Diff: tests_moteurs.cpp

Revision:

0:6ca63d45f0ee

Child:

1:0690cf83f060

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tests_moteurs.cpp	Tue Dec 11 19:12:55 2018 +0000
@@ -0,0 +1,88 @@
+
+#include "mbed.h"
+
+#include "tests_moteurs.h"
+#include "hardware.h"
+#include "reglages.h"
+#include "math_precalc.h"
+
+#include "odometrie.h"
+
+void test_ligne_droite(long int distance, int vitesse, int angle_vise)
+{
+    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 vitesse_D = vitesse;
+    int vitesse_G = vitesse;
+    
+    double angle = 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)
+    {
+        //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)));
+        
+        set_PWM_moteur_D(vitesse_D);
+        set_PWM_moteur_G(vitesse_G);
+        actualise_position();
+        x_actuel = get_x_actuel();
+        y_actuel = get_y_actuel();
+        printf("x recu  : %ld", x_actuel);// 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();
+}
+
+
+void test_rotation(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();
+}
\ No newline at end of file