Jimmy Maingam / Mbed 2 deprecated odometrieDecembre

assert1

Dependencies:   mbed X_NUCLEO_IHM02A1

tests_moteurs.cpp@3:06cbe2f6c494, 2019-03-30 (annotated)

Committer:
JimmyAREM
Date:
Sat Mar 30 15:10:23 2019 +0000
Revision:
3:06cbe2f6c494
Parent:
2:977799d72329 
odo

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
Pierr 2:977799d72329 11 void test_ligne_droite(double distance, int vitesse, double angle_vise)
Coconara 0:6ca63d45f0ee 12 {
Coconara 1:0690cf83f060 13 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
Coconara 0:6ca63d45f0ee 14 motors_on();
Pierr 2:977799d72329 15 double x_ini = get_x_actuel();
Pierr 2:977799d72329 16 double y_ini = get_y_actuel();
Coconara 0:6ca63d45f0ee 17
Pierr 2:977799d72329 18 double x_actuel = get_x_actuel();
Pierr 2:977799d72329 19 double y_actuel = get_y_actuel();
Coconara 0:6ca63d45f0ee 20
Coconara 0:6ca63d45f0ee 21 int vitesse_D = vitesse;
Coconara 0:6ca63d45f0ee 22 int vitesse_G = vitesse;
Coconara 0:6ca63d45f0ee 23
Coconara 0:6ca63d45f0ee 24 double angle = get_angle();
Pierr 2:977799d72329 25 if(angle_vise == 0.00001) angle_vise = get_angle(); // si on n'a pas passé d'argument pour l'angle, on prend maintient l'angle de départ
Coconara 0:6ca63d45f0ee 26
Coconara 0:6ca63d45f0ee 27
Pierr 2:977799d72329 28 while ((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel) < distance*distance) // a revoir : utilisation de sqrt
Coconara 0:6ca63d45f0ee 29 {
Coconara 0:6ca63d45f0ee 30 angle = get_angle();
Pierr 2:977799d72329 31 vitesse_G = (int) (vitesse * (1 + 0.05*diff_angle(angle_vise,angle))); // petit asser en angle
Pierr 2:977799d72329 32 vitesse_D = (int) (vitesse * (1 - 0.05*diff_angle(angle_vise,angle)));
Coconara 1:0690cf83f060 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();
Pierr 2:977799d72329 39 //printf("x recu : %lf xinit : %lf , distance : %lf\n", x_actuel,x_ini,distance);// 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));
Pierr 2:977799d72329 40 //printf("y recu : %lf yinit : %lf , distance : %lf\n\n", y_actuel,y_ini,(x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel), 0.5);
Pierr 2:977799d72329 41 //printf("vd : %d , vg : %d\n",vitesse_D,vitesse_G);
Pierr 2:977799d72329 42
Coconara 0:6ca63d45f0ee 43 }
Coconara 0:6ca63d45f0ee 44
Coconara 1:0690cf83f060 45 set_PWM_moteur_D(0);
Coconara 1:0690cf83f060 46 set_PWM_moteur_G(0);
Pierr 2:977799d72329 47 //wait(0.5);
Coconara 1:0690cf83f060 48 motors_stop();
Coconara 1:0690cf83f060 49 }
Coconara 1:0690cf83f060 50
Coconara 1:0690cf83f060 51
Coconara 1:0690cf83f060 52 void test_rotation_rel(double angle_vise, int vitesse, int nombreTours)
Coconara 1:0690cf83f060 53 {
Coconara 1:0690cf83f060 54 // rotation de angle_vise
Coconara 1:0690cf83f060 55 motors_on();
Coconara 1:0690cf83f060 56
Coconara 1:0690cf83f060 57 int compteur = 0;
Coconara 1:0690cf83f060 58
Coconara 1:0690cf83f060 59 int sensRotation = 1;
Coconara 1:0690cf83f060 60 if(angle_vise>0)
Coconara 1:0690cf83f060 61 {
Coconara 1:0690cf83f060 62 sensRotation = -1;
Coconara 1:0690cf83f060 63 }
Coconara 1:0690cf83f060 64
Coconara 1:0690cf83f060 65 int vitesse_D = vitesse*sensRotation;
Coconara 1:0690cf83f060 66 int vitesse_G = -vitesse*sensRotation;
Coconara 1:0690cf83f060 67
Coconara 1:0690cf83f060 68 double angle = get_angle();
Coconara 1:0690cf83f060 69 angle_vise+=angle;
Coconara 1:0690cf83f060 70 borne_angle_d(angle_vise);
Coconara 1:0690cf83f060 71
Coconara 1:0690cf83f060 72 while (abs(diff_angle(angle,angle_vise))>1 || (compteur != nombreTours))
Coconara 1:0690cf83f060 73 {
Coconara 1:0690cf83f060 74 set_PWM_moteur_D(vitesse_D);
Coconara 1:0690cf83f060 75 set_PWM_moteur_G(vitesse_G);
Coconara 1:0690cf83f060 76 actualise_position();
Coconara 1:0690cf83f060 77 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
Coconara 1:0690cf83f060 78 {
Coconara 1:0690cf83f060 79 compteur++;
Coconara 1:0690cf83f060 80 }
Coconara 1:0690cf83f060 81 angle = get_angle();
Coconara 1:0690cf83f060 82 //printf("angle recu : %lf", angle);
Coconara 1:0690cf83f060 83 }
Coconara 1:0690cf83f060 84
Coconara 0:6ca63d45f0ee 85 //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 86
Coconara 0:6ca63d45f0ee 87 set_PWM_moteur_D(0);
Coconara 0:6ca63d45f0ee 88 set_PWM_moteur_G(0);
Pierr 2:977799d72329 89 //wait(0.5);
Coconara 0:6ca63d45f0ee 90 motors_stop();
Coconara 0:6ca63d45f0ee 91 }
Coconara 0:6ca63d45f0ee 92
Coconara 0:6ca63d45f0ee 93
Coconara 1:0690cf83f060 94 void test_rotation_abs(double angle_vise, int vitesse, int nombre)
Coconara 0:6ca63d45f0ee 95 {
Coconara 1:0690cf83f060 96 double angle_rel = borne_angle_d(angle_vise-get_angle());
Coconara 1:0690cf83f060 97 test_rotation_rel(angle_rel, vitesse, nombre);
Coconara 0:6ca63d45f0ee 98 }