Diff: tests_moteurs.cpp

Revision:

1:0690cf83f060

Parent:

0:6ca63d45f0ee

Child:

2:977799d72329

--- a/tests_moteurs.cpp	Tue Dec 11 19:12:55 2018 +0000
+++ b/tests_moteurs.cpp	Wed Dec 12 20:03:07 2018 +0000
@@ -8,8 +8,9 @@
 
 #include "odometrie.h"
 
-void test_ligne_droite(long int distance, int vitesse, int angle_vise)
+void test_ligne_droite(long int distance, int vitesse)
 {
+    // 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();
@@ -21,16 +22,15 @@
     int vitesse_G = vitesse;
     
     double angle = get_angle();
+    double angle_vise = get_angle();
     
     
-    while (pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5) < distance)//x_actuel < distance)
+    while (pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5) < distance) // a revoir : utilisation de sqrt
     {
-        //float a = racine((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel));
-        printf(" SQRT : %lf", pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5));
         angle = get_angle();
-        vitesse_G = (int) (vitesse * (1 + 0.05*(angle_vise-angle)));
-        vitesse_D = (int) (vitesse * (1 - 0.05*(angle_vise-angle)));
-        
+        vitesse_G = (int) (vitesse * (1 - 0.05*diff_angle(angle_vise,angle))); // petit asser en angle
+        vitesse_D = (int) (vitesse * (1 + 0.05*diff_angle(angle_vise,angle)));
+         
         set_PWM_moteur_D(vitesse_D);
         set_PWM_moteur_G(vitesse_G);
         actualise_position();
@@ -40,6 +40,46 @@
     
     }
     
+    set_PWM_moteur_D(0);
+    set_PWM_moteur_G(0);
+    wait(0.5);
+    motors_stop();
+}
+
+
+void test_rotation_rel(double angle_vise, int vitesse, int nombreTours)
+{
+    // rotation de angle_vise
+    motors_on();
+    
+    int compteur = 0;
+    
+    int sensRotation = 1;
+    if(angle_vise>0)
+    {
+        sensRotation = -1;
+    }
+    
+    int vitesse_D = vitesse*sensRotation;
+    int vitesse_G = -vitesse*sensRotation;
+    
+    double angle = get_angle();
+    angle_vise+=angle;
+    borne_angle_d(angle_vise);
+        
+    while (abs(diff_angle(angle,angle_vise))>1 || (compteur != nombreTours))
+    {
+        set_PWM_moteur_D(vitesse_D);
+        set_PWM_moteur_G(vitesse_G);
+        actualise_position();
+        if (angle*get_angle() < 0 && angle > 100 && compteur<nombreTours) // angle>100 pour que compteur ne s'incremente qu'au passage de 180 a -180, pas de -1 a 1
+        {
+            compteur++;
+        }
+        angle = get_angle();
+        //printf("angle recu  : %lf", angle);
+    }
+    
     //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));
     
     set_PWM_moteur_D(0);
@@ -49,40 +89,8 @@
 }
 
 
-void test_rotation(double angle_vise, int vitesse, int nombre)
+void test_rotation_abs(double angle_vise, int vitesse, int nombre)
 {
-    motors_on();
-    /*long int x_ini = get_x_actuel();
-    long int y_ini = get_y_actuel();
-    
-    long int x_actuel = get_x_actuel();
-    long int y_actuel = get_y_actuel();*/
-    
-    int compteur = 0;
-    
-    int vitesse_D = -vitesse;
-    int vitesse_G = vitesse;
-    
-    double angle = get_angle();
-    
-    while ((angle < angle_vise - 1) || (angle > angle_vise + 1) || (compteur != nombre) || ((angle < -180 + 1) && (angle_vise == 180)))
-    {
-        set_PWM_moteur_D(vitesse_D);
-        set_PWM_moteur_G(vitesse_G);
-        actualise_position();
-        if (angle*get_angle() < 0 && angle > 20)
-        {
-            compteur++;
-        }
-        angle = get_angle();
-        printf("angle recu  : %lf", 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 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));
-    
-    set_PWM_moteur_D(0);
-    set_PWM_moteur_G(0);
-    wait(0.5);
-    motors_stop();
+    double angle_rel = borne_angle_d(angle_vise-get_angle());
+    test_rotation_rel(angle_rel, vitesse, nombre);
 }
\ No newline at end of file