AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
odo_asserv.cpp@5:34ed652f8c31, 2020-07-21 (annotated)
- Committer:
- Nanaud
- Date:
- Tue Jul 21 19:33:38 2020 +0000
- Revision:
- 5:34ed652f8c31
- Parent:
- 3:3ba377aafdfd
- Child:
- 6:ea6b30c4bb01
Commande Bluetooth test moteur
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 | |
Nanaud | 2:094c09903a9c | 4 | // Variables globales |
Nanaud | 2:094c09903a9c | 5 | // cpt_cdgA est le compteur d'impulsion du codeur de gauche |
Nanaud | 2:094c09903a9c | 6 | // cpt_cddA est le compteur d'impulsion du codeur de droite |
Nanaud | 3:3ba377aafdfd | 7 | float posX, posY; // Position et orientation |
Nanaud | 2:094c09903a9c | 8 | float theta; |
Nanaud | 2:094c09903a9c | 9 | |
Nanaud | 2:094c09903a9c | 10 | void odometrie(){ |
Nanaud | 2:094c09903a9c | 11 | |
Nanaud | 5:34ed652f8c31 | 12 | float distG = (cpt_cdgA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse |
Nanaud | 5:34ed652f8c31 | 13 | float distD = (cpt_cddA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse |
Nanaud | 2:094c09903a9c | 14 | |
Nanaud | 3:3ba377aafdfd | 15 | float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm) |
Nanaud | 5:34ed652f8c31 | 16 | float rayon = (ecartCodeuse /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle |
Nanaud | 2:094c09903a9c | 17 | |
Nanaud | 3:3ba377aafdfd | 18 | float dTheta = distRobot / rayon; // Changement d'orientation => Commande |
Nanaud | 3:3ba377aafdfd | 19 | float posX0 = posX - rayon*cos(theta); |
Nanaud | 3:3ba377aafdfd | 20 | float posY0 = posY - rayon*sin(theta); |
Nanaud | 2:094c09903a9c | 21 | |
Nanaud | 2:094c09903a9c | 22 | theta = theta + dTheta; // Mise à jour des coordonnées |
Nanaud | 2:094c09903a9c | 23 | posX = posX0 + rayon*cos(theta); |
Nanaud | 2:094c09903a9c | 24 | posY = posY0 + rayon*sin(theta); |
Nanaud | 2:094c09903a9c | 25 | } |