Nous allons ici apprendre à faire en sorte qu’un robot se déplace de façon autonome.

Le but étant qu’à la fin de ce cours, votre robot soit capable d’aller d’un point A à un point B de façon complétement autonome. L’objectif ici est de se déplacer en ligne droite, mais quand on dit ligne droite, c’est vraiment droit ! On va ainsi voir comment faire pour que le robot sache où il est, où il en est de son parcours et comment faire pour qu’il se dirige dans la bonne direction et de façon le plus propre possible.

Matériel

Pour réaliser tout cela, nous aurons besoin d’un peu de matériel :
– Des moteurs CC, si vous avez les moyens, on conseille des moteurs Maxon, référence sur le marché. Testés, éprouvés et conseillés ! Sinon l’émergence du marché chinois permet de trouver des moteurs pour des prix abordables.
– Un pont en H pour pouvoir contrôler les moteurs, composant dépendant directement de vos moteurs. Nous utilisons des ponts en H Pololu que nous conseillons !
– Des codeurs incrémentaux, le nombre de tics influencera la précision de votre déplacement. Avoir 1000 tics par tours de roues permet d’avoir une précision plus que satisfaisante. Si vous avez les moyens, on a testé les codeurs de marque Hengstler, ce sont de très bons produits ! Sinon, les codeurs incrémentaux premier prix de chez Vicatronic font également l’affaire !
– Une carte électronique permettant de programmer l’ensemble des composants. Pour commencer, une carte Arduino Uno fera très bien le job.

Comme on peut le voir, on conseille l’achat de codeurs externes plutôt que de codeurs directement reliés aux moteurs. On préfère ce mode de fonctionnement pour éviter de se perdre en cas de patinage des moteurs. Supposons que votre robot se bloque contre un mur mais que ses moteurs continuent de tourner, la position ne correspondrait plus du tout avec la réalité, alors qu’avec des codeurs externes, si le robot est bloqué, les roues codeuses ne tourneront pas. Votre robot ne se perdra donc jamais (ou presque jamais) !

Odométrie

Entrons maintenant dans le vif du sujet et commençons par le plus important : l’odométrie !

L’odométrie est une technique permettant de connaître la position, en temps réel, du robot par rapport à une position d’origine. On utilise pour cela les informations que nous donnent les codeurs externes et qui correspondent aux déplacements des roues motrices du robot. On va ainsi être capable de calculer la distance parcourue par chaque roue et ainsi d’en déduire une nouvelle position du robot.

Récupérer les informations de vos codeurs

La première étape pour réaliser une odométrie est de récupérer les tics que génère votre codeur. Si, comme nous, vous avez des codeurs bi-canaux alors les informations que renvoient celui-ci se traduisent par un même signal sur les canaux A et B déphasé de 90° l’un par rapport à l’autre. Ce déphasage permet de déterminer le sens de rotation du codeur (et donc de la roue). En effet, le principe est de regarder l’état du canal B lors d’un front montant et/ou descendant du canal A. Si le premier est à l’état bas alors on se déplace dans un sens et s’il est à l’état haut alors on se déplace dans l’autre sens. Voici un schéma illustrant le principe :

En pratique, si vous utilisez une Arduino Uno, on va connecter le canal A de chaque codeur sur une pin d’interruption (ça tombe bien, il y en a tout juste deux sur une Uno !) et chaque canal B sur une pin numérique de la carte (ça tombe bien, il y en a plein sur une Uno !). Il suffit maintenant, à chaque front d’un des canaux A, de regarder l’état du canal B correspondant et d’incrémenter ou décrémenter un compteur en fonction du sens de rotation.
Un tutoriel dédié au codeur est disponible ici !
Et voilà ! Bravo ! Vous êtes capable de compter les tics de vos codeurs !

Convertir les informations des codeurs en millimètres et degrés

