AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

odo_asserv.cpp@2:094c09903a9c, 2020-07-09 (annotated)

Committer:
Nanaud
Date:
Thu Jul 09 21:03:01 2020 +0000
Revision:
2:094c09903a9c
Child:
3:3ba377aafdfd
Debut du code pour l'odometrie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nanaud 2:094c09903a9c 1 //Nom du fichier : odo_asserv.cpp
Nanaud 2:094c09903a9c 2 #include "pins.h"
Nanaud 2:094c09903a9c 3 #define Pi 3.14159265359
Nanaud 2:094c09903a9c 4
Nanaud 2:094c09903a9c 5 #define ecart 120 // Distance en mm entre les deux roues motrices
Nanaud 2:094c09903a9c 6 #define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
Nanaud 2:094c09903a9c 7 #define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)
Nanaud 2:094c09903a9c 8
Nanaud 2:094c09903a9c 9 // Variables globales
Nanaud 2:094c09903a9c 10 // cpt_cdgA est le compteur d'impulsion du codeur de gauche
Nanaud 2:094c09903a9c 11 // cpt_cddA est le compteur d'impulsion du codeur de droite
Nanaud 2:094c09903a9c 12 int posX, posY; // Position et orientation
Nanaud 2:094c09903a9c 13 float theta;
Nanaud 2:094c09903a9c 14
Nanaud 2:094c09903a9c 15 void odometrie(){
Nanaud 2:094c09903a9c 16
Nanaud 2:094c09903a9c 17 int distG = (cpt_cdgA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse
Nanaud 2:094c09903a9c 18 int distD = (cpt_cddA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse
Nanaud 2:094c09903a9c 19
Nanaud 2:094c09903a9c 20 int distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
Nanaud 2:094c09903a9c 21 int rayon = (ecart /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
Nanaud 2:094c09903a9c 22
Nanaud 2:094c09903a9c 23 int dTheta = distRobot / rayon; // Changement d'orientation => Commande
Nanaud 2:094c09903a9c 24
Nanaud 2:094c09903a9c 25 int posX0 = posX - rayon*cos(theta);
Nanaud 2:094c09903a9c 26 int posY0 = posY - rayon*sin(theta);
Nanaud 2:094c09903a9c 27
Nanaud 2:094c09903a9c 28 theta = theta + dTheta; // Mise à jour des coordonnées
Nanaud 2:094c09903a9c 29 posX = posX0 + rayon*cos(theta);
Nanaud 2:094c09903a9c 30 posY = posY0 + rayon*sin(theta);
Nanaud 2:094c09903a9c 31 }