RobotGEII 16-17

Présentation

Présentation du projet

Ce projet consiste à réaliser un robot pour participer à la coupe de robotique des GEII. Pour fabriquer ce robot nous devons respecter un cahier des charges défini par le règlement. Le principe de ce concours est d'aller le plus rapidement possible d'un coin à l'autre d'une arène carrée de 8 mètres de coté, tout en esquivant les obstacles jonchant le parcours et les autres robots. Une fois arrivé, le robot doit percer un ballon qui lui est attaché en début de course.

Cahier des charges fonctionnel

Schéma fonctionnel de degré II

Schéma fonctionnelle 1er degres

Solutions techniques

Pour fabriquer ce robot nous avions plusieurs choix pour procéder à l’évitement des obstacles, ainsi qu'au guidage vers la zone. Nous avons choisi pour guider le robot un système hybride composé de roues codeuses et d'une caméra CMUCam Pixy, spécialisée dans la reconnaissance d'objets. La roue codeuse permettra d'effectuer le début du parcours, et sera remplacée par la caméra, plus précise, une fois la distance de détection atteinte. L'évitement des obstacles sera assuré par trois capteurs infrarouges SHARP GP2Y0A21YK0F, permettant la détection d'objets jusqu'à 80 centimètres de distance, qui, placés à l'avant du robot, permettront l'esquive si un obstacle est rencontré.

Etude et Réalisation des Differentes Parties

Alimentation et Régulation

Batterie

La batterie est imposée :

Tension	12 V
Capacité	7 Ah

Batterie

Moteurs de roues

Les moteurs sont imposés.

• Caracteristique du moteur:

Marque	Dunkermotoren G 42*25
Tension	15V
In	1.45 A
Ifm	10.9A
Rpm	3300 tr/mn

Dunkermotoren G 42*25

• Tests en conditions réeles sur les moteurs montés sur le robot ( U=12V ) :
  

Résistance électrique	4 ohm
Courant à vide	0.28 A
Courant en charge nominale robot	0.7A
Courant en charge au démarage robot	1.6A

• Consommation maximum en courant pour les 2 moteurs du robot :
  

Imax = 2*1.6A = 3.2A

Régulation de la tension d'alimentation

• Les besoins :

Pour Arduino MEGA:

Caractéristiques techniques :

Operating Voltage	5V
Input Voltage (recommended)	7-12V
Input Voltage (limit)	6-20V
Total output current MAX	800mA

On constate qu'il est possible d'alimenter la carte Arduino MEGA directement avec la tension de la batterie ( 12.8V chargée).
Ce n'est toutefois pas recommandé, car le régulateur intégré dans l'Arduino chaufferait, ce qui pourrait endommager le microcontrôleur.

Solutions Alimentation Arduino MEGA:

1. Tension d'alimentation inférieure à 12 V
   
2. Tension régulé de 5V qu'on fait venir directement sur les pattes VCC d'Arduino:
   

Le courant maximum requis: 800mA

Pour les Moteurs de Roues :

1. Tension maximum requise: 15V
   
2. Courant maximum requis: 3.2A
   

• Utilisation d'un Convertisseur DC/DC

Nous avons utilisé une carte faite par des étudiants des années précédents.

Carte d'alimentation

Cette carte contient deux régulateurs à découpage:

1. LM2596S fournit 5V ( 3A max )
   
2. XL6009E1 qui fournit 12V ( 4A max )
   

Les régulateurs fournissent largement le nécessaire en tension et courant.
Pour le semestre suivant nous prévoyons à réaliser une autre carte d'alimentation, qui sera plus compacte.

Contrôle des moteurs CC par un dual H-Bridge (L298P)

Principe de fonctionnement d'un H-Bridge (PONT-H)

Le pont-H est une structure utilisée en électronique de puissance pour:

1. controle moteurs
2. convertisseurs et hacheurs
3. onduleurs

• Principe: On active les commutateurs avec differents cominaisons pour obtenir le branchement voulu. Le courant va circuler dans un sens ou dans l'autre dans le moteur, ce qui va permettre d'inverser les sens de rotation du moteur. Avec le pont-H on peut également varier la vitesse en modulant la tension aux bornes du moteur.
  

Principe Pont-H

Freinage Magnétique

Combinaisons de commutateurs possibles pour commander un moteur DC:

Sens +	Fermer A et D
Sens -	Fermer B et C
Freinage magnétique	A et C / B et D
Arret libre	A,B,C,D ouverts
Autres combinaison	INTERDITES

Le composant L298N

Nous allons utiliser pour notre robot le composant L298N (traversant) qui a le meme principe de fonctionement que celui en CMS (L298P).

Caractéristiques L298N

Dans la figure suivante on peux voir le cablage du composant, les signaux de commande et les sorties d'alimentation MOTEUR. Dans le tableau nous avons les 4 modes possibles en actionant les entrées logique C et D ainsi que Venable (PWM) pour varier la tension d'alimentation des moteurs (0-12V).

Operating L298N

