AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
odo_asserv.cpp@3:3ba377aafdfd, 2020-07-15 (annotated)
- Committer:
- Nanaud
- Date:
- Wed Jul 15 17:51:04 2020 +0000
- Revision:
- 3:3ba377aafdfd
- Parent:
- 2:094c09903a9c
- Child:
- 5:34ed652f8c31
Correction odometrie, Ajout fonction debug pour test de rotation;
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 | 3:3ba377aafdfd | 5 | #define capt 1000 // Nb d'impulsions |
Nanaud | 3:3ba377aafdfd | 6 | #define ecart 245 // Distance en mm entre les deux roues codeuses |
Nanaud | 2:094c09903a9c | 7 | #define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm |
Nanaud | 2:094c09903a9c | 8 | #define perimetreRoueCodeuse (diametreRoueCodeuse * Pi) |
Nanaud | 2:094c09903a9c | 9 | |
Nanaud | 2:094c09903a9c | 10 | // Variables globales |
Nanaud | 2:094c09903a9c | 11 | // cpt_cdgA est le compteur d'impulsion du codeur de gauche |
Nanaud | 2:094c09903a9c | 12 | // cpt_cddA est le compteur d'impulsion du codeur de droite |
Nanaud | 3:3ba377aafdfd | 13 | float posX, posY; // Position et orientation |
Nanaud | 2:094c09903a9c | 14 | float theta; |
Nanaud | 2:094c09903a9c | 15 | |
Nanaud | 2:094c09903a9c | 16 | void odometrie(){ |
Nanaud | 2:094c09903a9c | 17 | |
Nanaud | 3:3ba377aafdfd | 18 | float distG = (cpt_cdgA * perimetreRoueCodeuse) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse |
Nanaud | 3:3ba377aafdfd | 19 | float distD = (cpt_cddA * perimetreRoueCodeuse) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse |
Nanaud | 2:094c09903a9c | 20 | |
Nanaud | 3:3ba377aafdfd | 21 | float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm) |
Nanaud | 3:3ba377aafdfd | 22 | float rayon = (ecart /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle |
Nanaud | 2:094c09903a9c | 23 | |
Nanaud | 3:3ba377aafdfd | 24 | float dTheta = distRobot / rayon; // Changement d'orientation => Commande |
Nanaud | 3:3ba377aafdfd | 25 | float posX0 = posX - rayon*cos(theta); |
Nanaud | 3:3ba377aafdfd | 26 | float 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 | } |