Diff: odo_asserv.cpp

Revision:

3:3ba377aafdfd

Parent:

2:094c09903a9c

Child:

5:34ed652f8c31

--- a/odo_asserv.cpp	Thu Jul 09 21:03:01 2020 +0000
+++ b/odo_asserv.cpp	Wed Jul 15 17:51:04 2020 +0000
@@ -2,28 +2,28 @@
 #include "pins.h"
 #define Pi 3.14159265359
 
-#define ecart 120 // Distance en mm entre les deux roues motrices
+#define capt 1000 // Nb d'impulsions
+#define ecart 245 // Distance en mm entre les deux roues codeuses
 #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 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 
+    float distG = (cpt_cdgA * perimetreRoueCodeuse) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    float distD = (cpt_cddA * perimetreRoueCodeuse) / capt; // 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
+    float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
+    float 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);
+    float dTheta = distRobot / rayon; // Changement d'orientation => Commande
+    float posX0 = posX - rayon*cos(theta); 
+    float posY0 = posY - rayon*sin(theta);
     
     theta = theta + dTheta; // Mise à jour des coordonnées
     posX = posX0 + rayon*cos(theta);