Cours:MiniQ
Corrections uniquement accessibles aux enseignants
Sommaire
[masquer]documentations
programmation avec arduino
| ||
 |
|
 |
En résumé :
- La carte Arduino équivalente est la : Arduino nano /w ATMega328
- Le programmateur d'origine est : AVR ISP
- Le port série est en général /dev/ACMO sous Linux
Description et utilisation des commandes moteurs
L'environnement Arduino vous propose une librairie native pour faire de la MLI. Il s'agit de la primitive analogWrite() que nous allons apprendre à utiliser maintenant.
Utilisation de analogWrite() pour la MLI
La MLI est essentiellement utilisée pour la puissance des moteurs. C'est ce que nous alllons détailler maintenant.
Commande des deux moteurs
Le timer 0 comme les autres timers permet une commande simultanée de deux moteurs. Puisqu'il s'agit du timer 0, les deux bits associés sont OC0A en PD6 (bit 6 du PORTD) et OC0B en PD5. Si le concepteur du robot a cablé ces deux bits comme commande des moteurs il est naturel d'utiliser le timer 0. C'est le cas comme on peut le voir sur le schéma de principe. Vous voyez bien PD5 connecté à EN1... mais par contre PD6 connecté à IN2. C'est forcément une erreur : PD6 doit être connecté à EN2.
Le tableau donne les positions de ces 4 signaux :
| Moteur | INx | ENx |
|---|---|---|
| 1 | PD4 (Arduino 4) | PD5 (Arduino 5) |
| 2 | PD7 (Arduino 7) | PD6 (Arduino 6) |
Côté puissance, la commande nécessite deux signaux :
- ENi qui reçoit donc la MLI
- INi qui détermine le sens de rotation
avec i=1 ou i=2 pour chacun des moteurs.
Le code d'utilisation de la PWM (ou MLI) est examiné maintenant.
Code pour utiliser les moteurs
Il est possible de trouver des exemples de programmation du robot. A partir de ces exemples nous présentons un code source légèrement modifié pour être conforme au schéma de principe. Nous avons aussi renommé les paramètres pour savoir qui est moteur gauche et droit et diminué aussi leur taille.
// les constantes FORW et BACK sont définies dans l'exercice 1
void Motor_Control(char MD_DIR,char MD_EN,char MG_DIR,char MG_EN){
//////////MGauche////////////////////////
if(MG_DIR==FORW)//M1 motor direction
digitalWrite(IN1,FORW);//forward
else
digitalWrite(IN1,BACK);//back
analogWrite(EN1,MG_EN);//set speed
///////////MDroit//////////////////////
if(MD_DIR==FORW)
digitalWrite(IN2,FORW);
else
digitalWrite(IN2,BACK);
analogWrite(EN2,MD_EN);
}
Le caractère 'D' désigne la droite. Le caractère 'G' désigne la gauche. Ainsi les deux premiers paramètres de "motor_Control" désignent le moteur droit avec MD_DIR ne prenant que les valeurs "FORW" pour marche avant et "BACK" pour marche arrière. "MD_EN" quant à lui, prend des valeurs entre 0 et 255. Il désigne un rapport cyclique. S'il vaut 80, alors le rapport cyclique est de 80/255 ! Plus le rapport cyclique est grand plus le moteur tourne vite !
Travail à faire : Exercice 1 (arduino)
Vous devez réaliser une commande du robot pour qu'il réalise un huit (deux cercles connectés par un point). Vous choisirez le rayon de vos cercles en jouant sur les rapports cycliques. Puis vous jouerez sur les temporisations pour gérer tout cela. Bien entendu, le fait qu'il n'y ait pas d'information de retour sur l'endroit où vous êtes complique un peu la mise au point.
1°) Voici un code complet d'initialisation et d'utilisation des moteurs :
#define EN1 6//pin for run the right motor
#define IN1 7//pin for control right motor direction
#define EN2 5//pin for run the left motor
#define IN2 4//pin for control left motor direction
#define FORW 1//go forward
#define BACK 0//go back
void setup(){
pinMode(EN1,OUTPUT);
pinMode(EN2,OUTPUT);
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
}
void Motor_Control(char MD_DIR,char MD_EN,char MG_DIR,char MG_EN){
//////////MGauche////////////////////////
if(MG_DIR==FORW)//M1 motor direction
digitalWrite(IN1,FORW);//forward
else
digitalWrite(IN1,BACK);//back
analogWrite(EN1,MG_EN);//set speed
///////////MDroit//////////////////////
if(MD_DIR==FORW)
digitalWrite(IN2,FORW);
else
digitalWrite(IN2,BACK);
analogWrite(EN2,MD_EN);
}
void loop(){
Motor_Control(1,100,1,100);
delay(100);
Motor_Control(0,0,0,0);
delay(1000);
Motor_Control(0,100,0,100);
delay(100);
Motor_Control(0,0,0,0);
delay(1000);
while(1);
}
Modifier ce code pour réaliser un simple cercle. Seule la partie loop est à modifier !
Attention : Vos essais seront faits sur la table de travail.
- Il y a des risques de chutes pour le robot qui ne s'en remettrait pas ! Soyez donc prudents !
- Ne mettez pas de vitesses excessives ! Pas au-delà de 100 pour le rapport cyclique !
- N'oubliez pas que votre câble USB n'est pas infini !
- N'oubliez pas que loop est appelé sans arrêt. Un moyen de coincer le programme est de faire la boucle infinie de l'exemple (le "while(1);" final du "loop()" !)
2°) NE PAS FAIRE CETTE QUESTION 2. PASSEZ A LA QUESTION 3. L'inconvénient du code présenté plus haut est que pendant que les moteurs tournent le micro-contrôleur ne peut rien faire d'autre. Nous allons donc modifier profondément notre philosophie d'écriture du code.
// définition d'une durée de 2,4s
#define temps_ARRET 2400
void loop(){
unsigned long time = millis();
static char rapport_cyclique,avant;
if ((time>1000)&&(time<1200)){
rapport_cyclique = 100;avant=FORW;
}
if ((time>1200)&&(time<2200)) {
rapport_cyclique = 0;avant=FORW;
}
if ((time>2200)&&(time<2400)) {
rapport_cyclique = 100;avant=BACK;
}
//****** c'est ici qu'on arrete tout
if (time > temps_ARRET){ // au bout de 20s
rapport_cyclique = 0;avant=1;
}
Motor_Control(avant,rapport_cyclique,avant,rapport_cyclique);
}
On voit que le code n'est pas bloqué par un "while(1)" et que les actions à entreprendre sont faites en fonction du temps courant qui est obtenu avec "millis()". On voit aussi qu'en fin de boucle on a prévu un arrêt des moteurs. Ceci est important à réaliser systématiquement. Réaliser le cercle de la première question avec cette technique. (Réalisation optionnelle dans un premier temps)
3°) Réaliser maintenant votre huit (deux cercles se touchant par un point). On pourra réaliser tout cela dans le noir avec une prise de trajectoire par appareil photo à pose longue.
4°) Étude du rayon du cercle (NE PAS FAIRE).
Nous avons vu en cours que Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): R(t) = \frac{L}{2} \frac{v_g(t)+v_d(t)}{v_g(t)-v_d(t)}
Essayez d'étudier si le rayon peut s'écrire Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): R = K \frac{\alpha_g+\alpha_d}{\alpha_g-\alpha_d} où Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \alpha_g et Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \alpha_d désignent le rapport cyclique des moteurs gauche et droit (qui varient entre 0 et 255) et K est une constante à déterminer. Une des difficultés de cette question est de trouver une méthode pour essayer d'évaluer le rayon du cercle.
Remarque : cette quatrième question n'a pas été réalisée avec les étudiants.
5°) Vous pouvez chercher à innover sur les trajectoires. Des étudiants, par exemple, ont mis deux obstales et cherché à réaliser un créneau entre ces deux obstacles. Faites-vous plaisir. Il vous faut à tout prix comprendre comment on va en ligne droite et comment on tourne.
Utilisation de l'infra-rouge pour détecter des obstacles
Il y a plusieurs détecteurs infra-rouges dans le robot mini-Q. Nous allons commencer par étudier ceux qui sont sur l'avant du robot et qui sont donc destinés à détecter des obstacles.
Utiliser les détecteurs infra-rouge frontaux
Le robot dispose de cinq détecteurs infra-rouge pour le suivi de lignes. Ce n'est pas ces détecteurs que nous allons étudier mais un détecteur et deux émetteurs dirigés respectivement vers la droite et vers la gauche. Cela devrait nous permettre de détecter des obstacles. Le détecteur est relié à INT0/PB0 et peut donc être géré par une interruption.
L'idée générale est d'envoyer un train d'impulsions en émission et de compter ce que l'on reçoit.
Code d'émission
Vous pouvez utiliser le code suivant :
// train d'impulsion Gauche
void L_Send40KHZ(void)//left ir transmitter sends 40kHZ pulse
{
int i;
for(i=0;i<24;i++)
{
digitalWrite(L_IR,LOW);
delayMicroseconds(8);
digitalWrite(L_IR,HIGH);
delayMicroseconds(8);
}
}
//train d'impulsion Droit
void R_Send40KHZ(void)//right ir transmitter sends 40kHZ pulse
{
int i;
for(i=0;i<24;i++)
{
digitalWrite(R_IR,LOW);
delayMicroseconds(8);
digitalWrite(R_IR,HIGH);
delayMicroseconds(8);
}
}
Si vous regardez attentivement le schéma de principe vous déduisez facilement que l'émission se fait à l'aide d'un zéro. Ce code nécessite les définitions suivantes
#define IR_IN 8//IR receiver pin
#define L_IR 9//left ir transmitter pin
#define R_IR 10//right ir transmitter pin
Réception par interruption
L'interruption est d'abord initialisée à l'aide du code :
void pcint0_init(void)
{
PCICR = 0X01;
PCMSK0 = 0X01;//Autorisation de l'interruption INT0
}
Le code de l'interruption est tout simple :
volatile unsigned int count=0;
ISR(PCINT0_vect)
{
count++;//on incrémente le compteur pour chaque impulsion reçue
}
setup() associé
Les lignes de code associé à la détection infra-rouge à mettre dans la partie setup() du programme sont :
pinMode(L_IR,OUTPUT);//init the left transmitter pin
pinMode(R_IR,OUTPUT);//init the right transmitter pin
pinMode(IR_IN,INPUT);//init the ir receiver pin
digitalWrite(R_IR,HIGH);
digitalWrite(L_IR,HIGH);
pcint0_init();
sei();
Un code pour éviter les obstacles
Tout ce qui précède peut être utilisé pour détecter et donc éviter les obstacles. L'idée générale est très simple :
- on initialise le compteur "count" à 0
- On émet à droite 24 impulsions plusieurs fois, on compte ensuite ce que l'on reçoit (cela est réalisé automatiquement à l'aide d'une interruption).
- si c'est supérieur à 20, on recule suffisamment on tourne du bon côté et on repart en marche avant.
- on initialise le compteur à 0
- On émet à gauche 24 impulsions plusieurs fois, on compte ce que l'on reçoit.
- si c'est supérieur à 20, on recule suffisamment on tourne du bon côté et on repart en marche avant.
La mise au point de ce code peut prendre beaucoup de temps.
Indications : le code pour émettre plusieurs fois peut être par exemple :
count = 0;
for(i=0;i<20;i++) { //left transmitter sends 20 pulses
L_Send40KHZ();
delayMicroseconds(600);
}
if(count>DISTANCE_IR)//if recieved a lot pulse , it means there's a obstacle
Ce code émet 20 fois 24 impulsions.
Le DISTANCE_IR du "if(count>DISTANCE_IR)" peut être défini par une constante. Divers essais chez moi on montré que la valeur 35 est un bon point de départ. Si vous augmentez cette valeur vous détecterez les obstacles plus près eet si vous la diminuez ce sera le contraire. Il est défini comme une constante DISTANCE_IR dans le code de la section suivante et nous vous encourageons très fortement d'utiliser cette technique pour la mise au point.
Travail à faire : exercice 2
Vous devez mettre au point un code qui permette de ne jamais percuter des obstacles, c'est à dire qui évite les obstacles qu'il détecte à droite comme à gauche.
Indications : vous n'avez plus à gérer de temps maintenant. Autrement dit la technique utilisant "millis()" n'est plus nécessaire. "delay()" peut suffire car votre programme n'est plus sensé s'arrêter. Le code complet du setup() vous est donné ainsi que le code de loop() pour vous montrer l'architecture du programme recherché :
#define EN1 6//pin for run the right motor
#define IN1 7//pin for control right motor direction
#define EN2 5//pin for run the left motor
#define IN2 4//pin for control left motor direction
#define FORW 1//go forward
#define BACK 0//go back
#define IR_IN 8//IR receiver pin
#define L_IR 9//left ir transmitter pin
#define R_IR 10//right ir transmitter pin
#define SPEED 60
#define DISTANCE_IR 35
int count;//count the motor speed pulse
void setup()
{
char i;
for(i=4;i<=7;i++)//init the motor driver pins
{
pinMode(i,OUTPUT);
}
pinMode(L_IR,OUTPUT);//init the left transmitter pin
pinMode(R_IR,OUTPUT);//init the right transmitter pin
pinMode(IR_IN,INPUT);//init the ir receiver pin
digitalWrite(R_IR,HIGH);
digitalWrite(L_IR,HIGH);
pcint0_init();
sei(); //enable the interrupt
Motor_Control(FORW,SPEED,FORW,SPEED);//run motor
}
void loop() {
Eviter_Obstacle();//obstacle avoidance
}
Ce code doit être naturellement complété par le code donné dans la section Utiliser les détecteurs infra-rouge frontaux ainsi que par l'écriture du sous-programme "Eviter_Obstacle()" qui est le cœur même de ce que l'on cherche à faire. Vous pouvez noter qu'une constante SPEED est définie à 60 ce qui évite de faire de la casse. Une constante DISTANCE_IR est définie à 35 et devra être utilisée dans le test après réception.
La mise au point de la détection infra-rouge peut se faire sans mouvement. On utilisera le robot couché sur le côté et un seul moteur celui qui n'est pas sur la table, par exemple, pour tester quand se fait la détection. On le fait avancer sauf quand il y a détection.
Les tests se feront dans le labyrinthe NAO (salle G006).
Correction uniquement accessible aux enseignants de l'exercice 2 dans cette autre page
Utilisation de l'infra-rouge pour suivre une ligne
Le robot mini-Q est équipé de cinq couples leds/photo-transistors (infra-rouge) dirigés vers le bas. Ils sont destinés à mesurer la réflexion du support sur lequel roule le robot. Si vous prenez un support blanc et que vous fixez un scotch noir vous arriverez à détecter le support noir qui n'a pas la même réflectance infra-rouge que le support blanc.
La mise au point sera difficile car sur le robot que nous avons essayé les sensibilités des capteurs ne sont pas identiques. La calibrage de chacun des capteurs peut se faire par le programme simple :
int data[5]={0X00,0X00,0X00,0X00,0x00};//save the analog value
void setup(){
Serial.begin(9600);
}
void Read_IRLine(void){ //read the analog value
// les capteurs A0, A1 sont peu sensibles sur le robot essayé !!!
data[0]=analogRead(A0); // gauche
data[1]=analogRead(A1);
data[2]=analogRead(A2);
data[3]=analogRead(A3);
data[4]=analogRead(A7); // droite
}
void loop(){
Read_IRLine();
for (char i=0;i<5;i++){
Serial.print(data[i]);
Serial.print(" ");
}
Serial.println(" ");
delay(1000);
}
qui affichera toutes les secondes les valeurs des 5 photo-transistors infra-rouges.
Travail à réaliser : Exercice 3
Le travail à réaliser se décompose en plusieurs étapes :
On vous demande de calibrer vos cinq capteurs avec une plaque blanche de test comprenant une piste en chatterton noir.
Ce travail consiste à utiliser la liaison série pour calibrer vos 5 capteurs. Calibrer veut dire ici, trouver les seuils en-dessous desquels on considère que l'on a du noir. Ces seuils ne sont pas tous identiques pour tous les robots et pour un même robot pour tous les capteurs. Pour les trouver, vous positionnez une fois le capteur sur du blanc, une fois sur du noir et le seuil sera à mi-chemin.
Un moyen de faire cela de manière assez facile à ajuster consiste à définir des constantes comme ci-dessous :
#define SEUILGG 985
#define SEUILG 970
#define SEUIL 750
#define SEUILD 700
#define SEUILDD 750
GG veut dire le plus à gauche, DD le plus à droite.
Une fois les seuils de chaque capteur trouvé, convertir l'ensemble des données en un nombre binaire sur 5 bits. Le bit de poids faible sera à 1 si une ligne noire est présente sous le capteur A0 et ainsi de suite jusqu'au bit b4. Tester de nouveau avec un affichage binaire. Commencez ensuite à lister les valeurs aberrantes et les valeurs possibles de ce nombre binaire.
Indication : Serial.print(78, BIN) donne "1001110"
Le prototype de "Read_IRLine sera changé comme :
char Read_IRLine(void)
pour en faire une fonction qui retourne un nombre binaire.
Indication : La conversion binaire vous est partiellement donnée maintenant, mais il vous faut adapter les seuils de vos capteurs... et tester...
//****** seuils très dépendants des ROBOTs
#define SEUILGG 941
#define SEUILG 500
#define SEUIL 400
#define SEUILD 880
#define SEUILDD 600
char Read_IRLine(void){ //read the analog value
char result=0;
data[0]=analogRead(A0); // gauche manque sensibilité sur mon robot
data[1]=analogRead(A1); // manque sensibilité sur mon robot
data[2]=analogRead(A2);
data[3]=analogRead(A3);
data[4]=analogRead(A7); // droite
if (data[0] < SEUILGG) result |= 0x01; else result &= 0xFE;
if (data[1] < SEUILG) result |= else result &= ;
if (data[2] < SEUIL) result |= else result &= ;
if (data[3] < SEUILD) result |= else result &= ;
if (data[4] < SEUILDD) result |= else result &= ;
return result;
}
Vous commencerez ensuite à essayer de réaliser un programme qui suit une ligne fermée en avançant. Le principe est d'accélérer le bon moteur quand on dévie de la ligne.
Indication : vous pourrez utiliser un "switch" en C qui n'utilise que les valeurs autorisées de Read_IRLine. La valeur 0x00 sera traitée comme valeur aberrante dans un premier temps.
Travail à réaliser : Exercice 3 (2eme version)
Le travail de calibrage reste identique à l'exercice 3 première version.
Mais plutôt que de convertir en binaire, on réalise une pondération +4, +2, 0, -2, -4 sur les 5 capteurs. Puis, à partir des résultats on calcule la somme pondérée que l'on divise par le nombre de 1. Ce résultat s'appelle deviation, par exemple.
Ce résultat est alors multiplié par une constante K (qu'il vous faut trouver expérimentalement) pour donner une variation de vitesse deltaV. Ainsi :
- deltaV = deviation * K
On calcule ensuite
- vitesseDroite = vitesseTranslation - deltaV
- vitesseGauche = vitesseTranslation + deltaV
Il est bon de s'arranger pour que vitesseTranslation ne soit pas trop élevé.
Travail d'évaluation
Ce travail d'évaluation a été déporté dans une page protégée.
Utilisation de l'infra-rouge pour savoir où l'on est ?
Chaque roue est équipée de capteurs infra-rouges destinés à mesurer, la vitesse ou la position (nombre de tours) de chacune des deux roues.
Pourquoi le problème est difficile
Avant de se poser des questions il faut essayer de bien comprendre ce que veut dire connaître exactement l'état du robot. Celui-ci est donné par trois paramètres même si l'on est en deux dimensions : Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): (x,y,\theta) . x et y sont naturellement le repérage de la position sur les deux axes x et y et Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \theta est l'orientation du robot par rapport à l'axe des x. Regardez la figure ci-contre pour vous convaincre de la pertinence de ces trois données.
Imaginons que vous ayez fait déplacer votre robot pendant un temps T quelconque. Vous relevez les compteurs de chacune des roues et convertissez en centimètres parcourus et trouvez deux nombres A et B. Est-il possible d'en déduire la nouvelle position (et orientation) sachant que l'on connaît exactement la position de départ ? Autre manière de poser le problème : je connais parfaitement Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): (x_d,y_d,\theta_d) ainsi que la distance parcourue par chacune des roues est-il possible d'en déduire Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): (x_f,y_f,\theta_f) ? Malheureusement la réponse à cette question est partiellement négative ! Il n'est possible que d'avoir Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \theta_f mais pas le reste.
Tout cela est résumé simplement par la figure ci-contre. Dans cette figure, les deux longueurs (en pointillés) rouges sont identiques (et égales à A) et les deux longueurs (en pointillés) bleues sont elles aussi identiques (et égales à B) et pourtant on obtient deux points d'arrivées différents. Seule l'orientation finale est identique. Cela montre qu'aucune formule ne permet le calcul de xf à partir de xd, A et B.
Doit-on abandonner l'idée de trouver le point final à partir du point de départ ? Non, heureusement. En fait le problème présenté devient possible si les deux distances parcourues A et B deviennent différentielles (c'est à dire toutes petites). C'est quand même une mauvaise nouvelle malgré tout. Cela veut dire qu'il nous faut mémoriser les distances parcourues par chacune des roues à des instants précis et assez fréquemment pour se rapprocher de l'idée mathématique de différentielle. Nous essaierons dans un premier temps dix mesures par seconde. Nous utiliserons des compteurs sur 8 bits en complément à deux... et dix fois par seconde nous mémoriserons donc les valeurs des deux compteurs (droit et gauche) dans une mémoire.
Le problème de la gestion de la mémoire sera examiné plus loin. Pour ce qui est de la taille, une mémoire de 2ko permet de mémoriser 1024 valeurs de 16 bits correspondant aux deux compteurs (roue droite et roue gauche). 1024 valeurs correspondent à 100 s (car 10 enregistrements par seconde) soit une minute et demi. Pas grandiose ! mais suffisant pour examiner quelques problèmes intéressants avec des étudiants.
Travail sur les rotations de chacune des roues: exercice 4
Présentation théorique du problème
On peut montrer facilement que la distance parcourue et l'angle sont donnés par :
Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): D(t)=\frac{1}{2}(l_d(t)+l_g(t))
D(t) est la distance parcourue par le centre de l'axe des moteurs, Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): l_g et Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): l_d représentent les distances parcourues par respectivement la roue gauche et la roue droite. Quant à l'angle, il est donné par :
Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \alpha(t)= \frac{l_d(t)-l_g(t)} {L}
si L est l’entre-axe des roues.
En temps que variations, ces formules deviennent facilement :
Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \Delta D(t)=\frac{1}{2}(\Delta l_d(t)+\Delta l_g(t))
Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \Delta \alpha(t)= \frac{\Delta l_d(t)-\Delta l_g(t)} {L}
L'intégration se fait alors par :
Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): \alpha(t+\Delta t)=\alpha(t)+\Delta \alpha(t)
Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): x(t+\Delta t)= x(t)+ \Delta D(t) cos(\alpha(t+\Delta t))
Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): y(t+\Delta t)= y(t)+ \Delta D(t) sin(\alpha(t+\Delta t))
On peut déterminer les variations de l'angle et de la position du robot comme le squelette de programme le fait ci-après :
dAlpha = (dRight-dLeft); //variation de l'angle
dDelta = (dRight+dLeft)/2; //variation de l'avancement
//conversion en radian
alpha += dAlpha / entraxeEnTick; //calcul des décalages selon X et Y
dX = cosf(alpha) * dDelta;
dY = sinf(alpha) * dDelta;
//conversion de la position en mètre
X += dX / tickParMetre;
Y += dY / tickParMetre;
Ce code devra être adapté à vos unités (voir plus loin ce sont des cm qui sont demandés)
ATTENTION : Le calcul du cosinus et du sinus sont des opérations lourdes dans les architectures 8 bits. Ils ne devront ainsi en aucun cas se trouver dans le code d'une interruption.
Code de départ
Nous avons déjà utilisé l'interruption INT0 dans le premier TP du module M2103. Nous allons lui ajouter l'interruption INT1. INT0 sera responsable du comptage du moteur droit tandis que INT1 sera responsable du comptage sur le moteur droit.
// variables globales
int count_r=0,count_l=0;
//encoder value
void interrupt01_init(void)
{
EICRA = 0X0F;
EIMSK = 0X03;
}
ISR(INT0_vect)//motor encoder interrupt
{
if(++count_r==120)
count_r=0;
}
ISR(INT1_vect)
{
if(++count_l==120)
count_l=0;
}
Travail à faire
On désire noter les positions de chacun des compteurs 10 fois par seconde. Pour cela on ajoutera une gestion du timer2 par interruption. Cette interruption sera responsable de prendre la valeur des compteurs droits et gauches et de les remettre à zéro ainsi que de les mémoriser dans un tableau de 100 cases (10 secondes d'enregistrement). Le calcul des positions et orientations Échec d'analyse (L’exécutable <code>texvc</code> est introuvable. Lisez math/README pour le configurer.): (x,y,\theta) sera fait une fois le mouvement terminé.
Modifier le code ci-dessus pour prendre en compte le cahier des charges décrit ci-dessus.Les coordonnées x et y seront exprimées en cm et l'angle en radians. Tester l'ensemble avec le code ci-dessous et modifiez ce code pour tester plus avant votre calcul.
void loop(){
Motor_Control(1,60,1,60);
delay(1000);
Motor_Control(0,0,0,0);
delay(100);
Motor_Control(1,70,1,50);
delay(1000);
Motor_Control(0,0,0,0);
delay(100);
Motor_Control(1,50,1,70);
delay(500);
Motor_Control(0,0,0,0);
delay(100);
Motor_Control(1,70,1,0);
delay(500);
Motor_Control(0,0,0,0);
delay(100);
Motor_Control(1,60,1,60);
delay(1000);
Motor_Control(0,0,0,0);
delay(100);
// Il est grand temps de calculer la trajectoire maintenant
while(1);
}
Remarque : les mouvements sont réalisés en marche avant seulement car les capteurs infra-rouge des roues sont incapables de distinguer la marche avant de la marche arrière !
