AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed DRV8825

Files at this revision

API Documentation at this revision

Revision 14:dd3c756c6d48, committed 2020-09-16

Comitter:
Nanaud
Date:
Wed Sep 16 12:31:54 2020 +0000
Parent:
13:a72b0752aa6f
Child:
15:43f5bda97488
Child:
16:ae65ce77b1f9
Commit message:
Odo 16.09.2020

Changed in this revision

debugPC.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
odo_asserv.cpp Show annotated file Show diff for this revision Revisions of this file
odo_asserv.h Show annotated file Show diff for this revision Revisions of this file
pins.h Show annotated file Show diff for this revision Revisions of this file 
--- 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