Revision 14:dd3c756c6d48, committed 2020-09-16

Comitter:

Nanaud

Date:

Wed Sep 16 12:31:54 2020 +0000

Parent:

13:a72b0752aa6f

Child:

15:43f5bda97488

Commit message:

Odo 16.09.2020

Changed in this revision

--- a/debugPC.cpp	Fri Sep 11 14:15:57 2020 +0000
+++ b/debugPC.cpp	Wed Sep 16 12:31:54 2020 +0000
@@ -118,7 +118,7 @@
             pc.printf("cofr : Enregistrement nb tics et reset\r\n");
             pc.printf("cofa : Affichage nb tics\r\n");
             pc.printf("parc : Distance a parcourir\r\n");
-            pc.printf("odom : Odometrie\r\n");
+            pc.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
             pc.printf("\n\r");
             // bt
             bt.printf("\n\n\r###HELP###\n\r");
@@ -135,7 +135,7 @@
             bt.printf("cofr : Enregistrement nb tics et reset\r\n");
             bt.printf("cofa : Affichage nb tics\r\n");
             bt.printf("parc : Distance a parcourir\r\n");
-            bt.printf("odom : Odometrie\r\n");
+            bt.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
             bt.printf("\n\r");
             break;
         case 1:     //usao
@@ -274,12 +274,11 @@
             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];
+        case 20: // odom
+            bt.printf("Odometrie :\n\r");
+            bt.printf("xB = %f\n\r", xB);
+            bt.printf("yB = %f\n\r", yB);
+            bt.printf("phiB = %f\n\r", phiB*180/_PI_);
             break;
         default:
             pc.printf("Commande invalide\n\r");
@@ -386,7 +385,7 @@
 
 void affOdo()
 {
-    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*180/Pi); 
+    if(aff_odo[0]) bt.printf("x = %f\n\r", xB);
+    if(aff_odo[1]) bt.printf("y = %f\n\r", yB);
+    if(aff_odo[2]) bt.printf("phi = %f\n\r", phiB*180/_PI_);
 }

--- a/main.cpp	Fri Sep 11 14:15:57 2020 +0000
+++ b/main.cpp	Wed Sep 16 12:31:54 2020 +0000
@@ -1,11 +1,12 @@
 // Nom du fichier : main.cpp
 #include "pins.h"
 
-void btnFct(){
+void btnFct()
+{
     mot_dis();
     aff_cd[0] = 0;
     aff_cd[1] = 0;
-    
+
     pc.printf("comptG = %d\r\n",comptG);
     pc.printf("comptD = %d\r\n",comptD);
     bt.printf("comptG = %d\r\n",comptG);
@@ -29,10 +30,9 @@
 
     //ticker_US.attach(&captUS_trig,0.2); // On apelle cette fonction toutes 0.2 secondes
     //ticker_affUS.attach(&affUltrasons,1.0);
-    //ticker_affcd.attach(&affCodeurs,1.0);
-    //tickTest.attach(&test4,0.1);
-    ticker_odo.attach(&odometrie,0.02); 
-    ticker_affodo.attach(&affOdo,1);
+    ticker_affcd.attach(&affCodeurs,1.0);
+    //ticker_odo.attach(&odoGeonobot1,0.2);
+    //ticker_affodo.attach(&affOdo,1.0);
 
     // Init capteurs à ultrasons
     captUS_init();
@@ -57,10 +57,8 @@
     cddA.rise(&cddaRise);
     cdgA.mode(PullUp);
     cddA.mode(PullUp);
-    
-    // Odometrie
-    
 
-
-    while(1) {}
+    while(1) {
+        
+    }
 }

--- 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;
+}
+*/

--- a/odo_asserv.h	Fri Sep 11 14:15:57 2020 +0000
+++ b/odo_asserv.h	Wed Sep 16 12:31:54 2020 +0000
@@ -11,7 +11,8 @@
 
 //ODOMETRIE
 extern Ticker ticker_odo;
-void odometrie();
-extern double x;
-extern double y;
-extern double phi;
+void odoGeonobot1();
+void odoGeonobot2();
+extern double xB;
+extern double yB;
+extern double phiB;

--- a/pins.h	Fri Sep 11 14:15:57 2020 +0000
+++ b/pins.h	Wed Sep 16 12:31:54 2020 +0000
@@ -10,7 +10,8 @@
 #include "odo_asserv.h"
 
 // #define
-#define Pi 3.14159265359
+//#define Pi 3.14159265359
+#define _PI_ 3.14159265359
 //#define NbPulseCodeur 1000
 //#define ecartCodeuse 245 // Distance en mm entre les deux roues codeuses
 #define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm