deplacement.cpp

00001 #include "deplacement.h"
00002 #include "mbed.h"
00003 #include "odometrie.h"
00004 #include "hardware.h"
00005 #include "math_precalc.h"
00006 #include "reglages.h"
00007 
00008 
00009 deplacement::deplacement(){
00010     consigne_D = 0;
00011     consigne_G = 0;
00012     somme_erreur_D = 0;
00013     somme_erreur_G = 0;
00014     erreur_precedente_D = 0;
00015     erreur_precedente_G = 0;
00016     compteur_asser =0;
00017     somme_y=0;
00018     
00019     for (int k =0; k<5;k++){
00020         erreur_glissee_D[k] = 0;
00021         erreur_glissee_G[k] = 0;
00022     }
00023     compteur_glisse = 0;
00024     
00025     Kp_D = 1.5;//1
00026     Ki_D = 0.12;//0.15
00027     Kd_D = 0.5;//1
00028     
00029     Kp_G = 1;//1
00030     Ki_G = 0.13;//0.15
00031     Kd_G = 1.2;//1
00032     
00033     tick_prec_D=0;
00034     tick_prec_G = 0;
00035     dix_ms = 0;
00036     for (int k =0; k<TAILLE_TAB;k++){
00037         tab_cmd_G[k]=0;
00038         tab_cmd_D[k]=0;
00039         vtab_G[k]=0;
00040         vtab_D[k]=0;
00041         c_D[k]=0;
00042         c_G[k]=0;
00043     }
00044     consigne_tab[0][0]=0;
00045     consigne_tab[0][1]=0;
00046     
00047     consigne_tab[1][0]=10;
00048     consigne_tab[1][1]=10;
00049     
00050     consigne_tab[2][0]=20;
00051     consigne_tab[2][1]=20;
00052     
00053     consigne_tab[3][0]=30;
00054     consigne_tab[3][1]=30;
00055     
00056     consigne_tab[4][0]=40;
00057     consigne_tab[4][1]=40;
00058     
00059    /* consigne_tab[5][0]=3*5;
00060     consigne_tab[5][1]=3*5;
00061     
00062     consigne_tab[6][0]=3*6;
00063     consigne_tab[6][1]=3*6;
00064     
00065     consigne_tab[7][0]=3*7;
00066     consigne_tab[7][1]=3*7;
00067     
00068     consigne_tab[8][0]=3*8;
00069     consigne_tab[8][1]=3*8;
00070     
00071     consigne_tab[9][0]=3*9;
00072     consigne_tab[9][1]=3*9;
00073     
00074     consigne_tab[10][0]=3*10;
00075     consigne_tab[10][1]=3*10;
00076     
00077     consigne_tab[11][0]=3*11;
00078     consigne_tab[11][1]=3*11;
00079     
00080     consigne_tab[12][0]=3*12;
00081     consigne_tab[12][1]=3*12;
00082     
00083     consigne_tab[13][0]=3*13;
00084     consigne_tab[13][1]=3*13;
00085     
00086     consigne_tab[14][0]=3*14;
00087     consigne_tab[14][1]=3*14;
00088     
00089     consigne_tab[15][0]=0;
00090     consigne_tab[15][1]=0;
00091     
00092     consigne_tab[16][0]=0;
00093     consigne_tab[16][1]=0;
00094     
00095     consigne_tab[17][0]=0;
00096     consigne_tab[17][1]=0;
00097     
00098     consigne_tab[18][0]=0;
00099     consigne_tab[18][1]=0;
00100     
00101     consigne_tab[19][0]=0;
00102     consigne_tab[19][1]=0;*/
00103 }
00104 
00105 void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM, en ticks/ms
00106     
00107     int sens_G=signe(vitesse_G);
00108     int sens_D=signe(vitesse_D);
00109     double vitesse_local_G=abs(vitesse_G);
00110     double vitesse_local_D=abs(vitesse_D);
00111     
00112     if(abs(vitesse_G) > 900){
00113         vitesse_local_G=900;
00114     }
00115     if(abs(vitesse_G)<5){
00116         vitesse_local_G=2;
00117     }
00118     if(abs(vitesse_D) > 900){
00119         vitesse_local_D=900;
00120     }
00121     if(abs(vitesse_D)< 5){
00122         vitesse_local_D=2;
00123     
00124     }
00125     ;
00126     int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
00127     int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
00128     float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
00129     float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
00130     float centieme_D = (VD_f-VD_int)*1000;
00131     float centieme_G = (VG_f-VG_int)*1000;
00132     if ((rand()%1000) < centieme_G){
00133         VG_int+=1;
00134     }
00135     if ((rand()%1000) < centieme_D){
00136         VD_int+=1;
00137     }
00138     //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
00139     set_PWM_moteur_G(VD_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
00140     set_PWM_moteur_D(VG_int);//
00141 }
00142 void deplacement::vitesse_nulle_G(int zero){
00143     if(zero == 0){
00144         set_PWM_moteur_G(0);
00145     }
00146 }
00147 void deplacement::vitesse_nulle_D(int zero){
00148     if(zero == 0){
00149         set_PWM_moteur_D(0);
00150     }
00151 }
00152 void deplacement::reculer_un_peu(int distance)
00153 {
00154     somme_y=0;
00155     // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
00156     motors_on();
00157     actualise_position();
00158     double x_ini = get_x_actuel();
00159     double y_ini = get_y_actuel();
00160     double angle_vise_deg = get_angle();
00161     double angle_vise=angle_vise_deg*3.1416/180;
00162     double angle = get_angle();
00163     
00164     double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
00165     double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
00166     
00167     double x_actuel = get_x_actuel();
00168     double y_actuel = get_y_actuel();
00169     
00170     
00171     double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00172     double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00173     
00174     //long int y_local_prec = y_local;
00175     float vitesse_G;
00176     float vitesse_D;
00177     
00178     angle = get_angle();
00179     float Kip=0;
00180     float Kpp= 0.05 ;
00181     float Kdp= 10;
00182     while (distance-x_local<0){
00183             
00184             vitesse_G = (distance-x_local)/70;
00185             vitesse_D = vitesse_G;
00186             if(vitesse_G >400){
00187                 vitesse_G=400;
00188                 vitesse_D=400;
00189             }
00190             if (vitesse_G<-400){
00191                 vitesse_G=-400;
00192                 vitesse_D=-400;
00193             }
00194             
00195             angle = get_angle();
00196             
00197             vitesse_G = vitesse_G  - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
00198             vitesse_D = vitesse_D  + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
00199             //consigne_D = vitesse_D;
00200             //consigne_G = vitesse_G;
00201             commande_vitesse(vitesse_G,vitesse_D);
00202             actualise_position();
00203             x_actuel = get_x_actuel();
00204             y_actuel = get_y_actuel();
00205             somme_y+=y_actuel;
00206             //y_local_prec = y_local;
00207             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00208             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00209             if (compteur_asser==150){
00210                 compteur_asser=0;
00211                 //printf("%lf\n",get_y_actuel());
00212             }
00213             compteur_asser++;
00214             printf("   VG : %f  VD : %f ; x_local : %lf, y_local : %lf, angle_vise : %f\n",vitesse_G,vitesse_D, x_local,y_local, 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));
00215     }
00216     //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
00217     test_rotation_abs(angle_vise_deg);
00218     //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
00219 }
00220 
00221 
00222 
00223 void deplacement::ligne_droite_v2(long int distance)
00224 {
00225     somme_y=0;
00226     // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
00227     motors_on();
00228     actualise_position();
00229     double x_ini = get_x_actuel();
00230     double y_ini = get_y_actuel();
00231     double angle_vise_deg = get_angle();
00232     double angle_vise=angle_vise_deg*3.1416/180;
00233     double angle = get_angle();
00234     
00235     double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
00236     double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
00237     
00238     double x_actuel = get_x_actuel();
00239     double y_actuel = get_y_actuel();
00240     
00241     
00242     double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00243     double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00244     
00245     //long int y_local_prec = y_local;
00246     float vitesse_G;
00247     float vitesse_D;
00248     
00249     angle = get_angle();
00250     float Kip=0;
00251     float Kpp= 0.05 ;
00252     float Kdp= 10;
00253     while (distance-x_local>0){
00254             
00255             vitesse_G = (distance-x_local)/70;
00256             vitesse_D = vitesse_G;
00257             if(vitesse_G >400){
00258                 vitesse_G=400;
00259                 vitesse_D=400;
00260             }
00261             if (vitesse_G<-400){
00262                 vitesse_G=-400;
00263                 vitesse_D=-400;
00264             }
00265             
00266             angle = get_angle();
00267             
00268             vitesse_G = vitesse_G  + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
00269             vitesse_D = vitesse_D  - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
00270             //consigne_D = vitesse_D;
00271             //consigne_G = vitesse_G;
00272             commande_vitesse(vitesse_G,vitesse_D);
00273             actualise_position();
00274             x_actuel = get_x_actuel();
00275             y_actuel = get_y_actuel();
00276             somme_y+=y_actuel;
00277             //y_local_prec = y_local;
00278             x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
00279             y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
00280             if (compteur_asser==150){
00281                 compteur_asser=0;
00282                 //printf("%lf\n",get_y_actuel());
00283             }
00284             compteur_asser++;
00285             //printf("   VG : %f  VD : %f ; x_local : %d, y_local : %d, angle_vise : %f",vitesse_G,vitesse_D, x_local,y_local, angle_vise_deg);// 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));
00286     }
00287     //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
00288     test_rotation_abs(angle_vise_deg);
00289     //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
00290 }
00291 
00292 void deplacement::test_rotation_rel(double angle_vise)
00293 {
00294     // rotation de angle_vise
00295     motors_on();
00296     double vitesse=180;
00297     int sens;
00298     double angle = get_angle();
00299     angle_vise+=angle;
00300     borne_angle_d(angle_vise);
00301     if (diff_angle(angle,angle_vise)<=0){
00302         sens = -1;
00303         //printf("negatif\n");
00304     }
00305     else{
00306         sens = 1;
00307         
00308         //printf("positif\n");
00309     }
00310     //printf("diff : %lf ",diff_angle(angle,angle_vise));
00311     while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
00312     {
00313         actualise_position();
00314         angle = get_angle();
00315         vitesse=1.5*sens*abs(diff_angle(angle,angle_vise));
00316         
00317         commande_vitesse(-vitesse,vitesse);
00318         if (compteur_asser==150){
00319                 compteur_asser=0;
00320                 //printf("%lf\n",get_y_actuel());
00321             }
00322         compteur_asser++;
00323         //printf("vitesse : %lf ", vitesse);
00324     }
00325     //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
00326     //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));
00327     //consigne_D = 0;
00328     //consigne_G = 0;
00329     vitesse_nulle_G(0);
00330     vitesse_nulle_D(0);
00331     wait(0.2);
00332     motors_stop();
00333 }
00334 
00335 
00336 void deplacement::test_rotation_abs(double angle_vise)
00337 {
00338     actualise_position();
00339     //printf("bite");
00340     double angle_rel = borne_angle_d(angle_vise-get_angle());
00341     test_rotation_rel(angle_rel);
00342 }
00343 
00344 void deplacement::asservissement(){
00345     long int tick_D = get_nbr_tick_D();
00346     long int tick_G = get_nbr_tick_G();
00347     
00348     long int tick_D_passe = tick_D-tick_prec_D;
00349     long int tick_G_passe = tick_G-tick_prec_G;
00350     
00351     tick_prec_D=tick_D;
00352     tick_prec_G=tick_G;
00353     
00354     float vitesse_codeuse_D = tick_D_passe;
00355     float vitesse_codeuse_G = tick_G_passe;
00356     
00357     float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
00358     float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
00359     
00360     if (compteur_glisse == 5)
00361         compteur_glisse = 0;
00362     
00363     if (compteur_glisse == -1)
00364     {
00365         compteur_glisse = 0;
00366         for (int i = 0; i<5; i++){
00367             erreur_glissee_D[compteur_glisse] = erreur_D;
00368             erreur_glissee_G[compteur_glisse] = erreur_G;
00369         }
00370     }
00371     
00372     erreur_glissee_D[compteur_glisse] = erreur_D;
00373     erreur_glissee_G[compteur_glisse] = erreur_G;
00374     compteur_glisse++;
00375     
00376     erreur_D = erreur_glissee_D[0];
00377     erreur_G = erreur_glissee_G[0];
00378     for (int i=1; i<5; i++)
00379     {
00380          erreur_D += erreur_glissee_D[i];
00381          erreur_G += erreur_glissee_G[i];
00382     }
00383     
00384     erreur_D = erreur_D/5.0;
00385     erreur_G = erreur_G/5.0; // erreur est maintenant la moyenne des 5 erreurs prec
00386     
00387     somme_erreur_D += erreur_D;
00388     somme_erreur_G += erreur_G;
00389     
00390     float delta_erreur_D = erreur_D-erreur_precedente_D;
00391     float delta_erreur_G = erreur_G-erreur_precedente_G;
00392     
00393     erreur_precedente_G = erreur_G;
00394     erreur_precedente_D = erreur_D;
00395     
00396     float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D;
00397     float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G;
00398     
00399     if (cmd_G <0){
00400         cmd_G = 0;
00401     }
00402     if (cmd_G > 500){
00403         cmd_G = 500;
00404     }
00405     if (cmd_D <0){
00406         cmd_D = 0;
00407     }
00408     if (cmd_D > 500){
00409         cmd_D = 500;
00410     }
00411     c_D[dix_ms]=consigne_D;
00412     c_G[dix_ms]=consigne_G;
00413     //printf("%d\n",c[i]);
00414     tab_cmd_D[dix_ms] = cmd_D;
00415     tab_cmd_G[dix_ms] = cmd_G;
00416     vtab_D[dix_ms] = vitesse_codeuse_D;
00417     vtab_G[dix_ms] = vitesse_codeuse_G;
00418     commande_vitesse(cmd_G,cmd_D);
00419     dix_ms++;
00420     //printf("%d\n",i);
00421     //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
00422     //printf("%f,%f\n",cmd_G,cmd_D);
00423     //printf("oui");
00424 }
00425 
00426 void deplacement::printftab(){
00427 
00428     for (int j =0;j<TAILLE_TAB;j++){
00429         if(j==500)
00430             bouton();
00431         printf("%f,%f,%f,%f,%f,%f\n",tab_cmd_G[j],10*vtab_G[j],10*c_D[j],tab_cmd_D[j],10*vtab_D[j],10*c_G[j]);
00432     }
00433         /*if (j<5)
00434             printf("%f,%f,%f,%f,%f\n",tab_cmd_G[j],10*vtab_G[j],10*c[j],tab_cmd_D[j],10*vtab_D[j]);
00435         else
00436             printf("%f,%f,%f,%f,%f\n",tab_cmd_G[j],2*(vtab_G[j]+vtab_G[j-1]+vtab_G[j-2]+vtab_G[j-3]+vtab_G[j-4]),10*c[j],tab_cmd_D[j],2*(vtab_D[j]+vtab_D[j-1]+vtab_D[j-2]+vtab_D[j-3]+vtab_D[j-4]));
00437     }*/
00438     
00439     /*for (int j =0;j<TAILLE_TAB;j++){
00440         printf("%f,%f,%d\n",2*(vtab_G[j]+vtab_G[j-1]+vtab_G[j-2]+vtab_G[j-3]+vtab_G[j-4]), 2*(vtab_D[j]+vtab_D[j-1]+vtab_D[j-2]+vtab_D[j-3]+vtab_D[j-4]), j);
00441     }*/
00442 }
00443 
00444 void deplacement::test(){
00445     Timer t;
00446     t.start();
00447     for (int i =0;i<5 ;i++){
00448         changement_consigne(consigne_tab[i][0], consigne_tab[i][1]);
00449         while(t.read()<0.5){
00450             //actualise_positio n();
00451         }
00452         //printf("t.read() : %f\n",t.read());
00453         //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G);
00454         t.reset();
00455     }
00456 }
00457 
00458 void deplacement::changement_consigne(int cons_D, int cons_G){
00459     consigne_D = cons_D;
00460     consigne_G = cons_G;
00461     compteur_glisse = -1;   
00462 }
00463 
00464 void deplacement::bouton(){
00465     DigitalIn depart(USER_BUTTON);
00466     while (depart){}
00467 }
00468 
00469 void deplacement::poussette(){
00470     motors_on();
00471     commande_vitesse(150,150);
00472     wait(2);
00473     vitesse_nulle_G(0);
00474     vitesse_nulle_D(0);
00475     motors_stop();
00476 }