Outils pour utilisateurs

Outils du site


cppu_moteur

Différences

Ci-dessous, les différences entre deux révisions de la page.

Lien vers cette vue comparative

Les deux révisions précédentes Révision précédente
Prochaine révision
Révision précédente
cppu_moteur [2020/10/14 16:17]
bvandepo [Achat de matériel]
cppu_moteur [2021/02/19 20:20] (Version actuelle)
Ligne 14: Ligne 14:
 ===== Partie 1 - Présentation générale de la maquette ===== ===== Partie 1 - Présentation générale de la maquette =====
  
-===Contexte le servomoteur===+=== Définition L'énergie ===
  
-Un servomoteur ..+L'énergie est la somme dans le temps des contributions instantanées de la puissance. 
 +Cela veut dire que l'énergie est exprimé par le produit de la puissance et le temps :  
 +  
 +<latex>
  
-Présenter le servomoteur et expliquer comment il se pilote: https://www.tutorialspoint.com/arduino/arduino_servo_motor.htm+\begin{center}  
 + 
 +\begin{equation} E_{X} = P_{X} \cdot temps  \end{equation} 
 + 
 +\end{center} 
 +</latex> 
 + 
 +Où :  
 +  * E c'est l'énergie en joule ou watt.heure 
 +  * P c'est la puissance en watt 
 +  * temps est le temps en secondes 
 +  * X représente un domaine de la physique (électrique, chimique, mécanique et etc) 
 + 
 + 
 +=== Définition - énergie électrique et énergie mécanique === 
 + 
 +Les génies électrique et mécaniques se consacrent à créer des sources qui délivrent leur puissance correspondante.  
 + 
 +La puissance électrique est décrite par l'équation suivante : 
 + 
 +<latex> 
 + 
 +\begin{center}  
 + 
 +\begin{equation} P_{électrique} = V \cdot i  \end{equation} 
 + 
 +\end{center} 
 +</latex> 
 + 
 +Où :  
 +  * V c'est la tension en volts 
 +  * i c'est le courant en ampères 
 + 
 +La tension électrique représente la différence d'accumulation des charges qui existent entre deux fils électriques. En courant continu on parle suivant du pôle positif (+) et du pôle négatif (-). Le courant électrique représente la quantité de charges qui sortent d'un fil pour aller vers l'autre. La puissance électrique est dite positive lorsque le courant sort du fil positif vers le négatif.  
 + 
 +Une source électrique capable de délivrer un grand courant (donc une grande quantité de charges) sans pour autant présenter une diminution dans sa tension (la différence de concentration de charges) a une très forte puissance nominale. Si cette source est capable de délivrer cette puissance pendant un temps très long, cette source a une grande capacité d'énergie. 
 + 
 +La puissance mécanique est décrite par l'équation suivante : 
 + 
 +<latex> 
 + 
 +\begin{center}  
 + 
 +\begin{equation} P_{mécanique} = \Gamma_{mot} \cdot \omega_{mot} \end{equation} 
 + 
 +\end{center} 
 +</latex> 
 + 
 +Où :  
 +  * ω c'est la vitesse angulaire en radian par secondes  
 +  * Γ c'est le couple mécanique en Newton mètre 
 + 
 +La vitesse angulaire représente l'allure dont un moteur tourne. Dans certains domaines on parle en rotations par minutes ou RPM.  Le couple est la capacité d'un moteur à délivrer une force sur son axe. La puissance mécanique est dite positive lorsque le couple est dans le même sens que la vitesse.   
 + 
 +Une source mécanique capable de délivrer un couple (une force sur son axe) sans pour autant perdre de la vitesse angulaire (l'allure de ses rotations) a une très forte puissance nominale. Si cette source est capable de délivrer cette puissance pendant très longtemps, cette source a une grande capacité d'énergie.   
 + 
 + 
 +=== Définition - le moteur à courant continu === 
 + 
 +Un moteur à courant continu permet de convertir l'énergie électrique en énergie mécanique. Mathématiquement nous pouvons exprimer cela par le jeu d'équation ci-dessous :  
 + 
 +<latex> 
 + 
 +\begin{center}  
 + 
 +\begin{equation} E_{elec} = E_{meca}  \end{equation} 
 +\begin{equation} P_{elec} \cdot temps = P_{meca} \cdot temps  \end{equation} 
 +\begin{equation} P_{elec} = P_{meca}  \end{equation} 
 +\begin{equation} V_{mot} \cdot i_{mot} = \Gamma_{mot} \cdot \omega_{mot}  \end{equation} 
 + 
 +\end{center} 
 +</latex> 
 + 
 +Où:  
 +  * elec c'est électrique  
 +  * meca c'est mécanique  
 + 
 +Ce transfert de puissance est illustré par la figure ci-dessous :  
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/source_elec_meca_1.png}} 
 + 
 + 
 +A partir de maintenant nous pouvons réfléchir en terme de point d'opération ou point d'équilibre.  
 +Une fois que la source mécanique est connéctée à une charge, une puissance mécanique lui sera délivré.  
 +En pratique cela signifie que le point d'opération ou le point d'équilibre du système change. 
 +Cela implique des changements dans la tension, le courant, le couple et la vitesse. Nous pouvons vite être dans des situations très complexes à comprendre.   
 + 
 +Pour simplifier l'analyse, nous pouvons fixer des grandeur physiques. La tension peut suivant être considérée comme fixe pour une source électrique. La vitesse nominal d'un moteur est aussi suivant fixée. Cela est répresenté par la figure ci-dessous :  
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/source_elec_meca_charge_1.png}}  
 + 
 + 
 +Nous pouvons faire un parallèle entre les grandeur électriques et mécaniques.  
 + 
 +La tension électrique est liée à la vitesse de rotation. Pour une tension $V_{mot}$ donnée, le moteur tournera à une vitesse $\omega_{mot}$ spécifique.  
 + 
 +Le courant est lié au couple. Lorqu'un moteur est à vide, le couple sur son arbre est très faible (frottement internes), donc le courant délivré par la source électrique est aussi très faible. Lorsqu'un moteur est mis sous charge, le couple sur son axe augmente et la source électrique délivre un courant plus important.  
 + 
 + 
 +=== Contexte - le moteur à courant continu commandé === 
 + 
 +Un moteur à courant continu a un point d'opération qui dépend de sa tension d'entrée et de son couple résistant. Normalement nous ne pouvons pas anticiper comment la charge mécanique va se comporter, donc on a très peu de contrôle sur celle-ci. Il nous reste à contrôler la tension d'entrée du moteur.  
 + 
 +D'un point de vue global, nous pouvons illustrer un moteur commandé par le schémas blocs ci-dessous.  
 + 
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/moteur_1.png}} 
 + 
 + 
 +Le système appelé "commande" adapte la tension à l'entrée du moteur en fonction des signaux de commande qu'il reçoit. Il utilise une source d'énergie électrique extérieure dont la tension est constante et nommée VCC. 
 + 
 +Le bloc appelé "reducteur" permet de changer le rapport de vitesse angulaire et couple sans pour autant changer la puissance. Dans notre cas le "réducteur" diminue la vitesse et augmente le couple.   
 + 
 + 
 +=== Contexte - le servomoteur === 
 + 
 + 
 +Un servomoteur est un moteur à courant continu dont le mouvement de rotation est contrôlé en termes d'angle plutôt qu'en vitesse. Le schématique ci-dessous illustre ce type de moteur       
 + 
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/servo_mot_1.png}} 
 + 
 + 
 +Un potentiomètre est installé sur l'axe du servomoteur et lui donne une mesure sur l'angle de cet axe. Cette mesure représente une tension électrique qui varie avec l'angle de l'axe. La mesure set récupérée par le bloc de commande et utilisée pour contrôler la tension du moteur.  
 + 
 +Le fait de récupérer une mesure et l'utiliser pour contrôler un système est appelé "asservissement". Dans le cas de cet enseignement l'asservissement sera numérique.
  