Tests éffectués avec un SHIELD Arduino (L298P) (

Pour commander les moteurs nous allons utiliser le pont H L298P [ datasheet].

L298P

Le composant est ci-dessous:

L298P

Pour faire des tests nous avons utlisé le Motor Shield For Arduino. En connectant ce shield à l'arduino nous pouvons commander les deux moteurs (commande du sens et de la vitesse).

L298P

• PWM

Nous allons utiliser le shield en mode PWM, on placera donc les jumpers en conséquence.

L298P

• Borne du moteur

Nous avons deux bornes (bleues) pour connecter les moteurs CC. Les connecteurs mâles derrière sont identiques à celui des bornes bleues.

L298P

• PWRIN

Les moteurs peuvent être alimentés par une alimentation externe lorsque le courant du moteur dépasse les limites fournies par l'Arduino (Il est conseillé de séparer les alimentations d’Arduino et des moteurs). Le changement entre la puissance externe et l'Arduino est mis en œuvre par deux jumpers .

PWRIN: Alimentation externe.
VIN: Alimentation du Arduino.

On placera donc les jumpers d’alimentation sur PWRIN.

On doit avoir quelque chose comme cela:

L298P

• Signal de contrôle Tableau de vérité

E1	M1		E2	M2	Texte de l’en-tête
L	X	Moteur 1 désactivé	L	X	Moteur 2 désactivé
H	H	Moteur 1 en arrière	H	H	Moteur 2 en arrière
H	L	Moteur 1 en avant	H	L	Moteur 2 en avant
PWM	X	Contrôle vitesse PWM	PWM	X	Contrôle vitesse PWM

NOTE:

H: Niveau haut
L: Niveau bas
X: N'importe quel niveau.

Mode PWM

Commande	Moteur	Pin Arduino	Pin Atmega328p	Signification
M1	Gauche	4	PD4	Contrôle du sens de rotation
E1 (PWM)	Gauche	5	PD5	Contrôle de la vitesse de rotation
M2	Droit	7	PD7	Contrôle du sens de rotation
E2 (PWM)	Droit	6	PD6	Contrôle de la vitesse de rotation

• Exemple de code

Nous allons gérer les moteurs par des signaux PWM (-255 a 255), le signe moins (-) indique que le moteur fonctionne en marche arrière, et le signe plus (+) qu'il fonctionne en marche avant. Ce code nous permet de gérer les deux moteurs par la fonction setVitesse(vG,vD). Dans la suite nous allons l'utiliser pour gérer le déplacement du robot.

Code exemple

#include <avr/io.h>//Librairie AVR
#define topPWM 255 //Valeur Max du PWM (8bits)

void initMoteur()//PWM sur PD5 et PD6
{
  //fpwm = fq / (topPWM * p)
  //fpwmi:fréquence du PWM
  //fq:fréquence du quark
  //topPWM: valeur maximum du PWM
  //p:prédiviseur

  //Déclaration de sorties
  DDRD |= (1 << PD4);//Sens du moteur Gauche
  DDRD |= (1 << PD5);//PWM du moteur Gauche
  DDRD |= (1 << PD7);//Sens du moteur Droit
  DDRD |= (1 << PD6);//PWM du moteur Droit

  TCCR0B |= (1 << CS00) | (1 << CS01);//Prédiviseur P=64

  TCCR0A |= (1 << WGM00) | (1 << WGM01);//Mode FAST PWM

  TCCR0A |= 1 << COM0A1;//PWM sur OC0A

  TCCR0A |= 1 << COM0B1;//PWM sur OC0B

  OCR0A = 0;//Valeur de comparaison pour A --> PD6

  OCR0B = 0;//Valeur de comparaison pour B --> PD5
}

void setMoteurG(int16_t vit)//fonction pour gérer le moteur gauche
{
  if (vit < 0)
  {
    vit = -vit;
    PORTD |= (1 << PD4);//Moteur Gauche en arrière
  }
  else PORTD &= ~ (1 << PD4);//Moteur Gauche en avant

  if (vit > topPWM) vit = topPWM;//Si jamais on met une valeur supérieure à 255, la vitesse maximum sera 255

  OCR0B = vit;//Action sur le PWM --> PD5
}

void setMoteurD(int16_t vit)//fonction pour gérer le moteur droit
{
  if (vit < 0)
  {
    vit = -vit;
    PORTD |= (1 << PD7);//Moteur Droit en arrière
  }
  else PORTD &= ~ (1 << PD7);//Moteur Droit en avant

  if (vit > topPWM) vit = topPWM;//Si jamais on met une valeur supérieure a 255, la vitesse maximum sera 255

  OCR0A = vit;//Action sur le PWM --> PD6
}

void setVitesse(int16_t vG, int16_t vD)//cette fonction nous permet gérer les deux moteurs avec "une seule" ligne
{
  setMoteurD(vD);
  setMoteurG(vG);
}

int main()
{
  initMoteur();

 while(1)
 {
  setVitesse(100,100);//Exemple d’utilisation
 }
}

Positionnement du robot: Explication du principe

Approximation par des segments de droites

Le positionnement du robot est obtenu par odométrie, c'est à dire que la position est obtenue par intégration de petits déplacements. L'intérêt de l'odométrie est qu'elle est assez simple à mettre en oeuvre et qu'elle est fiable. Par contre, quand on intègre les déplacements, on intègre aussi l'erreur ce qui fait que l'erreur de position croît avec le temps.

Entre deux lectures, on peut savoir de combien s'est déplacée chaque roue codeuse et il faut à partir de cela en déduire la position du robot.

Appelons ∆d et ∆g les distances (en mm) parcourues respectivement par les roues droites et gauches entre deux lectures des LM soit un intervalle de temps Te. Connaissant la position du robot à l'instant n-1, on cherche la pose à l'instant n.

On a donc : ∆moy_n = (∆d_n + ∆g_n)/2

∆dif_n = ∆d_n - ∆g_n

∆x_n = ∆moy_n cos(theta_n-1)

∆y_n = ∆moy_n sin(theta_n-1)

∆theta_n = ∆dif_n/L

x_n = x_n-1 + ∆x_n

y_n = y_n-1 + ∆y_n

theta_n = theta_n-1 + ∆theta_n

Estimation de la position du robot

Etude et Réalisation Carte Encodeurs

L'odométrie nous permettra d'estimer la position du robot en mouvement, des le début et jusqu'à la détection de la balise par la caméra. C'est à partir de la mesure des déplacements des roues, qu'on pourra reconstituer le mouvement du robot. En partant d'une position initiale connue et en intégrant les déplacements mesurés, on peut ainsi calculer à chaque instant la position courante du véhicule.

Pour mesurer le déplacement des roues, nous allons utiliser un encodeur, monté sur l'axe de chaque roue.

• ENCODEUR DE BASE:

Ce montage basique permet de mesurer la vitesse de rotation à partir de la fréquence, mais il ne permet pas de connaître le sens de rotation.

principe

• ENCODEUR EN QUADRATURE:

Celui-ci nous permettra de connaître à la fois le sens et la vitesse des roues. Il est composé d’un disque rotatif, 1 led infrarouge et 2 capteurs optique décalé un par rapport à l’autre de 90°. C’est justement ce décalage la qui va nous permettre de connaître le sens de rotation de la roue. Suivant le sens de rotation, nous auront deux signaux déphasés en avance/retard de 90°. La vitesse sera déterminé en fonction de la fréquence.

            Les signaux de sortie à l’oscilloscope

• CAPTEUR TCUT 1300. Nous allons utiliser ce capteur il nous permettra d'avoir le sens et la vitesse de chaque roue.

The TCUT1300X01 is a compact transmissive sensor that includes an infrared emitter and two phototransistor detectors, located face-to-face in a surface mount package.

• Schéma et dimensionnement des composants

Emiter: If = 10mA (Vf = 1.2V), Re = (5V-Vf)/If = 380 ohm.

Collector: Ic sat = 0.4mA (If = 10mA), Rmin = Vce /Ic = 12.5 kohm. Vce sat = 0.4V max (pour Ic sat = 0.4mA, If = 10mA ). On prendra Rc = 15 kohm.

Nous devons obtenir les signaux correspondents:

Présence dent	0 V
Absence dent	5 V

• Schéma et routage en Eagle

• Réalisation carte (2 exemplaires) et installation

- Carte simple face CMS.

TEST RÉALISÉ SUR LA CARTE FINIE:

Présence dent	0 V
Absence dent	4.8 V

Conclusion: Les valeurs sont cohérentes

Roue codeuse

Pour determiner la position du robot nous allons utiliser deux Roues Codeuses et deux capteurs (TCUT 1300). Nous avons créé le roue sur le logiciel GEFAO et fabrication avec la machine charlyrobot. Vous trouverez ici le fichier de la roue (Fichier:RoueCodeuse.zip).

Roue Codeuse composée de 30 dents

Valeurs recu par les capteurs

Cette roue sera placer sur l'arbre des motors et sur les deux capteurs, lorsque le moteur est en train de tourner les capteurs va nous envoyer une signal qui se resamblera à:

• Si la roue en avant

Il faut remarquer que la signal 1 est en avance. Donc sur chaque front montant de la signal 1, la signal 2 est à 0. Et sur chanque front descendate de la signal 1, la signal 2 est à 1. Ce raisonnement sera utiliser dans le code du proogramme.

• Si la roue en arriere

Il faut remarquer que la signal 1 est en retard. Donc sur chaque front montant de la signal 1, la signal 2 est à 1. Et sur chanque front descendate de la signal 1, la signal 2 est à 0. Ce raisonnement sera aussi utiliser dans le code du proogramme.

Il faut remarquer que si le robot avance, la roue codeuse est en arriere et si le robot recule la roue codeuse est en avant. Ce effet est du aux engranages qui sont sur les moteurs et la roue.

Interruptions et timer

• Interruptions INT0 et INT1: Code example

Dans cette partie nous allons utiliser les interruptions et les timers. On utilisera les inturruptions (INT0 et INT1) pour dechancher un code a chaque fois que les dents de la roue codeuse travers le flux lumineuse du capteurs (sur le front montant et front descendant). Nous allons aussi utliser un timer qui sera dechanché sur un temps assez petit ( <10ms) pour calculer un deltaX, un deltaY et un detaTeta qui nous permetront de determiner la position du robot a chaque instante.

Code example

#include <avr/io.h>//Libreries AVR

volatile int8_t NbDentsD = 0; //Nombre de dents captés par le capteur (TCUT 1300) de la roue codeuse Droit
volatile int8_t NbDentsG = 0; //Nombre de dents captés par le capteur (TCUT 1300) de la Roue Codeuse Gauge


ISR(INT0_vect)
{
  //Sur chaque roue nous avons un emeteur et deux recepteurs, ces deux seront lus sur le pates PD2 et PB0
  uint8_t Pin2 = PIND & (1 << PD2); //INTERRUPTION INT0 sur PD2
  uint8_t Pin8 = PINB & (1 << PB0);

  if (Pin2 != 0)
  {
    if (Pin8 == 0) NbDentsD++;//Si la roue droit avance dont on aditionne le dents
    else NbDentsD--;//sinon on soustrait les dents, car cela signifie que le roue est en arrière
  }
  else if (Pin2 == 0)
  {
    if (Pin8 != 0) NbDentsD++;
    else NbDentsD--;
  }
}

ISR(INT1_vect)
{
  uint8_t Pin3 = PIND & (1 << PD3); //INTERRUPTION INT1 sur PD3
  uint8_t Pin9 = PINB & (1 << PB1);

  if (Pin3 != 0)
  {
    if (Pin9 == 0) NbDentsG++;//Si la roue gauge avance dont on aditionne le dents
    else {
      NbDentsG--;//sinon on soustrait les dents, car cela signifie que le roue est en arrière
    }
  }
  else if (Pin3 == 0)
  {
    if (Pin9 != 0) NbDentsG++;
    else {
      NbDentsG--;
    }
  }
}

void int0()
{
  //CONFIGURATION INTERRUPTION INT0
  //Pin2 --> PD2--> INT0 entrée

  EIMSK |= 1 << INT0; //Autorisation interruption INT0

  EICRA |= 1 << ISC00; //Activation sur changement d'etat sur PD2
}

void int1()
{
  //CONFIGURATION INTERRUPTION INT1
  //PIN3 -->PD3--> INT1 entrée

  EIMSK |= 1 << INT1; //Autorisation interruption INT1

  EICRA |= 1 << ISC10; //Activation sur changement d'etat sur PD3
}

void sei_interruption()
{
  sei(); //Autorisation global d'interruptions
}

int main()
{
  int0();//Interruption 0
  int1();//Interruption 1
  sei_interruption();//Autorisation d'interruptions

  while(1)
  {

  }
}

• TIMER 1

En faite nous allons calculer la position du robot en ajoutant de petits deltas de postition lequels seront calculer sur un temps suffisamment petit ( <20ms) pour eviter que l'erreur soit grand. On utilisera un drapeau pour eviter de faire de calculs dans le ISR(TIMER1_COMPA_vect), nous allons les faire plutot sur while(1), cela nous permettra de reduire l'erreur.

Code example

#include <avr/io.h>//Libreries AVR

void timer1()//CONFIGURATION DU TIMER1
{
  //Tt1=(n*p)/fq
  //Tt1: periode du timer
  //n=valuer de comparaison (OCR1A)
  //p:prediviseur
  //fq:frequence du quark (arduino uno 16Mhz)

  TCCR1B |= (1 << CS10) | (1 << CS11); //p=64

  TCCR1B |= 1 << WGM12; //RAZ mode CTC

  TIMSK1 |= 1 << OCIE1A; //Autorisation d'interruption de comparaison A

  OCR1A = 7500; //Comparaison n=7500
}

ISR(TIMER1_COMPA_vect)
{
  newCalc = true;//drapeau
}

int main()
{
  timer1();
  while (1)
  {
    if (newCalc == true)
    {

     //Calculs de la position

      newCalc = false;
    }

  }
}

Suivons une ligne droite

On sait que le moteurs n'ont pas la vitesse, donc il faut essayer de faire cela par code. On va donc faire setVitesse(Vmax * cos(Teta + PI / 4.0), Vmax * sin(Teta + PI / 4.0)), les cos et sin nous permettra jouer sur la vitesse et de cette facon faire que le robot suis une ligne droite. Il faut remarquer qu'il ne va pas s'arreter.

Code de Suivons une ligne droite

#include <avr/io.h>//Libreries AVR
#include <math.h>//Libreries Math pour les calculs de sinus, cosinus, etc;

#define topPWM 255 //Valeur Max du PWM (8bits)

float d = 51.2; //Rayon des roues
float Delta_Dent = PI * d / 60.0; //Espacement entre chaque dent de la roue.
float L = 230.0; //Distence entre les centre des roues.

volatile int8_t NbDentsD = 0; //Nombre de dents captés par le capteur TCUT 1300 de la roue codeuse Droit
volatile int8_t NbDentsG = 0; //Nombre de dents captés par le capteur TCUT 1300 de la Roue Codeuse Gauge

float Delta_D = 0;
float Delta_G = 0;

float Delta_moy = 0;
float Delta_dif = 0;
float Delta_teta = 0;

float Delta_X = 0;
float Delta_Y = 0;

float Position_X = 0;
float Position_X_precendent = 0;

float Position_Y = 0;
float Position_Y_precendent = 0;

float Teta = 0;
float Teta_precedent = 0;

float Teta_consigne = 0; //Angle qu'on veux attaindre
float X_consigne = 0; //Position en X qu'on veux attaindre
float Y_consigne = 0; //Position en Y qu'on veux attaindre

volatile boolean newCalc = true;//Drapeau qui nous permetra de

float Vmax = 0;

void int0()
{
  //CONFIGURATION INTERRUPTION INT0
  //Pin2 --> PD2--> INT0 entrée

  EIMSK |= 1 << INT0; //Autorisation interruption INT0

  EICRA |= 1 << ISC00; //Activation sur changement d'etat sur PD2
}

void int1()
{
  //CONFIGURATION INTERRUPTION INT1
  //PIN3 -->PD3--> INT1 entrée

  EIMSK |= 1 << INT1; //Autorisation interruption INT1

  EICRA |= 1 << ISC10; //Activation sur changement d'etat sur PD3
}

void timer1()//CONFIGURATION DU TIMER1
{
  //Tt1=(n*p)/fq
  //Tt1: periode du timer
  //n=valuer de comparaison (OCR1A)
  //p:prediviseur
  //fq:frequence du quark (arduino uno 16Mhz)

  TCCR1B |= (1 << CS10) | (1 << CS11); //p=64

  TCCR1B |= 1 << WGM12; //RAZ mode CTC

  TIMSK1 |= 1 << OCIE1A; //Autorisation d'interruption de comparaison A

  OCR1A = 7500; //Comparaison n=7500
}

void initMoteur()//PWM sur PD5 et PD6
{
  //fpwm = fq / (topPWM * p)
  //fpwmi:frequence du PWM
  //fq:frequance du quark
  //topPWM: valeur maximun du PWM
  //p:prediviseur

  //Declaration de sorties
  DDRD |= (1 << PD4);//Sens du motor Gauge
  DDRD |= (1 << PD5);//PWM du motor Gauge
  DDRD |= (1 << PD7);//Sens du motor Droit
  DDRD |= (1 << PD6);//PWM du motor Droit

  TCCR0B |= (1 << CS00) | (1 << CS01);//Prediviseur P=64

  TCCR0A |= (1 << WGM00) | (1 << WGM01);//Mode FAST PWM

  TCCR0A |= 1 << COM0A1;//PWM sur OC0A

  TCCR0A |= 1 << COM0B1;//PWM sur OC0B

  OCR0A = 0;//Valeur de comparaison pour A --> PD6

  OCR0B = 0;//Valeur de comparaison pour B --> PD5
}

void sei_interruption()
{
  sei(); //Autorisation global d'interruptions
}

ISR(INT0_vect)
{
  //Sur chaque roue nous avons un emeteur et deux recepteurs, ces deux seront lu sur le pate PD2 et PB0
  uint8_t Pin2 = PIND & (1 << PD2); //INTERRUPTION INT0 sur PD2
  uint8_t Pin8 = PINB & (1 << PB0);

  if (Pin2 != 0)
  {
    if (Pin8 == 0) NbDentsD++;//Si la roue droit avance dont on aditionne le dents
    else NbDentsD--;//sinon on soustrait les dents, car cela signifie que le roue est en arrière
  }
  else if (Pin2 == 0)
  {
    if (Pin8 != 0) NbDentsD++;
    else NbDentsD--;
  }
}

ISR(INT1_vect)
{
  uint8_t Pin3 = PIND & (1 << PD3); //INTERRUPTION INT1 sur PD3
  uint8_t Pin9 = PINB & (1 << PB1);

  if (Pin3 != 0)
  {
    if (Pin9 == 0) NbDentsG++;//Si la roue gauge avance dont on aditionne le dents
    else {
      NbDentsG--;//sinon on soustrait les dents, car cela signifie que le roue est en arrière
    }
  }
  else if (Pin3 == 0)
  {
    if (Pin9 != 0) NbDentsG++;
    else {
      NbDentsG--;
    }
  }
}

ISR(TIMER1_COMPA_vect)
{
  newCalc = true;
}

void setMoteurG(int16_t vit)//fontion pour gener le moteur gauge
{
  if (vit < 0)
  {
    vit = -vit;
    PORTD |= (1 << PD4);//Moteur Gauge en arrière
  }
  else PORTD &= ~ (1 << PD4);//Moteur Gauge en avant

  if (vit > topPWM) vit = topPWM;//Si jamais on met une vauleur superior a 255, la vitesse maximun sera 255

  OCR0B = vit;//Action sur le PWM --> PD5
}

void setMoteurD(int16_t vit)//fontion pour gener le moteur droit
{
  if (vit < 0)
  {
    vit = -vit;
    PORTD |= (1 << PD7);//Moteur Droite en arrière
  }
  else PORTD &= ~ (1 << PD7);//Moteur Droite en avant

  if (vit > topPWM) vit = topPWM;//Si jamais on met une vauleur superior a 255, la vitesse maximun sera 255

  OCR0A = vit;//Action sur le PWM --> PD6
}

void setVitesse(int16_t vG, int16_t vD)//cette fontion nous permet gerer les deux moteurs avec "une seule" ligne
{
  setMoteurD(vD);
  setMoteurG(vG);
}

int main()
{
  int0();//Interruption 0
  int1();//Interruption 1
  timer1();//Timer 1
  initMoteur();//PWM
  sei_interruption();//Autorisation d'interruptions

  while (1)//Boucle infinie
  {
    if (newCalc == true)
    {
      Delta_D = (NbDentsD * Delta_Dent);//pas de la roue droit à chaque declanchement du TIMER 1.
      Delta_G = (NbDentsG * Delta_Dent); //pas de la roue gauge à chaque declanchement du TIMER 1.

      NbDentsG = 0;//Remise à zéro à chaque declanchement du TIMER 1.
      NbDentsD = 0;//Remise à zéro sà chaque declanchement du TIMER 1.

      Delta_moy = (1 / 2.0) * (Delta_D + Delta_G);//pas du robot à chaque declanchement du TIMER 1.
      Delta_dif = Delta_D - Delta_G;
      Delta_teta = Delta_dif / L;//Calcul de Delta_teta en rad à chaque declanchement du TIMER 1.

      Delta_X = Delta_moy * cos(Teta_precedent);//pas sur X du deplacement du robot à chaque declanchement du TIMER 1.
      Delta_Y = Delta_moy * sin(Teta_precedent);//pas sur Y du deplacement du robot à chaque declanchement du TIMER 1.

      Teta = Teta_precedent + Delta_teta;//Angle de desviation par rapport à l'axe X du robot.
      Teta_precedent = Teta;//Angle de desviation precedent par rapport à l'axe X du robot.

      Position_X = Position_X_precendent + Delta_X;//Potition actuelle du robot en X
      Position_X_precendent = Position_X;//Position precedente du robot en X

      Position_Y = Position_Y_precendent + Delta_Y;//Potition actuelle du robot en Y
      Position_Y_precendent = Position_Y;//Position precedente du robot en Y

      Vmax = 250;//valeur maximun qu'on impose aux moteurs

      setVitesse(Vmax * cos(Teta + PI / 4.0), Vmax * sin(Teta + PI / 4.0));

      newCalc = false;
    }
  }
}

Suivons une consigne

Cette fois nous pourrons donner une valeur en X ey Y et le robot sera capable d'arriver au point et s'arreté. La valeurs doit etre donnée en cm.

ws2812_config.h

//Code precedente (suivons un ligne)

int main()
{
  //Configurations
  
  while (1)//Boucle infinie
  {
    if (newCalc == true)
    {
      
     //Calculs

      X_consigne = 100;//valeur donné en cm
      Y_consigne = 50;//valeur donné en cm
      Teta_consigne = atan((Y_consigne - Position_Y / 50.0) / (X_consigne - Position_X / 50.0)) - Teta;//Nous divisons par 50 pour avoir les valeur en cm

      Vmax = 250;//valeur maximun qu'on impose aux moteurs

      setVitesse(Vmax * cos(Teta_consigne + PI / 4.0), Vmax * sin(Teta_consigne + PI / 4.0));

      newCalc = false;
    }
  }
}

Caméra

Choix camera

Nous avons testé 3 cameras différentes, la PiCam, la CMUCam3 et la CMUCam5 Pixy

vignette

Nous avons choisi d'utiliser la CMUCam 5(site CMU cam 5) car elle est beaucoup plus simple d'utilisation que les deux autres. En effet, celle ci dispose d'une interface dre réglage, PixyMon, lui permettant d'enregistrer les signatures des objets à détecter, et de régler l'acquisition pour restreindre la détection à ces signatures précises. De plus, celle ci dispose d'un support mû par des servomoteurs permettant d'élargir son champ de vision.

Camera CMU cam 5

Tout d’abord nous avons réalisé une simple reconnaissance d'objet grâce au logiciel, il suffit pour cela de sélectionner l'objet en question via une interface, Pixymon. Nous avons ensuite choisi d'utiliser une balise lumineuse pour que la camera la repère le plus loin possible. balise test.

Grâce à cette balise nous avons pu déterminer la distance maximale de détection avec une balise de taille réglementaire. Nous avons ainsi déterminé que la balise était capable d'effectuer une détection à approximativement 6m.

Programme de gestion du cap

Nous avons réalisé un programme permettant de récupérer la position en X d'un objet par rapport à la caméra

Code exemple

//////////////////////////////////////////
// Fonction cap
//////////////////////////////////////////
/**************************************************************

Description : indique le cap à suivre grace à la camera Pixy.
Entrées : Aucune
Sorties : Un int allant de -160 à 160. Une valeur nulle indique
          que la cible se trouve au centre du champ de vision 
          de Pixy. Une valeur positive indique que la cible se
          trouve à droite de Pixy, une valeur négative indique 
          la gauche.
***************************************************************/

int cap()
{
  int compteur1;
  uint16_t blocks;
  /*Récupération des "blocs". Un bloc est une zone rectangulaire 
  définie par Pixy, possedant plusieurs caractéristiques (hauteur,
  position en x/y, couleur...)*/
  blocks = pixy.getBlocks();
  /*Dans le cas ou un bloc est détecté, la caméra renverra la
  position en x de celui ci. La valeur est centrée en zero.*/
  if (blocks)
  {
    return(pixy.blocks[0].x );
  }    
}

Cette fonction a été testée avec le main suivant

Code exemple

#include <SPI.h>  
#include <Pixy.h>

Pixy pixy;

void setup()
{
  Serial.begin(9600);
  Serial.print("Starting...\n");
  pixy.init();
}


int cap()
{
  int compteur1;
  uint16_t blocks;
  blocks = pixy.getBlocks();
  if ((blocks) && (pixy.blocks[0].x != 0))
  {
    return(pixy.blocks[0].x );
  }    
}

void loop()
{ 
  delay(100);
  Serial.println(cap());
  delay(500);
}

Il est nécessaire d'appeler les bibliothèques SPI.h et Pixy.h, et de déclarer et d'initialiser Pixy dans le setup. Ceprogramme ne permet cepandant pas l'usage des servomoteurs, limitant le champ de vision.

Mise à l’arrêt du robot et le perçage du ballon

La mise à l’arrêt du robot et le perçage du ballon doivent avoir lieu simultanément. Cela doit se produire quand le robot est arrivé dans le coin opposé.

Capteur de "Mise à l’arrêt" du robot

Pour se diriger vers le bon coin, le robot est guidé par les roues codeuses et par la caméra qui suit une balise lumineuse. Maintenant qu'il est guidé dans la bonne direction, nous devons procéder à une mise à l’arrêt rapide des qu il franchi le coin opposé. Le coin possède la particularité d'avoir une surface au sol de couleur blanche, alors que le reste du sol est bleu. Une solution évidente est d’utiliser le principe d’une diode émettrice infrarouge et d'un photo transistor récepteur infrarouge. Ainsi on pourra arrêter le robot quand il franchit une surface blanche.

• Le capteur CNY70 :

Nous pourrons utiliser ce capteur infrarouge pour détecter la couleur du sol.

• Schéma et dimensionement des composants

Emetteur: If = 20mA (Vf = 1.15V), Re = (5V-Vf)/If = 195 ohm.

Collecteur: Ic = 0.5mA (pour If = 20mA, Vce = 5V, d = 2mm), Rc = Vce /Ic = 10 kohm.

Nous devons obtenir les signaux correspondents:

Sol noir/surface non reflechissante	env 0 V
Sol bleu	env 3 V
Sol blanc/surface bien reflechissante	env 5 V

• Schéma électrique, routage en Eagle et fabrication de la carte

Systeme "Perçage du Ballon"

• La partie mécanique

Nous avons testé deux matériaux pour faire le percage: le fer et le plastique.
Pour l'aiguille, nous avons choisi d'utiliser du fer, plus facile à affûter.

• La commande électrique du moteur

Nous avons testé deux moteurs ,un moteur normal et un SM-S4303R. Finalement nous avons choisi le moteur normal,car le SM-S4303R est moins puissant et ne tourne pas assez vite pour percer le ballon.

Nous avons un moteur 5V qui tourne à un régime élevé. Au démarrage il demande beaucoup de courant. Comme l'Arduino ne peut pas fournir des courants supérieur à 20mA, pour commander le moteur nous allons utiliser un transistor MOSFET. Ce transistor permet de commuter des courants assez importants.

Pour que le transistor devient passant, nous commandons le gate en +5V avec Arduino. On rajoute une résistance de tirage de 1Mohm entre le gate et la masse, pour éviter que le transistor devienne passant à cause d'une perturbation externe.

Caractéristique transistor MOS:

Marque	IRF2204
Vdss	40 V
Rds	3.6 mohm
Id	210 A

Schéma électrique et routage sur Eagle

Fabrication carte et installation

Réalisation Carte des "Entrées et Sorties" et du Pont H

Objectifs et Composants utilisés )

