Revision 12:2c312916a621, committed 2020-09-11

Comitter:

plmir

Date:

Fri Sep 11 13:47:56 2020 +0000

Parent:

11:e62133022f88

Child:

13:a72b0752aa6f

Commit message:

Odometrie

Changed in this revision

--- a/debug.h	Fri Sep 11 11:11:56 2020 +0000
+++ b/debug.h	Fri Sep 11 13:47:56 2020 +0000
@@ -8,8 +8,11 @@
 extern Serial bt;
 extern Ticker ticker_affUS;
 extern Ticker ticker_affcd;
+extern Ticker ticker_affodo;
+
 extern bool aff_US[6];
 extern bool aff_cd[4];
+extern bool aff_odo[3];
 extern int cmdType;
 
 // Prototypes
@@ -23,3 +26,4 @@
 void StringToDist(char *cmd);
 void affUltrasons();
 void affCodeurs();
+void affOdo();

--- a/debugPC.cpp	Fri Sep 11 11:11:56 2020 +0000
+++ b/debugPC.cpp	Fri Sep 11 13:47:56 2020 +0000
@@ -5,8 +5,11 @@
 Serial pc(USBTX, USBRX);
 Ticker ticker_affUS;
 Ticker ticker_affcd;
+Ticker ticker_affodo;
+
 bool aff_US[6];
 bool aff_cd[4];
+bool aff_odo[3];
 bool m_dis = true;    // 0 : START // 1 : STOP
 bool m_avance = false;   // 0 : BACKWARD // 1 : FORWARD
 bool mtrt_sens = false;
@@ -85,6 +88,7 @@
         "cofr", //17
         "cofa", //18
         "parc", //19
+        "odom", //20
         0
     };
 
@@ -268,6 +272,13 @@
             bt.printf("Distance a parcourir : ");
             cmdType=3;
             break;
+        case 20:     //résultat odométrie
+            pc.printf("Results Odometry\n\r");
+            bt.printf("Results Odometry\n\r");
+            aff_odo[0]=!aff_odo[0];
+            aff_odo[1]=!aff_odo[1];
+            aff_odo[2]=!aff_odo[2];
+            break;
         default:
             pc.printf("Commande invalide\n\r");
             bt.printf("Commande invalide\n\r");
@@ -370,3 +381,14 @@
     if(aff_cd[0]) bt.printf("comptG = %d\n\r", comptG);
     if(aff_cd[1]) bt.printf("comptD = %d\n\r", comptD);
 }
+
+void affOdo()
+{
+    if(aff_odo[0]) pc.printf("x = %f\n\r", x);
+    if(aff_odo[1]) pc.printf("y = %f\n\r", y);
+    if(aff_odo[2]) pc.printf("phi = %f\n\r", phi_deg);
+
+    if(aff_odo[0]) bt.printf("x = %f\n\r", x);
+    if(aff_odo[1]) bt.printf("y = %f\n\r", y);
+    if(aff_odo[2]) bt.printf("phi = %f\n\r", phi_deg); 
+}

--- a/main.cpp	Fri Sep 11 11:11:56 2020 +0000
+++ b/main.cpp	Fri Sep 11 13:47:56 2020 +0000
@@ -31,6 +31,8 @@
     //ticker_affUS.attach(&affUltrasons,1.0);
     ticker_affcd.attach(&affCodeurs,1.0);
     //tickTest.attach(&test4,0.1);
+    ticker_odo.attach(&odometrie,0.2); 
+    ticker_affodo.attach(&affOdo,1);
 
     // Init capteurs à ultrasons
     captUS_init();
@@ -57,7 +59,7 @@
     cddA.mode(PullUp);
     
     // Odometrie
-
+    
 
 
     while(1) {}

--- a/odo_asserv.cpp	Fri Sep 11 11:11:56 2020 +0000
+++ b/odo_asserv.cpp	Fri Sep 11 13:47:56 2020 +0000
@@ -3,6 +3,8 @@
 
 ///// 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é
@@ -31,6 +33,7 @@
 #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
@@ -41,12 +44,17 @@
     y0 = y;
     phi0 = phi;
 
-    dDist = ((comptG * coeffGLong) + (comptD * coeffDLong)) / 2;
-    dAngl = (comptD * coeffDAngl) - (comptG * coeffGAngl);
+    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
+    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
 
-    x = x0 + dDist * cos(dAngl);
-    y = y0 + dDist * sin(dAngl);
+    x = x0 + dDist * cos(phi0);
+    y = y0 + dDist * sin(phi0);
     phi = phi0 + dAngl;
+    phi_deg = phi*180/Pi;
+    
+    
+    comptG = 0;
+    comptD = 0;
 }
 
 

--- a/odo_asserv.h	Fri Sep 11 11:11:56 2020 +0000
+++ b/odo_asserv.h	Fri Sep 11 13:47:56 2020 +0000
@@ -7,4 +7,11 @@
 
 // Prototypes
 void cdgaRise();
-void cddaRise();
\ No newline at end of file
+void cddaRise();
+
+//ODOMETRIE
+extern Ticker ticker_odo;
+void odometrie();
+extern float x;
+extern float y;
+extern float phi_deg;

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odo_asserv_2.h	Fri Sep 11 13:47:56 2020 +0000
@@ -0,0 +1,17 @@
+// Nom du fichier : odo_asserv_2.h
+
+// extern
+// CODEURS
+extern long comptG;
+extern long comptD;
+
+// Prototypes
+void cdgaRise();
+void cddaRise();
+
+//ODOMETRIE
+extern Ticker ticker_odo;
+void odometrie();
+extern float x;
+extern float y;
+extern float phi_deg;
\ No newline at end of file