Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

deplacement.cpp@4:eac6746544fb, 2019-05-16 (annotated)

Committer:
GuillaumeCH
Date:
Thu May 16 09:10:16 2019 +0000
Revision:
4:eac6746544fb
Parent:
3:d38aa400d5e7
Child:
5:3638d7e7c5c1
oui

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GuillaumeCH 2:3066e614372f 1 #include "deplacement.h"
GuillaumeCH 2:3066e614372f 2 #include "mbed.h"
GuillaumeCH 2:3066e614372f 3 #include "odometrie.h"
GuillaumeCH 2:3066e614372f 4 #include "hardware.h"
GuillaumeCH 2:3066e614372f 5 #include "math_precalc.h"
GuillaumeCH 2:3066e614372f 6 #include "reglages.h"
GuillaumeCH 2:3066e614372f 7
GuillaumeCH 2:3066e614372f 8
GuillaumeCH 2:3066e614372f 9 deplacement::deplacement(){
GuillaumeCH 2:3066e614372f 10 consigne_D = 0;
GuillaumeCH 2:3066e614372f 11 consigne_G = 0;
GuillaumeCH 2:3066e614372f 12 somme_erreur_D = 0;
GuillaumeCH 2:3066e614372f 13 somme_erreur_G = 0;
GuillaumeCH 2:3066e614372f 14 erreur_precedente_D = 0;
GuillaumeCH 2:3066e614372f 15 erreur_precedente_G = 0;
GuillaumeCH 2:3066e614372f 16 compteur_asser =0;
GuillaumeCH 2:3066e614372f 17 somme_y=0;
GuillaumeCH 2:3066e614372f 18
GuillaumeCH 2:3066e614372f 19 for (int k =0; k<5;k++){
GuillaumeCH 2:3066e614372f 20 erreur_glissee_D[k] = 0;
GuillaumeCH 2:3066e614372f 21 erreur_glissee_G[k] = 0;
GuillaumeCH 2:3066e614372f 22 }
GuillaumeCH 2:3066e614372f 23 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 24
GuillaumeCH 2:3066e614372f 25 Kp_D = 1.5;//1
GuillaumeCH 2:3066e614372f 26 Ki_D = 0.12;//0.15
GuillaumeCH 2:3066e614372f 27 Kd_D = 0.5;//1
GuillaumeCH 2:3066e614372f 28
GuillaumeCH 2:3066e614372f 29 Kp_G = 1;//1
GuillaumeCH 2:3066e614372f 30 Ki_G = 0.13;//0.15
GuillaumeCH 2:3066e614372f 31 Kd_G = 1.2;//1
GuillaumeCH 2:3066e614372f 32
GuillaumeCH 2:3066e614372f 33 tick_prec_D=0;
GuillaumeCH 2:3066e614372f 34 tick_prec_G = 0;
GuillaumeCH 2:3066e614372f 35 dix_ms = 0;
GuillaumeCH 2:3066e614372f 36 for (int k =0; k<TAILLE_TAB;k++){
GuillaumeCH 2:3066e614372f 37 tab_cmd_G[k]=0;
GuillaumeCH 2:3066e614372f 38 tab_cmd_D[k]=0;
GuillaumeCH 2:3066e614372f 39 vtab_G[k]=0;
GuillaumeCH 2:3066e614372f 40 vtab_D[k]=0;
GuillaumeCH 2:3066e614372f 41 c_D[k]=0;
GuillaumeCH 2:3066e614372f 42 c_G[k]=0;
GuillaumeCH 2:3066e614372f 43 }
GuillaumeCH 2:3066e614372f 44 consigne_tab[0][0]=0;
GuillaumeCH 2:3066e614372f 45 consigne_tab[0][1]=0;
GuillaumeCH 2:3066e614372f 46
GuillaumeCH 2:3066e614372f 47 consigne_tab[1][0]=10;
GuillaumeCH 2:3066e614372f 48 consigne_tab[1][1]=10;
GuillaumeCH 2:3066e614372f 49
GuillaumeCH 2:3066e614372f 50 consigne_tab[2][0]=20;
GuillaumeCH 2:3066e614372f 51 consigne_tab[2][1]=20;
GuillaumeCH 2:3066e614372f 52
GuillaumeCH 2:3066e614372f 53 consigne_tab[3][0]=30;
GuillaumeCH 2:3066e614372f 54 consigne_tab[3][1]=30;
GuillaumeCH 2:3066e614372f 55
GuillaumeCH 2:3066e614372f 56 consigne_tab[4][0]=40;
GuillaumeCH 2:3066e614372f 57 consigne_tab[4][1]=40;
GuillaumeCH 2:3066e614372f 58
GuillaumeCH 2:3066e614372f 59 /* consigne_tab[5][0]=3*5;
GuillaumeCH 2:3066e614372f 60 consigne_tab[5][1]=3*5;
GuillaumeCH 2:3066e614372f 61
GuillaumeCH 2:3066e614372f 62 consigne_tab[6][0]=3*6;
GuillaumeCH 2:3066e614372f 63 consigne_tab[6][1]=3*6;
GuillaumeCH 2:3066e614372f 64
GuillaumeCH 2:3066e614372f 65 consigne_tab[7][0]=3*7;
GuillaumeCH 2:3066e614372f 66 consigne_tab[7][1]=3*7;
GuillaumeCH 2:3066e614372f 67
GuillaumeCH 2:3066e614372f 68 consigne_tab[8][0]=3*8;
GuillaumeCH 2:3066e614372f 69 consigne_tab[8][1]=3*8;
GuillaumeCH 2:3066e614372f 70
GuillaumeCH 2:3066e614372f 71 consigne_tab[9][0]=3*9;
GuillaumeCH 2:3066e614372f 72 consigne_tab[9][1]=3*9;
GuillaumeCH 2:3066e614372f 73
GuillaumeCH 2:3066e614372f 74 consigne_tab[10][0]=3*10;
GuillaumeCH 2:3066e614372f 75 consigne_tab[10][1]=3*10;
GuillaumeCH 2:3066e614372f 76
GuillaumeCH 2:3066e614372f 77 consigne_tab[11][0]=3*11;
GuillaumeCH 2:3066e614372f 78 consigne_tab[11][1]=3*11;
GuillaumeCH 2:3066e614372f 79
GuillaumeCH 2:3066e614372f 80 consigne_tab[12][0]=3*12;
GuillaumeCH 2:3066e614372f 81 consigne_tab[12][1]=3*12;
GuillaumeCH 2:3066e614372f 82
GuillaumeCH 2:3066e614372f 83 consigne_tab[13][0]=3*13;
GuillaumeCH 2:3066e614372f 84 consigne_tab[13][1]=3*13;
GuillaumeCH 2:3066e614372f 85
GuillaumeCH 2:3066e614372f 86 consigne_tab[14][0]=3*14;
GuillaumeCH 2:3066e614372f 87 consigne_tab[14][1]=3*14;
GuillaumeCH 2:3066e614372f 88
GuillaumeCH 2:3066e614372f 89 consigne_tab[15][0]=0;
GuillaumeCH 2:3066e614372f 90 consigne_tab[15][1]=0;
GuillaumeCH 2:3066e614372f 91
GuillaumeCH 2:3066e614372f 92 consigne_tab[16][0]=0;
GuillaumeCH 2:3066e614372f 93 consigne_tab[16][1]=0;
GuillaumeCH 2:3066e614372f 94
GuillaumeCH 2:3066e614372f 95 consigne_tab[17][0]=0;
GuillaumeCH 2:3066e614372f 96 consigne_tab[17][1]=0;
GuillaumeCH 2:3066e614372f 97
GuillaumeCH 2:3066e614372f 98 consigne_tab[18][0]=0;
GuillaumeCH 2:3066e614372f 99 consigne_tab[18][1]=0;
GuillaumeCH 2:3066e614372f 100
GuillaumeCH 2:3066e614372f 101 consigne_tab[19][0]=0;
GuillaumeCH 2:3066e614372f 102 consigne_tab[19][1]=0;*/
GuillaumeCH 2:3066e614372f 103 }
GuillaumeCH 2:3066e614372f 104
GuillaumeCH 2:3066e614372f 105 void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
GuillaumeCH 2:3066e614372f 106
GuillaumeCH 2:3066e614372f 107 int sens_G=signe(vitesse_G);
GuillaumeCH 2:3066e614372f 108 int sens_D=signe(vitesse_D);
GuillaumeCH 2:3066e614372f 109 double vitesse_local_G=abs(vitesse_G);
GuillaumeCH 2:3066e614372f 110 double vitesse_local_D=abs(vitesse_D);
GuillaumeCH 2:3066e614372f 111
GuillaumeCH 2:3066e614372f 112 if(abs(vitesse_G) > 900){
GuillaumeCH 2:3066e614372f 113 vitesse_local_G=900;
GuillaumeCH 2:3066e614372f 114 }
GuillaumeCH 3:d38aa400d5e7 115 if(abs(vitesse_G)<10){
GuillaumeCH 3:d38aa400d5e7 116 vitesse_local_G=10;
GuillaumeCH 2:3066e614372f 117 }
GuillaumeCH 2:3066e614372f 118 if(abs(vitesse_D) > 900){
GuillaumeCH 2:3066e614372f 119 vitesse_local_D=900;
GuillaumeCH 2:3066e614372f 120 }
GuillaumeCH 3:d38aa400d5e7 121 if(abs(vitesse_D)< 10){
GuillaumeCH 3:d38aa400d5e7 122 vitesse_local_D=10;
GuillaumeCH 2:3066e614372f 123
GuillaumeCH 2:3066e614372f 124 }
GuillaumeCH 2:3066e614372f 125 ;
GuillaumeCH 2:3066e614372f 126 int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 127 int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 128 float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 129 float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 130 float centieme_D = (VD_f-VD_int)*1000;
GuillaumeCH 2:3066e614372f 131 float centieme_G = (VG_f-VG_int)*1000;
GuillaumeCH 2:3066e614372f 132 if ((rand()%1000) < centieme_G){
GuillaumeCH 2:3066e614372f 133 VG_int+=1;
GuillaumeCH 2:3066e614372f 134 }
GuillaumeCH 2:3066e614372f 135 if ((rand()%1000) < centieme_D){
GuillaumeCH 2:3066e614372f 136 VD_int+=1;
GuillaumeCH 2:3066e614372f 137 }
GuillaumeCH 2:3066e614372f 138 //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
GuillaumeCH 2:3066e614372f 139 set_PWM_moteur_G(VG_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
GuillaumeCH 2:3066e614372f 140 set_PWM_moteur_D(VD_int);//
GuillaumeCH 2:3066e614372f 141 }
GuillaumeCH 2:3066e614372f 142 void deplacement::vitesse_nulle_G(int zero){
GuillaumeCH 2:3066e614372f 143 if(zero == 0){
GuillaumeCH 2:3066e614372f 144 set_PWM_moteur_G(0);
GuillaumeCH 2:3066e614372f 145 }
GuillaumeCH 2:3066e614372f 146 }
GuillaumeCH 2:3066e614372f 147 void deplacement::vitesse_nulle_D(int zero){
GuillaumeCH 2:3066e614372f 148 if(zero == 0){
GuillaumeCH 2:3066e614372f 149 set_PWM_moteur_D(0);
GuillaumeCH 2:3066e614372f 150 }
GuillaumeCH 2:3066e614372f 151 }
GuillaumeCH 3:d38aa400d5e7 152 void deplacement::marche_arriere(int distance)
GuillaumeCH 2:3066e614372f 153 {
GuillaumeCH 2:3066e614372f 154 somme_y=0;
GuillaumeCH 2:3066e614372f 155 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 156 motors_on();
GuillaumeCH 2:3066e614372f 157 actualise_position();
GuillaumeCH 2:3066e614372f 158 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 159 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 160 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 161 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 162 double angle = get_angle();
GuillaumeCH 2:3066e614372f 163
GuillaumeCH 2:3066e614372f 164 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 165 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 166
GuillaumeCH 2:3066e614372f 167 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 168 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 169
GuillaumeCH 2:3066e614372f 170
GuillaumeCH 2:3066e614372f 171 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 172 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 173
GuillaumeCH 2:3066e614372f 174 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 175 float vitesse_G;
GuillaumeCH 2:3066e614372f 176 float vitesse_D;
GuillaumeCH 2:3066e614372f 177
GuillaumeCH 2:3066e614372f 178 angle = get_angle();
GuillaumeCH 2:3066e614372f 179 float Kip=0;
GuillaumeCH 2:3066e614372f 180 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 181 float Kdp= 10;
GuillaumeCH 2:3066e614372f 182 while (distance-x_local<0){
GuillaumeCH 2:3066e614372f 183
GuillaumeCH 2:3066e614372f 184 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 185 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 186 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 187 vitesse_G=400;
GuillaumeCH 2:3066e614372f 188 vitesse_D=400;
GuillaumeCH 2:3066e614372f 189 }
GuillaumeCH 2:3066e614372f 190 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 191 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 192 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 193 }
GuillaumeCH 2:3066e614372f 194
GuillaumeCH 2:3066e614372f 195 angle = get_angle();
GuillaumeCH 2:3066e614372f 196
GuillaumeCH 2:3066e614372f 197 vitesse_G = vitesse_G - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
GuillaumeCH 2:3066e614372f 198 vitesse_D = vitesse_D + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
GuillaumeCH 2:3066e614372f 199 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 200 //consigne_G = vitesse_G;
GuillaumeCH 2:3066e614372f 201 commande_vitesse(vitesse_G,vitesse_D);
GuillaumeCH 2:3066e614372f 202 actualise_position();
GuillaumeCH 2:3066e614372f 203 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 204 y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 205 somme_y+=y_actuel;
GuillaumeCH 2:3066e614372f 206 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 207 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 208 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 209 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 210 compteur_asser=0;
GuillaumeCH 2:3066e614372f 211 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 212 }
GuillaumeCH 2:3066e614372f 213 compteur_asser++;
GuillaumeCH 2:3066e614372f 214 //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));
GuillaumeCH 2:3066e614372f 215 }
GuillaumeCH 2:3066e614372f 216 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 217 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 218 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 219 }
GuillaumeCH 2:3066e614372f 220
GuillaumeCH 2:3066e614372f 221
GuillaumeCH 2:3066e614372f 222
GuillaumeCH 3:d38aa400d5e7 223 void deplacement::ligne_droite(long int distance)
GuillaumeCH 2:3066e614372f 224 {
GuillaumeCH 2:3066e614372f 225 somme_y=0;
GuillaumeCH 2:3066e614372f 226 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 227 motors_on();
GuillaumeCH 2:3066e614372f 228 actualise_position();
GuillaumeCH 2:3066e614372f 229 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 230 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 231 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 232 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 233 double angle = get_angle();
GuillaumeCH 2:3066e614372f 234
GuillaumeCH 2:3066e614372f 235 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 236 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 237
GuillaumeCH 2:3066e614372f 238 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 239 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 240
GuillaumeCH 2:3066e614372f 241
GuillaumeCH 2:3066e614372f 242 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 243 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 244
GuillaumeCH 2:3066e614372f 245 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 246 float vitesse_G;
GuillaumeCH 2:3066e614372f 247 float vitesse_D;
GuillaumeCH 2:3066e614372f 248
GuillaumeCH 2:3066e614372f 249 angle = get_angle();
GuillaumeCH 2:3066e614372f 250 float Kip=0;
GuillaumeCH 2:3066e614372f 251 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 252 float Kdp= 10;
GuillaumeCH 2:3066e614372f 253 while (distance-x_local>0){
GuillaumeCH 2:3066e614372f 254
GuillaumeCH 2:3066e614372f 255 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 256 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 257 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 258 vitesse_G=400;
GuillaumeCH 2:3066e614372f 259 vitesse_D=400;
GuillaumeCH 2:3066e614372f 260 }
GuillaumeCH 2:3066e614372f 261 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 262 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 263 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 264 }
GuillaumeCH 2:3066e614372f 265
GuillaumeCH 2:3066e614372f 266 angle = get_angle();
GuillaumeCH 2:3066e614372f 267
GuillaumeCH 2:3066e614372f 268 vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
GuillaumeCH 2:3066e614372f 269 vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
GuillaumeCH 2:3066e614372f 270 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 271 //consigne_G = vitesse_G;
GuillaumeCH 2:3066e614372f 272 commande_vitesse(vitesse_G,vitesse_D);
GuillaumeCH 2:3066e614372f 273 actualise_position();
GuillaumeCH 2:3066e614372f 274 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 275 y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 276 somme_y+=y_actuel;
GuillaumeCH 2:3066e614372f 277 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 278 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 279 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 280 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 281 compteur_asser=0;
GuillaumeCH 2:3066e614372f 282 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 283 }
GuillaumeCH 2:3066e614372f 284 compteur_asser++;
GuillaumeCH 2:3066e614372f 285 //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));
GuillaumeCH 2:3066e614372f 286 }
GuillaumeCH 2:3066e614372f 287 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 288 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 289 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 290 }
GuillaumeCH 2:3066e614372f 291
GuillaumeCH 3:d38aa400d5e7 292 void deplacement::rotation_rel(double angle_vise)
GuillaumeCH 2:3066e614372f 293 {
GuillaumeCH 2:3066e614372f 294 // rotation de angle_vise
GuillaumeCH 2:3066e614372f 295 motors_on();
GuillaumeCH 2:3066e614372f 296 double vitesse=180;
GuillaumeCH 2:3066e614372f 297 int sens;
GuillaumeCH 2:3066e614372f 298 double angle = get_angle();
GuillaumeCH 2:3066e614372f 299 angle_vise+=angle;
GuillaumeCH 2:3066e614372f 300 borne_angle_d(angle_vise);
GuillaumeCH 2:3066e614372f 301 if (diff_angle(angle,angle_vise)<=0){
GuillaumeCH 2:3066e614372f 302 sens = -1;
GuillaumeCH 2:3066e614372f 303 //printf("negatif\n");
GuillaumeCH 2:3066e614372f 304 }
GuillaumeCH 2:3066e614372f 305 else{
GuillaumeCH 2:3066e614372f 306 sens = 1;
GuillaumeCH 2:3066e614372f 307
GuillaumeCH 2:3066e614372f 308 //printf("positif\n");
GuillaumeCH 2:3066e614372f 309 }
GuillaumeCH 2:3066e614372f 310 //printf("diff : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 311 while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
GuillaumeCH 2:3066e614372f 312 {
GuillaumeCH 2:3066e614372f 313 actualise_position();
GuillaumeCH 2:3066e614372f 314 angle = get_angle();
GuillaumeCH 2:3066e614372f 315 vitesse=1.5*sens*abs(diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 316
GuillaumeCH 2:3066e614372f 317 commande_vitesse(-vitesse,vitesse);
GuillaumeCH 2:3066e614372f 318 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 319 compteur_asser=0;
GuillaumeCH 2:3066e614372f 320 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 321 }
GuillaumeCH 2:3066e614372f 322 compteur_asser++;
GuillaumeCH 2:3066e614372f 323 //printf("vitesse : %lf ", vitesse);
GuillaumeCH 2:3066e614372f 324 }
GuillaumeCH 2:3066e614372f 325 //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 326 //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 2:3066e614372f 327 //consigne_D = 0;
GuillaumeCH 2:3066e614372f 328 //consigne_G = 0;
GuillaumeCH 2:3066e614372f 329 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 330 vitesse_nulle_D(0);
GuillaumeCH 3:d38aa400d5e7 331 wait(0.1);
GuillaumeCH 2:3066e614372f 332 motors_stop();
GuillaumeCH 2:3066e614372f 333 }
GuillaumeCH 2:3066e614372f 334
GuillaumeCH 2:3066e614372f 335
GuillaumeCH 3:d38aa400d5e7 336 void deplacement::rotation_abs(double angle_vise)
GuillaumeCH 2:3066e614372f 337 {
GuillaumeCH 2:3066e614372f 338 actualise_position();
GuillaumeCH 2:3066e614372f 339 //printf("bite");
GuillaumeCH 2:3066e614372f 340 double angle_rel = borne_angle_d(angle_vise-get_angle());
GuillaumeCH 3:d38aa400d5e7 341 rotation_rel(angle_rel);
GuillaumeCH 2:3066e614372f 342 }
GuillaumeCH 2:3066e614372f 343
GuillaumeCH 2:3066e614372f 344 void deplacement::asservissement(){
GuillaumeCH 2:3066e614372f 345 long int tick_D = get_nbr_tick_D();
GuillaumeCH 2:3066e614372f 346 long int tick_G = get_nbr_tick_G();
GuillaumeCH 2:3066e614372f 347
GuillaumeCH 2:3066e614372f 348 long int tick_D_passe = tick_D-tick_prec_D;
GuillaumeCH 2:3066e614372f 349 long int tick_G_passe = tick_G-tick_prec_G;
GuillaumeCH 2:3066e614372f 350
GuillaumeCH 2:3066e614372f 351 tick_prec_D=tick_D;
GuillaumeCH 2:3066e614372f 352 tick_prec_G=tick_G;
GuillaumeCH 2:3066e614372f 353
GuillaumeCH 2:3066e614372f 354 float vitesse_codeuse_D = tick_D_passe;
GuillaumeCH 2:3066e614372f 355 float vitesse_codeuse_G = tick_G_passe;
GuillaumeCH 2:3066e614372f 356
GuillaumeCH 2:3066e614372f 357 float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
GuillaumeCH 2:3066e614372f 358 float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
GuillaumeCH 2:3066e614372f 359
GuillaumeCH 2:3066e614372f 360 if (compteur_glisse == 5)
GuillaumeCH 2:3066e614372f 361 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 362
GuillaumeCH 2:3066e614372f 363 if (compteur_glisse == -1)
GuillaumeCH 2:3066e614372f 364 {
GuillaumeCH 2:3066e614372f 365 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 366 for (int i = 0; i<5; i++){
GuillaumeCH 2:3066e614372f 367 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 2:3066e614372f 368 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 2:3066e614372f 369 }
GuillaumeCH 2:3066e614372f 370 }
GuillaumeCH 2:3066e614372f 371
GuillaumeCH 2:3066e614372f 372 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 2:3066e614372f 373 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 2:3066e614372f 374 compteur_glisse++;
GuillaumeCH 2:3066e614372f 375
GuillaumeCH 2:3066e614372f 376 erreur_D = erreur_glissee_D[0];
GuillaumeCH 2:3066e614372f 377 erreur_G = erreur_glissee_G[0];
GuillaumeCH 2:3066e614372f 378 for (int i=1; i<5; i++)
GuillaumeCH 2:3066e614372f 379 {
GuillaumeCH 2:3066e614372f 380 erreur_D += erreur_glissee_D[i];
GuillaumeCH 2:3066e614372f 381 erreur_G += erreur_glissee_G[i];
GuillaumeCH 2:3066e614372f 382 }
GuillaumeCH 2:3066e614372f 383
GuillaumeCH 3:d38aa400d5e7 384 erreur_D = erreur_D/5.0f;
GuillaumeCH 3:d38aa400d5e7 385 erreur_G = erreur_G/5.0f; // erreur est maintenant la moyenne des 5 erreurs prec
GuillaumeCH 2:3066e614372f 386
GuillaumeCH 2:3066e614372f 387 somme_erreur_D += erreur_D;
GuillaumeCH 2:3066e614372f 388 somme_erreur_G += erreur_G;
GuillaumeCH 2:3066e614372f 389
GuillaumeCH 2:3066e614372f 390 float delta_erreur_D = erreur_D-erreur_precedente_D;
GuillaumeCH 2:3066e614372f 391 float delta_erreur_G = erreur_G-erreur_precedente_G;
GuillaumeCH 2:3066e614372f 392
GuillaumeCH 2:3066e614372f 393 erreur_precedente_G = erreur_G;
GuillaumeCH 2:3066e614372f 394 erreur_precedente_D = erreur_D;
GuillaumeCH 2:3066e614372f 395
GuillaumeCH 2:3066e614372f 396 float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D;
GuillaumeCH 2:3066e614372f 397 float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G;
GuillaumeCH 2:3066e614372f 398
GuillaumeCH 2:3066e614372f 399 if (cmd_G <0){
GuillaumeCH 2:3066e614372f 400 cmd_G = 0;
GuillaumeCH 2:3066e614372f 401 }
GuillaumeCH 2:3066e614372f 402 if (cmd_G > 500){
GuillaumeCH 2:3066e614372f 403 cmd_G = 500;
GuillaumeCH 2:3066e614372f 404 }
GuillaumeCH 2:3066e614372f 405 if (cmd_D <0){
GuillaumeCH 2:3066e614372f 406 cmd_D = 0;
GuillaumeCH 2:3066e614372f 407 }
GuillaumeCH 2:3066e614372f 408 if (cmd_D > 500){
GuillaumeCH 2:3066e614372f 409 cmd_D = 500;
GuillaumeCH 2:3066e614372f 410 }
GuillaumeCH 2:3066e614372f 411 c_D[dix_ms]=consigne_D;
GuillaumeCH 2:3066e614372f 412 c_G[dix_ms]=consigne_G;
GuillaumeCH 2:3066e614372f 413 //printf("%d\n",c[i]);
GuillaumeCH 2:3066e614372f 414 tab_cmd_D[dix_ms] = cmd_D;
GuillaumeCH 2:3066e614372f 415 tab_cmd_G[dix_ms] = cmd_G;
GuillaumeCH 2:3066e614372f 416 vtab_D[dix_ms] = vitesse_codeuse_D;
GuillaumeCH 2:3066e614372f 417 vtab_G[dix_ms] = vitesse_codeuse_G;
GuillaumeCH 2:3066e614372f 418 commande_vitesse(cmd_G,cmd_D);
GuillaumeCH 2:3066e614372f 419 dix_ms++;
GuillaumeCH 2:3066e614372f 420 //printf("%d\n",i);
GuillaumeCH 2:3066e614372f 421 //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
GuillaumeCH 2:3066e614372f 422 //printf("%f,%f\n",cmd_G,cmd_D);
GuillaumeCH 2:3066e614372f 423 //printf("oui");
GuillaumeCH 2:3066e614372f 424 }
GuillaumeCH 2:3066e614372f 425
GuillaumeCH 2:3066e614372f 426 void deplacement::printftab(){
GuillaumeCH 2:3066e614372f 427
GuillaumeCH 2:3066e614372f 428 for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 2:3066e614372f 429 if(j==500)
GuillaumeCH 2:3066e614372f 430 bouton();
GuillaumeCH 2:3066e614372f 431 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 2:3066e614372f 432 }
GuillaumeCH 2:3066e614372f 433 /*if (j<5)
GuillaumeCH 2:3066e614372f 434 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 2:3066e614372f 435 else
GuillaumeCH 2:3066e614372f 436 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 2:3066e614372f 437 }*/
GuillaumeCH 2:3066e614372f 438
GuillaumeCH 2:3066e614372f 439 /*for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 2:3066e614372f 440 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 2:3066e614372f 441 }*/
GuillaumeCH 2:3066e614372f 442 }
GuillaumeCH 2:3066e614372f 443
GuillaumeCH 2:3066e614372f 444 void deplacement::test(){
GuillaumeCH 2:3066e614372f 445 Timer t;
GuillaumeCH 2:3066e614372f 446 t.start();
GuillaumeCH 2:3066e614372f 447 for (int i =0;i<5 ;i++){
GuillaumeCH 2:3066e614372f 448 changement_consigne(consigne_tab[i][0], consigne_tab[i][1]);
GuillaumeCH 3:d38aa400d5e7 449 while(t.read()<0.5f){
GuillaumeCH 2:3066e614372f 450 //actualise_positio n();
GuillaumeCH 2:3066e614372f 451 }
GuillaumeCH 2:3066e614372f 452 //printf("t.read() : %f\n",t.read());
GuillaumeCH 2:3066e614372f 453 //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G);
GuillaumeCH 2:3066e614372f 454 t.reset();
GuillaumeCH 2:3066e614372f 455 }
GuillaumeCH 2:3066e614372f 456 }
GuillaumeCH 2:3066e614372f 457
GuillaumeCH 2:3066e614372f 458 void deplacement::changement_consigne(int cons_D, int cons_G){
GuillaumeCH 2:3066e614372f 459 consigne_D = cons_D;
GuillaumeCH 2:3066e614372f 460 consigne_G = cons_G;
GuillaumeCH 2:3066e614372f 461 compteur_glisse = -1;
GuillaumeCH 2:3066e614372f 462 }
GuillaumeCH 2:3066e614372f 463
GuillaumeCH 3:d38aa400d5e7 464
GuillaumeCH 2:3066e614372f 465
GuillaumeCH 2:3066e614372f 466 void deplacement::poussette(){
GuillaumeCH 2:3066e614372f 467 motors_on();
GuillaumeCH 2:3066e614372f 468 commande_vitesse(150,150);
GuillaumeCH 3:d38aa400d5e7 469 wait(1.5);
GuillaumeCH 2:3066e614372f 470 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 471 vitesse_nulle_D(0);
GuillaumeCH 2:3066e614372f 472 motors_stop();
GuillaumeCH 3:d38aa400d5e7 473 }
GuillaumeCH 3:d38aa400d5e7 474 void deplacement::initialisation(){
GuillaumeCH 3:d38aa400d5e7 475 init_odometrie();
GuillaumeCH 3:d38aa400d5e7 476 init_hardware();
GuillaumeCH 3:d38aa400d5e7 477 srand(time(NULL));
GuillaumeCH 3:d38aa400d5e7 478 motors_on();
GuillaumeCH 4:eac6746544fb 479 }
GuillaumeCH 4:eac6746544fb 480
GuillaumeCH 4:eac6746544fb 481 void deplacement::arc(Coordonnees p1, Coordonnees p2){
GuillaumeCH 4:eac6746544fb 482 motors_on();
GuillaumeCH 4:eac6746544fb 483 actualise_position();
GuillaumeCH 4:eac6746544fb 484 float vitesse_G;
GuillaumeCH 4:eac6746544fb 485 float vitesse_D;
GuillaumeCH 4:eac6746544fb 486 double x_ini = get_x_actuel();
GuillaumeCH 4:eac6746544fb 487 double y_ini = get_y_actuel();
GuillaumeCH 4:eac6746544fb 488 double angle_vise_deg = get_angle();
GuillaumeCH 4:eac6746544fb 489 double angle_vise=angle_vise_deg*3.1416/180.0;
GuillaumeCH 4:eac6746544fb 490 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 491
GuillaumeCH 4:eac6746544fb 492 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 493 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 494
GuillaumeCH 4:eac6746544fb 495 double x_actuel = get_x_actuel();
GuillaumeCH 4:eac6746544fb 496 double y_actuel = get_y_actuel();
GuillaumeCH 4:eac6746544fb 497
GuillaumeCH 4:eac6746544fb 498
GuillaumeCH 4:eac6746544fb 499 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 4:eac6746544fb 500 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 4:eac6746544fb 501
GuillaumeCH 4:eac6746544fb 502 Coordonnees p0;
GuillaumeCH 4:eac6746544fb 503 p0.x = x_local;
GuillaumeCH 4:eac6746544fb 504 p0.y = y_local;
GuillaumeCH 4:eac6746544fb 505 printf("x_local : %lf, y_local : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 506 cercle(p0,p1,p2);
GuillaumeCH 4:eac6746544fb 507 double xc= point[0];
GuillaumeCH 4:eac6746544fb 508 double yc= point[1];
GuillaumeCH 4:eac6746544fb 509 double Rc= point[2];
GuillaumeCH 4:eac6746544fb 510 printf("xc: %lf, yc : %lf, Rc: %lf,vg: %lf,vd : %lf\n",xc,yc,Rc,150.0,150.0*(Rc + ECART_ROUE/2.0)/(Rc-ECART_ROUE/2));
GuillaumeCH 4:eac6746544fb 511
GuillaumeCH 4:eac6746544fb 512 if (p2.y > 0){
GuillaumeCH 4:eac6746544fb 513 while(p2.y-y_local>0){
GuillaumeCH 4:eac6746544fb 514 vitesse_D = 150*(Rc + ECART_ROUE/2.0)/(Rc-ECART_ROUE/2.0);
GuillaumeCH 4:eac6746544fb 515 vitesse_G = 150;
GuillaumeCH 4:eac6746544fb 516 double correction = int_ext_cercle(x_actuel,y_actuel);
GuillaumeCH 4:eac6746544fb 517 vitesse_G= vitesse_G+correction*0.02;
GuillaumeCH 4:eac6746544fb 518 vitesse_D= vitesse_D-correction*0.02;
GuillaumeCH 4:eac6746544fb 519 commande_vitesse(vitesse_G,vitesse_D);
GuillaumeCH 4:eac6746544fb 520 //printf("vitesse_G : %f, vitesse_D : %f",vitesse_G,vitesse_D);
GuillaumeCH 4:eac6746544fb 521 actualise_position();
GuillaumeCH 4:eac6746544fb 522 x_actuel = get_x_actuel();
GuillaumeCH 4:eac6746544fb 523 y_actuel = get_y_actuel();
GuillaumeCH 4:eac6746544fb 524 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 4:eac6746544fb 525 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 4:eac6746544fb 526 //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 527 }
GuillaumeCH 4:eac6746544fb 528 }
GuillaumeCH 4:eac6746544fb 529 if (p2.y < 0){
GuillaumeCH 4:eac6746544fb 530 while(p2.y-y_local<0){
GuillaumeCH 4:eac6746544fb 531 vitesse_G = 150.0*(Rc + ECART_ROUE/2.0)/(Rc-ECART_ROUE/2);
GuillaumeCH 4:eac6746544fb 532 vitesse_D = 150.0;
GuillaumeCH 4:eac6746544fb 533 double correction = int_ext_cercle(x_local,y_local);
GuillaumeCH 4:eac6746544fb 534 vitesse_G= vitesse_G+correction*0.001;
GuillaumeCH 4:eac6746544fb 535 vitesse_D= vitesse_D-correction*0.001;
GuillaumeCH 4:eac6746544fb 536 commande_vitesse(vitesse_G,vitesse_D);
GuillaumeCH 4:eac6746544fb 537 //printf("vitesse_G : %f, vitesse_D : %f",vitesse_G,vitesse_D);
GuillaumeCH 4:eac6746544fb 538 actualise_position();
GuillaumeCH 4:eac6746544fb 539 x_actuel = get_x_actuel();
GuillaumeCH 4:eac6746544fb 540 y_actuel = get_y_actuel();
GuillaumeCH 4:eac6746544fb 541 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 4:eac6746544fb 542 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 4:eac6746544fb 543 //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 544
GuillaumeCH 4:eac6746544fb 545 }
GuillaumeCH 4:eac6746544fb 546 }
GuillaumeCH 4:eac6746544fb 547 printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 548 vitesse_nulle_D(0);
GuillaumeCH 4:eac6746544fb 549 vitesse_nulle_G(0);
GuillaumeCH 4:eac6746544fb 550 wait(0.2);
GuillaumeCH 4:eac6746544fb 551 motors_stop();
GuillaumeCH 4:eac6746544fb 552
GuillaumeCH 4:eac6746544fb 553
GuillaumeCH 4:eac6746544fb 554 }
GuillaumeCH 4:eac6746544fb 555
GuillaumeCH 4:eac6746544fb 556
GuillaumeCH 4:eac6746544fb 557 int deplacement::cercle(Coordonnees a,Coordonnees b, Coordonnees c){
GuillaumeCH 4:eac6746544fb 558 double ax2 = pow(a.x,2);
GuillaumeCH 4:eac6746544fb 559 double ay2 = pow(a.y,2);
GuillaumeCH 4:eac6746544fb 560 double bx2 = pow(b.x,2);
GuillaumeCH 4:eac6746544fb 561 double by2 = pow(b.y,2);
GuillaumeCH 4:eac6746544fb 562 double cx2 = pow(c.x,2);
GuillaumeCH 4:eac6746544fb 563 double cy2 = pow(c.y,2);
GuillaumeCH 4:eac6746544fb 564 double pente_a;
GuillaumeCH 4:eac6746544fb 565 double ord_b;
GuillaumeCH 4:eac6746544fb 566 double pente_A;
GuillaumeCH 4:eac6746544fb 567 double ord_B;
GuillaumeCH 4:eac6746544fb 568 double xc;
GuillaumeCH 4:eac6746544fb 569 double yc;
GuillaumeCH 4:eac6746544fb 570 double Rc;
GuillaumeCH 4:eac6746544fb 571 if (a.y == b.y && b.y == c.y){
GuillaumeCH 4:eac6746544fb 572 printf("Les points sont alignes1\n");
GuillaumeCH 4:eac6746544fb 573 return 0;
GuillaumeCH 4:eac6746544fb 574 }
GuillaumeCH 4:eac6746544fb 575 if (b.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 576 pente_a = -1*(b.x-a.x)/(b.y-a.y);
GuillaumeCH 4:eac6746544fb 577 printf("a : %lf ",pente_a);
GuillaumeCH 4:eac6746544fb 578 ord_b = (by2-ay2+bx2-ax2)/(2*(b.y-a.y));
GuillaumeCH 4:eac6746544fb 579 printf("b : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 580
GuillaumeCH 4:eac6746544fb 581 }
GuillaumeCH 4:eac6746544fb 582 else{
GuillaumeCH 4:eac6746544fb 583 cercle(c,b,a);
GuillaumeCH 4:eac6746544fb 584
GuillaumeCH 4:eac6746544fb 585 }
GuillaumeCH 4:eac6746544fb 586 if (c.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 587 pente_A = -1*(c.x-a.x)/(c.y-a.y);
GuillaumeCH 4:eac6746544fb 588 printf("A : %lf ",pente_A);
GuillaumeCH 4:eac6746544fb 589 ord_B = (cy2-ay2+cx2-ax2)/(2*(c.y-a.y));
GuillaumeCH 4:eac6746544fb 590 printf("B : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 591
GuillaumeCH 4:eac6746544fb 592 }
GuillaumeCH 4:eac6746544fb 593 else{
GuillaumeCH 4:eac6746544fb 594 cercle(b,a,c);
GuillaumeCH 4:eac6746544fb 595 }
GuillaumeCH 4:eac6746544fb 596
GuillaumeCH 4:eac6746544fb 597
GuillaumeCH 4:eac6746544fb 598 if (pente_a-pente_A!=0){
GuillaumeCH 4:eac6746544fb 599 xc = (ord_B-ord_b)/(pente_a-pente_A);
GuillaumeCH 4:eac6746544fb 600 yc = pente_a*xc+ord_b;
GuillaumeCH 4:eac6746544fb 601 Rc = pow((pow(xc-a.x,2)+pow(yc-a.y,2)),0.5);
GuillaumeCH 4:eac6746544fb 602 point[0] = xc;
GuillaumeCH 4:eac6746544fb 603 point[1] = yc;
GuillaumeCH 4:eac6746544fb 604 point[2] = Rc;
GuillaumeCH 4:eac6746544fb 605 printf("xc : %f, yc : %f, Rc : %f\n",xc,yc,Rc);
GuillaumeCH 4:eac6746544fb 606 }
GuillaumeCH 4:eac6746544fb 607 else{
GuillaumeCH 4:eac6746544fb 608 printf("Les points sont alignes2\n");
GuillaumeCH 4:eac6746544fb 609 return 0;
GuillaumeCH 4:eac6746544fb 610 }
GuillaumeCH 4:eac6746544fb 611
GuillaumeCH 4:eac6746544fb 612
GuillaumeCH 4:eac6746544fb 613 return 0;
GuillaumeCH 4:eac6746544fb 614 }
GuillaumeCH 4:eac6746544fb 615 double deplacement::int_ext_cercle(double x, double y){
GuillaumeCH 4:eac6746544fb 616 double xc= point[0];
GuillaumeCH 4:eac6746544fb 617 double yc= point[1];
GuillaumeCH 4:eac6746544fb 618 double Rc= point[2];
GuillaumeCH 4:eac6746544fb 619 double rayon = pow((pow(xc-x,2)+pow(yc-y,2)),0.5);
GuillaumeCH 4:eac6746544fb 620 return Rc-rayon;
GuillaumeCH 4:eac6746544fb 621
GuillaumeCH 4:eac6746544fb 622 }
GuillaumeCH 4:eac6746544fb 623
GuillaumeCH 4:eac6746544fb 624 void deplacement::va_au_point(double x,double y, double cap){
GuillaumeCH 4:eac6746544fb 625 actualise_position();
GuillaumeCH 4:eac6746544fb 626 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 627 double x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 628 double y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 629 double x_projete = 10000.0*cos(angle*3.1416/180.0)+x_robot;
GuillaumeCH 4:eac6746544fb 630 double y_projete = 10000.0*sin(angle*3.1416/180.0)+y_robot;
GuillaumeCH 4:eac6746544fb 631 printf("angle ::: %lf\n",recup_angle_entre_trois_points_213(x_robot,y_robot,x_projete,y_projete,x,y));
GuillaumeCH 4:eac6746544fb 632 rotation_rel(recup_angle_entre_trois_points_213(x_robot,y_robot,x_projete,y_projete,x,y));
GuillaumeCH 4:eac6746544fb 633 //printf("oui\n");
GuillaumeCH 4:eac6746544fb 634 actualise_position();
GuillaumeCH 4:eac6746544fb 635 angle = get_angle();
GuillaumeCH 4:eac6746544fb 636 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 637 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 638 double distance = pow(pow(x-x_robot,2)+pow(y-y_robot,2),0.5);
GuillaumeCH 4:eac6746544fb 639 printf("distance : %lf\n",distance);
GuillaumeCH 4:eac6746544fb 640 ligne_droite(distance);
GuillaumeCH 4:eac6746544fb 641 actualise_position();
GuillaumeCH 4:eac6746544fb 642 angle = get_angle();
GuillaumeCH 4:eac6746544fb 643 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 644 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 645 rotation_abs(cap);
GuillaumeCH 4:eac6746544fb 646
GuillaumeCH 4:eac6746544fb 647
GuillaumeCH 4:eac6746544fb 648 }
GuillaumeCH 4:eac6746544fb 649
GuillaumeCH 4:eac6746544fb 650 double deplacement::recup_angle_entre_trois_points_213(double x1,double y1,double x2,double y2,double x3,double y3){
GuillaumeCH 4:eac6746544fb 651 double x12 = x2-x1;
GuillaumeCH 4:eac6746544fb 652 double y12 = y2-y1;
GuillaumeCH 4:eac6746544fb 653 double x13 = x3-x1;
GuillaumeCH 4:eac6746544fb 654 double y13 = y3-y1;
GuillaumeCH 4:eac6746544fb 655 double norme12 = pow(x12*x12+y12*y12,0.5);
GuillaumeCH 4:eac6746544fb 656 double norme13 = pow(x13*x13+y13*y13,0.5);
GuillaumeCH 4:eac6746544fb 657 double prod_scal = x12*x13+y12*y13;
GuillaumeCH 4:eac6746544fb 658 //printf("%lf\n",acos(prod_scal/(norme12*norme13))*180.0/3.1416);
GuillaumeCH 4:eac6746544fb 659 return acos(prod_scal/(norme12*norme13))*180.0/3.1416;
GuillaumeCH 4:eac6746544fb 660 }
GuillaumeCH 4:eac6746544fb 661