• Objectif: concevoir une carte compacte qui va héberger le composant L298N pour le contrôle des moteurs ainsi que toutes les entrées et les sorties.La carte devra s’emboîter sur la carte Arduino MEGA.
  

Les Entrées	Les Sorties
Alimentation (5V)	Moteur Gauche
Alimentation moteurs (12V)	Moteur Droit
Encodeurs (TCUT1300)	Commande perçage Ballon
Camera (CMUCam 5)
3 capteurs obstacles (IR)
Couleur Sol (CNY70)
PWM Moteurs
Sens rotation Moteurs

• Références des composants utilisés:
  

Qté	Nom	Référence Eagle
1	Dual H-Bridge	L298n
1	Radiateur	pour L298n
4	Résistance	1 ohm package 207/10
8	Diode	1N4004
2	Condensateur	E 1.8-4 package 100nF
1	Connecteur ISP	AVR-ISP-6
1	Connecteur	Farnell 6 pins CMS
6	Connecteur	Molex 2 pins 22-27-2021-02 traversant
3	Connecteur	Molex 3 pins 22-27-2031-03 traversant

Schéma électrique de la carte (Eagle)

Schéma Complet

Routage et correspondance des pins (Eagle)

Schéma Complet

La commande des Moteurs de Roues (L298n H-Bridge)

