Diff: odo_asserv_2.cpp

Revision:

12:2c312916a621

diff -r e62133022f88 -r 2c312916a621 odo_asserv_2.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odo_asserv_2.cpp	Fri Sep 11 13:47:56 2020 +0000
@@ -0,0 +1,66 @@
+//Nom du fichier : odo_asserv_2.cpp
+#include "pins.h"
+
+///// VARIABLES
+
+Ticker ticker_odo_2;
+
+// 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 = 12*2*Pi*53791, coeffDAngl = 12*2*Pi*54972; // constantes permettant la transformation tic/radian
+
+long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
+
+///// INTERRUPTIONS CODEURS
+
+void cdgaRise()
+{
+    if(cdgB) comptG++;
+    else comptG--;
+}
+
+void cddaRise()
+{
+    if(cddB) comptD++;
+    else comptD--;
+}
+
+///// ODOMÉTRIE
+
+// Variables et constantes
+#define NbPulseCodeur 1000
+#define entraxe 245
+
+float x = 0, y = 0, phi = 0;
+float phi_deg;
+float x0 = 0, y0 = 0, phi0 = 0;
+float dDist = 0, dAngl = 0;
+float distG = 0, distD = 0; // Distance parcourue par chaque roue
+
+void odometrie()
+{
+    x0 = x;
+    y0 = y;
+    phi0 = phi;
+
+    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;
+    phi_deg = phi*180/Pi;
+    
+    
+    comptG = 0;
+    comptD = 0;
+}
+
+
+///// CONSIGNE ANGLE
+
+void angle()
+{
+    
+}
\ No newline at end of file