assert1
Dependencies: mbed X_NUCLEO_IHM02A1
tests_moteurs.cpp
- Committer:
- Pierr
- Date:
- 2019-02-23
- Revision:
- 2:977799d72329
- Parent:
- 1:0690cf83f060
File content as of revision 2:977799d72329:
#include "mbed.h" #include "tests_moteurs.h" #include "hardware.h" #include "reglages.h" #include "math_precalc.h" #include "odometrie.h" void test_ligne_droite(double distance, int vitesse, double angle_vise) { // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900) motors_on(); double x_ini = get_x_actuel(); double y_ini = get_y_actuel(); double x_actuel = get_x_actuel(); double y_actuel = get_y_actuel(); int vitesse_D = vitesse; int vitesse_G = vitesse; double angle = get_angle(); 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 while ((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel) < distance*distance) // a revoir : utilisation de sqrt { angle = get_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(); x_actuel = get_x_actuel(); y_actuel = get_y_actuel(); //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)); //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); //printf("vd : %d , vg : %d\n",vitesse_D,vitesse_G); } 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); set_PWM_moteur_G(0); //wait(0.5); motors_stop(); } void test_rotation_abs(double angle_vise, int vitesse, int nombre) { double angle_rel = borne_angle_d(angle_vise-get_angle()); test_rotation_rel(angle_rel, vitesse, nombre); }