Commande	Moteur	Pin Arduino	Pin ATmega2560	Signification
EnableG (PMW)	Gauche	5	PE3	Contrôle de la vitesse de rotation roue G
Input1	Gauche	10	PB4	Contrôle du sens de rotation (H/L)
Input2	Gauche	4	PG5	Contrôle du sens de rotation (H/L)
EnableD (PMW)	Droit	6	PH3	Contrôle de la vitesse de rotation roue D
Input3	Droit	11	PB5	Contrôle du sens de rotation (H/L)
Input4	Droit	7	PH4	Contrôle du sens de rotation (H/L)

Les signaux d'entrée et de sortie

Signal	Type	Pin Arduino	Pin ATmega2560	Signification
++roueG	digital INPUT	9	PH6	Capteur sens positif roue G
--roueG	digital INPUT	3	PE5(INT5)	Capteur sens négatif et vitesse roue G
++roueD	digital INPUT	8	PH5	Capteur sens positif roue D
--roueD	digital INPUT	2	PE4(INT4)	Capteur sens négatif et vitesse roue D
Out Ballon	digital OUTPUT	12	PB6	Signal de commande pour le Perçage du Ballon
IR gauche	analog INPUT	96	PF1	Signal distance obstacle capteur AV gauche
IR centre	analog INPUT	95	PF2	Signal distance obstacle capteur AV centre
IR droit	analog INPUT	94	PF3	Signal distance obstacle capteur AV droit
Couleur SOL	analog INPUT	93	PF4	Signal 0-5 V suivant couleur et nature du sol
MISO	liason ISP	50	PB3	Communication avec la Caméra
SCK	liason ISP	52	PB1	Communication avec la Caméra
SS	liason ISP	53	PB0	Communication avec la Caméra
MOSI	liason ISP	51	PB2	Communication avec la Caméra
RESET	liason ISP	30	RESET	Communication avec la Caméra

