Revision 2:094c09903a9c, committed 2020-07-09

Comitter:

Nanaud

Date:

Thu Jul 09 21:03:01 2020 +0000

Parent:

1:2fe8c402ee79

Child:

3:3ba377aafdfd

Commit message:

Debut du code pour l'odometrie

Changed in this revision

--- a/codeurs.cpp	Wed Jul 08 20:42:19 2020 +0000
+++ b/codeurs.cpp	Thu Jul 09 21:03:01 2020 +0000
@@ -3,24 +3,28 @@
 
 // Variables globales
 long cpt_cdgA=0; // Codeur de gauche
-long cpt_cdgB=0;
+//long cpt_cdgB=0;
 long cpt_cddA=0; // Codeur de droite
-long cpt_cddB=0;
+//long cpt_cddB=0;
 
 void cdgaRise(){
     if(cdgB) cpt_cdgA++;
     else cpt_cdgA--;
 }
 
+/*
 void cdgbRise(){
     if(cdgA) cpt_cdgB--;
     else cpt_cdgB++;    
 }
+*/
 
 void cddaRise(){
     
 }
 
+/*
 void cddbRise(){
     
-}
\ No newline at end of file
+}
+*/
\ No newline at end of file

--- a/codeurs.h	Wed Jul 08 20:42:19 2020 +0000
+++ b/codeurs.h	Thu Jul 09 21:03:01 2020 +0000
@@ -2,10 +2,10 @@
 
 // extern
 extern long cpt_cdgA;
-extern long cpt_cdgB;
+//extern long cpt_cdgB;
 extern long cpt_cddA;
-extern long cpt_cddB;
+//extern long cpt_cddB;
 
 // Prototypes
 void cdgaRise();
-void cdgbRise();
\ No newline at end of file
+//void cdgbRise();
\ No newline at end of file

--- a/debug.cpp	Wed Jul 08 20:42:19 2020 +0000
+++ b/debug.cpp	Thu Jul 09 21:03:01 2020 +0000
@@ -140,6 +140,6 @@
 
 void affCodeurs(){
     if(aff_cd[0]) printf("CdgA = %d\n\r", cpt_cdgA);   
-    if(aff_cd[1]) printf("CdgB = %d\n\r", cpt_cdgB); 
+    //if(aff_cd[1]) printf("CdgB = %d\n\r", cpt_cdgB); 
 }    
     
\ No newline at end of file

--- a/main.cpp	Wed Jul 08 20:42:19 2020 +0000
+++ b/main.cpp	Thu Jul 09 21:03:01 2020 +0000
@@ -27,7 +27,7 @@
     
     // Init codeurs 
     cdgA.rise(&cdgaRise);
-    cdgB.rise(&cdgbRise);
+    //cdgB.rise(&cdgbRise);
     cdgA.mode(PullUp);
     cdgB.mode(PullUp);
     
@@ -36,7 +36,6 @@
     ticker_affUS.attach(&affUltrasons,1.0);
     ticker_affcd.attach(&affCodeurs,1.0);
     aff_cd[0]=!aff_cd[0];
-    aff_cd[1]=!aff_cd[1];
     
     while(1) {}
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odo_asserv.cpp	Thu Jul 09 21:03:01 2020 +0000
@@ -0,0 +1,31 @@
+//Nom du fichier : odo_asserv.cpp
+#include "pins.h"
+#define Pi 3.14159265359
+
+#define ecart 120 // Distance en mm entre les deux roues motrices
+#define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
+#define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)
+
+// Variables globales
+// cpt_cdgA est le compteur d'impulsion du codeur de gauche
+// cpt_cddA est le compteur d'impulsion du codeur de droite
+int posX, posY; // Position et orientation 
+float theta; 
+
+void odometrie(){
+
+    int distG = (cpt_cdgA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    int distD = (cpt_cddA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    
+    int distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
+    int rayon = (ecart /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
+    
+    int dTheta = distRobot / rayon; // Changement d'orientation => Commande
+    
+    int posX0 = posX - rayon*cos(theta); 
+    int posY0 = posY - rayon*sin(theta);
+    
+    theta = theta + dTheta; // Mise à jour des coordonnées
+    posX = posX0 + rayon*cos(theta);
+    posY = posY0 + rayon*sin(theta);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/odo_asserv.h	Thu Jul 09 21:03:01 2020 +0000
@@ -0,0 +1,2 @@
+// Nom du fichier : odo.h
+

--- a/pins.h	Wed Jul 08 20:42:19 2020 +0000
+++ b/pins.h	Thu Jul 09 21:03:01 2020 +0000
@@ -2,11 +2,13 @@
 #include "mbed.h"
 #include "DRV8825.h"
 
-#include <string.h>   
+#include <string.h> 
+#include <math.h>  
 #include "debug.h"
 #include "captUS.h"
 #include "motors.h"
-#include "codeurs.h"   
+#include "codeurs.h"
+#include "odo_asserv.h"
 
 //Capteurs à ultrasons
 extern DigitalOut trigger;