commande_moteurs.cpp

00001 
00002 #include "mbed.h"
00003 
00004 #include "commande_moteurs.h"
00005 #include "hardware.h"
00006 #include "reglages.h"
00007 #include "math_precalc.h"
00008 
00009 #include "odometrie.h"
00010 
00011 
00012 void commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
00013     
00014     int sens_G=signe(vitesse_G);
00015     int sens_D=signe(vitesse_D);
00016     double vitesse_local_G=abs(vitesse_G);
00017     double vitesse_local_D=abs(vitesse_D);
00018     
00019     if(abs(vitesse_G) > 900){
00020         vitesse_local_G=900;
00021     }
00022     if(abs(vitesse_G)<10){
00023         vitesse_local_G=10;
00024     }
00025     if(abs(vitesse_D) > 900){
00026         vitesse_local_D=900;
00027     }
00028     if(abs(vitesse_D)< 10){
00029         vitesse_local_D=10;
00030     
00031     }
00032     ;
00033     int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
00034     int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
00035     float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
00036     float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
00037     float centieme_D = (VD_f-VD_int)*1000;
00038     float centieme_G = (VG_f-VG_int)*1000;
00039     if ((rand()%1000) < centieme_G){
00040         VG_int+=1;
00041     }
00042     if ((rand()%1000) < centieme_D){
00043         VD_int+=1;
00044     }
00045     //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
00046     set_PWM_moteur_G(VD_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
00047     set_PWM_moteur_D(VG_int);//
00048 }
00049 void vitesse_nulle_G(int zero){
00050     if(zero == 0){
00051         set_PWM_moteur_G(0);
00052     }
00053 }
00054 void vitesse_nulle_D(int zero){
00055     if(zero == 0){
00056         set_PWM_moteur_D(0);
00057     }
00058 }
00059 void reculer_un_peu(int distance){
00060     motors_on();
00061     long int x_ini = get_x_actuel();
00062     long int y_ini = get_y_actuel();
00063     double angle_vise_deg = get_angle();
00064     double angle_vise=angle_vise_deg*3.142/180;
00065     double angle = get_angle();
00066     
00067     long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
00068     long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
00069     
00070     long int x_actuel = get_x_actuel();
00071     long int y_actuel = get_y_actuel();
00072     long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00073     long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00074     
00075     float vitesse_G;
00076     float vitesse_D;
00077     
00078     angle = get_angle();
00079     
00080     //printf("YOOOO\n\n ");
00081     while (distance+x_local>0){
00082     
00083             vitesse_G = (distance+x_local)/70;
00084             vitesse_D = vitesse_G;
00085             if(vitesse_G >150){
00086                 vitesse_G=150;
00087                 vitesse_D=150;
00088             }
00089             if (vitesse_G<-150){
00090                 vitesse_G=-150;
00091                 vitesse_D=-150;
00092             }
00093             
00094             angle = get_angle();
00095             vitesse_G = vitesse_G  - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local; // petit asser en angle
00096             vitesse_D = vitesse_D  + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local;
00097                 
00098             commande_vitesse(-vitesse_G,-vitesse_D);
00099             actualise_position();
00100             x_actuel = get_x_actuel();
00101             y_actuel = get_y_actuel();
00102             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00103             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00104             printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// 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));
00105         
00106     }
00107     test_rotation_abs(angle_vise_deg);
00108     vitesse_nulle_G(0);
00109     vitesse_nulle_D(0);
00110     wait(0.3);
00111     motors_stop();
00112 }
00113 
00114 void ligne_droite(long int distance)
00115 {
00116     // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
00117     motors_on();
00118     long int x_ini = get_x_actuel();
00119     long int y_ini = get_y_actuel();
00120     double angle_vise_deg = get_angle();
00121     double angle_vise=angle_vise_deg*3.142/180;
00122     double angle = get_angle();
00123     
00124     long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
00125     long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
00126     
00127     long int x_actuel = get_x_actuel();
00128     long int y_actuel = get_y_actuel();
00129     long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00130     long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00131     
00132     float vitesse_G;
00133     float vitesse_D;
00134     
00135     angle = get_angle();
00136     
00137     while (distance-x_local>0){
00138     
00139             vitesse_G = (distance-x_local)/70;
00140             vitesse_D = vitesse_G;
00141             if(vitesse_G >400){
00142                 vitesse_G=400;
00143                 vitesse_D=400;
00144             }
00145             if (vitesse_G<-400){
00146                 vitesse_G=-400;
00147                 vitesse_D=-400;
00148             }
00149             
00150             angle = get_angle();
00151             vitesse_G = vitesse_G  + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local; // petit asser en angle
00152             vitesse_D = vitesse_D  - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local;
00153                 
00154             commande_vitesse(vitesse_G,vitesse_D);
00155             actualise_position();
00156             x_actuel = get_x_actuel();
00157             y_actuel = get_y_actuel();
00158             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00159             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00160             printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// 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));
00161         
00162     }
00163     test_rotation_abs(angle_vise_deg);
00164     vitesse_nulle_G(0);
00165     vitesse_nulle_D(0);
00166     wait(0.3);
00167     motors_stop();
00168 }
00169 void ligne_droite_v2(long int distance)
00170 {
00171     // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
00172     motors_on();
00173     long int x_ini = get_x_actuel();
00174     long int y_ini = get_y_actuel();
00175     double angle_vise_deg = get_angle();
00176     double angle_vise=angle_vise_deg*3.142/180;
00177     double angle = get_angle();
00178     
00179     long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
00180     long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
00181     
00182     long int x_actuel = get_x_actuel();
00183     long int y_actuel = get_y_actuel();
00184     
00185     long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00186     long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00187     
00188     long int y_local_prec = y_local;
00189     
00190     float vitesse_G;
00191     float vitesse_D;
00192     
00193     angle = get_angle();
00194     
00195     float Ki= 0.00007;
00196     float Kp= 0.03;
00197     while (distance-x_local>0){
00198     
00199             vitesse_G = (distance-x_local)/70;
00200             vitesse_D = vitesse_G;
00201             if(vitesse_G >400){
00202                 vitesse_G=400;
00203                 vitesse_D=400;
00204             }
00205             if (vitesse_G<-400){
00206                 vitesse_G=-400;
00207                 vitesse_D=-400;
00208             }
00209             
00210             angle = get_angle();
00211             //vitesse_G = vitesse_G + (y_local * 0.02) + (y_local - y_local_prec)*2;
00212             //vitesse_D = vitesse_D - (y_local * 0.02) - (y_local - y_local_prec)*2;
00213             vitesse_G = vitesse_G * (1 + Ki*y_local + Kp * diff_angle(angle_vise_deg, angle));
00214             vitesse_D = vitesse_D * (1 - Ki*y_local - Kp * diff_angle(angle_vise_deg, angle));
00215             
00216             commande_vitesse(vitesse_G,vitesse_D);
00217             actualise_position();
00218             x_actuel = get_x_actuel();
00219             y_actuel = get_y_actuel();
00220             y_local_prec = y_local;
00221             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00222             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00223             
00224             //printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise);// 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));
00225     }
00226     test_rotation_abs(angle_vise_deg);
00227     vitesse_nulle_G(0);
00228     vitesse_nulle_D(0);
00229     wait(0.3);
00230     motors_stop();
00231 }
00232 
00233 void test_rotation_rel(double angle_vise)
00234 {
00235     // rotation de angle_vise
00236     motors_on();
00237     float vitesse;
00238     double angle = get_angle();
00239     angle_vise+=angle;
00240     borne_angle_d(angle_vise);
00241         
00242     while (abs(diff_angle(angle,angle_vise))>0.05)
00243     {
00244         vitesse=1.3*diff_angle(angle,angle_vise);
00245         commande_vitesse(-vitesse,vitesse);
00246         actualise_position();
00247         angle = get_angle();
00248         printf("vitesse : %f", vitesse);
00249     }
00250     
00251     //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));
00252     
00253     vitesse_nulle_G(0);
00254     vitesse_nulle_D(0);
00255     wait(0.2);
00256     motors_stop();
00257 }
00258 
00259 
00260 void test_rotation_abs(double angle_vise)
00261 {
00262     double angle_rel = borne_angle_d(angle_vise-get_angle());
00263     test_rotation_rel(angle_rel);
00264 }