Média:Arduino-mega-pinout-diagram.png

La carte finale et les connecteurs

Figure 1 La carte finale

Figure 2 Carte Récto

Figure 3 La carte Vérso avec un défaut

Figure 4 Montage sur le Robot

Problèmes rencontrés:

Comme on peut voir sur la figure numéro 2, une piste à été coupé lors de la réalisation de la carte. Cet défaut s'est manifesté par une non cohérence entre le trajet demandé au robot et le trajet réel. Il nous a fallu 1 jour et demi pour trouver le problème et cela à l'aide du professeur et à travers les mesures avec un oscilloscope sur les signaux de commande des moteurs et sur les bornes de chaque moteur. Nous avons constaté des surtensions, des perturbations aux bornes d'un moteur. Cela venait d'une diode dont la piste était coupée. Le défaut a été remédié et nous avons constaté le fonctionnement normal du robot.

Leçons à tirer: Vérification avec rigueur la continuité des toutes les pistes en 2 étapes:

1. après la fabrication de la carte
   
2. après avoir soudé tous les composants
   

De même se servir toujours de l'oscilloscope pour visualiser les différents signaux en temps réel.

Code complet

Pour gérer les différents fonctionalités du robot nous avons utilisé le code suivant:

Code exemple

#include <avr/io.h>
#include <math.h>
#include <SPI.h>//Libreries   
#include <Pixy.h>//Libreries 

