Diff: odo_asserv.cpp

Revision:

2:094c09903a9c

Child:

3:3ba377aafdfd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odo_asserv.cpp	Thu Jul 09 21:03:01 2020 +0000
@@ -0,0 +1,31 @@
+//Nom du fichier : odo_asserv.cpp
+#include "pins.h"
+#define Pi 3.14159265359
+
+#define ecart 120 // Distance en mm entre les deux roues motrices
+#define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
+#define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)
+
+// Variables globales
+// cpt_cdgA est le compteur d'impulsion du codeur de gauche
+// cpt_cddA est le compteur d'impulsion du codeur de droite
+int posX, posY; // Position et orientation 
+float theta; 
+
+void odometrie(){
+
+    int distG = (cpt_cdgA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    int distD = (cpt_cddA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    
+    int distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
+    int rayon = (ecart /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
+    
+    int dTheta = distRobot / rayon; // Changement d'orientation => Commande
+    
+    int posX0 = posX - rayon*cos(theta); 
+    int posY0 = posY - rayon*sin(theta);
+    
+    theta = theta + dTheta; // Mise à jour des coordonnées
+    posX = posX0 + rayon*cos(theta);
+    posY = posY0 + rayon*sin(theta);
+}
\ No newline at end of file