AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

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?

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
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 }