Diff: odo_asserv.cpp

Revision:

14:dd3c756c6d48

Parent:

13:a72b0752aa6f

--- a/odo_asserv.cpp	Fri Sep 11 14:15:57 2020 +0000
+++ b/odo_asserv.cpp	Wed Sep 16 12:31:54 2020 +0000
@@ -1,17 +1,18 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
+//#define _PI_ 3.14159265359
 
 ///// VARIABLES
-
 Ticker ticker_odo;
 
 // Coeff à définir empiriquement
 const double coeffGLong = 5.956, coeffDLong = -5.956; // constantes permettant la transformation tic/millimètre
-//const double coeffGAngl = 12.4516203705, coeffDAngl = 12.725; // constantes permettant la transformation tic/degré
-const double coeffGAngl = 53791/(12*2*Pi), coeffDAngl = 54972/(12*2*Pi); // constantes permettant la transformation tic/radian
+//const double coeffGAngl = 53791/(12*2*_PI_), coeffDAngl = 54972/(12*2*_PI_); // constantes permettant la transformation tic/radian
+const double coeffGAngl = 713.425, coeffDAngl = 729.089; // constantes permettant la transformation tic/radian
 
 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
 
+
 ///// INTERRUPTIONS CODEURS
 
 void cdgaRise()
@@ -26,31 +27,70 @@
     else comptD--;
 }
 
-///// ODOMÉTRIE
+///*
+///// 1) ODOMÉTRIE Geonobot : Approximation par segment de droite
 
-// Variables et constantes
-//#define NbPulseCodeur 1000
-#define entraxe 245
-
-double x = 0, y = 0, phi = 0;
-double x0 = 0, y0 = 0, phi0 = 0;
-double dDist = 0, dAngl = 0;
+#define entraxe 245 // Distance entre les roues codeuses
+double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées
+double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées
+double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
 double distG = 0, distD = 0; // Distance parcourue par chaque roue
 
-// Approximation par segment de droite
-void odometrie()
+void odoGeonobot1()
 {
-    x0 = x;
-    y0 = y;
-    phi0 = phi;
+    xA = xB;
+    yA = yB;
+    phiA = phiB;
 
     dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
     dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
 
-    x = x0 + dDist * cos(phi0);
-    y = y0 + dDist * sin(phi0);
-    phi = phi0 + dAngl;
-    
+    xB = xA + dDist * cos(phiA);
+    yB = yA + dDist * sin(phiA);
+    phiB = phiA + dAngl;
+
     comptG = 0;
     comptD = 0;
-}
\ No newline at end of file
+}
+//*/
+
+/*
+///// 2) ODOMÉTRIE Geonobot : Approximation par arc de cercle
+
+#define entraxe 245 // Distance entre les roues codeuses
+double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées
+double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées
+double xO = 0, yO = 0; // Centre du cercle de rayon R
+double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
+double distG = 0, distD = 0; // Distance parcourue par chaque roue
+double rayon = 0;
+
+void odoGeonobot2()
+{
+    xA = xB;
+    yA = yB;
+    phiA = phiB;
+
+    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
+    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
+    rayon = dDist / dAngl;
+
+    xO = xA - R*sin(phiA);
+    yO = yA + R*cos(phiA);
+
+    phiB = phiA + dAngl;
+
+    if (dAngl == 0) {
+        xB = xO + R*sin(phiB);
+        yB = yO + R*cos(phiB);
+    }
+
+    else {
+        xB = xO + dDist*cos(phiB);
+        yB = yO + dDist*sin(phiB);
+    }
+
+    comptG = 0;
+    comptD = 0;
+}
+*/