-Dire qu'on en construit un sur la base d'un moteur DC réducté avec asservissement numérique. 
  
 === Schéma-blocs de la maquette à étudier === === Schéma-blocs de la maquette à étudier ===
Ligne 26: Ligne 153:
 La maquette étudiée lors de ses deux séances peut être résumée dans le schémas-bloc ci-dessous :  La maquette étudiée lors de ses deux séances peut être résumée dans le schémas-bloc ci-dessous : 
  
-Inclure une image avec les blocs+{{https://bvdp.inetdoc.net/files/cppu/maquette_1.png}}
  
-Décrire les blocs+Vous pouvez remarquer une différence par rapport au modèle de servomoteur présenté. Notre maquette découpe la commande dans deux parties, l'**Arduino+E/S** et le **shield moteur**.  
 + 
 +Le bloc Arduino+E/S reçoit la mesure du potentiomètre et génère des signaux de commande. Le shield du moteur reçoit les signaux de commande de l'Arduino et pilote la tension et le courant du moteur en fonction de ce signaux.  
 + 
 +Ce découplage s'impose du fait que la carte Arduino n'est pas capable de générer les courant nécessaires au pilotage du moteur.   
  
  
Ligne 44: Ligne 175:
 ==== Vue d'ensemble de la maquette ==== ==== Vue d'ensemble de la maquette ====
  
-Mettre une image qui montre chaque composant+Les composants de la maquette sont montrés dans l'image ci-dessous: 
 + 
 +//Mettre une image qui montre chaque composant//
  
  
Ligne 57: Ligne 190:
  
 ===== Partie 2 - Contrôle du moteur en vitesse, en sens et en position ===== ===== Partie 2 - Contrôle du moteur en vitesse, en sens et en position =====
 +
 +Pour prendre en main la maquette et comprendre le fonctionnement d'un moteur, nous allons procéder par petit pas. 
  
 ==== Contrôle de la vitesse du moteur ==== ==== Contrôle de la vitesse du moteur ====
  
-Décrire en pas à pas :  +La première étape de notre étude est nous familiariser avec le contrôle de vitesse du moteur à courant continu. 
-    * L'assemblage mécanique (pas de potentiomètre de position) +
-    * Quelques principes de code à faire pour étudier la vitesse +
-    * Des idées libres d'étude de la vitesse +
-        * Un contrôle par le potentiomètre de signal +
-        * Un contrôle par les entrées de la carte E/S +
-    * Une vidéo de fonctionnement+
  
-==== Contrôle du sens de rotation du moteur ====+=== Assemblage mécanique === 
 + 
 +Le schématique ci-dessus donne une vue d'ensemble du montage à faire pour cette étude.  
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/etude_vitesse_1.png}} 
 + 
 + 
 +=== Consignes en vitesse et en sens === 
 + 
 +La carte shield et sa librairie associé facilitent énormément l'envoi des consignes de vitesse. Les commandes les plus importantes sont résumées ci-dessous :   
 + 
 +<file cpp consignes_vitesse> 
 + 
 +#include <AFMotor.h>  //inclusion de la librairie du moteur à utiliser 
 + 
 +motor_id 1; //identifiant moteur 
 +AF_DCMotor motor(motor_id,MOTOR12_1KHZ);//, MOTOR12_8KHZ); // déclaration du moteur à utiliser 
 +motor.setSpeed(consigne_vitesse); // passage de la valeur de la vitesse à la variable 
 +motor.run(FORWARD); // choix du sense : avant      
 +motor.run(RELEASE); // choix du sense : axe libre       
 +motor.run(BACKWARD); // choix du sense : arrière        
 + 
 +</file> 
 + 
 +==== Etude en vitesse et en sens ==== 
 + 
 +A l'aide de la carte Arduino, créez un programme qui vous permet : 
 +  * d'envoyer une consigne en vitesse  
 +  * d'envoyer une consigne de sens 
 +  * de visualiser la consigne envoyé à l'aide de l'interface Serial  
 +  * de modifier vos consignes à l'aide de l'interface Serial 
 +  * de piloter la vitesse à l'aide d'un potentiomètre   
 + 
 +A l'aide de votre programme étudiez : 
 +  * la valeur de consigne pour l'amorçage de votre machine  
 +  * la valeur de consigne pour l'arrêt de votre machine 
 +  * la valeur de consignes ci-dessus en fonction du sens de rotation 
 +  * de piloter la vitesse à l'aide d'un potentiomètre   
 + 
 +Voici une vidéo exemple de fonctionnement :  
 + 
 +//mettre un lien vidéo ici//
  
