Revision 4:deef042e9c02, committed 2019-05-08

Comitter:

GuillaumeCH

Date:

Wed May 08 20:46:46 2019 +0000

Parent:

3:1dba6eca01ad

Commit message:

f

Changed in this revision

--- a/commande_moteurs.cpp	Mon May 06 13:48:45 2019 +0000
+++ b/commande_moteurs.cpp	Wed May 08 20:46:46 2019 +0000
@@ -221,9 +221,9 @@
             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
             
-            printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+            //printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
     }
-    //test_rotation_abs(angle_vise_deg);
+    test_rotation_abs(angle_vise_deg);
     vitesse_nulle_G(0);
     vitesse_nulle_D(0);
     wait(0.3);

--- a/deplacement.cpp	Mon May 06 13:48:45 2019 +0000
+++ b/deplacement.cpp	Wed May 08 20:46:46 2019 +0000
@@ -13,6 +13,8 @@
     somme_erreur_G = 0;
     erreur_precedente_D = 0;
     erreur_precedente_G = 0;
+    compteur_asser =0;
+    somme_y=0;
     
     for (int k =0; k<5;k++){
         erreur_glissee_D[k] = 0;
@@ -110,14 +112,14 @@
     if(abs(vitesse_G) > 900){
         vitesse_local_G=900;
     }
-    if(abs(vitesse_G)<10){
-        vitesse_local_G=0;
+    if(abs(vitesse_G)<5){
+        vitesse_local_G=2;
     }
     if(abs(vitesse_D) > 900){
         vitesse_local_D=900;
     }
-    if(abs(vitesse_D)< 10){
-        vitesse_local_D=0;
+    if(abs(vitesse_D)< 5){
+        vitesse_local_D=2;
     
     }
     ;
@@ -147,86 +149,38 @@
         set_PWM_moteur_D(0);
     }
 }
-void deplacement::reculer_un_peu(int distance){
+void deplacement::reculer_un_peu(int distance)
+{
+    somme_y=0;
+    // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
     motors_on();
-    long int x_ini = get_x_actuel();
-    long int y_ini = get_y_actuel();
+    actualise_position();
+    double x_ini = get_x_actuel();
+    double y_ini = get_y_actuel();
     double angle_vise_deg = get_angle();
-    double angle_vise=angle_vise_deg*3.142/180;
+    double angle_vise=angle_vise_deg*3.1416/180;
     double angle = get_angle();
     
-    long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
-    long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+    double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
+    double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+    
+    double x_actuel = get_x_actuel();
+    double y_actuel = get_y_actuel();
     
-    long int x_actuel = get_x_actuel();
-    long int y_actuel = get_y_actuel();
-    long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
-    long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
     
+    double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+    double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+    
+    //long int y_local_prec = y_local;
     float vitesse_G;
     float vitesse_D;
     
     angle = get_angle();
-    
-    //printf("YOOOO\n\n ");
-    while (distance+x_local>0){
-    
-            vitesse_G = (distance+x_local)/70;
-            vitesse_D = vitesse_G;
-            if(vitesse_G >150){
-                vitesse_G=150;
-                vitesse_D=150;
-            }
-            if (vitesse_G<-150){
-                vitesse_G=-150;
-                vitesse_D=-150;
-            }
+    float Kip=0;
+    float Kpp= 0.05 ;
+    float Kdp= 10;
+    while (distance-x_local<0){
             
-            angle = get_angle();
-            vitesse_G = vitesse_G  - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local; // petit asser en angle
-            vitesse_D = vitesse_D  + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local;
-                
-            commande_vitesse(-vitesse_G,-vitesse_D);
-            actualise_position();
-            x_actuel = get_x_actuel();
-            y_actuel = get_y_actuel();
-            x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
-            y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
-            //printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
-        
-    }
-    test_rotation_abs(angle_vise_deg);
-    vitesse_nulle_G(0);
-    vitesse_nulle_D(0);
-    wait(0.3);
-    motors_stop();
-}
-
-void deplacement::ligne_droite(long int distance)
-{
-    // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
-    motors_on();
-    long int x_ini = get_x_actuel();
-    long int y_ini = get_y_actuel();
-    double angle_vise_deg = get_angle();
-    double angle_vise=angle_vise_deg*3.142/180;
-    double angle = get_angle();
-    
-    long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
-    long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
-    
-    long int x_actuel = get_x_actuel();
-    long int y_actuel = get_y_actuel();
-    long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
-    long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
-    
-    float vitesse_G;
-    float vitesse_D;
-    
-    angle = get_angle();
-    
-    while (distance-x_local>0){
-    
             vitesse_G = (distance-x_local)/70;
             vitesse_D = vitesse_G;
             if(vitesse_G >400){
@@ -239,54 +193,65 @@
             }
             
             angle = get_angle();
-            vitesse_G = vitesse_G  + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local; // petit asser en angle
-            vitesse_D = vitesse_D  - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local;
-                
+            
+            vitesse_G = vitesse_G  - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
+            vitesse_D = vitesse_D  + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
+            //consigne_D = vitesse_D;
+            //consigne_G = vitesse_G;
             commande_vitesse(vitesse_G,vitesse_D);
             actualise_position();
             x_actuel = get_x_actuel();
             y_actuel = get_y_actuel();
+            somme_y+=y_actuel;
+            //y_local_prec = y_local;
             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
-            //printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
-        
+            if (compteur_asser==150){
+                compteur_asser=0;
+                //printf("%lf\n",get_y_actuel());
+            }
+            compteur_asser++;
+            printf("   VG : %f  VD : %f ; x_local : %lf, y_local : %lf, angle_vise : %f\n",vitesse_G,vitesse_D, x_local,y_local, angle);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
     }
+    //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
     test_rotation_abs(angle_vise_deg);
-    vitesse_nulle_G(0);
-    vitesse_nulle_D(0);
-    wait(0.3);
-    motors_stop();
+    //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
 }
+
+
+
 void deplacement::ligne_droite_v2(long int distance)
 {
+    somme_y=0;
     // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
     motors_on();
-    long int x_ini = get_x_actuel();
-    long int y_ini = get_y_actuel();
+    actualise_position();
+    double x_ini = get_x_actuel();
+    double y_ini = get_y_actuel();
     double angle_vise_deg = get_angle();
-    double angle_vise=angle_vise_deg*3.142/180;
+    double angle_vise=angle_vise_deg*3.1416/180;
     double angle = get_angle();
     
-    long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
-    long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
+    double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
+    double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
     
-    long int x_actuel = get_x_actuel();
-    long int y_actuel = get_y_actuel();
+    double x_actuel = get_x_actuel();
+    double y_actuel = get_y_actuel();
     
-    long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
-    long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
+    
+    double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
+    double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
     
     //long int y_local_prec = y_local;
-    
     float vitesse_G;
     float vitesse_D;
     
     angle = get_angle();
-    
-    float Ki2= 0.000015;
-    float Kp2= 0.04;
+    float Kip=0;
+    float Kpp= 0.05 ;
+    float Kdp= 10;
     while (distance-x_local>0){
-    
+            
             vitesse_G = (distance-x_local)/70;
             vitesse_D = vitesse_G;
             if(vitesse_G >400){
@@ -299,52 +264,65 @@
             }
             
             angle = get_angle();
-            //vitesse_G = vitesse_G + (y_local * 0.02) + (y_local - y_local_prec)*2;
-            //vitesse_D = vitesse_D - (y_local * 0.02) - (y_local - y_local_prec)*2;
-            vitesse_G = vitesse_G * (1 + Ki2*y_local + Kp2 * diff_angle(angle_vise_deg, angle));
-            vitesse_D = vitesse_D * (1 - Ki2*y_local - Kp2 * diff_angle(angle_vise_deg, angle));
+            
+            vitesse_G = vitesse_G  + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
+            vitesse_D = vitesse_D  - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
+            //consigne_D = vitesse_D;
+            //consigne_G = vitesse_G;
             commande_vitesse(vitesse_G,vitesse_D);
             actualise_position();
             x_actuel = get_x_actuel();
             y_actuel = get_y_actuel();
+            somme_y+=y_actuel;
             //y_local_prec = y_local;
             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
-            
-            printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
+            if (compteur_asser==150){
+                compteur_asser=0;
+                //printf("%lf\n",get_y_actuel());
+            }
+            compteur_asser++;
+            //printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise_deg);// sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
     }
+    //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
     test_rotation_abs(angle_vise_deg);
-    vitesse_nulle_G(0);
-    vitesse_nulle_D(0);
-    wait(0.3);
-    motors_stop();
+    //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
 }
 
 void deplacement::test_rotation_rel(double angle_vise)
 {
     // rotation de angle_vise
     motors_on();
-    float vitesse;
+    double vitesse=180;
     int sens;
     double angle = get_angle();
     angle_vise+=angle;
     borne_angle_d(angle_vise);
     if (diff_angle(angle,angle_vise)<=0){
         sens = -1;
+        //printf("negatif\n");
     }
     else{
         sens = 1;
+        
+        //printf("positif\n");
     }
-    while (sens*diff_angle(angle,angle_vise)>0)
+    //printf("diff : %lf ",diff_angle(angle,angle_vise));
+    while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
     {
-        vitesse=diff_angle(angle,angle_vise);
+        actualise_position();
+        angle = get_angle();
+        vitesse=1.5*sens*abs(diff_angle(angle,angle_vise));
         
         commande_vitesse(-vitesse,vitesse);
-        actualise_position();
-        angle = get_angle();
-        //printf("vitesse : %f", vitesse);
+        if (compteur_asser==150){
+                compteur_asser=0;
+                //printf("%lf\n",get_y_actuel());
+            }
+        compteur_asser++;
+        //printf("vitesse : %lf ", vitesse);
     }
-    
+    //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
     //printf(" x et y recu : %lf, %ld. distance parcourue : %ld ", sqrt((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel)), y_actuel, (x_actuel - x_ini)*(x_actuel - x_ini) + (y_actuel - y_ini)*(y_actuel - y_ini));
     //consigne_D = 0;
     //consigne_G = 0;
@@ -358,7 +336,7 @@
 void deplacement::test_rotation_abs(double angle_vise)
 {
     actualise_position();
-    printf("bite");
+    //printf("bite");
     double angle_rel = borne_angle_d(angle_vise-get_angle());
     test_rotation_rel(angle_rel);
 }
@@ -486,4 +464,13 @@
 void deplacement::bouton(){
     DigitalIn depart(USER_BUTTON);
     while (depart){}
+}
+
+void deplacement::poussette(){
+    motors_on();
+    commande_vitesse(150,150);
+    wait(2);
+    vitesse_nulle_G(0);
+    vitesse_nulle_D(0);
+    motors_stop();
 }
\ No newline at end of file

--- a/deplacement.h	Mon May 06 13:48:45 2019 +0000
+++ b/deplacement.h	Wed May 08 20:46:46 2019 +0000
@@ -19,7 +19,7 @@
         void test(void);
         void changement_consigne(int cons_D, int cons_G);
         void bouton();
-
+        void poussette();
     
     
     
@@ -54,6 +54,8 @@
         float c_G[TAILLE_TAB];
         int dix_ms;
         int consigne_tab[20][2];
+        int compteur_asser;
+        double somme_y;
 };
 
 

--- a/hardware.cpp	Mon May 06 13:48:45 2019 +0000
+++ b/hardware.cpp	Wed May 08 20:46:46 2019 +0000
@@ -9,17 +9,17 @@
 // PWM_MAX est définit dans réglage;
 bool moteurs_arret = false;
 
-//mot G
+
 XNucleoIHM02A1 *x_nucleo_ihm02a1; //Création d'une entité pour la carte de contôle des pas à pas
 L6470_init_t init[L6470DAISYCHAINSIZE] = {
-    /* First Motor. */
+    /* First Motor.G */
     {
         12,                           /* Motor supply voltage in V. */
         200,                           /* Min number of steps per revolution for the motor. */
         4,                           /* Max motor phase voltage in A. */
         7,                          /* Max motor phase voltage in V. */
         300,                         /* Motor initial speed [step/s]. */
-        247.4,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+        246.5,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
         1500.0,                        /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
         992.0,                         /* Motor maximum speed [step/s]. */
         0.0,                           /* Motor minimum speed [step/s]. */

--- a/main.cpp	Mon May 06 13:48:45 2019 +0000
+++ b/main.cpp	Wed May 08 20:46:46 2019 +0000
@@ -8,11 +8,15 @@
 
 int main()
 {
-    //init
+    //ini
     init_odometrie();
     init_hardware();
     srand(time(NULL));
     motors_on();
+    AnalogIn Ain(A0);
+    if(Ain.read()<0.5){
+          return 0;
+    }
     /*DigitalIn depart(USER_BUTTON);
     //Pause pour sauver le robot et l'ordi
     while(depart);*/
@@ -21,67 +25,25 @@
     Ticker asser;
     Timer t;
     t.start();
-    asser.attach(callback(&robot,&deplacement::asservissement),0.01);
-    robot.test();
-    asser.detach();
-    robot.vitesse_nulle_G(0);
-    robot.vitesse_nulle_D(0);
-    wait(0.2);
-    motors_stop();
-    robot.printftab();
-    //actualise_position();
-    //while(t.read()<5){
-        //debugEncoder();
-    //}
-    
-    //commande_vitesse(600,600);
-    
-    //robot.ligne_droite_v2(150000);
-    //asser.detach();
+    //asser.attach(callback(&robot,&deplacement::asservissement),0.01);
+    //robot.ligne_droite_v2(210000);
+
     //robot.ligne_droite_v2(210000);
-    //robot.test_rotation_rel(-90);
-    //ligne_droite_v2(210000);
-    /*while(t.read()<3){
-        //actualise_position();
-        //actualise_position_test();
-        //debugEncoder();
-        //("bite");
-    }*/
-    //wait(1);
-    /*for(int i =0;i<50;i++){
-        robot.test_rotation_rel(180);
-    }*/
-    //robot.ligne_droite_v2(30000);
-    /*robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);
-    robot.ligne_droite_v2(100000);*/
-    //robot.ligne_droite_v2(210000);
-    /*for (int i =0;i<4;i++){
-        robot.ligne_droite_v2(50000);
-        robot.test_rotation_rel(-90);
-    }*/
-    //robot.ligne_droite_v2(140000);
-    //vitesse_nulle_G(0);
-    //vitesse_nulle_D(0);
+    robot.commande_vitesse(500,0);
+    //robot.ligne_droite_v2(150000);
+    //robot.test_rotation_rel(90);
+    //robot.ligne_droite_v2(40000);
+    //robot.poussette();
+    //robot.reculer_un_peu(-50000);
+    //robot.test();
+    //asser.detach();
+    //robot.vitesse_nulle_G(0);
+    //robot.vitesse_nulle_D(0);
+    //wait(0.2);
     //motors_stop();
-    //robot.tab();
-    //ligne_droite(200000);
-    //ligne_droite_v2(200000);
-    //commande_vitesse(500,500);
-    //test_rotation_rel(90);
-    //test_rotation_rel(-90);
-    //test_rotation_rel(180);
-    /*for (int i =0;i<6;i++){
-        robot.test_rotation_rel(180);
-        wait(1);
-    }*/
+    //robot.printftab();
     
+    
+    //printf("x : %lf,y : %lf,angle : %lf\n",get_x_actuel(),get_y_actuel(),get_angle());*/
     return 0;
 }

--- a/odometrie.cpp	Mon May 06 13:48:45 2019 +0000
+++ b/odometrie.cpp	Wed May 08 20:46:46 2019 +0000
@@ -4,8 +4,8 @@
 #include "reglages.h"
 #include "math_precalc.h"
 
-long int x_actuel;
-long int y_actuel;
+double x_actuel;
+double y_actuel;
 double angle; // angle du robot
 
 
@@ -68,83 +68,26 @@
         
     	angle = borne_angle_r(angle);
 
-        x_actuel = (int) (cx + R * sin(angle) + 0.5);
-        y_actuel = (int) (cy - R * cos(angle) + 0.5);
+        x_actuel = cx + R * sin(angle);
+        y_actuel = cy - R * cos(angle);
     }
     else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite
-        x_actuel += (int) (dep_roue_G * cos(angle) + 0.5);
-        y_actuel += (int) (dep_roue_D * sin(angle) + 0.5);
+        x_actuel +=dep_roue_G * cos(angle);
+        y_actuel +=dep_roue_D * sin(angle);
     }
         
-    printf("tick d : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
+    //printf("tick d : %d, tick g : %d, x : %lf, y : %lf. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
+    
 } 
 
-void actualise_position_test()
-{
-    
-    //on suppose les valeurs de vd et vg constantes pendant t, la trajectoire decrite par le robot est alors un cercle
-    
-
-	//------recuperation de la rotation des roues---------
-	long int nbr_tick_D=get_nbr_tick_D();
-	long int nbr_tick_G=get_nbr_tick_G();
-	
-	//calcul du nombre de tick
-	long int nbr_tick_D_actuel=nbr_tick_D-nbr_tick_D_prec;
-	long int nbr_tick_G_actuel=nbr_tick_G-nbr_tick_G_prec;
-	
-	//sauvegarde
-	nbr_tick_D_prec=nbr_tick_D;
-	nbr_tick_G_prec=nbr_tick_G;
-	
-	double dep_roue_G = nbr_tick_G_actuel * DISTANCE_PAR_TICK_G; // deplacement des roues
-    double dep_roue_D = nbr_tick_D_actuel * DISTANCE_PAR_TICK_D;
-	
-	
-	//------calcul de la trajectoire---------
-	
-    // determination du cercle décrit par la trajectoire et de la vitesse du robot sur ce cercle
-    if (dep_roue_G != dep_roue_D){
-        
-    	double R = 0; // rayon du cercle decrit par la trajectoire
-	    double d = 0; // vitesse du robot
-        double cx = 0; // position du centre du cercle decrit par la trajectoire
-        double cy = 0;
-        
-        R = ECART_ROUE / 2 * (dep_roue_D + dep_roue_G) / (dep_roue_D - dep_roue_G); // rayon du cercle
-        cx = x_actuel - R * sin(angle);
-        cy = y_actuel + R * cos(angle);
-        d = (dep_roue_G + dep_roue_D) / 2;
-
-        // mise à jour des coordonnées du robot
-        if (dep_roue_G + dep_roue_D != 0){
-            angle += d / R;
-        }
-        else{
-            angle += dep_roue_D * 2 / ECART_ROUE;
-        }
-        
-    	angle = borne_angle_r(angle);
-
-        x_actuel = (int) (cx + R * sin(angle) + 0.5);
-        y_actuel = (int) (cy - R * cos(angle) + 0.5);
-        printf("cx : %lf cy : %lf ",cx + R * sin(angle) + 0.5,cy - R * cos(angle) + 0.5);
-    }
-    else if (dep_roue_G == dep_roue_D){ // cas où la trajectoire est une parfaite ligne droite
-        x_actuel += (int) (dep_roue_G * cos(angle) + 0.5);
-        y_actuel += (int) (dep_roue_D * sin(angle) + 0.5);
-    }
-        
-    printf("tick d : %d, tick g : %d, x : %d, y : %d. angle : %lf\n", nbr_tick_D, nbr_tick_G, x_actuel, y_actuel, angle*180/PI);
-}
 
 
-long int get_x_actuel()
+double get_x_actuel()
 {
 	return x_actuel;
 }
 
-long int get_y_actuel()
+double get_y_actuel()
 {
 	return y_actuel;
 }

--- a/odometrie.h	Mon May 06 13:48:45 2019 +0000
+++ b/odometrie.h	Wed May 08 20:46:46 2019 +0000
@@ -4,8 +4,8 @@
 
 void init_odometrie(void);
 void actualise_position(void);
-long int get_x_actuel(void);
-long int get_y_actuel(void);
+double get_x_actuel(void);
+double get_y_actuel(void);
 double get_angle(void);
 void actualise_position_test(void);
 

--- a/reglages.h	Mon May 06 13:48:45 2019 +0000
+++ b/reglages.h	Wed May 08 20:46:46 2019 +0000
@@ -4,16 +4,16 @@
 #define THETA_INIT 0
 
 //propre a chaque robot
-#define ECART_ROUE 31200 // a augmenter si l'angle reel est plus grand que l'angle vise //31190
-#define DISTANCE_PAR_TICK_D 100000/11862
-#define DISTANCE_PAR_TICK_G 100000/11862
+#define ECART_ROUE 30000 // a augmenter si l'angle reel est plus grand que l'angle vise //31190
+#define DISTANCE_PAR_TICK_D 8.5 // si le robot va trop loin, à augmenter//8.5
+#define DISTANCE_PAR_TICK_G 8.5 
 
 
 
 
 //correction mécanique
 #define COEFF_MOTEUR_D 1.00 //1.085
-#define COEFF_MOTEUR_G 0.98   //1.10
+#define COEFF_MOTEUR_G 1.00   //1.10
 
 //contraintes mecaniques
 #define PWM_MAX 900  //PWM maximal, à cette valeur le robot est à sa vitesse maximale admissible