AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

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?

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