-Décrire en pas à pas :  
-    * L'assemblage mécanique (pas de potentiomètre de position) 
-    * Quelques principes de code à faire pour étudier le sens 
-    * Des idées libres d'étude du sens 
-        * Un contrôle par les entrées de la carte E/S 
-    * Une vidéo de fonctionnement 
  
 ==== Contrôle de la position angulaire du moteur ==== ==== Contrôle de la position angulaire du moteur ====
  
-Décrire en pas à pas :  +Maintenant que nous pouvons contrôler la vitesse du moteur, nous allons étudier comment utiliser un potentiomètre et sa mesure de tension associée pour piloter la position angulaire de l'axe du moteur.  
-    L'assemblage mécanique (avec de potentiomètre de position) + 
-    * Quelques principes de code à faire pour étudier le sens +=== Assemblage mécanique === 
-    Des idées libres d'étude de la position + 
-        * Un contrôle par le potentiomètre de signal +Le schématique ci-dessus donne une vue d'ensemble du montage à faire pour cette étude.  
-        Un contrôle par les entrées de la carte E/S + 
-        Un contrôle automatique par génération de consigne par l'Arduino +{{https://bvdp.inetdoc.net/files/cppu/etude_angle_1.png}} 
-    Une vidéo de fonctionnement+ 
 +On remarque bien que le potentiomètre n'est pas encore connecté mécaniquement au moteur à courant continu. Nous allons utiliser ce découplage pour illustrer le fonctionnement de l'asservissement.  
 + 
 +=== Un peut de théorie sur l'asservissement === 
 + 
 +L'idée centrale à tout asservissement est de minimiser l'erreur entre une mesure et une consigne.  
 + 
 +S'il est possible d'asservir directement la variable d'intérêt (dans notre cas précédent c'était la vitesse) aucune astuce mathématique est nécessaire, comme nous avons vu précédement. Cela s'exprime par le schéma directe ci-dessous.  
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/asservissement_direct_1.png}} 
 + 
 +Si nous n'avons pas accès à la variable asservie, les choses sont plus compliqués. Ceci est le cas maintenant où nous souhaitons asservir un déplacement angulaire en agissant sur une vitesse angulaire. Cela s'exprime par le schéma indirect ci-dessous. 
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/asservissement_indirecte_1.png}} 
 + 
 +La difficulté est de trouver le moyen de "traduire" une consigne en déplacement dans une consigne en vitesse. Cette traduction est faite par un correcteur qui a pour but de minimiser l'erreur entre le déplacement de sortie et la consigne de déplacement. Cette "traduction" est illustrée par l'image ci-dessous.        
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/asservissement_total_1.png}} 
 + 
 + 
 +=== Le PID, un correcteur "magique" === 
 + 
 +La nature de la "traduction" faite par le correcteur dépend des types de variables en entrée en en sortie. Il existe une technique qui s'applique à tout cas de figure sans pour autant nécessiter de connaissances claires sur le système. Ce correcteur s'appelle PID. 
 + 
 +Le correcteur PID applique un traitement mathématique directement à l'erreur. Il consiste en trois termes différents qui vont appliquer des corrections différentes. Ces termes sont Kp, Ki et Kd comme montre l'image ci-dessous. 
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/theorie_PID_1.png}} 
 + 
 +Où :  
 +  Le terme KP s'applique directement à l'erreur: $K_p \cdot erreur$ 
 +  * Le terme Ki s'applique à la somme moyenne des erreurs: $K_i \cdot \dfrac{erreur[1]+...+erreur[n]}{n}$ 
 +  * Le terme Kd s'applique à la différence entre deux erreurs: $K_d \cdot (erreur[n]-erreur[1])$  
 + 
 + 
 +En pratique nous allons utiliser un PID numérique. Il consiste dans une suite de valeurs d'erreur appelée "buffer circulaire". Il est circulaire car le nombre de valeurs admissibles est limitées et lorsque le programme arrive à la fin de la suite il continue à enregistrer les valeurs par l'écrasement des premières valeurs mesurées. Le buffer circulaire est illustré par l'image ci-dessous.  
 + 
 + 
 +{{https://bvdp.inetdoc.net/files/cppu/buffer_circulaire_1.png}} 
 + 
 + 
 +Dans notre application, si nous sommes à la case 1 du buffer circulaire, les valeurs nécessaires à la correction son calculés comme montre l'image ci-dessous :  
 + 
 +  
 +{{https://bvdp.inetdoc.net/files/cppu/buffer_circulaire_pid_1.png}} 
 + 
 +Il nous reste de choisir les valeurs de Kp, Ki et Kd. Cela se fait par une méthode itérative qui sera décrite ci-dessous.   
 + 
 +==== Etude de l'asservissement angulaire ==== 
 + 
 +<file cpp PID_program> 
 +//-------------PID CONTROL------------------------------ 
 +// PID CONTROL VARIABLES DECLARATION 
 +#define TAB_SIZE 10 
 + 
 +int16_t erreur_tab[TAB_SIZE]; 
 + 
 +uint8_t ind_ecr_erreur_tab=0; 
 +int16_t integrale_erreur=0; 
 +int16_t derivee_erreur=0; 
 + 
 +int16_t Kd=0;  //de 0 à 50 
 +int16_t Ki=0;   
 +int16_t Kp=10; 
 + 
 +uint16_t consigne_posit =  0; // variable that holds the value of the position reference 
 +uint16_t posit =  0;  //variable that holds the value of the position measurement 
 +int16_t erreur = 0;   // variable that calculates the error 
 + 
 + 
 + 
 +void update_tab_erreur(int16_t new_erreur) 
 +
 +  //calcul intégrale 
 +  integrale_erreur=integrale_erreur-erreur_tab[ind_ecr_erreur_tab]; 
 +  integrale_erreur=integrale_erreur+new_erreur; 
 + 
 +  //calcul dérivée 
 +  uint8_t ind_lec_erreur_tab=ind_ecr_erreur_tab+5; 
 +  if (ind_lec_erreur_tab>=TAB_SIZE )  //gestion modulo 
 +  ind_lec_erreur_tab-=TAB_SIZE; 
 +  derivee_erreur=new_erreur-erreur_tab[ind_lec_erreur_tab]; 
 +   
 +  //maj tableau  
 +  erreur_tab[ind_ecr_erreur_tab]=new_erreur; 
 +  ind_ecr_erreur_tab++; 
 +  if (ind_ecr_erreur_tab>=TAB_SIZE) 
 +  ind_ecr_erreur_tab=0;   
 +}//end of update_tab_erreur 
 +//----------------------------------------------- 
 + 
 +void pid_correction(int16_t erreur){   
 +  int16_t commande=0;        //control signal 
 +  int16_t commande_max=250;  //maximum control signal 
 +   
 +  erreur  = erreur*Kp; 
 +  erreur += Kd*derivee_erreur; //quand l'erreur diminue, ce terme est négatif et permet d'éviter le dépassement 
 +  erreur += (Ki*integrale_erreur)/TAB_SIZE; 
 +   
 +  commande=abs(erreur);  // updates the control signal     
 +  if (commande>commande_max) commande=commande_max; // checks for saturation 
 +     
 +  if (commande<3) 
 +  { 
 +    motor.run(RELEASE); 
 +    motor.setSpeed(0); 
 +  } 
 +  else 
 +  if (erreur<0) 
 +  { 
 +    motor.run(FORWARD);        
 +    motor.setSpeed(commande); 
 +  } 
 +  else 
 +  { 
 +    motor.run(BACKWARD);        
 +    motor.setSpeed(commande); 
 +  }   
 +}//end of pid_correction 
 +//------------------------------------------------ 
 + 
 + 
 +</file> 
 + 
 +<file cpp PID_usage> 
 + 
 +void setup()  
 +{  
 +  // setup control table 
 +  for (int i=0;i<TAB_SIZE;i++) 
 +  erreur_tab[i]=0; 
 +
 + 
 +void loop()   
 +{  
 +    consigne_posit =  analogRead(A0);  //reads the position reference 
 +    posit =  analogRead(A1);           // reads the position 
 +    erreur = posit - consigne_posit;   // calculates the error 
 +    update_tab_erreur(erreur);         // updates the tab 
 +    pid_correction(erreur);            // updates the control 
 +     
 +    // Prints system variables 
 +    Serial.print(consigne_posit,DEC);  //prints the position reference 
 +    Serial.print("\t"); 
 +    Serial.print(erreur,DEC);          //prints the erro 
 +    Serial.print("\t"); 
 +    Serial.print(posit,DEC);           //prints the position 
 +    Serial.print("\t"); 
 +    Serial.println(""); 
 +    delay(10); 
 + 
 + 
 +
 +</file> 
 + 
 +A l'aide de la carte Arduino, et le squelette de programme avec le PID intégré écrivez un programme que vous permet: 
 +  * de changer les paramètres Kp, Ki et Kd  
 +  * d'envoyer une consigne de déplacement angulaire  
 +  * de monitorer l'angle par l'interface série  
 +  de modifier vos consignes à l'aide de l'interface série 
 +  * de monitorer l'erreur par l'interface série 
 +  * de récupérer la mesure d'angle à travers un deuxième potentiomètre  
 + 
 +A l'aide de votre programme étudiez : 
 +  * le comportement de votre moteur lorsque vous lui donnez une consigne angulaire  
 +  * le comportement de votre moteur lorsque vous agissez sur le deuxième potentiomètre 
 +  * l'erreur 
 +   
 +Voici une vidéo exemple de fonctionnement :  
 + 
 +//mettre un lien vidéo ici// 
  
  
Ligne 137: Ligne 471:
 ===Controle du moteur en vitesse et en sens=== ===Controle du moteur en vitesse et en sens===
  
-{{http://homepages.laas.fr/bvandepo/files/cppu/IMG_20200918_002851_crop_resize.jpg}}+{{https://bvdp.inetdoc.net/files/cppu/IMG_20200918_002851_crop_resize.jpg}}
  
  
 ===Controle du moteur en position=== ===Controle du moteur en position===
  
-{{http://homepages.laas.fr/bvandepo/files/cppu/IMG_20200924_130008_crop_resize.jpg}}+{{https://bvdp.inetdoc.net/files/cppu/IMG_20200924_130008_crop_resize.jpg}}
  
 ====Installation de la librarie pour le shield moteur==== ====Installation de la librarie pour le shield moteur====
Ligne 158: Ligne 492:
  
 ====Mettre ici des liens sur les fonctions de librairies Arduino à utiliser==== ====Mettre ici des liens sur les fonctions de librairies Arduino à utiliser====
 +
 +
 +
 +=== Exemple de code - Consigne en vitesse ===
 +
 +
 +<file cpp motor_shield_speed.ino>
 +//B. Vandeportaele 09/2020
 +//L. Villa 10/2020
 +
 +//Todo gestion du temps pour que l'asservissement ait des paramètres fixes/ contenu du programme
 +
 +#include <AFMotor.h>  
 +#include <Wire.h>
 +
 +//adresses codées sur 7bits
 +#define SLAVE_ADDR_8574_A 0x38+6
 +#define SLAVE_ADDR_8574_B 0x38+7
 +
 +
 +AF_DCMotor motor(1,MOTOR12_1KHZ);//, MOTOR12_8KHZ);
 +
 +int led = 13; //numéro de la broche Arduino pilotant la LED sur le shield PERIPH
 +
 +//////////////////////////////////////////
 +char readPort8574(char addr, char * ptr_value)
 +/*addr, l'adresse du PCF8574
 +ptr_value, pointeur pour renvoyer la valeur lue sur le port
 +retourne -1 si échec 0 sinon*/
 +{
 +    Wire.requestFrom((byte)addr, (byte)1);// demande la lecture d'1 octet depuis l'adresse du pérpiphérique
 +    if (Wire.available()==1) //si l'octet est disponible
 +    {
 +        (* ptr_value) = Wire.read(); // lire l'octet
 +        return 0;
 +    }
 +    else
 +    {
 +        (* ptr_value) =0; //valeur par défaut si le composant n'a pas acquité
 +        return -1;
 +    }
 +}
 +//////////////////////////////////////////
 +char writePort8574(char addr, char value)
 +/*addr, l'adresse du PCF8574
 +value, la valeur à écrire sur le port
 +retourne -1 si échec 0 sinon */
 +{
 +    Wire.beginTransmission(addr);//démarre la transmission avec l'adresse du pérpiphérique
 +    Wire.write((byte)value);     //envoie la donnée
 +    if (Wire.endTransmission()==0)      //stoppe la transmission
 +        return 0;
 +    else
 +        return -1;
 +}
 +//////////////////////////////////////////
 +
 +//unité en milliseconde 
 +uint32_t cadence_it=3;
 +uint32_t time_next_it=0;
 +
 +
 +
 +void setup() 
 +
 +  // initialize the digital pin as an output.
 +  pinMode(led, OUTPUT);   
 +  Serial.begin(115200);  // start serial port at 9600 bps:
 +        
 +  Wire.begin();        // joindre le bus i2c en tant que maître
 +  writePort8574( SLAVE_ADDR_8574_B , 0xff); //configure le composant B en entrée
 +  writePort8574(SLAVE_ADDR_8574_A,0); //allume les leds
 +  delay(1000);
 +  writePort8574(SLAVE_ADDR_8574_A,~0); //eteind les leds
 +  
 +}
 +
 +
 +void loop()  
 +{
 +
 +  controleur();
 +
 +}
 +
 +
 +void controleur()  
 +{  
 +byte consigne_vitesse;  // une variable pour récupérer la valeur de la vitesse
 +
 +readPort8574(SLAVE_ADDR_8574_B,&consigne_vitesse); // contrôle de la valeur de la vitesse
 +
 +writePort8574(SLAVE_ADDR_8574_A,~consigne_vitesse); // affichage de la valeur de la vitesse 
 +
 +Serial.println(consigne_vitesse); // affichage de la valeur de la vitesse
 +
 +motor.setSpeed(consigne_vitesse); // passage de la valeur de la vitesse à la variable
 +
 +
 +motor.run(FORWARD); // choix du sense : avant     
 +
 +delay(1000);  
 +
 +motor.run(RELEASE); // choix du sense : lâcher l'axe      
 +
 +delay(1000); 
 +
 +motor.run(BACKWARD); // choix du sense : arrière       
 +
 +delay(1000); 
 +
 +motor.run(RELEASE); // choix du sense : lâcher l'axe      
 +
 +delay(1000); 
 +
 +
 +</file>  
 +
  
  
Ligne 488: Ligne 940:
  
  
 +<file cpp interaction_serial.ino>
 +#include <Wire.h>
 +//adresses codées sur 7bits
 +#define SLAVE_ADDR_8574_A 0x3E
 +#define SLAVE_ADDR_8574_B 0x3F
 +
 +//////////////////////////////////////////
 +char readPort8574(char addr, char * ptr_value)
 +/*addr, l'adresse du PCF8574
 +  ptr_value, pointeur pour renvoyer la valeur lue sur le port
 +  retourne -1 si échec 0 sinon*/
 +{
 +  Wire.requestFrom((byte)addr, (byte)1);// demande la lecture d'1 octet depuis l'adresse du pérpiphérique
 +  if (Wire.available() == 1) //si l'octet est disponible
 +  {
 +    (* ptr_value) = Wire.read(); // lire l'octet
 +    return 0;
 +  }
 +  else
 +  {
 +    (* ptr_value) = 0; //valeur par défaut si le composant n'a pas acquité
 +    return -1;
 +  }
 +}
 +//////////////////////////////////////////
 +char writePort8574(char addr, char value)
 +/*addr, l'adresse du PCF8574
 +  value, la valeur à écrire sur le port
 +  retourne -1 si échec 0 sinon */
 +{
 +  Wire.beginTransmission((byte)addr);//démarre la transmission avec l'adresse du pérpiphérique
 +  Wire.write((byte)value);     //envoie la donnée
 +  if (Wire.endTransmission() == 0)    //stoppe la transmission
 +    return 0;
 +  else
 +    return -1;
 +}
 +//////////////////////////////////////////
 +void setup()
 +{
 +  Serial.begin(9600);  // start serial port at 9600 bps:
 +  Serial.print("Bonjour");
 +  Wire.begin();        // joindre le bus i2c en tant que maître
 +  writePort8574( SLAVE_ADDR_8574_B , 0xff); //configure le composant B en entrée
 +}
 +//////////////////////////////////////////
 +void loop()
 +{
 +  char  val;
 +
 +  if (Serial.available() > 0) {
 +    val = Serial.read();
 +    writePort8574(SLAVE_ADDR_8574_A, &val);
 +    switch (val) {
 +      case '0':
 +        break;
 +      case '1':
 +        break;
 +      case '2':
 +        break;
 +      default:
 +        break;
 +
 +    }
 +    Serial.println((int)val, DEC);
 +  }
 +  /*
 +
 +    if (readPort8574(SLAVE_ADDR_8574_B,&val)==0)
 +      {
 +    //      Serial.print("lecture  8574  OK, ");
 +         writePort8574(SLAVE_ADDR_8574_A,val);
 +      }
 +
 +    delay(100);*/
 +
 +}
 +</file>
 ====Modèles des pièces 3D==== ====Modèles des pièces 3D====
 <file cpp connecteur_moteur_potar_2.scad>  <file cpp connecteur_moteur_potar_2.scad> 
- 
 //B. Vandeportaele 09/2020 //B. Vandeportaele 09/2020
 //jeu=0.5; //pour connecteur //jeu=0.5; //pour connecteur
Ligne 496: Ligne 1025:
  
 ///////////////////////////// /////////////////////////////
-module tige_biseau(){+module tige_biseau(){ // coté moteur
 difference(){         difference(){        
 cylinder(h=20,d=5.5+jeu,$fn=120); cylinder(h=20,d=5.5+jeu,$fn=120);
Ligne 510: Ligne 1039:
  
 ///////////////////////////// /////////////////////////////
- module tige_biseau2(){+ module tige_biseau2(){ //coté potar
 difference(){         difference(){        
 cylinder(h=20,d=6+jeu,$fn=120); cylinder(h=20,d=6+jeu,$fn=120);
Ligne 520: Ligne 1049:
 ///////////////////////////// /////////////////////////////
 module connecteur(){ module connecteur(){
 +    rotate([180,0,0])
 difference(){ difference(){
-cylinder(h=20,d=13,$fn=120); +cylinder(h=16,d=11,$fn=120); 
-    translate([0,0,-10]) +    translate([0,0,-12]) 
-    tige_biseau2(); +    //tige_biseau2(); 
-    //#cylinder(h=20,d=6,$fn=120); +    #cylinder(h=20,d=6.40,$fn=120); 
-     translate([0,0,+12])+     translate([0,0,+7.95])
         tige_biseau();         tige_biseau();
 } }
 } }
 ///////////////////////////// /////////////////////////////
-module chape(ep=3,long=30){+module chape(angle=0,ep=3,long=30){
 difference(){ difference(){
 hull() hull()
 { {
-cylinder(h=3,d=10,$fn=120);+cylinder(h=ep,d=10,$fn=120);
   translate([long,0,0])   translate([long,0,0])
-cylinder(h=3,d=4,$fn=120);+cylinder(h=ep,d=4,$fn=120);
 } }
 translate([0,0,-10]) translate([0,0,-10])
-tige_biseau();+rotate([0,0,angle]) 
 +  tige_biseau();
 translate([long,0,-10]) translate([long,0,-10])
 cylinder(h=20,d=2,$fn=120); cylinder(h=20,d=2,$fn=120);
Ligne 552: Ligne 1083:
 h=21.4; h=21.4;
 ep=2; ep=2;
-entraxe=11;+entraxe=11.5;
 diamvis=2.8; diamvis=2.8;
-hauteur=36.5;+hauteur=21;
 union(){ union(){
     difference(){     difference(){
Ligne 573: Ligne 1104:
              translate([33.5-22,h/2,-50])                      translate([33.5-22,h/2,-50])        
                  #cylinder(h=100,d=8,$fn=180);                         #cylinder(h=100,d=8,$fn=180);       
 +            translate([33.5-22,h/2+8,-50])        
 +                 #cylinder(h=100,d=2.9,$fn=18);       
         }         }
 } }
 } }
 ///////////////////////////// /////////////////////////////
-//tige_biseau2();+ 
 + 
 +///////////////////////////// 
 +//pour 75mm de long+15mm partie male et - 7mm partie femelle 
 +module rallonge(longueur){ 
 +union(){ 
 +    difference(){ 
 +    cylinder(h=longueur,d=13,$fn=120); 
 +    translate([0,0,+longueur-7]) 
 +        //scale([1,1,5]) 
 +             tige_biseau(); 
 +
 + translate([0,0,-15]) 
 +        tige_biseau(); //de base il fait 20 de long 
 +
 + 
 +
 +///////////////////////////// 
 +//rotate([90,0,0])  rallonge(75); 
 +   //tige_biseau(); 
 +    //tige_biseau2();
 //connecteur(); //connecteur();
 //chape(); //chape();
-rotate([90,0,0]) 
-equerre(); 
  
 +
 +chape(90,4,40);
 +/*for  (i=[1:1:10]){
 +translate([0,i*15,0])
 +chape(90,4,20+i*10);
 +}
 +*/
 +//rotate([90,0,0]) equerre();
 +
 +//connecteur();
 </file> </file>
 +
 +
 +
  
 ===== Informations pratiques ===== ===== Informations pratiques =====
cppu_moteur.txt · Dernière modification: 2021/02/19 20:20 (modification externe)