Guillaume Chauvon / Mbed 2 deprecated Asserve12

asser1

Dependencies:   mbed asser1

deplacement.cpp@3:1dba6eca01ad, 2019-05-06 (annotated)

Committer:
GuillaumeCH
Date:
Mon May 06 13:48:45 2019 +0000
Revision:
3:1dba6eca01ad
Child:
4:deef042e9c02
O

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GuillaumeCH 3:1dba6eca01ad 1 #include "deplacement.h"
GuillaumeCH 3:1dba6eca01ad 2 #include "mbed.h"
GuillaumeCH 3:1dba6eca01ad 3 #include "odometrie.h"
GuillaumeCH 3:1dba6eca01ad 4 #include "hardware.h"
GuillaumeCH 3:1dba6eca01ad 5 #include "math_precalc.h"
GuillaumeCH 3:1dba6eca01ad 6 #include "reglages.h"
GuillaumeCH 3:1dba6eca01ad 7
GuillaumeCH 3:1dba6eca01ad 8
GuillaumeCH 3:1dba6eca01ad 9 deplacement::deplacement(){
GuillaumeCH 3:1dba6eca01ad 10 consigne_D = 0;
GuillaumeCH 3:1dba6eca01ad 11 consigne_G = 0;
GuillaumeCH 3:1dba6eca01ad 12 somme_erreur_D = 0;
GuillaumeCH 3:1dba6eca01ad 13 somme_erreur_G = 0;
GuillaumeCH 3:1dba6eca01ad 14 erreur_precedente_D = 0;
GuillaumeCH 3:1dba6eca01ad 15 erreur_precedente_G = 0;
GuillaumeCH 3:1dba6eca01ad 16
GuillaumeCH 3:1dba6eca01ad 17 for (int k =0; k<5;k++){
GuillaumeCH 3:1dba6eca01ad 18 erreur_glissee_D[k] = 0;
GuillaumeCH 3:1dba6eca01ad 19 erreur_glissee_G[k] = 0;
GuillaumeCH 3:1dba6eca01ad 20 }
GuillaumeCH 3:1dba6eca01ad 21 compteur_glisse = 0;
GuillaumeCH 3:1dba6eca01ad 22
GuillaumeCH 3:1dba6eca01ad 23 Kp_D = 1.5;//1
GuillaumeCH 3:1dba6eca01ad 24 Ki_D = 0.12;//0.15
GuillaumeCH 3:1dba6eca01ad 25 Kd_D = 0.5;//1
GuillaumeCH 3:1dba6eca01ad 26
GuillaumeCH 3:1dba6eca01ad 27 Kp_G = 1;//1
GuillaumeCH 3:1dba6eca01ad 28 Ki_G = 0.13;//0.15
GuillaumeCH 3:1dba6eca01ad 29 Kd_G = 1.2;//1
GuillaumeCH 3:1dba6eca01ad 30
GuillaumeCH 3:1dba6eca01ad 31 tick_prec_D=0;
GuillaumeCH 3:1dba6eca01ad 32 tick_prec_G = 0;
GuillaumeCH 3:1dba6eca01ad 33 dix_ms = 0;
GuillaumeCH 3:1dba6eca01ad 34 for (int k =0; k<TAILLE_TAB;k++){
GuillaumeCH 3:1dba6eca01ad 35 tab_cmd_G[k]=0;
GuillaumeCH 3:1dba6eca01ad 36 tab_cmd_D[k]=0;
GuillaumeCH 3:1dba6eca01ad 37 vtab_G[k]=0;
GuillaumeCH 3:1dba6eca01ad 38 vtab_D[k]=0;
GuillaumeCH 3:1dba6eca01ad 39 c_D[k]=0;
GuillaumeCH 3:1dba6eca01ad 40 c_G[k]=0;
GuillaumeCH 3:1dba6eca01ad 41 }
GuillaumeCH 3:1dba6eca01ad 42 consigne_tab[0][0]=0;
GuillaumeCH 3:1dba6eca01ad 43 consigne_tab[0][1]=0;
GuillaumeCH 3:1dba6eca01ad 44
GuillaumeCH 3:1dba6eca01ad 45 consigne_tab[1][0]=10;
GuillaumeCH 3:1dba6eca01ad 46 consigne_tab[1][1]=10;
GuillaumeCH 3:1dba6eca01ad 47
GuillaumeCH 3:1dba6eca01ad 48 consigne_tab[2][0]=20;
GuillaumeCH 3:1dba6eca01ad 49 consigne_tab[2][1]=20;
GuillaumeCH 3:1dba6eca01ad 50
GuillaumeCH 3:1dba6eca01ad 51 consigne_tab[3][0]=30;
GuillaumeCH 3:1dba6eca01ad 52 consigne_tab[3][1]=30;
GuillaumeCH 3:1dba6eca01ad 53
GuillaumeCH 3:1dba6eca01ad 54 consigne_tab[4][0]=40;
GuillaumeCH 3:1dba6eca01ad 55 consigne_tab[4][1]=40;
GuillaumeCH 3:1dba6eca01ad 56
GuillaumeCH 3:1dba6eca01ad 57 /* consigne_tab[5][0]=3*5;
GuillaumeCH 3:1dba6eca01ad 58 consigne_tab[5][1]=3*5;
GuillaumeCH 3:1dba6eca01ad 59
GuillaumeCH 3:1dba6eca01ad 60 consigne_tab[6][0]=3*6;
GuillaumeCH 3:1dba6eca01ad 61 consigne_tab[6][1]=3*6;
GuillaumeCH 3:1dba6eca01ad 62
GuillaumeCH 3:1dba6eca01ad 63 consigne_tab[7][0]=3*7;
GuillaumeCH 3:1dba6eca01ad 64 consigne_tab[7][1]=3*7;
GuillaumeCH 3:1dba6eca01ad 65
GuillaumeCH 3:1dba6eca01ad 66 consigne_tab[8][0]=3*8;
GuillaumeCH 3:1dba6eca01ad 67 consigne_tab[8][1]=3*8;
GuillaumeCH 3:1dba6eca01ad 68
GuillaumeCH 3:1dba6eca01ad 69 consigne_tab[9][0]=3*9;
GuillaumeCH 3:1dba6eca01ad 70 consigne_tab[9][1]=3*9;
GuillaumeCH 3:1dba6eca01ad 71
GuillaumeCH 3:1dba6eca01ad 72 consigne_tab[10][0]=3*10;
GuillaumeCH 3:1dba6eca01ad 73 consigne_tab[10][1]=3*10;
GuillaumeCH 3:1dba6eca01ad 74
GuillaumeCH 3:1dba6eca01ad 75 consigne_tab[11][0]=3*11;
GuillaumeCH 3:1dba6eca01ad 76 consigne_tab[11][1]=3*11;
GuillaumeCH 3:1dba6eca01ad 77
GuillaumeCH 3:1dba6eca01ad 78 consigne_tab[12][0]=3*12;
GuillaumeCH 3:1dba6eca01ad 79 consigne_tab[12][1]=3*12;
GuillaumeCH 3:1dba6eca01ad 80
GuillaumeCH 3:1dba6eca01ad 81 consigne_tab[13][0]=3*13;
GuillaumeCH 3:1dba6eca01ad 82 consigne_tab[13][1]=3*13;
GuillaumeCH 3:1dba6eca01ad 83
GuillaumeCH 3:1dba6eca01ad 84 consigne_tab[14][0]=3*14;
GuillaumeCH 3:1dba6eca01ad 85 consigne_tab[14][1]=3*14;
GuillaumeCH 3:1dba6eca01ad 86
GuillaumeCH 3:1dba6eca01ad 87 consigne_tab[15][0]=0;
GuillaumeCH 3:1dba6eca01ad 88 consigne_tab[15][1]=0;
GuillaumeCH 3:1dba6eca01ad 89
GuillaumeCH 3:1dba6eca01ad 90 consigne_tab[16][0]=0;
GuillaumeCH 3:1dba6eca01ad 91 consigne_tab[16][1]=0;
GuillaumeCH 3:1dba6eca01ad 92
GuillaumeCH 3:1dba6eca01ad 93 consigne_tab[17][0]=0;
GuillaumeCH 3:1dba6eca01ad 94 consigne_tab[17][1]=0;
GuillaumeCH 3:1dba6eca01ad 95
GuillaumeCH 3:1dba6eca01ad 96 consigne_tab[18][0]=0;
GuillaumeCH 3:1dba6eca01ad 97 consigne_tab[18][1]=0;
GuillaumeCH 3:1dba6eca01ad 98
GuillaumeCH 3:1dba6eca01ad 99 consigne_tab[19][0]=0;
GuillaumeCH 3:1dba6eca01ad 100 consigne_tab[19][1]=0;*/
GuillaumeCH 3:1dba6eca01ad 101 }
GuillaumeCH 3:1dba6eca01ad 102
GuillaumeCH 3:1dba6eca01ad 103 void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
GuillaumeCH 3:1dba6eca01ad 104
GuillaumeCH 3:1dba6eca01ad 105 int sens_G=signe(vitesse_G);
GuillaumeCH 3:1dba6eca01ad 106 int sens_D=signe(vitesse_D);
GuillaumeCH 3:1dba6eca01ad 107 double vitesse_local_G=abs(vitesse_G);
GuillaumeCH 3:1dba6eca01ad 108 double vitesse_local_D=abs(vitesse_D);
GuillaumeCH 3:1dba6eca01ad 109
GuillaumeCH 3:1dba6eca01ad 110 if(abs(vitesse_G) > 900){
GuillaumeCH 3:1dba6eca01ad 111 vitesse_local_G=900;
GuillaumeCH 3:1dba6eca01ad 112 }
GuillaumeCH 3:1dba6eca01ad 113 if(abs(vitesse_G)<10){
GuillaumeCH 3:1dba6eca01ad 114 vitesse_local_G=0;
GuillaumeCH 3:1dba6eca01ad 115 }
GuillaumeCH 3:1dba6eca01ad 116 if(abs(vitesse_D) > 900){
GuillaumeCH 3:1dba6eca01ad 117 vitesse_local_D=900;
GuillaumeCH 3:1dba6eca01ad 118 }
GuillaumeCH 3:1dba6eca01ad 119 if(abs(vitesse_D)< 10){
GuillaumeCH 3:1dba6eca01ad 120 vitesse_local_D=0;
GuillaumeCH 3:1dba6eca01ad 121
GuillaumeCH 3:1dba6eca01ad 122 }
GuillaumeCH 3:1dba6eca01ad 123 ;
GuillaumeCH 3:1dba6eca01ad 124 int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 3:1dba6eca01ad 125 int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 3:1dba6eca01ad 126 float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 3:1dba6eca01ad 127 float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 3:1dba6eca01ad 128 float centieme_D = (VD_f-VD_int)*1000;
GuillaumeCH 3:1dba6eca01ad 129 float centieme_G = (VG_f-VG_int)*1000;
GuillaumeCH 3:1dba6eca01ad 130 if ((rand()%1000) < centieme_G){
GuillaumeCH 3:1dba6eca01ad 131 VG_int+=1;
GuillaumeCH 3:1dba6eca01ad 132 }
GuillaumeCH 3:1dba6eca01ad 133 if ((rand()%1000) < centieme_D){
GuillaumeCH 3:1dba6eca01ad 134 VD_int+=1;
GuillaumeCH 3:1dba6eca01ad 135 }
GuillaumeCH 3:1dba6eca01ad 136 //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
GuillaumeCH 3:1dba6eca01ad 137 set_PWM_moteur_G(VD_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
GuillaumeCH 3:1dba6eca01ad 138 set_PWM_moteur_D(VG_int);//
GuillaumeCH 3:1dba6eca01ad 139 }
GuillaumeCH 3:1dba6eca01ad 140 void deplacement::vitesse_nulle_G(int zero){
GuillaumeCH 3:1dba6eca01ad 141 if(zero == 0){
GuillaumeCH 3:1dba6eca01ad 142 set_PWM_moteur_G(0);
GuillaumeCH 3:1dba6eca01ad 143 }
GuillaumeCH 3:1dba6eca01ad 144 }
GuillaumeCH 3:1dba6eca01ad 145 void deplacement::vitesse_nulle_D(int zero){
GuillaumeCH 3:1dba6eca01ad 146 if(zero == 0){
GuillaumeCH 3:1dba6eca01ad 147 set_PWM_moteur_D(0);
GuillaumeCH 3:1dba6eca01ad 148 }
GuillaumeCH 3:1dba6eca01ad 149 }
GuillaumeCH 3:1dba6eca01ad 150 void deplacement::reculer_un_peu(int distance){
GuillaumeCH 3:1dba6eca01ad 151 motors_on();
GuillaumeCH 3:1dba6eca01ad 152 long int x_ini = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 153 long int y_ini = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 154 double angle_vise_deg = get_angle();
GuillaumeCH 3:1dba6eca01ad 155 double angle_vise=angle_vise_deg*3.142/180;
GuillaumeCH 3:1dba6eca01ad 156 double angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 157
GuillaumeCH 3:1dba6eca01ad 158 long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 3:1dba6eca01ad 159 long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 3:1dba6eca01ad 160
GuillaumeCH 3:1dba6eca01ad 161 long int x_actuel = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 162 long int y_actuel = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 163 long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 3:1dba6eca01ad 164 long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 3:1dba6eca01ad 165
GuillaumeCH 3:1dba6eca01ad 166 float vitesse_G;
GuillaumeCH 3:1dba6eca01ad 167 float vitesse_D;
GuillaumeCH 3:1dba6eca01ad 168
GuillaumeCH 3:1dba6eca01ad 169 angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 170
GuillaumeCH 3:1dba6eca01ad 171 //printf("YOOOO\n\n ");
GuillaumeCH 3:1dba6eca01ad 172 while (distance+x_local>0){
GuillaumeCH 3:1dba6eca01ad 173
GuillaumeCH 3:1dba6eca01ad 174 vitesse_G = (distance+x_local)/70;
GuillaumeCH 3:1dba6eca01ad 175 vitesse_D = vitesse_G;
GuillaumeCH 3:1dba6eca01ad 176 if(vitesse_G >150){
GuillaumeCH 3:1dba6eca01ad 177 vitesse_G=150;
GuillaumeCH 3:1dba6eca01ad 178 vitesse_D=150;
GuillaumeCH 3:1dba6eca01ad 179 }
GuillaumeCH 3:1dba6eca01ad 180 if (vitesse_G<-150){
GuillaumeCH 3:1dba6eca01ad 181 vitesse_G=-150;
GuillaumeCH 3:1dba6eca01ad 182 vitesse_D=-150;
GuillaumeCH 3:1dba6eca01ad 183 }
GuillaumeCH 3:1dba6eca01ad 184
GuillaumeCH 3:1dba6eca01ad 185 angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 186 vitesse_G = vitesse_G - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local; // petit asser en angle
GuillaumeCH 3:1dba6eca01ad 187 vitesse_D = vitesse_D + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local;
GuillaumeCH 3:1dba6eca01ad 188
GuillaumeCH 3:1dba6eca01ad 189 commande_vitesse(-vitesse_G,-vitesse_D);
GuillaumeCH 3:1dba6eca01ad 190 actualise_position();
GuillaumeCH 3:1dba6eca01ad 191 x_actuel = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 192 y_actuel = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 193 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 3:1dba6eca01ad 194 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 3:1dba6eca01ad 195 //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));
GuillaumeCH 3:1dba6eca01ad 196
GuillaumeCH 3:1dba6eca01ad 197 }
GuillaumeCH 3:1dba6eca01ad 198 test_rotation_abs(angle_vise_deg);
GuillaumeCH 3:1dba6eca01ad 199 vitesse_nulle_G(0);
GuillaumeCH 3:1dba6eca01ad 200 vitesse_nulle_D(0);
GuillaumeCH 3:1dba6eca01ad 201 wait(0.3);
GuillaumeCH 3:1dba6eca01ad 202 motors_stop();
GuillaumeCH 3:1dba6eca01ad 203 }
GuillaumeCH 3:1dba6eca01ad 204
GuillaumeCH 3:1dba6eca01ad 205 void deplacement::ligne_droite(long int distance)
GuillaumeCH 3:1dba6eca01ad 206 {
GuillaumeCH 3:1dba6eca01ad 207 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 3:1dba6eca01ad 208 motors_on();
GuillaumeCH 3:1dba6eca01ad 209 long int x_ini = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 210 long int y_ini = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 211 double angle_vise_deg = get_angle();
GuillaumeCH 3:1dba6eca01ad 212 double angle_vise=angle_vise_deg*3.142/180;
GuillaumeCH 3:1dba6eca01ad 213 double angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 214
GuillaumeCH 3:1dba6eca01ad 215 long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 3:1dba6eca01ad 216 long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 3:1dba6eca01ad 217
GuillaumeCH 3:1dba6eca01ad 218 long int x_actuel = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 219 long int y_actuel = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 220 long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 3:1dba6eca01ad 221 long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 3:1dba6eca01ad 222
GuillaumeCH 3:1dba6eca01ad 223 float vitesse_G;
GuillaumeCH 3:1dba6eca01ad 224 float vitesse_D;
GuillaumeCH 3:1dba6eca01ad 225
GuillaumeCH 3:1dba6eca01ad 226 angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 227
GuillaumeCH 3:1dba6eca01ad 228 while (distance-x_local>0){
GuillaumeCH 3:1dba6eca01ad 229
GuillaumeCH 3:1dba6eca01ad 230 vitesse_G = (distance-x_local)/70;
GuillaumeCH 3:1dba6eca01ad 231 vitesse_D = vitesse_G;
GuillaumeCH 3:1dba6eca01ad 232 if(vitesse_G >400){
GuillaumeCH 3:1dba6eca01ad 233 vitesse_G=400;
GuillaumeCH 3:1dba6eca01ad 234 vitesse_D=400;
GuillaumeCH 3:1dba6eca01ad 235 }
GuillaumeCH 3:1dba6eca01ad 236 if (vitesse_G<-400){
GuillaumeCH 3:1dba6eca01ad 237 vitesse_G=-400;
GuillaumeCH 3:1dba6eca01ad 238 vitesse_D=-400;
GuillaumeCH 3:1dba6eca01ad 239 }
GuillaumeCH 3:1dba6eca01ad 240
GuillaumeCH 3:1dba6eca01ad 241 angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 242 vitesse_G = vitesse_G + 1.5*diff_angle(angle_vise_deg,angle) + 0.015*y_local; // petit asser en angle
GuillaumeCH 3:1dba6eca01ad 243 vitesse_D = vitesse_D - 1.5*diff_angle(angle_vise_deg,angle) - 0.015*y_local;
GuillaumeCH 3:1dba6eca01ad 244
GuillaumeCH 3:1dba6eca01ad 245 commande_vitesse(vitesse_G,vitesse_D);
GuillaumeCH 3:1dba6eca01ad 246 actualise_position();
GuillaumeCH 3:1dba6eca01ad 247 x_actuel = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 248 y_actuel = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 249 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 3:1dba6eca01ad 250 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 3:1dba6eca01ad 251 //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));
GuillaumeCH 3:1dba6eca01ad 252
GuillaumeCH 3:1dba6eca01ad 253 }
GuillaumeCH 3:1dba6eca01ad 254 test_rotation_abs(angle_vise_deg);
GuillaumeCH 3:1dba6eca01ad 255 vitesse_nulle_G(0);
GuillaumeCH 3:1dba6eca01ad 256 vitesse_nulle_D(0);
GuillaumeCH 3:1dba6eca01ad 257 wait(0.3);
GuillaumeCH 3:1dba6eca01ad 258 motors_stop();
GuillaumeCH 3:1dba6eca01ad 259 }
GuillaumeCH 3:1dba6eca01ad 260 void deplacement::ligne_droite_v2(long int distance)
GuillaumeCH 3:1dba6eca01ad 261 {
GuillaumeCH 3:1dba6eca01ad 262 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 3:1dba6eca01ad 263 motors_on();
GuillaumeCH 3:1dba6eca01ad 264 long int x_ini = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 265 long int y_ini = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 266 double angle_vise_deg = get_angle();
GuillaumeCH 3:1dba6eca01ad 267 double angle_vise=angle_vise_deg*3.142/180;
GuillaumeCH 3:1dba6eca01ad 268 double angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 269
GuillaumeCH 3:1dba6eca01ad 270 long int x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 3:1dba6eca01ad 271 long int y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 3:1dba6eca01ad 272
GuillaumeCH 3:1dba6eca01ad 273 long int x_actuel = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 274 long int y_actuel = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 275
GuillaumeCH 3:1dba6eca01ad 276 long int x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 3:1dba6eca01ad 277 long int y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 3:1dba6eca01ad 278
GuillaumeCH 3:1dba6eca01ad 279 //long int y_local_prec = y_local;
GuillaumeCH 3:1dba6eca01ad 280
GuillaumeCH 3:1dba6eca01ad 281 float vitesse_G;
GuillaumeCH 3:1dba6eca01ad 282 float vitesse_D;
GuillaumeCH 3:1dba6eca01ad 283
GuillaumeCH 3:1dba6eca01ad 284 angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 285
GuillaumeCH 3:1dba6eca01ad 286 float Ki2= 0.000015;
GuillaumeCH 3:1dba6eca01ad 287 float Kp2= 0.04;
GuillaumeCH 3:1dba6eca01ad 288 while (distance-x_local>0){
GuillaumeCH 3:1dba6eca01ad 289
GuillaumeCH 3:1dba6eca01ad 290 vitesse_G = (distance-x_local)/70;
GuillaumeCH 3:1dba6eca01ad 291 vitesse_D = vitesse_G;
GuillaumeCH 3:1dba6eca01ad 292 if(vitesse_G >400){
GuillaumeCH 3:1dba6eca01ad 293 vitesse_G=400;
GuillaumeCH 3:1dba6eca01ad 294 vitesse_D=400;
GuillaumeCH 3:1dba6eca01ad 295 }
GuillaumeCH 3:1dba6eca01ad 296 if (vitesse_G<-400){
GuillaumeCH 3:1dba6eca01ad 297 vitesse_G=-400;
GuillaumeCH 3:1dba6eca01ad 298 vitesse_D=-400;
GuillaumeCH 3:1dba6eca01ad 299 }
GuillaumeCH 3:1dba6eca01ad 300
GuillaumeCH 3:1dba6eca01ad 301 angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 302 //vitesse_G = vitesse_G + (y_local * 0.02) + (y_local - y_local_prec)*2;
GuillaumeCH 3:1dba6eca01ad 303 //vitesse_D = vitesse_D - (y_local * 0.02) - (y_local - y_local_prec)*2;
GuillaumeCH 3:1dba6eca01ad 304 vitesse_G = vitesse_G * (1 + Ki2*y_local + Kp2 * diff_angle(angle_vise_deg, angle));
GuillaumeCH 3:1dba6eca01ad 305 vitesse_D = vitesse_D * (1 - Ki2*y_local - Kp2 * diff_angle(angle_vise_deg, angle));
GuillaumeCH 3:1dba6eca01ad 306 commande_vitesse(vitesse_G,vitesse_D);
GuillaumeCH 3:1dba6eca01ad 307 actualise_position();
GuillaumeCH 3:1dba6eca01ad 308 x_actuel = get_x_actuel();
GuillaumeCH 3:1dba6eca01ad 309 y_actuel = get_y_actuel();
GuillaumeCH 3:1dba6eca01ad 310 //y_local_prec = y_local;
GuillaumeCH 3:1dba6eca01ad 311 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 3:1dba6eca01ad 312 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 3:1dba6eca01ad 313
GuillaumeCH 3:1dba6eca01ad 314 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));
GuillaumeCH 3:1dba6eca01ad 315 }
GuillaumeCH 3:1dba6eca01ad 316 test_rotation_abs(angle_vise_deg);
GuillaumeCH 3:1dba6eca01ad 317 vitesse_nulle_G(0);
GuillaumeCH 3:1dba6eca01ad 318 vitesse_nulle_D(0);
GuillaumeCH 3:1dba6eca01ad 319 wait(0.3);
GuillaumeCH 3:1dba6eca01ad 320 motors_stop();
GuillaumeCH 3:1dba6eca01ad 321 }
GuillaumeCH 3:1dba6eca01ad 322
GuillaumeCH 3:1dba6eca01ad 323 void deplacement::test_rotation_rel(double angle_vise)
GuillaumeCH 3:1dba6eca01ad 324 {
GuillaumeCH 3:1dba6eca01ad 325 // rotation de angle_vise
GuillaumeCH 3:1dba6eca01ad 326 motors_on();
GuillaumeCH 3:1dba6eca01ad 327 float vitesse;
GuillaumeCH 3:1dba6eca01ad 328 int sens;
GuillaumeCH 3:1dba6eca01ad 329 double angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 330 angle_vise+=angle;
GuillaumeCH 3:1dba6eca01ad 331 borne_angle_d(angle_vise);
GuillaumeCH 3:1dba6eca01ad 332 if (diff_angle(angle,angle_vise)<=0){
GuillaumeCH 3:1dba6eca01ad 333 sens = -1;
GuillaumeCH 3:1dba6eca01ad 334 }
GuillaumeCH 3:1dba6eca01ad 335 else{
GuillaumeCH 3:1dba6eca01ad 336 sens = 1;
GuillaumeCH 3:1dba6eca01ad 337 }
GuillaumeCH 3:1dba6eca01ad 338 while (sens*diff_angle(angle,angle_vise)>0)
GuillaumeCH 3:1dba6eca01ad 339 {
GuillaumeCH 3:1dba6eca01ad 340 vitesse=diff_angle(angle,angle_vise);
GuillaumeCH 3:1dba6eca01ad 341
GuillaumeCH 3:1dba6eca01ad 342 commande_vitesse(-vitesse,vitesse);
GuillaumeCH 3:1dba6eca01ad 343 actualise_position();
GuillaumeCH 3:1dba6eca01ad 344 angle = get_angle();
GuillaumeCH 3:1dba6eca01ad 345 //printf("vitesse : %f", vitesse);
GuillaumeCH 3:1dba6eca01ad 346 }
GuillaumeCH 3:1dba6eca01ad 347
GuillaumeCH 3:1dba6eca01ad 348 //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));
GuillaumeCH 3:1dba6eca01ad 349 //consigne_D = 0;
GuillaumeCH 3:1dba6eca01ad 350 //consigne_G = 0;
GuillaumeCH 3:1dba6eca01ad 351 vitesse_nulle_G(0);
GuillaumeCH 3:1dba6eca01ad 352 vitesse_nulle_D(0);
GuillaumeCH 3:1dba6eca01ad 353 wait(0.2);
GuillaumeCH 3:1dba6eca01ad 354 motors_stop();
GuillaumeCH 3:1dba6eca01ad 355 }
GuillaumeCH 3:1dba6eca01ad 356
GuillaumeCH 3:1dba6eca01ad 357
GuillaumeCH 3:1dba6eca01ad 358 void deplacement::test_rotation_abs(double angle_vise)
GuillaumeCH 3:1dba6eca01ad 359 {
GuillaumeCH 3:1dba6eca01ad 360 actualise_position();
GuillaumeCH 3:1dba6eca01ad 361 printf("bite");
GuillaumeCH 3:1dba6eca01ad 362 double angle_rel = borne_angle_d(angle_vise-get_angle());
GuillaumeCH 3:1dba6eca01ad 363 test_rotation_rel(angle_rel);
GuillaumeCH 3:1dba6eca01ad 364 }
GuillaumeCH 3:1dba6eca01ad 365
GuillaumeCH 3:1dba6eca01ad 366 void deplacement::asservissement(){
GuillaumeCH 3:1dba6eca01ad 367 long int tick_D = get_nbr_tick_D();
GuillaumeCH 3:1dba6eca01ad 368 long int tick_G = get_nbr_tick_G();
GuillaumeCH 3:1dba6eca01ad 369
GuillaumeCH 3:1dba6eca01ad 370 long int tick_D_passe = tick_D-tick_prec_D;
GuillaumeCH 3:1dba6eca01ad 371 long int tick_G_passe = tick_G-tick_prec_G;
GuillaumeCH 3:1dba6eca01ad 372
GuillaumeCH 3:1dba6eca01ad 373 tick_prec_D=tick_D;
GuillaumeCH 3:1dba6eca01ad 374 tick_prec_G=tick_G;
GuillaumeCH 3:1dba6eca01ad 375
GuillaumeCH 3:1dba6eca01ad 376 float vitesse_codeuse_D = tick_D_passe;
GuillaumeCH 3:1dba6eca01ad 377 float vitesse_codeuse_G = tick_G_passe;
GuillaumeCH 3:1dba6eca01ad 378
GuillaumeCH 3:1dba6eca01ad 379 float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
GuillaumeCH 3:1dba6eca01ad 380 float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
GuillaumeCH 3:1dba6eca01ad 381
GuillaumeCH 3:1dba6eca01ad 382 if (compteur_glisse == 5)
GuillaumeCH 3:1dba6eca01ad 383 compteur_glisse = 0;
GuillaumeCH 3:1dba6eca01ad 384
GuillaumeCH 3:1dba6eca01ad 385 if (compteur_glisse == -1)
GuillaumeCH 3:1dba6eca01ad 386 {
GuillaumeCH 3:1dba6eca01ad 387 compteur_glisse = 0;
GuillaumeCH 3:1dba6eca01ad 388 for (int i = 0; i<5; i++){
GuillaumeCH 3:1dba6eca01ad 389 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 3:1dba6eca01ad 390 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 3:1dba6eca01ad 391 }
GuillaumeCH 3:1dba6eca01ad 392 }
GuillaumeCH 3:1dba6eca01ad 393
GuillaumeCH 3:1dba6eca01ad 394 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 3:1dba6eca01ad 395 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 3:1dba6eca01ad 396 compteur_glisse++;
GuillaumeCH 3:1dba6eca01ad 397
GuillaumeCH 3:1dba6eca01ad 398 erreur_D = erreur_glissee_D[0];
GuillaumeCH 3:1dba6eca01ad 399 erreur_G = erreur_glissee_G[0];
GuillaumeCH 3:1dba6eca01ad 400 for (int i=1; i<5; i++)
GuillaumeCH 3:1dba6eca01ad 401 {
GuillaumeCH 3:1dba6eca01ad 402 erreur_D += erreur_glissee_D[i];
GuillaumeCH 3:1dba6eca01ad 403 erreur_G += erreur_glissee_G[i];
GuillaumeCH 3:1dba6eca01ad 404 }
GuillaumeCH 3:1dba6eca01ad 405
GuillaumeCH 3:1dba6eca01ad 406 erreur_D = erreur_D/5.0;
GuillaumeCH 3:1dba6eca01ad 407 erreur_G = erreur_G/5.0; // erreur est maintenant la moyenne des 5 erreurs prec
GuillaumeCH 3:1dba6eca01ad 408
GuillaumeCH 3:1dba6eca01ad 409 somme_erreur_D += erreur_D;
GuillaumeCH 3:1dba6eca01ad 410 somme_erreur_G += erreur_G;
GuillaumeCH 3:1dba6eca01ad 411
GuillaumeCH 3:1dba6eca01ad 412 float delta_erreur_D = erreur_D-erreur_precedente_D;
GuillaumeCH 3:1dba6eca01ad 413 float delta_erreur_G = erreur_G-erreur_precedente_G;
GuillaumeCH 3:1dba6eca01ad 414
GuillaumeCH 3:1dba6eca01ad 415 erreur_precedente_G = erreur_G;
GuillaumeCH 3:1dba6eca01ad 416 erreur_precedente_D = erreur_D;
GuillaumeCH 3:1dba6eca01ad 417
GuillaumeCH 3:1dba6eca01ad 418 float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D;
GuillaumeCH 3:1dba6eca01ad 419 float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G;
GuillaumeCH 3:1dba6eca01ad 420
GuillaumeCH 3:1dba6eca01ad 421 if (cmd_G <0){
GuillaumeCH 3:1dba6eca01ad 422 cmd_G = 0;
GuillaumeCH 3:1dba6eca01ad 423 }
GuillaumeCH 3:1dba6eca01ad 424 if (cmd_G > 500){
GuillaumeCH 3:1dba6eca01ad 425 cmd_G = 500;
GuillaumeCH 3:1dba6eca01ad 426 }
GuillaumeCH 3:1dba6eca01ad 427 if (cmd_D <0){
GuillaumeCH 3:1dba6eca01ad 428 cmd_D = 0;
GuillaumeCH 3:1dba6eca01ad 429 }
GuillaumeCH 3:1dba6eca01ad 430 if (cmd_D > 500){
GuillaumeCH 3:1dba6eca01ad 431 cmd_D = 500;
GuillaumeCH 3:1dba6eca01ad 432 }
GuillaumeCH 3:1dba6eca01ad 433 c_D[dix_ms]=consigne_D;
GuillaumeCH 3:1dba6eca01ad 434 c_G[dix_ms]=consigne_G;
GuillaumeCH 3:1dba6eca01ad 435 //printf("%d\n",c[i]);
GuillaumeCH 3:1dba6eca01ad 436 tab_cmd_D[dix_ms] = cmd_D;
GuillaumeCH 3:1dba6eca01ad 437 tab_cmd_G[dix_ms] = cmd_G;
GuillaumeCH 3:1dba6eca01ad 438 vtab_D[dix_ms] = vitesse_codeuse_D;
GuillaumeCH 3:1dba6eca01ad 439 vtab_G[dix_ms] = vitesse_codeuse_G;
GuillaumeCH 3:1dba6eca01ad 440 commande_vitesse(cmd_G,cmd_D);
GuillaumeCH 3:1dba6eca01ad 441 dix_ms++;
GuillaumeCH 3:1dba6eca01ad 442 //printf("%d\n",i);
GuillaumeCH 3:1dba6eca01ad 443 //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
GuillaumeCH 3:1dba6eca01ad 444 //printf("%f,%f\n",cmd_G,cmd_D);
GuillaumeCH 3:1dba6eca01ad 445 //printf("oui");
GuillaumeCH 3:1dba6eca01ad 446 }
GuillaumeCH 3:1dba6eca01ad 447
GuillaumeCH 3:1dba6eca01ad 448 void deplacement::printftab(){
GuillaumeCH 3:1dba6eca01ad 449
GuillaumeCH 3:1dba6eca01ad 450 for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 3:1dba6eca01ad 451 if(j==500)
GuillaumeCH 3:1dba6eca01ad 452 bouton();
GuillaumeCH 3:1dba6eca01ad 453 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]);
GuillaumeCH 3:1dba6eca01ad 454 }
GuillaumeCH 3:1dba6eca01ad 455 /*if (j<5)
GuillaumeCH 3:1dba6eca01ad 456 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]);
GuillaumeCH 3:1dba6eca01ad 457 else
GuillaumeCH 3:1dba6eca01ad 458 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]));
GuillaumeCH 3:1dba6eca01ad 459 }*/
GuillaumeCH 3:1dba6eca01ad 460
GuillaumeCH 3:1dba6eca01ad 461 /*for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 3:1dba6eca01ad 462 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);
GuillaumeCH 3:1dba6eca01ad 463 }*/
GuillaumeCH 3:1dba6eca01ad 464 }
GuillaumeCH 3:1dba6eca01ad 465
GuillaumeCH 3:1dba6eca01ad 466 void deplacement::test(){
GuillaumeCH 3:1dba6eca01ad 467 Timer t;
GuillaumeCH 3:1dba6eca01ad 468 t.start();
GuillaumeCH 3:1dba6eca01ad 469 for (int i =0;i<5 ;i++){
GuillaumeCH 3:1dba6eca01ad 470 changement_consigne(consigne_tab[i][0], consigne_tab[i][1]);
GuillaumeCH 3:1dba6eca01ad 471 while(t.read()<0.5){
GuillaumeCH 3:1dba6eca01ad 472 //actualise_positio n();
GuillaumeCH 3:1dba6eca01ad 473 }
GuillaumeCH 3:1dba6eca01ad 474 //printf("t.read() : %f\n",t.read());
GuillaumeCH 3:1dba6eca01ad 475 //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G);
GuillaumeCH 3:1dba6eca01ad 476 t.reset();
GuillaumeCH 3:1dba6eca01ad 477 }
GuillaumeCH 3:1dba6eca01ad 478 }
GuillaumeCH 3:1dba6eca01ad 479
GuillaumeCH 3:1dba6eca01ad 480 void deplacement::changement_consigne(int cons_D, int cons_G){
GuillaumeCH 3:1dba6eca01ad 481 consigne_D = cons_D;
GuillaumeCH 3:1dba6eca01ad 482 consigne_G = cons_G;
GuillaumeCH 3:1dba6eca01ad 483 compteur_glisse = -1;
GuillaumeCH 3:1dba6eca01ad 484 }
GuillaumeCH 3:1dba6eca01ad 485
GuillaumeCH 3:1dba6eca01ad 486 void deplacement::bouton(){
GuillaumeCH 3:1dba6eca01ad 487 DigitalIn depart(USER_BUTTON);
GuillaumeCH 3:1dba6eca01ad 488 while (depart){}
GuillaumeCH 3:1dba6eca01ad 489 }