AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
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?
User | Revision | Line number | New 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 | } |