Maintenant que nous sommes capables de récupérer les informations des codeurs, la prochaine étape est de convertir tout ça en quelque chose de lisible, c’est-à-dire en millimètre pour les distances et en degrés pour les angles. Il y a ici deux façons de faire, la première est purement théorique, on connaît le nombre de tics du codeur, le diamètre de notre roue codeuse, on peut donc déterminer la distance correspondant à un tic. Cependant, cela revient à faire confiance aux données constructeur, ce qui est suicidaire dans la majorité des cas. En effet, la roue peut ne pas être exactement ronde, le codeur peut avoir une certaine erreur sur le nombre de tics …
Il y a donc une deuxième approche qui est plus empirique, on parcourt une certaine distance connue et précise « à la main » avec le robot tout en comptant le nombre de tics, on réalise ensuite un produit en croix pour trouver la distance en millimètre correspondant à 1 tic codeur. En ce qui concerne les angles, on fait la même opération que pour les distances mais en faisant tourner le robot sur lui-même d’un certain nombre de degrés, on va ainsi être capable d’obtenir le nombre de degrés correspondant à un tic codeur.
Pour information, nous réalisons ce type de calibration en faisant parcourir 1 mètre au robot et en tournant de 3600° sur nous-même (10 tours). On arrive à se positionner avec une précision au millimètre et au dixième de degrés près.

Calculer la position en x, y et orientation du robot

On sait désormais déterminer la distance parcourue par chaque roue ainsi que leurs variations en angle, on peut calculer la position du robot ! Pour commencer il faut fixer les conditions initiales de notre système, soit les positions x0 et y0 ainsi que l’orientation initiale. Une fois cela fixé, notre but va être de calculer des deltas en x, y et en angle.

Définissons les noms des variables que nous allons utiliser :
x0, y0 et θ0 respectivement, l’abscisse à l’origine, l’ordonnée à l’origine et l’orientation à l’origine.
dDist et dAngl la distance parcourue et la variation sur l’angle entre deux appels de nos fonctions.
coeffGLong et coeffDLong les coefficients permettant la transformation nombre de tics codeurs / distance parcourue.
coeffGAngl et coeffDAngl les coefficients permettant la transformation nombre de tics codeurs / rotation en degrés.
nbTicG et nbTicD les nombres de tics des codeurs gauche et droit.

Commençons par calculer la distance parcourue par le robot ainsi que sa variation d’angle. Pour calculer la distance parcourue, il suffit de faire la moyenne des distances parcourues par chaque roue. On va donc avoir dDist = (coeffGLong*nbTicG + coeffDLong*nbTicD)/2.
Pour la variation d’angle, il suffit de calculer la différence d’angle entre les deux roues, l’ordre dans lequel le calcul sera effectué déterminera le sens dans lequel sera le repère pour déterminer les angles. Pour se faciliter la tâche, plaçons-nous dans un repère trigonométrique. Ainsi le calcul sera :
dAngl = coeffDAngl*nbTicD – coeffGAngl*nbTicG.

On y est presque ! Encore un peu de trigonométrie et on en a terminé avec le positionnement !
On cherche maintenant dX et dY, respectivement, le déplacement suivant l’axe des abscisses et celui sur l’axe des ordonnées. Pour calculer cela, nous avons la distance parcourue par le robot ainsi que la variation qu’il a effectuée. Voici un schéma représentant ce que nous avons et ce que nous cherchons :

Il ne nous reste plus qu’à utiliser deux formules de trigonométrie de base et on en a terminé ! Pour calculer dX il suffit d’utiliser le cosinus de dAngl et pour dY le sinus.
On va donc avoir :
x = x0 + dDist*cos(dAngl)
y = y0 + dDist*sin(dAngl)
θ = θ0 + dAngl

Et voilà ! Vous savez comment positionner un robot autonome !

Déterminer où le robot doit aller

On sait maintenant où est le robot, on va donc maintenant le faire bouger ! Pour cela on a besoin de plusieurs choses : la première va être de fixer les conditions initiales, soit les coordonnées x0 et y0 au départ et l’angle d’origine θ0 du robot. Ensuite, il faut déterminer la cible à viser, soit les coordonnées xC et yC du point que vise le robot. Maintenant que tout cela est fixer, on doit vérifier si le robot est arrivé à destination ou pas. Pour cela rien de mieux que l’algorithme de Pythagore :

distanceCible = sqrt((xC-xR)*(xC-xR)+(yC-yR)*(yC-yR));

Maintenant qu’on connait la distance qu’il reste à parcourir, à nous de déterminer si celle-ci est suffisamment petite pour qu’on estime être arrivée à destination (espérer arriver à une distance nulle est utopique et n’arrivera jamais, il faut fixer une distance à laquelle on estime être arrivée !)