#define topPWM 255 //Valeur Max du PWM (8bits)

Pixy pixy;

float dD = 52.21;//52.21; //Diametre de la roue codeuse droite(mm)
float dG = 52.98;//52.98; //Diametre de la roue codeuse gauge(mm)

float Delta_DentD = PI * dD / 60.0;//Espacement entre chaque dent de la roue.
float Delta_DentG = PI * dG / 60.0;

float L = 233.0; //Distence entre les centre des roues.

volatile int8_t NbDentsD = 0; //Nombre de dents captés par le capteur **** de la roue codeuse Droit
volatile int8_t NbDentsG = 0; //Nombre de dents captés par le capteur **** de la Roue Codeuse Gauge

float Delta_D = 0;
float Delta_G = 0;

float Delta_moy = 0;
float Delta_dif = 0;
float Delta_teta = 0;

float Delta_X = 0;
float Delta_Y = 0;

float Position_X = 0;
float Position_X_precendent = 0;

float Position_Y = 0;
float Position_Y_precendent = 0;

float Teta = 0;
float Teta_precedent = 0;

float Teta_consigne = 0; //Angle qu'on veux attaindre
float X_consigne = 0;  //Position en X qu'on veux attaindre
float Y_consigne = 0; //Position en Y qu'on veux attaindre

