assert1
Dependencies: mbed X_NUCLEO_IHM02A1
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