Voici un schéma résumant les coordonnées en jeu et la distance que l’on cherche. Le cercle bleu représente le robot et celui en rouge la cible visée.

Maintenant qu’on sait la distance à parcourir, il faut s’assurer qu’on va dans la bonne direction, sinon on n’est pas rendu ! Pour cela, rien de mieux qu’un peu de trigonométrie. Si on reprend le schéma précédant, le 0° correspond à être parallèle à l’axe X et à tourner le dos à l’axe Y. De plus, on cherche un angle comprit entre 180 et -180° et non compris entre 0 et 360°. Pour différencier cela, on commence par regarder si le y du robot est supérieur ou inférieur au y de la cible. S’il est supérieur alors l’angle sera positif, sinon il est négatif. On obtient donc la formule suivante pour l’angle :

angle = signe * acos((xC-xR)/((xC-xR)*(xC-xR)*(yC-yR)*(yC-yR));

Et voilà, on sait positionner notre robot et maintenant on arrive même à savoir où on en est de notre but !

Avant de faire bouger notre robot, faisons un petit point sur le code :

/************************************************************
				   ___    _____  ____ 
				  / _ \  / ___/ / __ \
				 / , _/ / /__  / /_/ /
				/_/|_|  \___/  \____/ 

	Auteur : Association Robotique Club de l'Ouest
	
	Exemple de positionnement d'un robot
	
	Cet exemple permet de positionner un robot.

************************************************************/

//-----------------------------------------------------------
// Déclaration des constantes

// Numéro de l'intéruption 0, il correspond à la pin 2
const int interuptG = 0;
// Numéro de l'intéruption 1, il correspond à la pin 3
const int interuptD = 1; 	

// Pin 9 de l'Arduino Uno
const int canalBG 	= 9;	
// Pin 10 de l'Arduino Uno
const int canalBD 	= 10;

//Constantes permettant la transformation tic / millimètre
const double coeffGLong = 0.00017;
const double coeffDLong = 0.00017;	

//Constantes permettant la transformation tic / degrés
const double coeffGAngl = 0.0008;
const double coeffDAngl = 0.0008;

// Fin déclaration des constantes
//-----------------------------------------------------------

//-----------------------------------------------------------
// Déclaration des variables globales
  
// Variables (compteurs) qui vont stocker 
// le nombre de tics comptés pour chaque codeur
long comptD = 0;
long comptG = 0;


//Variables allant contenir les positions du robots
double xR = 0.;
double yR = 0.;

//Variables permettant de stocker la position de la cible du robot
double xC = 1000;
double yC = 1000;

//Variables allant contenir les delta position et angle
double dDist = 0.;
double dAngl = 0.;

//Variable contenant le cap du robot
double orientation = 0.;

//Variable contenant l'angle entre le robot et la cible
double consigneOrientation = 0.;

//Variable permettant de savoir si la cible est à gauche ou à droite du robot.
int signe = 1;

//Variable contenant la distance entre le robot et la cible
double distanceCible = 0.;


// Fin Déclaration des variables globales
//-----------------------------------------------------------

//-----------------------------------------------------------
// Fonction d'initialisation
void setup()
{
	// Permet un affichage du résultat sur la consol Série
	Serial.begin(9600);		

	// Initialisation des pin 9 et 10 en entrèes
	pinMode(canalBG, INPUT);	
	pinMode(canalBD, INPUT);

	// Déclaration des intéruptions
	// Ici les fonctions d'intéruptions s'éxecutent
	// sur fronts montants : RISING
	attachInterrupt(interuptG, Reagir, RISING);
 	attachInterrupt(interuptD, ReagirBis, RISING);
}
// Fin initialisation
//-----------------------------------------------------------


//-----------------------------------------------------------
// Fonction principale
// Avec l'utilisation des intéruptions la fonction loop qui

void loop()
{
		//On commence par calculer les variations de position en distance et en angle
	dDist = (coeffGLong*comptG + coeffDLong*comptD)/2.;
	dAngl = coeffDAngl*comptD - coeffGAngl*comptG;

	//Actualisation de la position du robot en xy et en orientation
	xR += dDist*cos(dAngl);
	yR += dDist*sin(dAngl);
	orientation += dAngl;

	//On calcule la distance séparant le robot de sa cible
	distanceCible = sqrt((xC-xR)*(xC-xR)+(yC-yR)*(yC-yR));

	//On regarde si la cible est à gauche ou à droite du robot
	if(yR > yC)
	{
		signe = 1;
	}else
	{
		signe = -1;
	}

	//On calcule l'angle entre le robot et la cible
	consigneOrientation = signe * acos((xC-xR)/((xC-xR)*(xC-xR)*(yC-yR)*(yC-yR));

}
// Fin fonction principale
//-----------------------------------------------------------

//-----------------------------------------------------------
// Fonction d'intéruption pour le codeur gauche
// 
// Fonction appelée par un tic du codeur gauche. 
// On regarde le sens de rotation de celui-ci 
// (on teste l'état de la voie déphasée) et on 
// incrémente ou décrémente le compteur comptG.
void Reagir()
{  
	if(digitalRead(canalBG) == HIGH)
	{
		comptG--;        
	}
	else
	{
		comptG++;      
	}      
}
// Fin fonction d'intéruption pour le codeur gauche
//-----------------------------------------------------------


//-----------------------------------------------------------
// Fonction d'intéruption pour le codeur droit
// 
// Fonction appelée par un tic du codeur droit. 
// On regarde le sens de rotation de celui-ci 
// (on teste l'état de la voie déphasée) et on 
// incrémente ou décrémente le compteur comptD.
void ReagirBis()
{     
	if(digitalRead(canalBD) == HIGH)
	{
		comptD ++;
	}
	else
	{
		comptD--;     
	}
}
// Fin fonction d'intéruption pour le codeur droit
//-----------------------------------------------------------

Et voilà, il est temps maintenant de faire bouger notre robot ! Nous allons pour cela utiliser des commandes PWM qui seront envoyées à un pont en H qui va permettre de faire bouger le robot dans un sens comme dans l’autre. Un tutoriel expliquant cela est en cours de rédaction. Nous devons donc tout simplement calculer les commandes à envoyer. On va diviser ce calcul en deux étapes : la première est de faire avancer le robot en marche avant et marche arrière, la deuxième sera de faire tourner le robot sur lui-même.

Faire avancer le robot

Pour faire avancer notre robot, on va comme on l’a dit utiliser les PWM disponibles sur notre carte, rappelons que cette commande varie de 0 à 255, 0 entraînant l’arrêt des moteurs, 255 les entraînant à leur pleine puissance. Comme on veut atteindre une cible et qu’on connait la distance séparant notre robot à celle-ci on va alors indiquer au pont en H que la commande est égale à la distance entre le robot et la cible si celle-ci est inférieur à 255 millimètres ou 255 si c’est supérieur.

On va appliquer cette commande aux deux moteurs, obtenant le code suivant :

void loop()
{
	//On commence par calculer les variations de position en distance et en angle
	dDist = (coeffGLong*comptG + coeffDLong*comptD)/2.;
	dAngl = coeffDAngl*comptD - coeffGAngl*comptG;

	//Actualisation de la position du robot en xy et en orientation
	xR += dDist*cos(dAngl);
	yR += dDist*sin(dAngl);
	orientation += dAngl;

	//On calcule la distance séparant le robot de sa cible
	distanceCible = sqrt((xC-xR)*(xC-xR)+(yC-yR)*(yC-yR));

	//On regarde si la cible est à gauche ou à droite du robot
	if(yR > yC)
	{
		signe = 1;
	}else
	{
		signe = -1;
	}

	//On calcule l'angle entre le robot et la cible
	consigneOrientation = signe * acos((xC-xR)/((xC-xR)*(xC-xR)*(yC-yR)*(yC-yR));

	//On détermine les commandes à envoyer aux moteurs
	cmdD = abs((int)distanceCible);
	if(cmdD>255)
	{
		cmdD = 255;
	}
	cmdG = cmdD;
}

Maintenant que notre robot avance, c’est bien mais il ne fait que des lignes droites … et encore elles sont pas bien droites… Pour cela on va aussi lui demander de se tourner vers notre cible, et ça tout le temps !

Asservir un robot

Objectif

Qu’est ce qu’on entend par asservir un robot ? Et bien pas un, mais deux objectifs, rien que ça ! Le premier va être de s’assurer que quand on veut aller droit, on aille droit. ça à l’air bête mais ce n’est vraiment pas si simple que ça, surtout parce que les mécanos sont des rigolos qui ne font jamais les choses correctement entrainant forcément des petites erreurs entre les deux moteurs, une même commande sur les deux n’aura pas forcément le même résultat.

Le deuxième objectif est que le robot se tourne d’un angle donné quand on lui demande, le plus précisément possible et sans dépassement (ou un petit, j’aime bien avoir un petit dépassement, ça donne un comportement un peu plus pêchu au robot). Ce qu’on appelle dépassement, c’est par exemple quand le robot est à 0°, on lui demande de se tourner à 90°, il va d’abord tourner à 95° puis revenir à 90.

Implémentation

Comment implémente-t-on tout ça dans un robot ? Comme on est paresseux, on ne va en implémenter qu’un seul … qui fera le boulot des deux ! En effet je me suis toujours limité à ne faire que le deuxième objectif, et s’il est bien paramétré il permet aussi d’assurer la ligne droite. Çà a été le cas sur tous les robots qu’on a paramétré depuis maintenant plus de trois ans et ça a, à chaque fois, très bien marché.

On va voir l’asservissement en plusieurs étapes, on va commencer par essayer de tourner le robot sur lui-même pour qu’il ait la cible toujours bien devant lui. Ensuite on chercher à avancer ET tourner le tout en même temps.

Le fonctionnement est relativement simple, on commence par chercher l’erreur entre l’orientation voulue et l’orientation actuelle du robot, c’est simple, on connait tout ça ! L’orientation voulue est contenue dans la variable consigneOrientation et celle du robot est contenue dans la variable orientation. L’erreur est donc la différence entre la consigne et l’orientation, on peut aussi le formuler comme ça : « De combien le robot doit-il se tourner pour avoir la cible bien en face de lui ? ».

Voici un schéma pour récapituler tout ça :

On obtient ainsi une valeur qui sera plus ou moins grande en fonction de combien le robot doit tourner sur lui-même. Il suffit maintenant d’envoyer cette commande aux moteurs, en multipliant par -1 la valeur sur le moteur droit, sinon le robot ira tout droit.

Asservissement proportionnel

On pourrait s’arrêter là, le robot va bien chercher à compenser son erreur, par contre le comportement n’est pas satisfaisant, il n’est pas bien en face de la cible, il ne l’atteint pas, ou alors c’est tout le contraire il va trop loin ! Pour améliorer ça on va réaliser un asservissement proportionnel, rien de bien méchant il va seulement falloir multiplier l’erreur trouvée précédemment par un coefficient. Le but du jeu va donc être de trouver la bonne valeur pour ce coefficient.

Pour cela il va falloir testée des valeurs jusqu’à trouver la meilleure possible ! Un coefficient trop grand entraînera des dépassements, voire même un déplacement non stop du robot. Un coefficient trop petit fera que le robot n’atteint jamais la consigne.
Voici une image tirée du site de Ferdinand Piette :

Il illustre très bien le comportement du robot en fonction du paramètre P. On va chercher a avoir la meilleur courbe possible (ici la rouge), on corrigera l’erreur qui reste par la suite (erreur qu’on appelle erreur statique).

Asservissement proportionnel dérivé

Afin de contrer l’erreur restante on va calculer la différence entre l’erreur actuelle et l’erreur à l’instant précédant. On peut appeler cette différence une « vitesse de correction », ce n’est pas tout à fait exacte mais l’expression est jolie ! De la même façon que pour le premier paramètre, on multiplie cette erreur par un coefficient. Le but sera de configurer ce paramètre jusqu’à avoir une réponse du robot satisfaisante ! Ainsi, la valeur envoyée aux moteurs sera la somme des valeurs trouvées avec les deux corrections !

Si on récapitule, on obtient l’algorithme suivante :

1. Calcule de l’erreur entre l’angle demandé et le cap actuel du robot.
2. Multiplication du calcul 1 par un paramètre.
3. Calcul de la différence entre l’erreur actuelle et l’erreur précédente.
4. Multiplication du calcul 3 par un paramètre.
5. Somme des calculs 2 et 4.
6. Envoi des commandes au robot.

Et voilà, vous avez maintenant un robot qui se tournera toujours bien vers sa cible !

Avancer et tourner

Maintenant qu’on sait tourner sur nous même bien comme il faut, il ne reste plus qu’à combiner avancer et tourner sur nous-mêmes pour avoir un robot autonome ! On y est presque.

En fait, on a déjà tout, il ne nous reste presque plus rien à faire, en effet, on à déjà une commande pour avancer tout droit (souvenez-vous, celle qu’on calcule avec la distance à la cible) et une commande pour tourner (celle avec l’asservissement). Il suffit simplement d’ajouter les deux et … voilà, c’est tout.

Si, comme nous, vous utilisez une Arduino il y a quand même un petit détail à prendre en compte, c’est la limite de la valeur PWM de 255. En effet il va falloir ici faire deux écrêtements, un pour le calcul de la commande d’avancer en ligne droite et un second une fois toutes les commandes sommées. Si on ne fait pas le premier, les commandes d’angles peuvent ne pas être prisent en compte lors du déplacement et entraîner un comportement erratique du robot…

Et voilà, on a terminé, voici un algorithme et le code associé pour l’asservissement en angle et en ligne droite du robot :

1. Calcule de la distance entre le robot et la cible.
2. Écrêtement de la valeur trouvé en 2 à 255.
3. Calcule de l’erreur entre l’angle demandé et le cap actuel du robot.
4. Multiplication du calcul 3 par un coefficient.
5. Calcule de la différence entre l’erreur actuelle et l’erreur précédente.
6. Multiplication du calcul 5 par un coefficient.
7. Somme des calculs 4 et 6.
8. Ajout du calcul 7 et du calcul 2 pour le moteur gauche.
9. Soustraction du calcul 2 et du calcul 7 pour le moteur droit (2-7)
10. Écrêtement à 255.
11. Envoi des commandes au robot.

Et voilà, maintenant voici le code complet pour un robot allant tout seul à une cible :

/************************************************************
				   ___    _____  ____ 
				  / _ \  / ___/ / __ \
				 / , _/ / /__  / /_/ /
				/_/|_|  \___/  \____/ 

	Auteur : Association Robotique Club de l'Ouest
	
	Exemple de positionnement d'un robot
	
	Cet exemple permet de positionner un robot.

************************************************************/

//-----------------------------------------------------------
// Déclaration des constantes

// Numéro de l'intéruption 0, il correspond à la pin 2
const int interuptG = 0;
// Numéro de l'intéruption 1, il correspond à la pin 3
const int interuptD = 1; 	

// Pin 9 de l'Arduino Uno
const int canalBG 	= 9;	
// Pin 10 de l'Arduino Uno
const int canalBD 	= 10;

//Constantes permettant la transformation tic / millimètre
const double coeffGLong = 0.00017;
const double coeffDLong = 0.00017;	

//Constantes permettant la transformation tic / degrés
const double coeffGAngl = 0.0008;
const double coeffDAngl = 0.0008;

// Fin déclaration des constantes
//-----------------------------------------------------------

//-----------------------------------------------------------
// Déclaration des variables globales
  
// Variables (compteurs) qui vont stocker 
// le nombre de tics comptés pour chaque codeur
long comptD = 0;
long comptG = 0;


//Variables allant contenir les positions du robots
double xR = 0.;
double yR = 0.;

//Variables permettant de stocker la position de la cible du robot
double xC = 1000;
double yC = 1000;

//Variables allant contenir les delta position et angle
double dDist = 0.;
double dAngl = 0.;

//Variable contenant le cap du robot
double orientation = 0.;

//Variable contenant l'angle entre le robot et la cible
double consigneOrientation = 0.;

//Variable permettant de savoir si la cible est à gauche ou à droite du robot.
int signe = 1;

//Variable contenant la distance entre le robot et la cible
double distanceCible = 0.;

//Variables parametrant l'asservissement en angle du robot
double coeffP = 5.0;
double coeffD = 3.0;

//Variables utilisées pour asservir le robot
double erreurAngle = 0.;
double erreurPre = 0.;
double  deltaErreur = 0.;

//Variables représentant les commandes à envoyer aux moteurs
int cmdG = 0;//Commande gauche
int cmdD = 0; //commande droite


// Fin Déclaration des variables globales
//-----------------------------------------------------------

//-----------------------------------------------------------
// Fonction d'initialisation
void setup()
{
	// Permet un affichage du résultat sur la consol Série
	Serial.begin(9600);		

	// Initialisation des pin 9 et 10 en entrèes
	pinMode(canalBG, INPUT);	
	pinMode(canalBD, INPUT);

	// Déclaration des intéruptions
	// Ici les fonctions d'intéruptions s'éxecutent
	// sur fronts montants : RISING
	attachInterrupt(interuptG, Reagir, RISING);
 	attachInterrupt(interuptD, ReagirBis, RISING);
}
// Fin initialisation
//-----------------------------------------------------------


//-----------------------------------------------------------
// Fonction principale
// Avec l'utilisation des intéruptions la fonction loop qui

void loop()
{
		//On commence par calculer les variations de position en distance et en angle
	dDist = (coeffGLong*comptG + coeffDLong*comptD)/2.;
	dAngl = coeffDAngl*comptD - coeffGAngl*comptG;

	//Actualisation de la position du robot en xy et en orientation
	xR += dDist*cos(dAngl);
	yR += dDist*sin(dAngl);
	orientation += dAngl;

	//On calcule la distance séparant le robot de sa cible
	distanceCible = sqrt((xC-xR)*(xC-xR)+(yC-yR)*(yC-yR));

	//On regarde si la cible est à gauche ou à droite du robot
	if(yR > yC)
	{
		signe = 1;
	}else
	{
		signe = -1;
	}

	//On calcule l'angle entre le robot et la cible
	consigneOrientation = signe * acos((xC-xR)/((xC-xR)*(xC-xR)*(yC-yR)*(yC-yR));
						
	//On détermine les commandes à envoyer aux moteurs
	cmdD = abs((int)distanceCible);
	if(cmdD>255)
	{
	  	cmdD = 255;
	}
	cmdG = cmdD;
									   
	//Calcul de l'erreur
	erreurAngle =  consigneOrientation - orientation;
									   
	//Calcul de la différence entre l'erreur au coup précédent et l'erreur actuelle.
	deltaErreur = erreurAngle - erreurPre;

	//Mise en mémoire de l'erreur actuelle
	erreurPre  = erreurAngle;
				
	//Calcul de la valeur à envoyer aux moteurs pour tourner
	int detltaCommande = coeffP*erreurAngle + coeffD * deltaErreur;
									   
	cmdG += deltaCommande;
	cmdD -= deltaCommande;
									   
	if(cmdD>255)
	{
	  	cmdD = 255;
	}else if(cmdD < 0)
	{
	  	cmdD = 0;
	}
									   
	if(cmdG>255)
	{
	  	cmdG = 255;
	}else if(cmdG < 0)
	{
	  	cmdG = 0;
	}
		
	
}
// Fin fonction principale
//-----------------------------------------------------------

//-----------------------------------------------------------
// Fonction d'intéruption pour le codeur gauche
// 
// Fonction appelée par un tic du codeur gauche. 
// On regarde le sens de rotation de celui-ci 
// (on teste l'état de la voie déphasée) et on 
// incrémente ou décrémente le compteur comptG.
void Reagir()
{  
	if(digitalRead(canalBG) == HIGH)
	{
		comptG--;        
	}
	else
	{
		comptG++;      
	}      
}
// Fin fonction d'intéruption pour le codeur gauche
//-----------------------------------------------------------


//-----------------------------------------------------------
// Fonction d'intéruption pour le codeur droit
// 
// Fonction appelée par un tic du codeur droit. 
// On regarde le sens de rotation de celui-ci 
// (on teste l'état de la voie déphasée) et on 
// incrémente ou décrémente le compteur comptD.
void ReagirBis()
{     
	if(digitalRead(canalBD) == HIGH)
	{
		comptD ++;
	}
	else
	{
		comptD--;     
	}
}
// Fin fonction d'intéruption pour le codeur droit
//-----------------------------------------------------------