volatile boolean newCalc = true;//Drapeau qui nous permetra de

int Vmax = 0;

int cm_IR_G;
int cm_IR_C;
int cm_IR_D;

//ROUES CODEUSES

void int4()
{
  //CONFIGURATION INTERRUPTION INT4
  //Pin2 --> PE4--> INT4 entrée

  EIMSK |= 1 << INT4; //Autorisation interruption INT4

  EICRB |= (1 << ISC40); //Activation sur changement d'etat de PE4
}

ISR(INT4_vect)
{
  //Sur chaque roue nous avons un emeteur et deux recepteurs, ces deux seront lu sur le pate PE4 et PH5
  uint8_t Pin2 = PINE & (1 << PE4); //INTERRUPTION INT4 sur PE4
  uint8_t Pin8 = PINH & (1 << PH5);

  if (Pin2 != 0)
  {
    if (Pin8 == 0) NbDentsD++;//Si la roue droit avance dont on aditionne le dents
    else NbDentsD--;//sinon on soustrait les dents, car cela signifie que le roue est en arrière
  }
  else if (Pin2 == 0)
  {
    if (Pin8 != 0) NbDentsD++;
    else NbDentsD--;
  }
}

void int5()
{
  //CONFIGURATION INTERRUPTION INT5
  //Pin3 -->PE5--> INT5 entrée

  EIMSK |= 1 << INT5; //Autorisation interruption INT5

  EICRB |= (1 << ISC50); //Activation sur changement d'etat de PE5
}

ISR(INT5_vect)
{
  uint8_t Pin3 = PINE & (1 << PE5); //INTERRUPTION INT5 sur PE5
  uint8_t Pin9 = PINH & (1 << PH6);

  if (Pin3 != 0)
  {
    if (Pin9 == 0) NbDentsG++;//Si la roue gauge avance dont on aditionne le dents
    else {
      NbDentsG--;//sinon on soustrait les dents, car cela signifie que le roue est en arrière
    }
  }
  else if (Pin3 == 0)
  {
    if (Pin9 != 0) NbDentsG++;
    else {
      NbDentsG--;
    }
  }
}

//CONFIGURATION DU TIMER1

void timer1()
{
  //Tt1=(n*p)/fq
  //n=Tt1*fq/p
  //Tt1: periode du timer
  //n=valuer de comparaison (OCR1A)
  //p:prediviseur
  //fq:frequence du quark (arduino Mega 16Mhz)

  TCCR1B |= (1 << CS10) | (1 << CS11); //p=64

  TCCR1B |= 1 << WGM12; //RAZ mode CTC

  TIMSK1 |= 1 << OCIE1A; //Autorisation d'interruption de comparaison A

  OCR1A = 5000; //Comparaison , Tt1=20ms,
}

ISR(TIMER1_COMPA_vect)
{
  newCalc = true;
}

//PONT H (Moteurs)

void initMoteur()//PWM sur PE3 (Gauge) et PH3(Droit)
{
  //fpwm = fq / (topPWM * p)
  //fpwmi:frequence du PWM
  //fq:frequance du quark
  //topPWM: valeur maximun du PWM
  //p:prediviseur

  //Declaration de sorties
  DDRG |= (1 << PG5);//Sens du motor Gauge
  DDRB |= (1 << PB4);//Sens du motor Gauge
  DDRE |= (1 << PE3);//PWM du motor Gauge OC3A
  DDRH |= (1 << PH4);//Sens du motor Droit
  DDRB |= (1 << PB5);//Sens du motor Gauge
  DDRH |= (1 << PH3);//PWM du motor Droit OC4A

  //Timer3 PE3

  TCCR3A |= (1 << WGM30);//Mode FAST PWM
  TCCR3B |= (1 << WGM32);

  TCCR3B |= (1 << CS30) | (1 << CS31);//Prediviseur p=64

  //Timer4 PH3

  TCCR4A |= (1 << WGM40);//Mode FAST PWM
  TCCR4B |= (1 << WGM42);

  TCCR4B |= (1 << CS40) | (1 << CS41);//Prediviseur P=64


  TCCR3A |= 1 << COM3A1;//PWM sur OC3A

  TCCR4A |= 1 << COM4A1;//PWM sur OC4A

  OCR3A = 0;//Valeur de comparaison initial --> PE3

  OCR4A = 0;//Valeur de comparaison initial --> PH3
}

void sei_interruption()
{
  sei(); //Autorisation global d'interruptions
}

void setMoteurG(int16_t vit)//fontion pour gener le moteur gauge
{
  if (vit < 0)
  {
    vit = -vit;
    PORTG |= (1 << PG5);//Moteur Gauge en arrière
    PORTB &= ~ (1 << PB4);
  }
  else {
    PORTG &= ~ (1 << PG5);//Moteur Gauge en avant
    PORTB |= (1 << PB4);
  }
  if (vit > topPWM) vit = topPWM;//Si jamais on met une vauleur superior a 255, la vitesse maximun sera 255

  OCR3A = vit;//Action sur le PWM --> PE3
}

void setMoteurD(int16_t vit)//fontion pour gener le moteur droit
{
  if (vit < 0)
  {
    vit = -vit;
    PORTH &= ~ (1 << PH4); //Moteur Droite en avant
    PORTB |=  (1 << PB5);
  }
  else
  {
    PORTH |= (1 << PH4); //Moteur Droite en arrière
    PORTB &= ~ (1 << PB5);
  }

  if (vit > topPWM) vit = topPWM;//Si jamais on met une vauleur superior a 255, la vitesse maximun sera 255

  OCR4A = vit;//Action sur le PWM --> PH3
}

void setVitesse(int16_t vG, int16_t vD)//cette fontion nous permet gerer les deux moteurs avec "une seule" ligne
{
  setMoteurD(vD);
  setMoteurG(vG);
}

float cap()
{
  int compteur1;
  int blocks;
  /*Récupération des "blocs". Un bloc est une zone rectangulaire
    définie par Pixy, possedant plusieurs caractéristiques (hauteur,
    position en x/y, couleur...)*/
  blocks = pixy.getBlocks();
  /*Dans le cas ou un bloc est détecté, la caméra renverra la
    position en x de celui ci. La valeur est centrée en zero.*/

  return (pixy.blocks[0].x);

}

