tests_moteurs.cpp

00001 
00002 #include "mbed.h"
00003 
00004 #include "tests_moteurs.h"
00005 #include "hardware.h"
00006 #include "reglages.h"
00007 #include "math_precalc.h"
00008 
00009 #include "odometrie.h"
00010 
00011 void test_ligne_droite(double distance, int vitesse, double angle_vise)
00012 {
00013     // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
00014     motors_on();
00015     double x_ini = get_x_actuel();
00016     double y_ini = get_y_actuel();
00017     
00018     double x_actuel = get_x_actuel();
00019     double y_actuel = get_y_actuel();
00020     
00021     int vitesse_D = vitesse;
00022     int vitesse_G = vitesse;
00023     
00024     double angle = get_angle();
00025     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 
00026     
00027     
00028     while ((x_ini - x_actuel)*(x_ini - x_actuel) + (y_ini - y_actuel)*(y_ini - y_actuel) < distance*distance) // a revoir : utilisation de sqrt
00029     {
00030         angle = get_angle();
00031         vitesse_G = (int) (vitesse * (1 + 0.05*diff_angle(angle_vise,angle))); // petit asser en angle
00032         vitesse_D = (int) (vitesse * (1 - 0.05*diff_angle(angle_vise,angle)));
00033          
00034         set_PWM_moteur_D(vitesse_D);
00035         set_PWM_moteur_G(vitesse_G);
00036         actualise_position();
00037         x_actuel = get_x_actuel();
00038         y_actuel = get_y_actuel();
00039         //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));
00040         //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);
00041         //printf("vd : %d , vg : %d\n",vitesse_D,vitesse_G);
00042         
00043     }
00044     
00045     set_PWM_moteur_D(0);
00046     set_PWM_moteur_G(0);
00047     //wait(0.5);
00048     motors_stop();
00049 }
00050 
00051 
00052 void test_rotation_rel(double angle_vise, int vitesse, int nombreTours)
00053 {
00054     // rotation de angle_vise
00055     motors_on();
00056     
00057     int compteur = 0;
00058     
00059     int sensRotation = 1;
00060     if(angle_vise>0)
00061     {
00062         sensRotation = -1;
00063     }
00064     
00065     int vitesse_D = vitesse*sensRotation;
00066     int vitesse_G = -vitesse*sensRotation;
00067     
00068     double angle = get_angle();
00069     angle_vise+=angle;
00070     borne_angle_d(angle_vise);
00071         
00072     while (abs(diff_angle(angle,angle_vise))>1 || (compteur != nombreTours))
00073     {
00074         set_PWM_moteur_D(vitesse_D);
00075         set_PWM_moteur_G(vitesse_G);
00076         actualise_position();
00077         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
00078         {
00079             compteur++;
00080         }
00081         angle = get_angle();
00082         //printf("angle recu  : %lf", angle);
00083     }
00084     
00085     //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));
00086     
00087     set_PWM_moteur_D(0);
00088     set_PWM_moteur_G(0);
00089     //wait(0.5);
00090     motors_stop();
00091 }
00092 
00093 
00094 void test_rotation_abs(double angle_vise, int vitesse, int nombre)
00095 {
00096     double angle_rel = borne_angle_d(angle_vise-get_angle());
00097     test_rotation_rel(angle_rel, vitesse, nombre);
00098 }