Guillaume Chauvon
/
Asservissment_robot2_v16_05
l
Diff: deplacement.cpp
- Revision:
- 2:3066e614372f
- Child:
- 3:d38aa400d5e7
diff -r 0d76bc4e1aea -r 3066e614372f deplacement.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/deplacement.cpp Wed May 08 21:19:10 2019 +0000 @@ -0,0 +1,476 @@ +#include "deplacement.h" +#include "mbed.h" +#include "odometrie.h" +#include "hardware.h" +#include "math_precalc.h" +#include "reglages.h" + + +deplacement::deplacement(){ + consigne_D = 0; + consigne_G = 0; + somme_erreur_D = 0; + somme_erreur_G = 0; + erreur_precedente_D = 0; + erreur_precedente_G = 0; + compteur_asser =0; + somme_y=0; + + for (int k =0; k<5;k++){ + erreur_glissee_D[k] = 0; + erreur_glissee_G[k] = 0; + } + compteur_glisse = 0; + + Kp_D = 1.5;//1 + Ki_D = 0.12;//0.15 + Kd_D = 0.5;//1 + + Kp_G = 1;//1 + Ki_G = 0.13;//0.15 + Kd_G = 1.2;//1 + + tick_prec_D=0; + tick_prec_G = 0; + dix_ms = 0; + for (int k =0; k<TAILLE_TAB;k++){ + tab_cmd_G[k]=0; + tab_cmd_D[k]=0; + vtab_G[k]=0; + vtab_D[k]=0; + c_D[k]=0; + c_G[k]=0; + } + consigne_tab[0][0]=0; + consigne_tab[0][1]=0; + + consigne_tab[1][0]=10; + consigne_tab[1][1]=10; + + consigne_tab[2][0]=20; + consigne_tab[2][1]=20; + + consigne_tab[3][0]=30; + consigne_tab[3][1]=30; + + consigne_tab[4][0]=40; + consigne_tab[4][1]=40; + + /* consigne_tab[5][0]=3*5; + consigne_tab[5][1]=3*5; + + consigne_tab[6][0]=3*6; + consigne_tab[6][1]=3*6; + + consigne_tab[7][0]=3*7; + consigne_tab[7][1]=3*7; + + consigne_tab[8][0]=3*8; + consigne_tab[8][1]=3*8; + + consigne_tab[9][0]=3*9; + consigne_tab[9][1]=3*9; + + consigne_tab[10][0]=3*10; + consigne_tab[10][1]=3*10; + + consigne_tab[11][0]=3*11; + consigne_tab[11][1]=3*11; + + consigne_tab[12][0]=3*12; + consigne_tab[12][1]=3*12; + + consigne_tab[13][0]=3*13; + consigne_tab[13][1]=3*13; + + consigne_tab[14][0]=3*14; + consigne_tab[14][1]=3*14; + + consigne_tab[15][0]=0; + consigne_tab[15][1]=0; + + consigne_tab[16][0]=0; + consigne_tab[16][1]=0; + + consigne_tab[17][0]=0; + consigne_tab[17][1]=0; + + consigne_tab[18][0]=0; + consigne_tab[18][1]=0; + + consigne_tab[19][0]=0; + consigne_tab[19][1]=0;*/ +} + +void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM + + int sens_G=signe(vitesse_G); + int sens_D=signe(vitesse_D); + double vitesse_local_G=abs(vitesse_G); + double vitesse_local_D=abs(vitesse_D); + + if(abs(vitesse_G) > 900){ + vitesse_local_G=900; + } + if(abs(vitesse_G)<2){ + vitesse_local_G=2; + } + if(abs(vitesse_D) > 900){ + vitesse_local_D=900; + } + if(abs(vitesse_D)< 2){ + vitesse_local_D=2; + + } + ; + int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G; + int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D; + float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G; + float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D; + float centieme_D = (VD_f-VD_int)*1000; + float centieme_G = (VG_f-VG_int)*1000; + if ((rand()%1000) < centieme_G){ + VG_int+=1; + } + if ((rand()%1000) < centieme_D){ + VD_int+=1; + } + //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int); + set_PWM_moteur_G(VG_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots + set_PWM_moteur_D(VD_int);// +} +void deplacement::vitesse_nulle_G(int zero){ + if(zero == 0){ + set_PWM_moteur_G(0); + } +} +void deplacement::vitesse_nulle_D(int zero){ + if(zero == 0){ + set_PWM_moteur_D(0); + } +} +void deplacement::reculer_un_peu(int distance) +{ + somme_y=0; + // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900) + motors_on(); + actualise_position(); + double x_ini = get_x_actuel(); + double y_ini = get_y_actuel(); + double angle_vise_deg = get_angle(); + double angle_vise=angle_vise_deg*3.1416/180; + double angle = get_angle(); + + double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise); + double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise); + + double x_actuel = get_x_actuel(); + double y_actuel = get_y_actuel(); + + + double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini; + double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini; + + //long int y_local_prec = y_local; + float vitesse_G; + float vitesse_D; + + angle = get_angle(); + float Kip=0; + float Kpp= 0.05 ; + float Kdp= 10; + while (distance-x_local<0){ + + vitesse_G = (distance-x_local)/70; + vitesse_D = vitesse_G; + if(vitesse_G >400){ + vitesse_G=400; + vitesse_D=400; + } + if (vitesse_G<-400){ + vitesse_G=-400; + vitesse_D=-400; + } + + angle = get_angle(); + + vitesse_G = vitesse_G - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y; + vitesse_D = vitesse_D + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y; + //consigne_D = vitesse_D; + //consigne_G = vitesse_G; + commande_vitesse(vitesse_G,vitesse_D); + actualise_position(); + x_actuel = get_x_actuel(); + y_actuel = get_y_actuel(); + somme_y+=y_actuel; + //y_local_prec = y_local; + x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini; + y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini; + if (compteur_asser==150){ + compteur_asser=0; + //printf("%lf\n",get_y_actuel()); + } + compteur_asser++; + //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)); + } + //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle()); + test_rotation_abs(angle_vise_deg); + //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle()); +} + + + +void deplacement::ligne_droite_v2(long int distance) +{ + somme_y=0; + // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900) + motors_on(); + actualise_position(); + double x_ini = get_x_actuel(); + double y_ini = get_y_actuel(); + double angle_vise_deg = get_angle(); + double angle_vise=angle_vise_deg*3.1416/180; + double angle = get_angle(); + + double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise); + double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise); + + double x_actuel = get_x_actuel(); + double y_actuel = get_y_actuel(); + + + double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini; + double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini; + + //long int y_local_prec = y_local; + float vitesse_G; + float vitesse_D; + + angle = get_angle(); + float Kip=0; + float Kpp= 0.05 ; + float Kdp= 10; + while (distance-x_local>0){ + + vitesse_G = (distance-x_local)/70; + vitesse_D = vitesse_G; + if(vitesse_G >400){ + vitesse_G=400; + vitesse_D=400; + } + if (vitesse_G<-400){ + vitesse_G=-400; + vitesse_D=-400; + } + + angle = get_angle(); + + vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y; + vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y; + //consigne_D = vitesse_D; + //consigne_G = vitesse_G; + commande_vitesse(vitesse_G,vitesse_D); + actualise_position(); + x_actuel = get_x_actuel(); + y_actuel = get_y_actuel(); + somme_y+=y_actuel; + //y_local_prec = y_local; + x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini; + y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini; + if (compteur_asser==150){ + compteur_asser=0; + //printf("%lf\n",get_y_actuel()); + } + compteur_asser++; + //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)); + } + //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle()); + test_rotation_abs(angle_vise_deg); + //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle()); +} + +void deplacement::test_rotation_rel(double angle_vise) +{ + // rotation de angle_vise + motors_on(); + double vitesse=180; + int sens; + double angle = get_angle(); + angle_vise+=angle; + borne_angle_d(angle_vise); + if (diff_angle(angle,angle_vise)<=0){ + sens = -1; + //printf("negatif\n"); + } + else{ + sens = 1; + + //printf("positif\n"); + } + //printf("diff : %lf ",diff_angle(angle,angle_vise)); + while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100) + { + actualise_position(); + angle = get_angle(); + vitesse=1.5*sens*abs(diff_angle(angle,angle_vise)); + + commande_vitesse(-vitesse,vitesse); + if (compteur_asser==150){ + compteur_asser=0; + //printf("%lf\n",get_y_actuel()); + } + compteur_asser++; + //printf("vitesse : %lf ", vitesse); + } + //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise)); + //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)); + //consigne_D = 0; + //consigne_G = 0; + vitesse_nulle_G(0); + vitesse_nulle_D(0); + wait(0.2); + motors_stop(); +} + + +void deplacement::test_rotation_abs(double angle_vise) +{ + actualise_position(); + //printf("bite"); + double angle_rel = borne_angle_d(angle_vise-get_angle()); + test_rotation_rel(angle_rel); +} + +void deplacement::asservissement(){ + long int tick_D = get_nbr_tick_D(); + long int tick_G = get_nbr_tick_G(); + + long int tick_D_passe = tick_D-tick_prec_D; + long int tick_G_passe = tick_G-tick_prec_G; + + tick_prec_D=tick_D; + tick_prec_G=tick_G; + + float vitesse_codeuse_D = tick_D_passe; + float vitesse_codeuse_G = tick_G_passe; + + float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D; + float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G; + + if (compteur_glisse == 5) + compteur_glisse = 0; + + if (compteur_glisse == -1) + { + compteur_glisse = 0; + for (int i = 0; i<5; i++){ + erreur_glissee_D[compteur_glisse] = erreur_D; + erreur_glissee_G[compteur_glisse] = erreur_G; + } + } + + erreur_glissee_D[compteur_glisse] = erreur_D; + erreur_glissee_G[compteur_glisse] = erreur_G; + compteur_glisse++; + + erreur_D = erreur_glissee_D[0]; + erreur_G = erreur_glissee_G[0]; + for (int i=1; i<5; i++) + { + erreur_D += erreur_glissee_D[i]; + erreur_G += erreur_glissee_G[i]; + } + + erreur_D = erreur_D/5.0; + erreur_G = erreur_G/5.0; // erreur est maintenant la moyenne des 5 erreurs prec + + somme_erreur_D += erreur_D; + somme_erreur_G += erreur_G; + + float delta_erreur_D = erreur_D-erreur_precedente_D; + float delta_erreur_G = erreur_G-erreur_precedente_G; + + erreur_precedente_G = erreur_G; + erreur_precedente_D = erreur_D; + + float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D; + float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G; + + if (cmd_G <0){ + cmd_G = 0; + } + if (cmd_G > 500){ + cmd_G = 500; + } + if (cmd_D <0){ + cmd_D = 0; + } + if (cmd_D > 500){ + cmd_D = 500; + } + c_D[dix_ms]=consigne_D; + c_G[dix_ms]=consigne_G; + //printf("%d\n",c[i]); + tab_cmd_D[dix_ms] = cmd_D; + tab_cmd_G[dix_ms] = cmd_G; + vtab_D[dix_ms] = vitesse_codeuse_D; + vtab_G[dix_ms] = vitesse_codeuse_G; + commande_vitesse(cmd_G,cmd_D); + dix_ms++; + //printf("%d\n",i); + //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D); + //printf("%f,%f\n",cmd_G,cmd_D); + //printf("oui"); +} + +void deplacement::printftab(){ + + for (int j =0;j<TAILLE_TAB;j++){ + if(j==500) + bouton(); + 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]); + } + /*if (j<5) + 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]); + else + 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])); + }*/ + + /*for (int j =0;j<TAILLE_TAB;j++){ + 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); + }*/ +} + +void deplacement::test(){ + Timer t; + t.start(); + for (int i =0;i<5 ;i++){ + changement_consigne(consigne_tab[i][0], consigne_tab[i][1]); + while(t.read()<0.5){ + //actualise_positio n(); + } + //printf("t.read() : %f\n",t.read()); + //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G); + t.reset(); + } +} + +void deplacement::changement_consigne(int cons_D, int cons_G){ + consigne_D = cons_D; + consigne_G = cons_G; + compteur_glisse = -1; +} + +void deplacement::bouton(){ + DigitalIn depart(USER_BUTTON); + while (depart){} +} + +void deplacement::poussette(){ + motors_on(); + commande_vitesse(150,150); + wait(1); + vitesse_nulle_G(0); + vitesse_nulle_D(0); + motors_stop(); +} \ No newline at end of file