Jimmy Maingam / Mbed 2 deprecated odometrieDecembre

assert1

Dependencies:   mbed X_NUCLEO_IHM02A1

tests_moteurs.cpp@0:6ca63d45f0ee, 2018-12-11 (annotated)

Committer:
Coconara
Date:
Tue Dec 11 19:12:55 2018 +0000
Revision:
0:6ca63d45f0ee
Child:
1:0690cf83f060
asser

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Coconara 0:6ca63d45f0ee 1
Coconara 0:6ca63d45f0ee 2 #include "mbed.h"
Coconara 0:6ca63d45f0ee 3
Coconara 0:6ca63d45f0ee 4 #include "tests_moteurs.h"
Coconara 0:6ca63d45f0ee 5 #include "hardware.h"
Coconara 0:6ca63d45f0ee 6 #include "reglages.h"
Coconara 0:6ca63d45f0ee 7 #include "math_precalc.h"
Coconara 0:6ca63d45f0ee 8
Coconara 0:6ca63d45f0ee 9 #include "odometrie.h"
Coconara 0:6ca63d45f0ee 10
Coconara 0:6ca63d45f0ee 11 void test_ligne_droite(long int distance, int vitesse, int angle_vise)
Coconara 0:6ca63d45f0ee 12 {
Coconara 0:6ca63d45f0ee 13 motors_on();
Coconara 0:6ca63d45f0ee 14 long int x_ini = get_x_actuel();
Coconara 0:6ca63d45f0ee 15 long int y_ini = get_y_actuel();
Coconara 0:6ca63d45f0ee 16
Coconara 0:6ca63d45f0ee 17 long int x_actuel = get_x_actuel();
Coconara 0:6ca63d45f0ee 18 long int y_actuel = get_y_actuel();
Coconara 0:6ca63d45f0ee 19
Coconara 0:6ca63d45f0ee 20 int vitesse_D = vitesse;
Coconara 0:6ca63d45f0ee 21 int vitesse_G = vitesse;
Coconara 0:6ca63d45f0ee 22
Coconara 0:6ca63d45f0ee 23 double angle = get_angle();
Coconara 0:6ca63d45f0ee 24
Coconara 0:6ca63d45f0ee 25
Coconara 0:6ca63d45f0ee 26 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)
Coconara 0:6ca63d45f0ee 27 {
Coconara 0:6ca63d45f0ee 28 //float a = racine((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel));
Coconara 0:6ca63d45f0ee 29 printf(" SQRT : %lf", pow((double) (x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5));
Coconara 0:6ca63d45f0ee 30 angle = get_angle();
Coconara 0:6ca63d45f0ee 31 vitesse_G = (int) (vitesse * (1 + 0.05*(angle_vise-angle)));
Coconara 0:6ca63d45f0ee 32 vitesse_D = (int) (vitesse * (1 - 0.05*(angle_vise-angle)));
Coconara 0:6ca63d45f0ee 33
Coconara 0:6ca63d45f0ee 34 set_PWM_moteur_D(vitesse_D);
Coconara 0:6ca63d45f0ee 35 set_PWM_moteur_G(vitesse_G);
Coconara 0:6ca63d45f0ee 36 actualise_position();
Coconara 0:6ca63d45f0ee 37 x_actuel = get_x_actuel();
Coconara 0:6ca63d45f0ee 38 y_actuel = get_y_actuel();
Coconara 0:6ca63d45f0ee 39 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));
Coconara 0:6ca63d45f0ee 40
Coconara 0:6ca63d45f0ee 41 }
Coconara 0:6ca63d45f0ee 42
Coconara 0:6ca63d45f0ee 43 //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));
Coconara 0:6ca63d45f0ee 44
Coconara 0:6ca63d45f0ee 45 set_PWM_moteur_D(0);
Coconara 0:6ca63d45f0ee 46 set_PWM_moteur_G(0);
Coconara 0:6ca63d45f0ee 47 wait(0.5);
Coconara 0:6ca63d45f0ee 48 motors_stop();
Coconara 0:6ca63d45f0ee 49 }
Coconara 0:6ca63d45f0ee 50
Coconara 0:6ca63d45f0ee 51
Coconara 0:6ca63d45f0ee 52 void test_rotation(double angle_vise, int vitesse, int nombre)
Coconara 0:6ca63d45f0ee 53 {
Coconara 0:6ca63d45f0ee 54 motors_on();
Coconara 0:6ca63d45f0ee 55 /*long int x_ini = get_x_actuel();
Coconara 0:6ca63d45f0ee 56 long int y_ini = get_y_actuel();
Coconara 0:6ca63d45f0ee 57
Coconara 0:6ca63d45f0ee 58 long int x_actuel = get_x_actuel();
Coconara 0:6ca63d45f0ee 59 long int y_actuel = get_y_actuel();*/
Coconara 0:6ca63d45f0ee 60
Coconara 0:6ca63d45f0ee 61 int compteur = 0;
Coconara 0:6ca63d45f0ee 62
Coconara 0:6ca63d45f0ee 63 int vitesse_D = -vitesse;
Coconara 0:6ca63d45f0ee 64 int vitesse_G = vitesse;
Coconara 0:6ca63d45f0ee 65
Coconara 0:6ca63d45f0ee 66 double angle = get_angle();
Coconara 0:6ca63d45f0ee 67
Coconara 0:6ca63d45f0ee 68 while ((angle < angle_vise - 1) || (angle > angle_vise + 1) || (compteur != nombre) || ((angle < -180 + 1) && (angle_vise == 180)))
Coconara 0:6ca63d45f0ee 69 {
Coconara 0:6ca63d45f0ee 70 set_PWM_moteur_D(vitesse_D);
Coconara 0:6ca63d45f0ee 71 set_PWM_moteur_G(vitesse_G);
Coconara 0:6ca63d45f0ee 72 actualise_position();
Coconara 0:6ca63d45f0ee 73 if (angle*get_angle() < 0 && angle > 20)
Coconara 0:6ca63d45f0ee 74 {
Coconara 0:6ca63d45f0ee 75 compteur++;
Coconara 0:6ca63d45f0ee 76 }
Coconara 0:6ca63d45f0ee 77 angle = get_angle();
Coconara 0:6ca63d45f0ee 78 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));
Coconara 0:6ca63d45f0ee 79
Coconara 0:6ca63d45f0ee 80 }
Coconara 0:6ca63d45f0ee 81
Coconara 0:6ca63d45f0ee 82 //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));
Coconara 0:6ca63d45f0ee 83
Coconara 0:6ca63d45f0ee 84 set_PWM_moteur_D(0);
Coconara 0:6ca63d45f0ee 85 set_PWM_moteur_G(0);
Coconara 0:6ca63d45f0ee 86 wait(0.5);
Coconara 0:6ca63d45f0ee 87 motors_stop();
Coconara 0:6ca63d45f0ee 88 }