Revision 13:a72b0752aa6f, committed 2020-09-11

Comitter:

Nanaud

Date:

Fri Sep 11 14:15:57 2020 +0000

Parent:

12:2c312916a621

Child:

14:dd3c756c6d48

Commit message:

Vers odo 2

Changed in this revision

--- a/debugPC.cpp	Fri Sep 11 13:47:56 2020 +0000
+++ b/debugPC.cpp	Fri Sep 11 14:15:57 2020 +0000
@@ -118,6 +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("\n\r");
             // bt
             bt.printf("\n\n\r###HELP###\n\r");
@@ -134,6 +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("\n\r");
             break;
         case 1:     //usao
@@ -384,11 +386,7 @@
 
 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); 
+    if(aff_odo[2]) bt.printf("phi = %f\n\r", phi*180/Pi); 
 }

--- a/main.cpp	Fri Sep 11 13:47:56 2020 +0000
+++ b/main.cpp	Fri Sep 11 14:15:57 2020 +0000
@@ -29,9 +29,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);
+    //ticker_affcd.attach(&affCodeurs,1.0);
     //tickTest.attach(&test4,0.1);
-    ticker_odo.attach(&odometrie,0.2); 
+    ticker_odo.attach(&odometrie,0.02); 
     ticker_affodo.attach(&affOdo,1);
 
     // Init capteurs à ultrasons

--- a/odo_asserv.cpp	Fri Sep 11 13:47:56 2020 +0000
+++ b/odo_asserv.cpp	Fri Sep 11 14:15:57 2020 +0000
@@ -8,7 +8,7 @@
 // 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
+const double coeffGAngl = 53791/(12*2*Pi), coeffDAngl = 54972/(12*2*Pi); // constantes permettant la transformation tic/radian
 
 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
 
@@ -29,15 +29,15 @@
 ///// ODOMÉTRIE
 
 // Variables et constantes
-#define NbPulseCodeur 1000
+//#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
+double x = 0, y = 0, phi = 0;
+double x0 = 0, y0 = 0, phi0 = 0;
+double dDist = 0, dAngl = 0;
+double distG = 0, distD = 0; // Distance parcourue par chaque roue
 
+// Approximation par segment de droite
 void odometrie()
 {
     x0 = x;
@@ -50,17 +50,7 @@
     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

--- a/odo_asserv.h	Fri Sep 11 13:47:56 2020 +0000
+++ b/odo_asserv.h	Fri Sep 11 14:15:57 2020 +0000
@@ -12,6 +12,6 @@
 //ODOMETRIE
 extern Ticker ticker_odo;
 void odometrie();
-extern float x;
-extern float y;
-extern float phi_deg;
+extern double x;
+extern double y;
+extern double phi;

--- a/odo_asserv_2.cpp	Fri Sep 11 13:47:56 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,66 +0,0 @@
-//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

--- a/odo_asserv_2.h	Fri Sep 11 13:47:56 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,17 +0,0 @@
-// 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