Revision 3:1dba6eca01ad, committed 2019-05-06

Comitter:

GuillaumeCH

Date:

Mon May 06 13:48:45 2019 +0000

Parent:

2:5764f89a27f6

Child:

4:deef042e9c02

Commit message:

O

Changed in this revision

--- a/commande_moteurs.cpp	Wed Apr 17 18:55:22 2019 +0000
+++ b/commande_moteurs.cpp	Mon May 06 13:48:45 2019 +0000
@@ -42,7 +42,7 @@
     if ((rand()%1000) < centieme_D){
         VD_int+=1;
     }
-    printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
+    //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
     set_PWM_moteur_G(VD_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
     set_PWM_moteur_D(VG_int);//
 }
@@ -77,7 +77,7 @@
     
     angle = get_angle();
     
-    printf("YOOOO\n\n ");
+    //printf("YOOOO\n\n ");
     while (distance+x_local>0){
     
             vitesse_G = (distance+x_local)/70;
@@ -134,7 +134,6 @@
     
     angle = get_angle();
     
-    printf("YOOOO\n\n ");
     while (distance-x_local>0){
     
             vitesse_G = (distance-x_local)/70;
@@ -167,6 +166,69 @@
     wait(0.3);
     motors_stop();
 }
+void ligne_droite_v2(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;
+    
+    long int y_local_prec = y_local;
+    
+    float vitesse_G;
+    float vitesse_D;
+    
+    angle = get_angle();
+    
+    float Ki= 0.00007;
+    float Kp= 0.03;
+    while (distance-x_local>0){
+    
+            vitesse_G = (distance-x_local)/70;
+            vitesse_D = vitesse_G;
+            if(vitesse_G >400){
+                vitesse_G=400;
+                vitesse_D=400;
+            }
+            if (vitesse_G<-400){
+                vitesse_G=-400;
+                vitesse_D=-400;
+            }
+            
+            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 + Ki*y_local + Kp * diff_angle(angle_vise_deg, angle));
+            vitesse_D = vitesse_D * (1 - Ki*y_local - Kp * diff_angle(angle_vise_deg, angle));
+            
+            commande_vitesse(vitesse_G,vitesse_D);
+            actualise_position();
+            x_actuel = get_x_actuel();
+            y_actuel = get_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));
+    }
+    //test_rotation_abs(angle_vise_deg);
+    vitesse_nulle_G(0);
+    vitesse_nulle_D(0);
+    wait(0.3);
+    motors_stop();
+}
 
 void test_rotation_rel(double angle_vise)
 {
@@ -177,7 +239,7 @@
     angle_vise+=angle;
     borne_angle_d(angle_vise);
         
-    while (abs(diff_angle(angle,angle_vise))>0.08)
+    while (abs(diff_angle(angle,angle_vise))>0.05)
     {
         vitesse=1.3*diff_angle(angle,angle_vise);
         commande_vitesse(-vitesse,vitesse);

--- a/commande_moteurs.h	Wed Apr 17 18:55:22 2019 +0000
+++ b/commande_moteurs.h	Mon May 06 13:48:45 2019 +0000
@@ -13,4 +13,6 @@
 void vitesse_nulle_D(int zero);
 void vitesse_nulle_G(int zero);
 void reculer_un_peu(int distance);
+void ligne_droite_v2(long int distance);
+
 #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/deplacement.cpp	Mon May 06 13:48:45 2019 +0000
@@ -0,0 +1,489 @@
+#include "deplacement.h"
+#include "mbed.h"
+#include "odometrie.h"
+#include "hardware.h"
+#include "math_precalc.h"
+#include "reglages.h"
+
+
+deplacement::deplacement(){
+    consigne_D = 0;
+    consigne_G = 0;
+    somme_erreur_D = 0;
+    somme_erreur_G = 0;
+    erreur_precedente_D = 0;
+    erreur_precedente_G = 0;
+    
+    for (int k =0; k<5;k++){
+        erreur_glissee_D[k] = 0;
+        erreur_glissee_G[k] = 0;
+    }
+    compteur_glisse = 0;
+    
+    Kp_D = 1.5;//1
+    Ki_D = 0.12;//0.15
+    Kd_D = 0.5;//1
+    
+    Kp_G = 1;//1
+    Ki_G = 0.13;//0.15
+    Kd_G = 1.2;//1
+    
+    tick_prec_D=0;
+    tick_prec_G = 0;
+    dix_ms = 0;
+    for (int k =0; k<TAILLE_TAB;k++){
+        tab_cmd_G[k]=0;
+        tab_cmd_D[k]=0;
+        vtab_G[k]=0;
+        vtab_D[k]=0;
+        c_D[k]=0;
+        c_G[k]=0;
+    }
+    consigne_tab[0][0]=0;
+    consigne_tab[0][1]=0;
+    
+    consigne_tab[1][0]=10;
+    consigne_tab[1][1]=10;
+    
+    consigne_tab[2][0]=20;
+    consigne_tab[2][1]=20;
+    
+    consigne_tab[3][0]=30;
+    consigne_tab[3][1]=30;
+    
+    consigne_tab[4][0]=40;
+    consigne_tab[4][1]=40;
+    
+   /* consigne_tab[5][0]=3*5;
+    consigne_tab[5][1]=3*5;
+    
+    consigne_tab[6][0]=3*6;
+    consigne_tab[6][1]=3*6;
+    
+    consigne_tab[7][0]=3*7;
+    consigne_tab[7][1]=3*7;
+    
+    consigne_tab[8][0]=3*8;
+    consigne_tab[8][1]=3*8;
+    
+    consigne_tab[9][0]=3*9;
+    consigne_tab[9][1]=3*9;
+    
+    consigne_tab[10][0]=3*10;
+    consigne_tab[10][1]=3*10;
+    
+    consigne_tab[11][0]=3*11;
+    consigne_tab[11][1]=3*11;
+    
+    consigne_tab[12][0]=3*12;
+    consigne_tab[12][1]=3*12;
+    
+    consigne_tab[13][0]=3*13;
+    consigne_tab[13][1]=3*13;
+    
+    consigne_tab[14][0]=3*14;
+    consigne_tab[14][1]=3*14;
+    
+    consigne_tab[15][0]=0;
+    consigne_tab[15][1]=0;
+    
+    consigne_tab[16][0]=0;
+    consigne_tab[16][1]=0;
+    
+    consigne_tab[17][0]=0;
+    consigne_tab[17][1]=0;
+    
+    consigne_tab[18][0]=0;
+    consigne_tab[18][1]=0;
+    
+    consigne_tab[19][0]=0;
+    consigne_tab[19][1]=0;*/
+}
+
+void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
+    
+    int sens_G=signe(vitesse_G);
+    int sens_D=signe(vitesse_D);
+    double vitesse_local_G=abs(vitesse_G);
+    double vitesse_local_D=abs(vitesse_D);
+    
+    if(abs(vitesse_G) > 900){
+        vitesse_local_G=900;
+    }
+    if(abs(vitesse_G)<10){
+        vitesse_local_G=0;
+    }
+    if(abs(vitesse_D) > 900){
+        vitesse_local_D=900;
+    }
+    if(abs(vitesse_D)< 10){
+        vitesse_local_D=0;
+    
+    }
+    ;
+    int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
+    int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
+    float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
+    float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
+    float centieme_D = (VD_f-VD_int)*1000;
+    float centieme_G = (VG_f-VG_int)*1000;
+    if ((rand()%1000) < centieme_G){
+        VG_int+=1;
+    }
+    if ((rand()%1000) < centieme_D){
+        VD_int+=1;
+    }
+    //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
+    set_PWM_moteur_G(VD_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
+    set_PWM_moteur_D(VG_int);//
+}
+void deplacement::vitesse_nulle_G(int zero){
+    if(zero == 0){
+        set_PWM_moteur_G(0);
+    }
+}
+void deplacement::vitesse_nulle_D(int zero){
+    if(zero == 0){
+        set_PWM_moteur_D(0);
+    }
+}
+void deplacement::reculer_un_peu(int distance){
+    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();
+    
+    //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;
+            }
+            
+            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){
+                vitesse_G=400;
+                vitesse_D=400;
+            }
+            if (vitesse_G<-400){
+                vitesse_G=-400;
+                vitesse_D=-400;
+            }
+            
+            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_v2(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;
+    
+    //long int y_local_prec = y_local;
+    
+    float vitesse_G;
+    float vitesse_D;
+    
+    angle = get_angle();
+    
+    float Ki2= 0.000015;
+    float Kp2= 0.04;
+    while (distance-x_local>0){
+    
+            vitesse_G = (distance-x_local)/70;
+            vitesse_D = vitesse_G;
+            if(vitesse_G >400){
+                vitesse_G=400;
+                vitesse_D=400;
+            }
+            if (vitesse_G<-400){
+                vitesse_G=-400;
+                vitesse_D=-400;
+            }
+            
+            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));
+            commande_vitesse(vitesse_G,vitesse_D);
+            actualise_position();
+            x_actuel = get_x_actuel();
+            y_actuel = get_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));
+    }
+    test_rotation_abs(angle_vise_deg);
+    vitesse_nulle_G(0);
+    vitesse_nulle_D(0);
+    wait(0.3);
+    motors_stop();
+}
+
+void deplacement::test_rotation_rel(double angle_vise)
+{
+    // rotation de angle_vise
+    motors_on();
+    float vitesse;
+    int sens;
+    double angle = get_angle();
+    angle_vise+=angle;
+    borne_angle_d(angle_vise);
+    if (diff_angle(angle,angle_vise)<=0){
+        sens = -1;
+    }
+    else{
+        sens = 1;
+    }
+    while (sens*diff_angle(angle,angle_vise)>0)
+    {
+        vitesse=diff_angle(angle,angle_vise);
+        
+        commande_vitesse(-vitesse,vitesse);
+        actualise_position();
+        angle = get_angle();
+        //printf("vitesse : %f", vitesse);
+    }
+    
+    //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;
+    vitesse_nulle_G(0);
+    vitesse_nulle_D(0);
+    wait(0.2);
+    motors_stop();
+}
+
+
+void deplacement::test_rotation_abs(double angle_vise)
+{
+    actualise_position();
+    printf("bite");
+    double angle_rel = borne_angle_d(angle_vise-get_angle());
+    test_rotation_rel(angle_rel);
+}
+
+void deplacement::asservissement(){
+    long int tick_D = get_nbr_tick_D();
+    long int tick_G = get_nbr_tick_G();
+    
+    long int tick_D_passe = tick_D-tick_prec_D;
+    long int tick_G_passe = tick_G-tick_prec_G;
+    
+    tick_prec_D=tick_D;
+    tick_prec_G=tick_G;
+    
+    float vitesse_codeuse_D = tick_D_passe;
+    float vitesse_codeuse_G = tick_G_passe;
+    
+    float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
+    float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
+    
+    if (compteur_glisse == 5)
+        compteur_glisse = 0;
+    
+    if (compteur_glisse == -1)
+    {
+        compteur_glisse = 0;
+        for (int i = 0; i<5; i++){
+            erreur_glissee_D[compteur_glisse] = erreur_D;
+            erreur_glissee_G[compteur_glisse] = erreur_G;
+        }
+    }
+    
+    erreur_glissee_D[compteur_glisse] = erreur_D;
+    erreur_glissee_G[compteur_glisse] = erreur_G;
+    compteur_glisse++;
+    
+    erreur_D = erreur_glissee_D[0];
+    erreur_G = erreur_glissee_G[0];
+    for (int i=1; i<5; i++)
+    {
+         erreur_D += erreur_glissee_D[i];
+         erreur_G += erreur_glissee_G[i];
+    }
+    
+    erreur_D = erreur_D/5.0;
+    erreur_G = erreur_G/5.0; // erreur est maintenant la moyenne des 5 erreurs prec
+    
+    somme_erreur_D += erreur_D;
+    somme_erreur_G += erreur_G;
+    
+    float delta_erreur_D = erreur_D-erreur_precedente_D;
+    float delta_erreur_G = erreur_G-erreur_precedente_G;
+    
+    erreur_precedente_G = erreur_G;
+    erreur_precedente_D = erreur_D;
+    
+    float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D;
+    float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G;
+    
+    if (cmd_G <0){
+        cmd_G = 0;
+    }
+    if (cmd_G > 500){
+        cmd_G = 500;
+    }
+    if (cmd_D <0){
+        cmd_D = 0;
+    }
+    if (cmd_D > 500){
+        cmd_D = 500;
+    }
+    c_D[dix_ms]=consigne_D;
+    c_G[dix_ms]=consigne_G;
+    //printf("%d\n",c[i]);
+    tab_cmd_D[dix_ms] = cmd_D;
+    tab_cmd_G[dix_ms] = cmd_G;
+    vtab_D[dix_ms] = vitesse_codeuse_D;
+    vtab_G[dix_ms] = vitesse_codeuse_G;
+    commande_vitesse(cmd_G,cmd_D);
+    dix_ms++;
+    //printf("%d\n",i);
+    //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
+    //printf("%f,%f\n",cmd_G,cmd_D);
+    //printf("oui");
+}
+
+void deplacement::printftab(){
+
+    for (int j =0;j<TAILLE_TAB;j++){
+        if(j==500)
+            bouton();
+        printf("%f,%f,%f,%f,%f,%f\n",tab_cmd_G[j],10*vtab_G[j],10*c_D[j],tab_cmd_D[j],10*vtab_D[j],10*c_G[j]);
+    }
+        /*if (j<5)
+            printf("%f,%f,%f,%f,%f\n",tab_cmd_G[j],10*vtab_G[j],10*c[j],tab_cmd_D[j],10*vtab_D[j]);
+        else
+            printf("%f,%f,%f,%f,%f\n",tab_cmd_G[j],2*(vtab_G[j]+vtab_G[j-1]+vtab_G[j-2]+vtab_G[j-3]+vtab_G[j-4]),10*c[j],tab_cmd_D[j],2*(vtab_D[j]+vtab_D[j-1]+vtab_D[j-2]+vtab_D[j-3]+vtab_D[j-4]));
+    }*/
+    
+    /*for (int j =0;j<TAILLE_TAB;j++){
+        printf("%f,%f,%d\n",2*(vtab_G[j]+vtab_G[j-1]+vtab_G[j-2]+vtab_G[j-3]+vtab_G[j-4]), 2*(vtab_D[j]+vtab_D[j-1]+vtab_D[j-2]+vtab_D[j-3]+vtab_D[j-4]), j);
+    }*/
+}
+
+void deplacement::test(){
+    Timer t;
+    t.start();
+    for (int i =0;i<5 ;i++){
+        changement_consigne(consigne_tab[i][0], consigne_tab[i][1]);
+        while(t.read()<0.5){
+            //actualise_positio n();
+        }
+        //printf("t.read() : %f\n",t.read());
+        //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G);
+        t.reset();
+    }
+}
+
+void deplacement::changement_consigne(int cons_D, int cons_G){
+    consigne_D = cons_D;
+    consigne_G = cons_G;
+    compteur_glisse = -1;   
+}
+
+void deplacement::bouton(){
+    DigitalIn depart(USER_BUTTON);
+    while (depart){}
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/deplacement.h	Mon May 06 13:48:45 2019 +0000
@@ -0,0 +1,60 @@
+#ifndef DEPLACEMENT_H
+#define DEPLACEMENT_H
+
+#define TAILLE_TAB 250
+
+class deplacement{
+    public:
+        deplacement();
+        void asservissement(void);
+        void test_rotation_rel(double angle_vise);
+        void test_rotation_abs(double angle_vise);
+        void ligne_droite(long int distance);
+        void commande_vitesse(float vitesse_G, float vitesse_D);
+        void vitesse_nulle_D(int zero);
+        void vitesse_nulle_G(int zero);
+        void reculer_un_peu(int distance);
+        void ligne_droite_v2(long int distance);
+        void printftab(void);  
+        void test(void);
+        void changement_consigne(int cons_D, int cons_G);
+        void bouton();
+
+    
+    
+    
+    private:
+        float consigne;
+        int consigne_D;
+        int consigne_G;
+        float somme_erreur_D;
+        float somme_erreur_G;
+        float erreur_precedente_D;
+        float erreur_precedente_G;
+        float erreur_glissee_D[5];
+        float erreur_glissee_G[5];
+        int compteur_glisse;
+        float Kp_D;
+        float Ki_D;
+        float Kd_D;
+        float Kp_G;
+        float Ki_G;
+        float Kd_G;
+        long int tick_prec_D;
+        long int tick_prec_G;
+        float tab_cmd_D[TAILLE_TAB];
+        float tab_cmd_G[TAILLE_TAB];
+        float vtab_D[TAILLE_TAB];
+        float vtab_G[TAILLE_TAB];
+        float erreur_tab_G[TAILLE_TAB];
+        float erreur_tab_D[TAILLE_TAB];
+        float somme_erreur_tab_G[TAILLE_TAB];
+        float somme_erreur_tab_D[TAILLE_TAB];
+        float c_D[TAILLE_TAB];
+        float c_G[TAILLE_TAB];
+        int dix_ms;
+        int consigne_tab[20][2];
+};
+
+
+#endif
\ No newline at end of file

--- a/hardware.cpp	Wed Apr 17 18:55:22 2019 +0000
+++ b/hardware.cpp	Mon May 06 13:48:45 2019 +0000
@@ -4,36 +4,37 @@
 #include "hardware.h"
 #include "DevSPI.h"
 #include "XNucleoIHM02A1.h"
+#include "commande_moteurs.h"
 
 // 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. */
     {
-        10,                           /* Motor supply voltage in V. */
+        12,                           /* Motor supply voltage in V. */
         200,                           /* Min number of steps per revolution for the motor. */
         4,                           /* Max motor phase voltage in A. */
-        7.06,                          /* Max motor phase voltage in V. */
+        7,                          /* Max motor phase voltage in V. */
         300,                         /* Motor initial speed [step/s]. */
-        500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+        247.4,                         /* 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]. */
         602.7,                         /* Motor full-step speed threshold [step/s]. */
-        4.5,                          /* Holding kval [V]. */
-        4.5,                          /* Constant speed kval [V]. */
-        4.5,                          /* Acceleration starting kval [V]. */
-        4.5,                          /* Deceleration starting kval [V]. */
+        5,                          /* Holding kval [V]. */
+        5,                          /* Constant speed kval [V]. */
+        5,                          /* Acceleration starting kval [V]. */
+        5,                          /* Deceleration starting kval [V]. */
         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
         392.1569e-6,                   /* Start slope [s/step]. */
         643.1372e-6,                   /* Acceleration final slope [s/step]. */
         643.1372e-6,                   /* Deceleration final slope [s/step]. */
         0,                             /* Thermal compensation factor (range [0, 15]). */
-        4.5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
-        32,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+        5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+        5*1000*1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
         0xFF,                          /* Alarm conditions enable. */
         0x2E88                         /* Ic configuration. */
@@ -41,27 +42,27 @@
 
     /* Second Motor. */
     {
-        10,                           /* Motor supply voltage in V. */
+        12,                           /* Motor supply voltage in V. */
         200,                           /* Min number of steps per revolution for the motor. */
         4,                           /* Max motor phase voltage in A. */
-        7.06,                          /* Max motor phase voltage in V. */
+        7,                          /* Max motor phase voltage in V. */
         300,                         /* Motor initial speed [step/s]. */
-        500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+        251.0,                         /* 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]. */
         602.7,                         /* Motor full-step speed threshold [step/s]. */
-        4.5,                          /* Holding kval [V]. */
-        4.5,                          /* Constant speed kval [V]. */
-        4.5,                          /* Acceleration starting kval [V]. */
-        4.5,                          /* Deceleration starting kval [V]. */
+        5,                          /* Holding kval [V]. */
+        5,                          /* Constant speed kval [V]. */
+        5,                          /* Acceleration starting kval [V]. */
+        5,                          /* Deceleration starting kval [V]. */
         61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
         392.1569e-6,                   /* Start slope [s/step]. */
         643.1372e-6,                   /* Acceleration final slope [s/step]. */
         643.1372e-6,                   /* Deceleration final slope [s/step]. */
         0,                             /* Thermal compensation factor (range [0, 15]). */
-        4.5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
-        32,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+        5*1000*1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+        5*1000*1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
         StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
         0xFF,                          /* Alarm conditions enable. */
         0x2E88                         /* Ic configuration. */
@@ -92,7 +93,7 @@
 
 void init_hardware()
 {
-    pc.baud(115200); //Initialisation de l'USART pc 
+    pc.baud(2000000); //Initialisation de l'USART pc 
 
     /* Initializing Motor Control Expansion Board. */
     x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
@@ -295,7 +296,73 @@
     lastEncodedB = encodedB; //store this value for next time
 }
 
+
+
+
+/*void asservissement(){
+    long int tick_D = get_nbr_tick_D();
+    long int tick_G = get_nbr_tick_G();
+    
+    long int tick_D_passe = tick_D-tick_prec_D;
+    long int tick_G_passe = tick_G-tick_prec_G;
+    
+    tick_prec_D=tick_D;
+    tick_prec_G=tick_G;
+    
+    float vitesse_codeuse_D = tick_D_passe;
+    float vitesse_codeuse_G = tick_G_passe;
+    
+    float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
+    float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
+    
+    somme_erreur_D += erreur_D;
+    somme_erreur_G += erreur_G;
+    
+    float delta_erreur_D = erreur_D-erreur_precedente_D;
+    float delta_erreur_G = erreur_G-erreur_precedente_G;
+    
+    erreur_precedente_G = erreur_G;
+    erreur_precedente_D = erreur_D;
+    
+    float cmd_D = Kp*erreur_D+Ki*somme_erreur_D + Kd*delta_erreur_D;
+    float cmd_G = Kp*erreur_G+Ki*somme_erreur_G + Kd*delta_erreur_G;
+    
+    if (cmd_G <0){
+        cmd_G = 0;
+    }
+    if (cmd_G > 500){
+        cmd_G = 500;
+    }
+    if (cmd_D <0){
+        cmd_D = 0;
+    }
+    if (cmd_D > 500){
+        cmd_D = 500;
+    }
+    commande_vitesse(cmd_G,cmd_D);
+    //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
+    //printf("%f\n",vitesse_codeuse_G);
+    //printf("oui");
+}*/
+
 void debugEncoder()
 {
-    printf("codeuse droite : %d, codeuse gauche : %d\n", lastEncodedB, lastEncodedA);
-}
\ No newline at end of file
+    printf("tick_D : %ld, tick_G : %ld\n", get_nbr_tick_D(),get_nbr_tick_G());
+}
+long int get_position_G(){
+    /* Getting the current position. */
+    long int position = motors[1]->get_position();
+    return position;
+    /* Printing to the console. */
+    //printf("--> Getting the current position: %d\r\n", position);
+    
+}
+long int get_position_D(){
+    /* Getting the current position. */
+    long int position = motors[0]->get_position();
+    
+    /* Printing to the console. */
+    //printf("--> Getting the current position: %d\r\n", position);
+    return position;
+    
+}

--- a/hardware.h	Wed Apr 17 18:55:22 2019 +0000
+++ b/hardware.h	Mon May 06 13:48:45 2019 +0000
@@ -30,7 +30,9 @@
 
 float lecture_courrant_D();
 float lecture_courrant_G();
-
-void debugEncoder(void);
+long int get_position_D();
+long int get_position_G();
+void debugEncoder();
+void asservissement();
 
 #endif

--- a/main.cpp	Wed Apr 17 18:55:22 2019 +0000
+++ b/main.cpp	Mon May 06 13:48:45 2019 +0000
@@ -4,6 +4,7 @@
 #include "odometrie.h"
 #include "reglages.h"
 #include "commande_moteurs.h"
+#include "deplacement.h"
 
 int main()
 {
@@ -12,20 +13,75 @@
     init_hardware();
     srand(time(NULL));
     motors_on();
+    /*DigitalIn depart(USER_BUTTON);
     //Pause pour sauver le robot et l'ordi
-    wait(3);
-    //ligne_droite(150000);
+    while(depart);*/
+    deplacement robot;
+    robot.bouton();
+    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();
+    //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);
+    //motors_stop();
+    //robot.tab();
+    //ligne_droite(200000);
+    //ligne_droite_v2(200000);
     //commande_vitesse(500,500);
-    /*for(int j = 0; j<5;j++){
-        for (int i =0; i< 4;i++){
-            ligne_droite(50000);            
-            test_rotation_rel(90);
-        }
+    //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);
     }*/
-    reculer_un_peu(50000);
-    /*for (int i = 0; i< 10; i++){
-        test_rotation_rel(180);
-    }*/
+    
     return 0;
 }

--- a/odometrie.cpp	Wed Apr 17 18:55:22 2019 +0000
+++ b/odometrie.cpp	Mon May 06 13:48:45 2019 +0000
@@ -79,6 +79,66 @@
     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);
 } 
 
+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()
 {
 	return x_actuel;

--- a/odometrie.h	Wed Apr 17 18:55:22 2019 +0000
+++ b/odometrie.h	Mon May 06 13:48:45 2019 +0000
@@ -7,5 +7,6 @@
 long int get_x_actuel(void);
 long int get_y_actuel(void);
 double get_angle(void);
+void actualise_position_test(void);
 
 #endif

--- a/reglages.h	Wed Apr 17 18:55:22 2019 +0000
+++ b/reglages.h	Mon May 06 13:48:45 2019 +0000
@@ -4,16 +4,16 @@
 #define THETA_INIT 0
 
 //propre a chaque robot
-#define ECART_ROUE 32600 // a augmenter si l'angle reel est plus grand que l'angle vise
-#define DISTANCE_PAR_TICK_D 100000/11865
-#define DISTANCE_PAR_TICK_G 100000/11865
+#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
 
 
 
 
 //correction mécanique
 #define COEFF_MOTEUR_D 1.00 //1.085
-#define COEFF_MOTEUR_G 1.00   //1.10
+#define COEFF_MOTEUR_G 0.98   //1.10
 
 //contraintes mecaniques
 #define PWM_MAX 900  //PWM maximal, à cette valeur le robot est à sa vitesse maximale admissible