Diff: commande_moteurs.cpp

Revision:

3:1dba6eca01ad

Parent:

2:5764f89a27f6

Child:

4:deef042e9c02

--- 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);