Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

deplacement.cpp@5:3638d7e7c5c1, 2019-05-20 (annotated)

Committer:
JimmyAREM
Date:
Mon May 20 23:08:09 2019 +0000
Revision:
5:3638d7e7c5c1
Parent:
4:eac6746544fb
Child:
6:e1585b8bd07d
maj pour publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GuillaumeCH 2:3066e614372f 1 #include "deplacement.h"
JimmyAREM 5:3638d7e7c5c1 2 #include "Strategie.h"
GuillaumeCH 2:3066e614372f 3 #include "mbed.h"
GuillaumeCH 2:3066e614372f 4 #include "odometrie.h"
GuillaumeCH 2:3066e614372f 5 #include "hardware.h"
GuillaumeCH 2:3066e614372f 6 #include "math_precalc.h"
GuillaumeCH 2:3066e614372f 7 #include "reglages.h"
JimmyAREM 5:3638d7e7c5c1 8 #include "Ecran.h"
JimmyAREM 5:3638d7e7c5c1 9 #include "AnalyseArcLib.h"
GuillaumeCH 2:3066e614372f 10
JimmyAREM 5:3638d7e7c5c1 11 extern Serial pc;
JimmyAREM 5:3638d7e7c5c1 12 extern bool detectionUltrasons;
JimmyAREM 5:3638d7e7c5c1 13 bool stopCapteurs = false;
JimmyAREM 5:3638d7e7c5c1 14
JimmyAREM 5:3638d7e7c5c1 15 extern int distanceUltrasonGauche;
JimmyAREM 5:3638d7e7c5c1 16 extern int distanceUltrasonDroit;
JimmyAREM 5:3638d7e7c5c1 17 extern int distanceUltrasonArriere;
JimmyAREM 5:3638d7e7c5c1 18
JimmyAREM 5:3638d7e7c5c1 19 extern double posFinalRobotX;
JimmyAREM 5:3638d7e7c5c1 20 extern double posFinalRobotY;
JimmyAREM 5:3638d7e7c5c1 21
JimmyAREM 5:3638d7e7c5c1 22 extern double posInterRobotX;
JimmyAREM 5:3638d7e7c5c1 23 extern double posInterRobotY;
JimmyAREM 5:3638d7e7c5c1 24
JimmyAREM 5:3638d7e7c5c1 25 bool evitementValider = false;
JimmyAREM 5:3638d7e7c5c1 26 Coordonnees pointIntermediaire()
JimmyAREM 5:3638d7e7c5c1 27 {
JimmyAREM 5:3638d7e7c5c1 28 Coordonnees P1;
JimmyAREM 5:3638d7e7c5c1 29 double x_init = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 30 double y_init = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 31 P1.x = posInterRobotX *100 - x_init;
JimmyAREM 5:3638d7e7c5c1 32 P1.y = posInterRobotY *100 - y_init;
JimmyAREM 5:3638d7e7c5c1 33 return P1;
JimmyAREM 5:3638d7e7c5c1 34 }
JimmyAREM 5:3638d7e7c5c1 35
JimmyAREM 5:3638d7e7c5c1 36 Coordonnees pointFinale()
JimmyAREM 5:3638d7e7c5c1 37 {
JimmyAREM 5:3638d7e7c5c1 38 Coordonnees P1;
JimmyAREM 5:3638d7e7c5c1 39 double x_init = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 40 double y_init = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 41 P1.x = posFinalRobotX *100 - x_init;
JimmyAREM 5:3638d7e7c5c1 42 P1.y = posFinalRobotY *100 - y_init;
JimmyAREM 5:3638d7e7c5c1 43 return P1;
JimmyAREM 5:3638d7e7c5c1 44 }
GuillaumeCH 2:3066e614372f 45
GuillaumeCH 2:3066e614372f 46 deplacement::deplacement(){
GuillaumeCH 2:3066e614372f 47 consigne_D = 0;
GuillaumeCH 2:3066e614372f 48 consigne_G = 0;
GuillaumeCH 2:3066e614372f 49 somme_erreur_D = 0;
GuillaumeCH 2:3066e614372f 50 somme_erreur_G = 0;
GuillaumeCH 2:3066e614372f 51 erreur_precedente_D = 0;
GuillaumeCH 2:3066e614372f 52 erreur_precedente_G = 0;
GuillaumeCH 2:3066e614372f 53 compteur_asser =0;
GuillaumeCH 2:3066e614372f 54 somme_y=0;
GuillaumeCH 2:3066e614372f 55
GuillaumeCH 2:3066e614372f 56 for (int k =0; k<5;k++){
GuillaumeCH 2:3066e614372f 57 erreur_glissee_D[k] = 0;
GuillaumeCH 2:3066e614372f 58 erreur_glissee_G[k] = 0;
GuillaumeCH 2:3066e614372f 59 }
GuillaumeCH 2:3066e614372f 60 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 61
GuillaumeCH 2:3066e614372f 62 Kp_D = 1.5;//1
GuillaumeCH 2:3066e614372f 63 Ki_D = 0.12;//0.15
GuillaumeCH 2:3066e614372f 64 Kd_D = 0.5;//1
GuillaumeCH 2:3066e614372f 65
GuillaumeCH 2:3066e614372f 66 Kp_G = 1;//1
GuillaumeCH 2:3066e614372f 67 Ki_G = 0.13;//0.15
GuillaumeCH 2:3066e614372f 68 Kd_G = 1.2;//1
GuillaumeCH 2:3066e614372f 69
GuillaumeCH 2:3066e614372f 70 tick_prec_D=0;
GuillaumeCH 2:3066e614372f 71 tick_prec_G = 0;
GuillaumeCH 2:3066e614372f 72 dix_ms = 0;
GuillaumeCH 2:3066e614372f 73 for (int k =0; k<TAILLE_TAB;k++){
GuillaumeCH 2:3066e614372f 74 tab_cmd_G[k]=0;
GuillaumeCH 2:3066e614372f 75 tab_cmd_D[k]=0;
GuillaumeCH 2:3066e614372f 76 vtab_G[k]=0;
GuillaumeCH 2:3066e614372f 77 vtab_D[k]=0;
GuillaumeCH 2:3066e614372f 78 c_D[k]=0;
GuillaumeCH 2:3066e614372f 79 c_G[k]=0;
GuillaumeCH 2:3066e614372f 80 }
GuillaumeCH 2:3066e614372f 81 consigne_tab[0][0]=0;
GuillaumeCH 2:3066e614372f 82 consigne_tab[0][1]=0;
GuillaumeCH 2:3066e614372f 83
GuillaumeCH 2:3066e614372f 84 consigne_tab[1][0]=10;
GuillaumeCH 2:3066e614372f 85 consigne_tab[1][1]=10;
GuillaumeCH 2:3066e614372f 86
GuillaumeCH 2:3066e614372f 87 consigne_tab[2][0]=20;
GuillaumeCH 2:3066e614372f 88 consigne_tab[2][1]=20;
GuillaumeCH 2:3066e614372f 89
GuillaumeCH 2:3066e614372f 90 consigne_tab[3][0]=30;
GuillaumeCH 2:3066e614372f 91 consigne_tab[3][1]=30;
GuillaumeCH 2:3066e614372f 92
GuillaumeCH 2:3066e614372f 93 consigne_tab[4][0]=40;
GuillaumeCH 2:3066e614372f 94 consigne_tab[4][1]=40;
GuillaumeCH 2:3066e614372f 95
GuillaumeCH 2:3066e614372f 96 /* consigne_tab[5][0]=3*5;
GuillaumeCH 2:3066e614372f 97 consigne_tab[5][1]=3*5;
GuillaumeCH 2:3066e614372f 98
GuillaumeCH 2:3066e614372f 99 consigne_tab[6][0]=3*6;
GuillaumeCH 2:3066e614372f 100 consigne_tab[6][1]=3*6;
GuillaumeCH 2:3066e614372f 101
GuillaumeCH 2:3066e614372f 102 consigne_tab[7][0]=3*7;
GuillaumeCH 2:3066e614372f 103 consigne_tab[7][1]=3*7;
GuillaumeCH 2:3066e614372f 104
GuillaumeCH 2:3066e614372f 105 consigne_tab[8][0]=3*8;
GuillaumeCH 2:3066e614372f 106 consigne_tab[8][1]=3*8;
GuillaumeCH 2:3066e614372f 107
GuillaumeCH 2:3066e614372f 108 consigne_tab[9][0]=3*9;
GuillaumeCH 2:3066e614372f 109 consigne_tab[9][1]=3*9;
GuillaumeCH 2:3066e614372f 110
GuillaumeCH 2:3066e614372f 111 consigne_tab[10][0]=3*10;
GuillaumeCH 2:3066e614372f 112 consigne_tab[10][1]=3*10;
GuillaumeCH 2:3066e614372f 113
GuillaumeCH 2:3066e614372f 114 consigne_tab[11][0]=3*11;
GuillaumeCH 2:3066e614372f 115 consigne_tab[11][1]=3*11;
GuillaumeCH 2:3066e614372f 116
GuillaumeCH 2:3066e614372f 117 consigne_tab[12][0]=3*12;
GuillaumeCH 2:3066e614372f 118 consigne_tab[12][1]=3*12;
GuillaumeCH 2:3066e614372f 119
GuillaumeCH 2:3066e614372f 120 consigne_tab[13][0]=3*13;
GuillaumeCH 2:3066e614372f 121 consigne_tab[13][1]=3*13;
GuillaumeCH 2:3066e614372f 122
GuillaumeCH 2:3066e614372f 123 consigne_tab[14][0]=3*14;
GuillaumeCH 2:3066e614372f 124 consigne_tab[14][1]=3*14;
GuillaumeCH 2:3066e614372f 125
GuillaumeCH 2:3066e614372f 126 consigne_tab[15][0]=0;
GuillaumeCH 2:3066e614372f 127 consigne_tab[15][1]=0;
GuillaumeCH 2:3066e614372f 128
GuillaumeCH 2:3066e614372f 129 consigne_tab[16][0]=0;
GuillaumeCH 2:3066e614372f 130 consigne_tab[16][1]=0;
GuillaumeCH 2:3066e614372f 131
GuillaumeCH 2:3066e614372f 132 consigne_tab[17][0]=0;
GuillaumeCH 2:3066e614372f 133 consigne_tab[17][1]=0;
GuillaumeCH 2:3066e614372f 134
GuillaumeCH 2:3066e614372f 135 consigne_tab[18][0]=0;
GuillaumeCH 2:3066e614372f 136 consigne_tab[18][1]=0;
GuillaumeCH 2:3066e614372f 137
GuillaumeCH 2:3066e614372f 138 consigne_tab[19][0]=0;
GuillaumeCH 2:3066e614372f 139 consigne_tab[19][1]=0;*/
GuillaumeCH 2:3066e614372f 140 }
GuillaumeCH 2:3066e614372f 141
GuillaumeCH 2:3066e614372f 142 void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
GuillaumeCH 2:3066e614372f 143
GuillaumeCH 2:3066e614372f 144 int sens_G=signe(vitesse_G);
GuillaumeCH 2:3066e614372f 145 int sens_D=signe(vitesse_D);
GuillaumeCH 2:3066e614372f 146 double vitesse_local_G=abs(vitesse_G);
GuillaumeCH 2:3066e614372f 147 double vitesse_local_D=abs(vitesse_D);
GuillaumeCH 2:3066e614372f 148
GuillaumeCH 2:3066e614372f 149 if(abs(vitesse_G) > 900){
GuillaumeCH 2:3066e614372f 150 vitesse_local_G=900;
GuillaumeCH 2:3066e614372f 151 }
GuillaumeCH 3:d38aa400d5e7 152 if(abs(vitesse_G)<10){
GuillaumeCH 3:d38aa400d5e7 153 vitesse_local_G=10;
GuillaumeCH 2:3066e614372f 154 }
GuillaumeCH 2:3066e614372f 155 if(abs(vitesse_D) > 900){
GuillaumeCH 2:3066e614372f 156 vitesse_local_D=900;
GuillaumeCH 2:3066e614372f 157 }
GuillaumeCH 3:d38aa400d5e7 158 if(abs(vitesse_D)< 10){
GuillaumeCH 3:d38aa400d5e7 159 vitesse_local_D=10;
GuillaumeCH 2:3066e614372f 160
GuillaumeCH 2:3066e614372f 161 }
GuillaumeCH 2:3066e614372f 162 ;
GuillaumeCH 2:3066e614372f 163 int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 164 int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 165 float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 166 float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 167 float centieme_D = (VD_f-VD_int)*1000;
GuillaumeCH 2:3066e614372f 168 float centieme_G = (VG_f-VG_int)*1000;
GuillaumeCH 2:3066e614372f 169 if ((rand()%1000) < centieme_G){
GuillaumeCH 2:3066e614372f 170 VG_int+=1;
GuillaumeCH 2:3066e614372f 171 }
GuillaumeCH 2:3066e614372f 172 if ((rand()%1000) < centieme_D){
GuillaumeCH 2:3066e614372f 173 VD_int+=1;
GuillaumeCH 2:3066e614372f 174 }
GuillaumeCH 2:3066e614372f 175 //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
GuillaumeCH 2:3066e614372f 176 set_PWM_moteur_G(VG_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
GuillaumeCH 2:3066e614372f 177 set_PWM_moteur_D(VD_int);//
GuillaumeCH 2:3066e614372f 178 }
GuillaumeCH 2:3066e614372f 179 void deplacement::vitesse_nulle_G(int zero){
GuillaumeCH 2:3066e614372f 180 if(zero == 0){
GuillaumeCH 2:3066e614372f 181 set_PWM_moteur_G(0);
GuillaumeCH 2:3066e614372f 182 }
GuillaumeCH 2:3066e614372f 183 }
GuillaumeCH 2:3066e614372f 184 void deplacement::vitesse_nulle_D(int zero){
GuillaumeCH 2:3066e614372f 185 if(zero == 0){
GuillaumeCH 2:3066e614372f 186 set_PWM_moteur_D(0);
GuillaumeCH 2:3066e614372f 187 }
GuillaumeCH 2:3066e614372f 188 }
GuillaumeCH 3:d38aa400d5e7 189 void deplacement::marche_arriere(int distance)
GuillaumeCH 2:3066e614372f 190 {
GuillaumeCH 2:3066e614372f 191 somme_y=0;
GuillaumeCH 2:3066e614372f 192 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 193 motors_on();
GuillaumeCH 2:3066e614372f 194 actualise_position();
GuillaumeCH 2:3066e614372f 195 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 196 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 197 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 198 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 199 double angle = get_angle();
GuillaumeCH 2:3066e614372f 200
GuillaumeCH 2:3066e614372f 201 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 202 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 203
GuillaumeCH 2:3066e614372f 204 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 205 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 206
GuillaumeCH 2:3066e614372f 207
GuillaumeCH 2:3066e614372f 208 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 209 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 210
GuillaumeCH 2:3066e614372f 211 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 212 float vitesse_G;
GuillaumeCH 2:3066e614372f 213 float vitesse_D;
GuillaumeCH 2:3066e614372f 214
GuillaumeCH 2:3066e614372f 215 angle = get_angle();
GuillaumeCH 2:3066e614372f 216 float Kip=0;
GuillaumeCH 2:3066e614372f 217 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 218 float Kdp= 10;
GuillaumeCH 2:3066e614372f 219 while (distance-x_local<0){
JimmyAREM 5:3638d7e7c5c1 220 if(detectionUltrasons)
JimmyAREM 5:3638d7e7c5c1 221 {
JimmyAREM 5:3638d7e7c5c1 222 detectionUltrasons = false;
JimmyAREM 5:3638d7e7c5c1 223 //writeCapteurs();
JimmyAREM 5:3638d7e7c5c1 224 LectureI2CCarteCapteur(*this);
JimmyAREM 5:3638d7e7c5c1 225 }
GuillaumeCH 2:3066e614372f 226 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 227 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 228 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 229 vitesse_G=400;
GuillaumeCH 2:3066e614372f 230 vitesse_D=400;
GuillaumeCH 2:3066e614372f 231 }
GuillaumeCH 2:3066e614372f 232 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 233 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 234 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 235 }
GuillaumeCH 2:3066e614372f 236
GuillaumeCH 2:3066e614372f 237 angle = get_angle();
GuillaumeCH 2:3066e614372f 238
GuillaumeCH 2:3066e614372f 239 vitesse_G = vitesse_G - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
GuillaumeCH 2:3066e614372f 240 vitesse_D = vitesse_D + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
GuillaumeCH 2:3066e614372f 241 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 242 //consigne_G = vitesse_G;
JimmyAREM 5:3638d7e7c5c1 243 if (stopCapteurs == true)
JimmyAREM 5:3638d7e7c5c1 244 {
JimmyAREM 5:3638d7e7c5c1 245 vitesse_nulle_D(0);
JimmyAREM 5:3638d7e7c5c1 246 vitesse_nulle_G(0);
JimmyAREM 5:3638d7e7c5c1 247 }
JimmyAREM 5:3638d7e7c5c1 248 else
JimmyAREM 5:3638d7e7c5c1 249 {
JimmyAREM 5:3638d7e7c5c1 250 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 251 }
GuillaumeCH 2:3066e614372f 252 actualise_position();
GuillaumeCH 2:3066e614372f 253 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 254 y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 255 somme_y+=y_actuel;
GuillaumeCH 2:3066e614372f 256 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 257 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 258 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 259 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 260 compteur_asser=0;
GuillaumeCH 2:3066e614372f 261 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 262 }
GuillaumeCH 2:3066e614372f 263 compteur_asser++;
GuillaumeCH 2:3066e614372f 264 //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 265 }
GuillaumeCH 2:3066e614372f 266 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 267 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 268 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 269 }
GuillaumeCH 2:3066e614372f 270
GuillaumeCH 2:3066e614372f 271
GuillaumeCH 2:3066e614372f 272
GuillaumeCH 3:d38aa400d5e7 273 void deplacement::ligne_droite(long int distance)
GuillaumeCH 2:3066e614372f 274 {
GuillaumeCH 2:3066e614372f 275 somme_y=0;
GuillaumeCH 2:3066e614372f 276 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 277 motors_on();
GuillaumeCH 2:3066e614372f 278 actualise_position();
GuillaumeCH 2:3066e614372f 279 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 280 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 281 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 282 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 283 double angle = get_angle();
GuillaumeCH 2:3066e614372f 284
GuillaumeCH 2:3066e614372f 285 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 286 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 287
GuillaumeCH 2:3066e614372f 288 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 289 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 290
GuillaumeCH 2:3066e614372f 291
GuillaumeCH 2:3066e614372f 292 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 293 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 294
GuillaumeCH 2:3066e614372f 295 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 296 float vitesse_G;
GuillaumeCH 2:3066e614372f 297 float vitesse_D;
GuillaumeCH 2:3066e614372f 298
GuillaumeCH 2:3066e614372f 299 angle = get_angle();
GuillaumeCH 2:3066e614372f 300 float Kip=0;
GuillaumeCH 2:3066e614372f 301 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 302 float Kdp= 10;
GuillaumeCH 2:3066e614372f 303 while (distance-x_local>0){
GuillaumeCH 2:3066e614372f 304
JimmyAREM 5:3638d7e7c5c1 305 if(detectionUltrasons)
JimmyAREM 5:3638d7e7c5c1 306 {
JimmyAREM 5:3638d7e7c5c1 307 detectionUltrasons = false;
JimmyAREM 5:3638d7e7c5c1 308 evitementValider = false;
JimmyAREM 5:3638d7e7c5c1 309 //writeCapteurs();
JimmyAREM 5:3638d7e7c5c1 310 LectureI2CCarteCapteur(*this);
JimmyAREM 5:3638d7e7c5c1 311 }
GuillaumeCH 2:3066e614372f 312 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 313 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 314 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 315 vitesse_G=400;
GuillaumeCH 2:3066e614372f 316 vitesse_D=400;
GuillaumeCH 2:3066e614372f 317 }
GuillaumeCH 2:3066e614372f 318 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 319 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 320 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 321 }
GuillaumeCH 2:3066e614372f 322
GuillaumeCH 2:3066e614372f 323 angle = get_angle();
GuillaumeCH 2:3066e614372f 324
GuillaumeCH 2:3066e614372f 325 vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
GuillaumeCH 2:3066e614372f 326 vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
GuillaumeCH 2:3066e614372f 327 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 328 //consigne_G = vitesse_G;
JimmyAREM 5:3638d7e7c5c1 329 if (stopCapteurs == true)
JimmyAREM 5:3638d7e7c5c1 330 {
JimmyAREM 5:3638d7e7c5c1 331 if((distanceUltrasonGauche == 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
JimmyAREM 5:3638d7e7c5c1 332 {
JimmyAREM 5:3638d7e7c5c1 333
JimmyAREM 5:3638d7e7c5c1 334 if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE)
JimmyAREM 5:3638d7e7c5c1 335 {
JimmyAREM 5:3638d7e7c5c1 336 evitementValider = true;
JimmyAREM 5:3638d7e7c5c1 337 actualise_position();
JimmyAREM 5:3638d7e7c5c1 338 Coordonnees P1 = pointIntermediaire();
JimmyAREM 5:3638d7e7c5c1 339 Coordonnees P2 = pointFinale();
JimmyAREM 5:3638d7e7c5c1 340 arc(P1,P2);
JimmyAREM 5:3638d7e7c5c1 341 wait(2);
JimmyAREM 5:3638d7e7c5c1 342 }
JimmyAREM 5:3638d7e7c5c1 343 else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
JimmyAREM 5:3638d7e7c5c1 344 {
JimmyAREM 5:3638d7e7c5c1 345 evitementValider = true;
JimmyAREM 5:3638d7e7c5c1 346 actualise_position();
JimmyAREM 5:3638d7e7c5c1 347 Coordonnees P1 = pointIntermediaire();
JimmyAREM 5:3638d7e7c5c1 348 Coordonnees P2 = pointFinale();
JimmyAREM 5:3638d7e7c5c1 349 arc(P1,P2);
JimmyAREM 5:3638d7e7c5c1 350 wait(2);
JimmyAREM 5:3638d7e7c5c1 351 }
JimmyAREM 5:3638d7e7c5c1 352 }
JimmyAREM 5:3638d7e7c5c1 353 if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit == 1100) && (evitementValider == false))
JimmyAREM 5:3638d7e7c5c1 354 {
JimmyAREM 5:3638d7e7c5c1 355
JimmyAREM 5:3638d7e7c5c1 356 if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
JimmyAREM 5:3638d7e7c5c1 357 {
JimmyAREM 5:3638d7e7c5c1 358 evitementValider = true;
JimmyAREM 5:3638d7e7c5c1 359 actualise_position();
JimmyAREM 5:3638d7e7c5c1 360 Coordonnees P1 = pointIntermediaire();
JimmyAREM 5:3638d7e7c5c1 361 Coordonnees P2 = pointFinale();
JimmyAREM 5:3638d7e7c5c1 362 arc(P1,P2);
JimmyAREM 5:3638d7e7c5c1 363 wait(2);
JimmyAREM 5:3638d7e7c5c1 364 }
JimmyAREM 5:3638d7e7c5c1 365 else if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE)
JimmyAREM 5:3638d7e7c5c1 366 {
JimmyAREM 5:3638d7e7c5c1 367 evitementValider = true;
JimmyAREM 5:3638d7e7c5c1 368 actualise_position();
JimmyAREM 5:3638d7e7c5c1 369 Coordonnees P1 = pointIntermediaire();
JimmyAREM 5:3638d7e7c5c1 370 Coordonnees P2 = pointFinale();
JimmyAREM 5:3638d7e7c5c1 371 arc(P1,P2);
JimmyAREM 5:3638d7e7c5c1 372 wait(2);
JimmyAREM 5:3638d7e7c5c1 373 }
JimmyAREM 5:3638d7e7c5c1 374
JimmyAREM 5:3638d7e7c5c1 375 }
JimmyAREM 5:3638d7e7c5c1 376 if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
JimmyAREM 5:3638d7e7c5c1 377 {
JimmyAREM 5:3638d7e7c5c1 378 evitementValider = true;
JimmyAREM 5:3638d7e7c5c1 379 if(lancerUnEvitementParArcGauche(distanceUltrasonGauche) == SANS_OBSTACLE)
JimmyAREM 5:3638d7e7c5c1 380 {
JimmyAREM 5:3638d7e7c5c1 381 evitementValider = true;
JimmyAREM 5:3638d7e7c5c1 382 actualise_position();
JimmyAREM 5:3638d7e7c5c1 383 Coordonnees P1 = pointIntermediaire();
JimmyAREM 5:3638d7e7c5c1 384 Coordonnees P2 = pointFinale();
JimmyAREM 5:3638d7e7c5c1 385 arc(P1,P2);
JimmyAREM 5:3638d7e7c5c1 386 wait(2);
JimmyAREM 5:3638d7e7c5c1 387 }
JimmyAREM 5:3638d7e7c5c1 388 else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit) == SANS_OBSTACLE)
JimmyAREM 5:3638d7e7c5c1 389 {
JimmyAREM 5:3638d7e7c5c1 390 evitementValider = true;
JimmyAREM 5:3638d7e7c5c1 391 actualise_position();
JimmyAREM 5:3638d7e7c5c1 392 Coordonnees P1 = pointIntermediaire();
JimmyAREM 5:3638d7e7c5c1 393 Coordonnees P2 = pointFinale();
JimmyAREM 5:3638d7e7c5c1 394 arc(P1,P2);
JimmyAREM 5:3638d7e7c5c1 395 wait(2);
JimmyAREM 5:3638d7e7c5c1 396 }
JimmyAREM 5:3638d7e7c5c1 397 }
JimmyAREM 5:3638d7e7c5c1 398 else
JimmyAREM 5:3638d7e7c5c1 399 {
JimmyAREM 5:3638d7e7c5c1 400 vitesse_nulle_D(0);
JimmyAREM 5:3638d7e7c5c1 401 vitesse_nulle_G(0);
JimmyAREM 5:3638d7e7c5c1 402 }
JimmyAREM 5:3638d7e7c5c1 403
JimmyAREM 5:3638d7e7c5c1 404 }
JimmyAREM 5:3638d7e7c5c1 405 else
JimmyAREM 5:3638d7e7c5c1 406 {
JimmyAREM 5:3638d7e7c5c1 407 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 408 }
GuillaumeCH 2:3066e614372f 409 actualise_position();
GuillaumeCH 2:3066e614372f 410 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 411 y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 412 somme_y+=y_actuel;
GuillaumeCH 2:3066e614372f 413 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 414 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 415 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 416 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 417 compteur_asser=0;
GuillaumeCH 2:3066e614372f 418 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 419 }
GuillaumeCH 2:3066e614372f 420 compteur_asser++;
GuillaumeCH 2:3066e614372f 421 //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 422 }
GuillaumeCH 2:3066e614372f 423 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 424 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 425 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 426 }
GuillaumeCH 2:3066e614372f 427
GuillaumeCH 3:d38aa400d5e7 428 void deplacement::rotation_rel(double angle_vise)
GuillaumeCH 2:3066e614372f 429 {
GuillaumeCH 2:3066e614372f 430 // rotation de angle_vise
GuillaumeCH 2:3066e614372f 431 motors_on();
GuillaumeCH 2:3066e614372f 432 double vitesse=180;
GuillaumeCH 2:3066e614372f 433 int sens;
GuillaumeCH 2:3066e614372f 434 double angle = get_angle();
GuillaumeCH 2:3066e614372f 435 angle_vise+=angle;
GuillaumeCH 2:3066e614372f 436 borne_angle_d(angle_vise);
GuillaumeCH 2:3066e614372f 437 if (diff_angle(angle,angle_vise)<=0){
GuillaumeCH 2:3066e614372f 438 sens = -1;
GuillaumeCH 2:3066e614372f 439 //printf("negatif\n");
GuillaumeCH 2:3066e614372f 440 }
GuillaumeCH 2:3066e614372f 441 else{
GuillaumeCH 2:3066e614372f 442 sens = 1;
GuillaumeCH 2:3066e614372f 443
GuillaumeCH 2:3066e614372f 444 //printf("positif\n");
GuillaumeCH 2:3066e614372f 445 }
GuillaumeCH 2:3066e614372f 446 //printf("diff : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 447 while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
GuillaumeCH 2:3066e614372f 448 {
GuillaumeCH 2:3066e614372f 449 actualise_position();
GuillaumeCH 2:3066e614372f 450 angle = get_angle();
GuillaumeCH 2:3066e614372f 451 vitesse=1.5*sens*abs(diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 452
GuillaumeCH 2:3066e614372f 453 commande_vitesse(-vitesse,vitesse);
GuillaumeCH 2:3066e614372f 454 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 455 compteur_asser=0;
GuillaumeCH 2:3066e614372f 456 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 457 }
GuillaumeCH 2:3066e614372f 458 compteur_asser++;
GuillaumeCH 2:3066e614372f 459 //printf("vitesse : %lf ", vitesse);
GuillaumeCH 2:3066e614372f 460 }
GuillaumeCH 2:3066e614372f 461 //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 462 //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 463 //consigne_D = 0;
GuillaumeCH 2:3066e614372f 464 //consigne_G = 0;
GuillaumeCH 2:3066e614372f 465 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 466 vitesse_nulle_D(0);
GuillaumeCH 3:d38aa400d5e7 467 wait(0.1);
GuillaumeCH 2:3066e614372f 468 motors_stop();
GuillaumeCH 2:3066e614372f 469 }
GuillaumeCH 2:3066e614372f 470
GuillaumeCH 2:3066e614372f 471
GuillaumeCH 3:d38aa400d5e7 472 void deplacement::rotation_abs(double angle_vise)
GuillaumeCH 2:3066e614372f 473 {
GuillaumeCH 2:3066e614372f 474 actualise_position();
GuillaumeCH 2:3066e614372f 475 //printf("bite");
GuillaumeCH 2:3066e614372f 476 double angle_rel = borne_angle_d(angle_vise-get_angle());
GuillaumeCH 3:d38aa400d5e7 477 rotation_rel(angle_rel);
GuillaumeCH 2:3066e614372f 478 }
GuillaumeCH 2:3066e614372f 479
GuillaumeCH 2:3066e614372f 480 void deplacement::asservissement(){
GuillaumeCH 2:3066e614372f 481 long int tick_D = get_nbr_tick_D();
GuillaumeCH 2:3066e614372f 482 long int tick_G = get_nbr_tick_G();
GuillaumeCH 2:3066e614372f 483
GuillaumeCH 2:3066e614372f 484 long int tick_D_passe = tick_D-tick_prec_D;
GuillaumeCH 2:3066e614372f 485 long int tick_G_passe = tick_G-tick_prec_G;
GuillaumeCH 2:3066e614372f 486
GuillaumeCH 2:3066e614372f 487 tick_prec_D=tick_D;
GuillaumeCH 2:3066e614372f 488 tick_prec_G=tick_G;
GuillaumeCH 2:3066e614372f 489
GuillaumeCH 2:3066e614372f 490 float vitesse_codeuse_D = tick_D_passe;
GuillaumeCH 2:3066e614372f 491 float vitesse_codeuse_G = tick_G_passe;
GuillaumeCH 2:3066e614372f 492
GuillaumeCH 2:3066e614372f 493 float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
GuillaumeCH 2:3066e614372f 494 float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
GuillaumeCH 2:3066e614372f 495
GuillaumeCH 2:3066e614372f 496 if (compteur_glisse == 5)
GuillaumeCH 2:3066e614372f 497 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 498
GuillaumeCH 2:3066e614372f 499 if (compteur_glisse == -1)
GuillaumeCH 2:3066e614372f 500 {
GuillaumeCH 2:3066e614372f 501 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 502 for (int i = 0; i<5; i++){
GuillaumeCH 2:3066e614372f 503 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 2:3066e614372f 504 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 2:3066e614372f 505 }
GuillaumeCH 2:3066e614372f 506 }
GuillaumeCH 2:3066e614372f 507
GuillaumeCH 2:3066e614372f 508 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 2:3066e614372f 509 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 2:3066e614372f 510 compteur_glisse++;
GuillaumeCH 2:3066e614372f 511
GuillaumeCH 2:3066e614372f 512 erreur_D = erreur_glissee_D[0];
GuillaumeCH 2:3066e614372f 513 erreur_G = erreur_glissee_G[0];
GuillaumeCH 2:3066e614372f 514 for (int i=1; i<5; i++)
GuillaumeCH 2:3066e614372f 515 {
GuillaumeCH 2:3066e614372f 516 erreur_D += erreur_glissee_D[i];
GuillaumeCH 2:3066e614372f 517 erreur_G += erreur_glissee_G[i];
GuillaumeCH 2:3066e614372f 518 }
GuillaumeCH 2:3066e614372f 519
GuillaumeCH 3:d38aa400d5e7 520 erreur_D = erreur_D/5.0f;
GuillaumeCH 3:d38aa400d5e7 521 erreur_G = erreur_G/5.0f; // erreur est maintenant la moyenne des 5 erreurs prec
GuillaumeCH 2:3066e614372f 522
GuillaumeCH 2:3066e614372f 523 somme_erreur_D += erreur_D;
GuillaumeCH 2:3066e614372f 524 somme_erreur_G += erreur_G;
GuillaumeCH 2:3066e614372f 525
GuillaumeCH 2:3066e614372f 526 float delta_erreur_D = erreur_D-erreur_precedente_D;
GuillaumeCH 2:3066e614372f 527 float delta_erreur_G = erreur_G-erreur_precedente_G;
GuillaumeCH 2:3066e614372f 528
GuillaumeCH 2:3066e614372f 529 erreur_precedente_G = erreur_G;
GuillaumeCH 2:3066e614372f 530 erreur_precedente_D = erreur_D;
GuillaumeCH 2:3066e614372f 531
GuillaumeCH 2:3066e614372f 532 float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D;
GuillaumeCH 2:3066e614372f 533 float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G;
GuillaumeCH 2:3066e614372f 534
GuillaumeCH 2:3066e614372f 535 if (cmd_G <0){
GuillaumeCH 2:3066e614372f 536 cmd_G = 0;
GuillaumeCH 2:3066e614372f 537 }
GuillaumeCH 2:3066e614372f 538 if (cmd_G > 500){
GuillaumeCH 2:3066e614372f 539 cmd_G = 500;
GuillaumeCH 2:3066e614372f 540 }
GuillaumeCH 2:3066e614372f 541 if (cmd_D <0){
GuillaumeCH 2:3066e614372f 542 cmd_D = 0;
GuillaumeCH 2:3066e614372f 543 }
GuillaumeCH 2:3066e614372f 544 if (cmd_D > 500){
GuillaumeCH 2:3066e614372f 545 cmd_D = 500;
GuillaumeCH 2:3066e614372f 546 }
GuillaumeCH 2:3066e614372f 547 c_D[dix_ms]=consigne_D;
GuillaumeCH 2:3066e614372f 548 c_G[dix_ms]=consigne_G;
GuillaumeCH 2:3066e614372f 549 //printf("%d\n",c[i]);
GuillaumeCH 2:3066e614372f 550 tab_cmd_D[dix_ms] = cmd_D;
GuillaumeCH 2:3066e614372f 551 tab_cmd_G[dix_ms] = cmd_G;
GuillaumeCH 2:3066e614372f 552 vtab_D[dix_ms] = vitesse_codeuse_D;
GuillaumeCH 2:3066e614372f 553 vtab_G[dix_ms] = vitesse_codeuse_G;
GuillaumeCH 2:3066e614372f 554 commande_vitesse(cmd_G,cmd_D);
GuillaumeCH 2:3066e614372f 555 dix_ms++;
GuillaumeCH 2:3066e614372f 556 //printf("%d\n",i);
GuillaumeCH 2:3066e614372f 557 //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
GuillaumeCH 2:3066e614372f 558 //printf("%f,%f\n",cmd_G,cmd_D);
GuillaumeCH 2:3066e614372f 559 //printf("oui");
GuillaumeCH 2:3066e614372f 560 }
GuillaumeCH 2:3066e614372f 561
GuillaumeCH 2:3066e614372f 562 void deplacement::printftab(){
GuillaumeCH 2:3066e614372f 563
GuillaumeCH 2:3066e614372f 564 for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 2:3066e614372f 565 if(j==500)
GuillaumeCH 2:3066e614372f 566 bouton();
GuillaumeCH 2:3066e614372f 567 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 568 }
GuillaumeCH 2:3066e614372f 569 /*if (j<5)
GuillaumeCH 2:3066e614372f 570 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 571 else
GuillaumeCH 2:3066e614372f 572 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 573 }*/
GuillaumeCH 2:3066e614372f 574
GuillaumeCH 2:3066e614372f 575 /*for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 2:3066e614372f 576 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 577 }*/
GuillaumeCH 2:3066e614372f 578 }
GuillaumeCH 2:3066e614372f 579
GuillaumeCH 2:3066e614372f 580 void deplacement::test(){
GuillaumeCH 2:3066e614372f 581 Timer t;
GuillaumeCH 2:3066e614372f 582 t.start();
GuillaumeCH 2:3066e614372f 583 for (int i =0;i<5 ;i++){
GuillaumeCH 2:3066e614372f 584 changement_consigne(consigne_tab[i][0], consigne_tab[i][1]);
GuillaumeCH 3:d38aa400d5e7 585 while(t.read()<0.5f){
GuillaumeCH 2:3066e614372f 586 //actualise_positio n();
GuillaumeCH 2:3066e614372f 587 }
GuillaumeCH 2:3066e614372f 588 //printf("t.read() : %f\n",t.read());
GuillaumeCH 2:3066e614372f 589 //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G);
GuillaumeCH 2:3066e614372f 590 t.reset();
GuillaumeCH 2:3066e614372f 591 }
GuillaumeCH 2:3066e614372f 592 }
GuillaumeCH 2:3066e614372f 593
GuillaumeCH 2:3066e614372f 594 void deplacement::changement_consigne(int cons_D, int cons_G){
GuillaumeCH 2:3066e614372f 595 consigne_D = cons_D;
GuillaumeCH 2:3066e614372f 596 consigne_G = cons_G;
GuillaumeCH 2:3066e614372f 597 compteur_glisse = -1;
GuillaumeCH 2:3066e614372f 598 }
GuillaumeCH 2:3066e614372f 599
GuillaumeCH 3:d38aa400d5e7 600
JimmyAREM 5:3638d7e7c5c1 601 void deplacement::poussette(float temps){
GuillaumeCH 2:3066e614372f 602 motors_on();
JimmyAREM 5:3638d7e7c5c1 603 commande_vitesse(100,100);
JimmyAREM 5:3638d7e7c5c1 604 wait_ms(temps);
GuillaumeCH 2:3066e614372f 605 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 606 vitesse_nulle_D(0);
GuillaumeCH 2:3066e614372f 607 motors_stop();
GuillaumeCH 3:d38aa400d5e7 608 }
JimmyAREM 5:3638d7e7c5c1 609
GuillaumeCH 3:d38aa400d5e7 610 void deplacement::initialisation(){
GuillaumeCH 3:d38aa400d5e7 611 init_odometrie();
GuillaumeCH 3:d38aa400d5e7 612 init_hardware();
GuillaumeCH 3:d38aa400d5e7 613 srand(time(NULL));
GuillaumeCH 3:d38aa400d5e7 614 motors_on();
GuillaumeCH 4:eac6746544fb 615 }
GuillaumeCH 4:eac6746544fb 616 void deplacement::arc(Coordonnees p1, Coordonnees p2){
GuillaumeCH 4:eac6746544fb 617 motors_on();
GuillaumeCH 4:eac6746544fb 618 actualise_position();
GuillaumeCH 4:eac6746544fb 619 float vitesse_G;
GuillaumeCH 4:eac6746544fb 620 float vitesse_D;
GuillaumeCH 4:eac6746544fb 621 double x_ini = get_x_actuel();
GuillaumeCH 4:eac6746544fb 622 double y_ini = get_y_actuel();
GuillaumeCH 4:eac6746544fb 623 double angle_vise_deg = get_angle();
GuillaumeCH 4:eac6746544fb 624 double angle_vise=angle_vise_deg*3.1416/180.0;
GuillaumeCH 4:eac6746544fb 625 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 626
GuillaumeCH 4:eac6746544fb 627 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 628 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 629
GuillaumeCH 4:eac6746544fb 630 double x_actuel = get_x_actuel();
GuillaumeCH 4:eac6746544fb 631 double y_actuel = get_y_actuel();
GuillaumeCH 4:eac6746544fb 632
GuillaumeCH 4:eac6746544fb 633
GuillaumeCH 4:eac6746544fb 634 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 4:eac6746544fb 635 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 4:eac6746544fb 636
GuillaumeCH 4:eac6746544fb 637 Coordonnees p0;
GuillaumeCH 4:eac6746544fb 638 p0.x = x_local;
GuillaumeCH 4:eac6746544fb 639 p0.y = y_local;
JimmyAREM 5:3638d7e7c5c1 640 //printf("x_local : %lf, y_local : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 641 cercle(p0,p1,p2);
JimmyAREM 5:3638d7e7c5c1 642 double xc = point[0];
JimmyAREM 5:3638d7e7c5c1 643 double yc = point[1];
JimmyAREM 5:3638d7e7c5c1 644 double Rc = point[2];
GuillaumeCH 4:eac6746544fb 645
JimmyAREM 5:3638d7e7c5c1 646 double angle_tan;
JimmyAREM 5:3638d7e7c5c1 647 /*if (yc != 0.0){
JimmyAREM 5:3638d7e7c5c1 648 angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 649 }
JimmyAREM 5:3638d7e7c5c1 650 else {
JimmyAREM 5:3638d7e7c5c1 651 if (p1.y > 0){
JimmyAREM 5:3638d7e7c5c1 652 angle_tan = 90.0;
JimmyAREM 5:3638d7e7c5c1 653 }
JimmyAREM 5:3638d7e7c5c1 654 else if (p1.y <0){
JimmyAREM 5:3638d7e7c5c1 655 angle_tan = -90.0;
JimmyAREM 5:3638d7e7c5c1 656 }
JimmyAREM 5:3638d7e7c5c1 657 }*/
JimmyAREM 5:3638d7e7c5c1 658 printf("x : %lf, y : %lf bite ",p0.x,p0.y);
JimmyAREM 5:3638d7e7c5c1 659 double angle_a_parcourir = recup_angle_entre_trois_points_213(xc,yc,p0.x,p0.y,p2.x,p2.y);
JimmyAREM 5:3638d7e7c5c1 660 if (angle_a_parcourir >179.999){
JimmyAREM 5:3638d7e7c5c1 661 if (recup_angle_entre_trois_points_213(xc,yc,p1.x,p1.y,p2.x,p2.y)<0){
JimmyAREM 5:3638d7e7c5c1 662 angle_a_parcourir = -179.999;
GuillaumeCH 4:eac6746544fb 663 }
GuillaumeCH 4:eac6746544fb 664 }
JimmyAREM 5:3638d7e7c5c1 665 /*if (angle_a_parcourir < 0){
JimmyAREM 5:3638d7e7c5c1 666 angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 667 }
JimmyAREM 5:3638d7e7c5c1 668 else{
JimmyAREM 5:3638d7e7c5c1 669 angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 670 }*/
JimmyAREM 5:3638d7e7c5c1 671 angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 672 rotation_abs(angle_tan);
JimmyAREM 5:3638d7e7c5c1 673 actualise_position();
JimmyAREM 5:3638d7e7c5c1 674 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 675 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 676 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 677 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 678 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 5:3638d7e7c5c1 679 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));
JimmyAREM 5:3638d7e7c5c1 680 printf("x_loc : %lf, y_loc : %lf, angle : %lf,angle_tan : %lf,angle_a_parcourir : %lf\n",x_local,y_local,angle,angle_tan,angle_a_parcourir);
JimmyAREM 5:3638d7e7c5c1 681 motors_on();
JimmyAREM 5:3638d7e7c5c1 682 if (angle_a_parcourir < 0){
JimmyAREM 5:3638d7e7c5c1 683 while(!(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)>0 && abs(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y))<100)){
JimmyAREM 5:3638d7e7c5c1 684 //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y));
JimmyAREM 5:3638d7e7c5c1 685 vitesse_D = -15*recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 5:3638d7e7c5c1 686 if (vitesse_D>300){
JimmyAREM 5:3638d7e7c5c1 687 vitesse_D = 300;
JimmyAREM 5:3638d7e7c5c1 688 }
JimmyAREM 5:3638d7e7c5c1 689 vitesse_G = vitesse_D*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
JimmyAREM 5:3638d7e7c5c1 690 double correction = int_ext_cercle(x_actuel,y_actuel);
JimmyAREM 5:3638d7e7c5c1 691 //printf("correction : %lf",correction);
JimmyAREM 5:3638d7e7c5c1 692 //printf("diff_angle : %lf ",diff_angle(angle_tan,angle));
JimmyAREM 5:3638d7e7c5c1 693 vitesse_G= vitesse_G+correction*0.02 + 0.8*diff_angle(angle_tan,angle);
JimmyAREM 5:3638d7e7c5c1 694 vitesse_D= vitesse_D-correction*0.02 - 0.8*diff_angle(angle_tan,angle);
JimmyAREM 5:3638d7e7c5c1 695 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 696 //printf("vitesse_G : %f, vitesse_D : %f ",vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 697 actualise_position();
JimmyAREM 5:3638d7e7c5c1 698 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 699 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 700 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 701 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 702 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 5:3638d7e7c5c1 703 if (y_local >=0){
JimmyAREM 5:3638d7e7c5c1 704 angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 705 }
JimmyAREM 5:3638d7e7c5c1 706 else{
JimmyAREM 5:3638d7e7c5c1 707 angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 708 }
JimmyAREM 5:3638d7e7c5c1 709 //angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 710 //printf("x_loc : %lf, y_loc : %lf, angle : %lf,angle_tan : %lf\n",x_local,y_local,angle,angle_tan);
JimmyAREM 5:3638d7e7c5c1 711 }
JimmyAREM 5:3638d7e7c5c1 712 }
JimmyAREM 5:3638d7e7c5c1 713 if (angle_a_parcourir>= 0){//arc vers la droite
JimmyAREM 5:3638d7e7c5c1 714 //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y));
JimmyAREM 5:3638d7e7c5c1 715 while(!(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y)<0 && abs(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y))<100)){
JimmyAREM 5:3638d7e7c5c1 716 printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y));
JimmyAREM 5:3638d7e7c5c1 717 vitesse_G = 15*(recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y));
JimmyAREM 5:3638d7e7c5c1 718 if (abs(vitesse_G)>300){
JimmyAREM 5:3638d7e7c5c1 719 vitesse_G = 300;
JimmyAREM 5:3638d7e7c5c1 720 }
JimmyAREM 5:3638d7e7c5c1 721 vitesse_D = vitesse_G*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
JimmyAREM 5:3638d7e7c5c1 722 double correction = int_ext_cercle(x_local,y_local);
JimmyAREM 5:3638d7e7c5c1 723 //printf("correction : %lf",correction);
JimmyAREM 5:3638d7e7c5c1 724 //printf("diff_angle : %lf",diff_angle(angle_tan,angle));
JimmyAREM 5:3638d7e7c5c1 725 vitesse_D= vitesse_D+correction*0.02 - 0.8*diff_angle(angle_tan,angle);
JimmyAREM 5:3638d7e7c5c1 726 vitesse_G= vitesse_G-correction*0.02 + 0.8*diff_angle(angle_tan,angle);
JimmyAREM 5:3638d7e7c5c1 727 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 728 //printf("vitesse_G : %f, vitesse_D : %f",vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 729 actualise_position();
JimmyAREM 5:3638d7e7c5c1 730 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 731 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 732 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 733 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 734 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 5:3638d7e7c5c1 735
JimmyAREM 5:3638d7e7c5c1 736 if (y_local >=0){
JimmyAREM 5:3638d7e7c5c1 737 angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 738 }
JimmyAREM 5:3638d7e7c5c1 739 else{
JimmyAREM 5:3638d7e7c5c1 740 angle_tan = atan((x_local-xc)/(yc-y_local))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 741 }
JimmyAREM 5:3638d7e7c5c1 742 //printf("x_loc : %lf, y_loc : %lf, angle : %lf,angle_tan : %lf\n",x_local,y_local,angle,angle_tan);
GuillaumeCH 4:eac6746544fb 743
GuillaumeCH 4:eac6746544fb 744 }
GuillaumeCH 4:eac6746544fb 745 }
JimmyAREM 5:3638d7e7c5c1 746 //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y));
JimmyAREM 5:3638d7e7c5c1 747 //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 748 vitesse_nulle_D(0);
GuillaumeCH 4:eac6746544fb 749 vitesse_nulle_G(0);
GuillaumeCH 4:eac6746544fb 750 wait(0.2);
GuillaumeCH 4:eac6746544fb 751 motors_stop();
GuillaumeCH 4:eac6746544fb 752
GuillaumeCH 4:eac6746544fb 753
GuillaumeCH 4:eac6746544fb 754 }
GuillaumeCH 4:eac6746544fb 755
GuillaumeCH 4:eac6746544fb 756
GuillaumeCH 4:eac6746544fb 757 int deplacement::cercle(Coordonnees a,Coordonnees b, Coordonnees c){
GuillaumeCH 4:eac6746544fb 758 double ax2 = pow(a.x,2);
GuillaumeCH 4:eac6746544fb 759 double ay2 = pow(a.y,2);
GuillaumeCH 4:eac6746544fb 760 double bx2 = pow(b.x,2);
GuillaumeCH 4:eac6746544fb 761 double by2 = pow(b.y,2);
GuillaumeCH 4:eac6746544fb 762 double cx2 = pow(c.x,2);
GuillaumeCH 4:eac6746544fb 763 double cy2 = pow(c.y,2);
JimmyAREM 5:3638d7e7c5c1 764 double pente_a=0;
GuillaumeCH 4:eac6746544fb 765 double ord_b;
JimmyAREM 5:3638d7e7c5c1 766 double pente_A=0;
GuillaumeCH 4:eac6746544fb 767 double ord_B;
GuillaumeCH 4:eac6746544fb 768 double xc;
GuillaumeCH 4:eac6746544fb 769 double yc;
GuillaumeCH 4:eac6746544fb 770 double Rc;
GuillaumeCH 4:eac6746544fb 771 if (a.y == b.y && b.y == c.y){
GuillaumeCH 4:eac6746544fb 772 printf("Les points sont alignes1\n");
GuillaumeCH 4:eac6746544fb 773 return 0;
GuillaumeCH 4:eac6746544fb 774 }
GuillaumeCH 4:eac6746544fb 775 if (b.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 776 pente_a = -1*(b.x-a.x)/(b.y-a.y);
GuillaumeCH 4:eac6746544fb 777 printf("a : %lf ",pente_a);
GuillaumeCH 4:eac6746544fb 778 ord_b = (by2-ay2+bx2-ax2)/(2*(b.y-a.y));
GuillaumeCH 4:eac6746544fb 779 printf("b : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 780
GuillaumeCH 4:eac6746544fb 781 }
GuillaumeCH 4:eac6746544fb 782 else{
JimmyAREM 5:3638d7e7c5c1 783 cercle(c,a,b);
JimmyAREM 5:3638d7e7c5c1 784 return 0;
GuillaumeCH 4:eac6746544fb 785
GuillaumeCH 4:eac6746544fb 786 }
GuillaumeCH 4:eac6746544fb 787 if (c.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 788 pente_A = -1*(c.x-a.x)/(c.y-a.y);
GuillaumeCH 4:eac6746544fb 789 printf("A : %lf ",pente_A);
GuillaumeCH 4:eac6746544fb 790 ord_B = (cy2-ay2+cx2-ax2)/(2*(c.y-a.y));
GuillaumeCH 4:eac6746544fb 791 printf("B : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 792
GuillaumeCH 4:eac6746544fb 793 }
GuillaumeCH 4:eac6746544fb 794 else{
JimmyAREM 5:3638d7e7c5c1 795 cercle(b,c,a);
JimmyAREM 5:3638d7e7c5c1 796 return 0;
GuillaumeCH 4:eac6746544fb 797 }
GuillaumeCH 4:eac6746544fb 798
GuillaumeCH 4:eac6746544fb 799
GuillaumeCH 4:eac6746544fb 800 if (pente_a-pente_A!=0){
GuillaumeCH 4:eac6746544fb 801 xc = (ord_B-ord_b)/(pente_a-pente_A);
GuillaumeCH 4:eac6746544fb 802 yc = pente_a*xc+ord_b;
GuillaumeCH 4:eac6746544fb 803 Rc = pow((pow(xc-a.x,2)+pow(yc-a.y,2)),0.5);
GuillaumeCH 4:eac6746544fb 804 point[0] = xc;
GuillaumeCH 4:eac6746544fb 805 point[1] = yc;
GuillaumeCH 4:eac6746544fb 806 point[2] = Rc;
GuillaumeCH 4:eac6746544fb 807 printf("xc : %f, yc : %f, Rc : %f\n",xc,yc,Rc);
GuillaumeCH 4:eac6746544fb 808 }
GuillaumeCH 4:eac6746544fb 809 else{
GuillaumeCH 4:eac6746544fb 810 printf("Les points sont alignes2\n");
GuillaumeCH 4:eac6746544fb 811 return 0;
GuillaumeCH 4:eac6746544fb 812 }
GuillaumeCH 4:eac6746544fb 813
GuillaumeCH 4:eac6746544fb 814
GuillaumeCH 4:eac6746544fb 815 return 0;
GuillaumeCH 4:eac6746544fb 816 }
GuillaumeCH 4:eac6746544fb 817 double deplacement::int_ext_cercle(double x, double y){
GuillaumeCH 4:eac6746544fb 818 double xc= point[0];
GuillaumeCH 4:eac6746544fb 819 double yc= point[1];
GuillaumeCH 4:eac6746544fb 820 double Rc= point[2];
GuillaumeCH 4:eac6746544fb 821 double rayon = pow((pow(xc-x,2)+pow(yc-y,2)),0.5);
GuillaumeCH 4:eac6746544fb 822 return Rc-rayon;
GuillaumeCH 4:eac6746544fb 823
GuillaumeCH 4:eac6746544fb 824 }
GuillaumeCH 4:eac6746544fb 825
GuillaumeCH 4:eac6746544fb 826 void deplacement::va_au_point(double x,double y, double cap){
GuillaumeCH 4:eac6746544fb 827 actualise_position();
GuillaumeCH 4:eac6746544fb 828 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 829 double x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 830 double y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 831 double x_projete = 10000.0*cos(angle*3.1416/180.0)+x_robot;
GuillaumeCH 4:eac6746544fb 832 double y_projete = 10000.0*sin(angle*3.1416/180.0)+y_robot;
JimmyAREM 5:3638d7e7c5c1 833 printf("angle ::: %lf\n",recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete));
JimmyAREM 5:3638d7e7c5c1 834 rotation_rel(recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete));
GuillaumeCH 4:eac6746544fb 835 //printf("oui\n");
GuillaumeCH 4:eac6746544fb 836 actualise_position();
GuillaumeCH 4:eac6746544fb 837 angle = get_angle();
GuillaumeCH 4:eac6746544fb 838 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 839 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 840 double distance = pow(pow(x-x_robot,2)+pow(y-y_robot,2),0.5);
GuillaumeCH 4:eac6746544fb 841 printf("distance : %lf\n",distance);
GuillaumeCH 4:eac6746544fb 842 ligne_droite(distance);
GuillaumeCH 4:eac6746544fb 843 actualise_position();
GuillaumeCH 4:eac6746544fb 844 angle = get_angle();
GuillaumeCH 4:eac6746544fb 845 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 846 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 847 rotation_abs(cap);
GuillaumeCH 4:eac6746544fb 848
GuillaumeCH 4:eac6746544fb 849
GuillaumeCH 4:eac6746544fb 850 }
GuillaumeCH 4:eac6746544fb 851
GuillaumeCH 4:eac6746544fb 852 double deplacement::recup_angle_entre_trois_points_213(double x1,double y1,double x2,double y2,double x3,double y3){
JimmyAREM 5:3638d7e7c5c1 853 double x13 = x3-x1;
JimmyAREM 5:3638d7e7c5c1 854 double y13 = y3-y1;
GuillaumeCH 4:eac6746544fb 855 double x12 = x2-x1;
GuillaumeCH 4:eac6746544fb 856 double y12 = y2-y1;
GuillaumeCH 4:eac6746544fb 857 double norme12 = pow(x12*x12+y12*y12,0.5);
GuillaumeCH 4:eac6746544fb 858 double norme13 = pow(x13*x13+y13*y13,0.5);
JimmyAREM 5:3638d7e7c5c1 859 double ux = x13/norme13;
JimmyAREM 5:3638d7e7c5c1 860 double uy = y13/norme13;
JimmyAREM 5:3638d7e7c5c1 861 double wx = x12/norme12;
JimmyAREM 5:3638d7e7c5c1 862 double wy = y12/norme12;
JimmyAREM 5:3638d7e7c5c1 863 //printf("u : %lf,%lf ,v : %lf,%lf ,w : %lf,%lf\n",ux,uy,-uy,ux,wx,wy);
GuillaumeCH 4:eac6746544fb 864 double prod_scal = x12*x13+y12*y13;
JimmyAREM 5:3638d7e7c5c1 865 double cos_angle = prod_scal/(norme12*norme13);
JimmyAREM 5:3638d7e7c5c1 866 double sin_angle;
JimmyAREM 5:3638d7e7c5c1 867 if (uy!=0){
JimmyAREM 5:3638d7e7c5c1 868 sin_angle = -1*(wx - cos_angle*ux)/uy;
JimmyAREM 5:3638d7e7c5c1 869 }
JimmyAREM 5:3638d7e7c5c1 870 else{
JimmyAREM 5:3638d7e7c5c1 871 sin_angle = (wy - cos_angle*uy)/ux;
JimmyAREM 5:3638d7e7c5c1 872 }
JimmyAREM 5:3638d7e7c5c1 873 //printf("cos : %lf sin : %lf\n",cos_angle,sin_angle);
JimmyAREM 5:3638d7e7c5c1 874 if (sin_angle >=0){
JimmyAREM 5:3638d7e7c5c1 875 return acos(prod_scal/(norme12*norme13))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 876 }
JimmyAREM 5:3638d7e7c5c1 877 else{
JimmyAREM 5:3638d7e7c5c1 878 return -acos(prod_scal/(norme12*norme13))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 879 }
GuillaumeCH 4:eac6746544fb 880 }
JimmyAREM 5:3638d7e7c5c1 881