void Delta()
{
  Delta_D = (NbDentsD * Delta_DentD);//pas de la roue droit à chaque declanchement du TIMER 1.
  Delta_G = (NbDentsG * Delta_DentG); //pas de la roue gauge à chaque declanchement du TIMER 1.

  NbDentsG = 0;//Remise à zéro à chaque declanchement du TIMER 1.
  NbDentsD = 0;//Remise à zéro sà chaque declanchement du TIMER 1.

  Delta_moy = (1 / 2.0) * (Delta_D + Delta_G);//pas du robot à chaque declanchement du TIMER 1.
  Delta_dif = Delta_D - Delta_G;
  Delta_teta = Delta_dif / L;//Calcul de Delta_teta en rad à chaque declanchement du TIMER 1.

  Delta_X = (1 / 50.0) * Delta_moy * cos(Teta_precedent); //pas sur X du deplacement du robot à chaque declanchement du TIMER 1.
  Delta_Y = (1 / 50.0) * Delta_moy * sin(Teta_precedent); //pas sur Y du deplacement du robot à chaque declanchement du TIMER 1.
}


void initADC1()
{
  ADCSRA |= 1 << ADEN;
  ADMUX |= 1 << REFS0;
  ADCSRA |= (1 << ADPS0) | (1 << ADPS1) | (1 << ADPS2);
}

boolean a = true;
int adcc;
int mpb = 0;


void MOteurBallon() { // fonctionnement du robot

  if (adcc < 150) // detecte que  la couleur au sol est bleu
  {
    setVitesse(-60, -60); // bloque les moteurs
    if (mpb < 1) { // permet a la boucle de s'effectuer qu'une seule fois
      _delay_ms(1000); // attend que le robot s'arrete totalement

      PORTB |= 1 << PB6; // demarre le moteur pour percer le ballon
      setVitesse(0, 0); // eteind totalement les moteurs du robot

      _delay_ms(2000); // permet de faire tourner le moteur pour percer le ballon pendant 2 seconde
      PORTB &= ~(1 << PB6); //eteind le moteur pour percer le ballon
      mpb++; // permet a la boucle de s'effectuer qu'une seule fois
      a = false; // permet de arrèter le programme de fonctionnement

    }
  }
  else // detecte que la couleur au sol est blanche
  {
    PORTB &= ~(1 << PB6); // permet d'etre sur que le moteur pour percer le ballon ne demarre pas

    if ((cap() > 90) && (cap() < 290)) // il n'y a pas d'obstacles et la camera detecte la balise
      setVitesse((240 + cap() / 2.0 - 159 / 2.0), (240 - cap() / 2.0 + 159 / 2.0)); // la vitesse des moteurs varies pour avoir
    //la balise au centre de la camera
    else // il n'y a pas d'obstacles et la camera ne detecte pas la balise
      //setVitesse(Vmax * cos(Teta_consigne + PI / 4.0), Vmax * sin(Teta_consigne + PI / 4.0));
      setVitesse(200, 250 ); // le robot tourne vers la gauche jusqu'a ce que la camera detecte a nouveau la balise
  }
}

int n = 0;
float dis = 0;
float k1 = 0;
float kmax = 0;





int main()
{

  int4();//Interruption 4
  int5();//Interruption 5
  timer1();//Timer 1
  initMoteur();//PWM
  initADC1();
  sei_interruption();//Autorisation d'interruptions
  Serial.begin(9600);
  Serial.print("Starting...\n");
  pixy.init();
  Serial.print("Pixy Initiated");

  while (1)//Boucle infinie
  {

    if (a == true) {
      if (newCalc == true)
      {

        Delta();

        Teta = Teta_precedent + Delta_teta / 5.0; //Angle de desviation par rapport à l'axe X du robot.
        Teta_precedent = Teta;//Angle de desviation precedent par rapport à l'axe X du robot.

        Position_X = Position_X_precendent + Delta_X;//Potition actuelle du robot en X
        Position_X_precendent = Position_X;//Position precedente du robot en X

        Position_Y = Position_Y_precendent + Delta_Y;//Potition actuelle du robot en Y
        Position_Y_precendent = Position_Y;//Position precedente du robot en Y

        X_consigne = 1200;//47;//502;//522;//140;//271;//800;//469;//383 ;//valeur donné en cm
        Y_consigne = 0;//502;//47;//140;//522;//469;//0;//271;// 383;//valeur donné en cm

        Teta_consigne = atan((Y_consigne - Position_Y) / (X_consigne - Position_X)) - Teta;

        dis = sqrt(pow(X_consigne - Position_X, 2) + pow(Y_consigne - Position_Y, 2));

        kmax = 1.37; //1.37;//0.95;//1.0;
        k1 = kmax;
        if (dis < 400)   k1 = 0;

        Vmax = k1 * 255; //valeur maximun qu'on impose aux moteurs

        ADMUX |= (1 << MUX0);
        ADCSRA |= 1 << ADSC;
        while ((ADCSRA & (1 << ADSC)) == 1); //{Serial.println("a");}
        //cm_IR_G = pow(3027.4/ADC,1.2134);
        //Serial.println(cm_IR_G);
        //Serial.print("  ");
        ADMUX &= ~(1 << MUX0);

        ADMUX |= (1 << MUX1);
        ADCSRA |= 1 << ADSC;
        while ((ADCSRA & (1 << ADSC)) == 1); //{Serial.println("a");}
        //cm_IR_C = pow(3027.4/ADC,1.2134);
        //Serial.println(cm_IR_C);
        //Serial.print("  ");
        ADMUX &= ~(1 << MUX1);

        ADMUX |= (1 << MUX0) | (1 << MUX1);
        ADCSRA |= 1 << ADSC;
        while ((ADCSRA & (1 << ADSC)) == 1); //{Serial.println("a");}
        //cm_IR_D = pow(3027.4/ADC,1.2134);
        //Serial.println(cm_IR_D);
        //Serial.print("  ");
        ADMUX &= ~((1 << MUX0) | (1 << MUX1));


        ADMUX |= (1 << MUX2);
        ADCSRA |= 1 << ADSC;
        while ((ADCSRA & (1 << ADSC)) == 1);
        adcc = ADC;
        ADMUX &= ~(1 << MUX2);

        if (Position_X > 250) { //le robot a fait plus de 2m
          MOteurBallon(); // execute le programme pour suivre la balise
        }
        else // le robot a parcouru moins de 2m */
          setVitesse(Vmax * cos(Teta_consigne + PI / 4.0), Vmax * sin(Teta_consigne + PI / 4.0));//le robot va droit grace a la roue codeuse

        Serial.print("  ");
        Serial.println(mpb);


        newCalc = false;
      }

    }
    else setVitesse(0, 0); //permet au moteur de ne pas redemarrer une fois que le robot est passer sur le bleu
  }
}

Vidéo de Démonstration

      VIDEO ICI [1]

Bibliographie/références

• positionnement d'un robot par roues codeuses