Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

deplacement.cpp@7:6b15a1feed2d, 2019-05-26 (annotated)

Committer:
JimmyAREM
Date:
Sun May 26 14:57:54 2019 +0000
Revision:
7:6b15a1feed2d
Parent:
6:e1585b8bd07d
Child:
8:f2425e4302fc
a

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 6:e1585b8bd07d 11
JimmyAREM 5:3638d7e7c5c1 12 extern Serial pc;
JimmyAREM 5:3638d7e7c5c1 13 extern bool detectionUltrasons;
JimmyAREM 5:3638d7e7c5c1 14 bool stopCapteurs = false;
JimmyAREM 6:e1585b8bd07d 15 extern bool typeEvitement;
JimmyAREM 5:3638d7e7c5c1 16
JimmyAREM 5:3638d7e7c5c1 17 extern int distanceUltrasonGauche;
JimmyAREM 5:3638d7e7c5c1 18 extern int distanceUltrasonDroit;
JimmyAREM 5:3638d7e7c5c1 19 extern int distanceUltrasonArriere;
JimmyAREM 5:3638d7e7c5c1 20
JimmyAREM 5:3638d7e7c5c1 21 extern double posFinalRobotX;
JimmyAREM 5:3638d7e7c5c1 22 extern double posFinalRobotY;
JimmyAREM 5:3638d7e7c5c1 23
JimmyAREM 5:3638d7e7c5c1 24 extern double posInterRobotX;
JimmyAREM 5:3638d7e7c5c1 25 extern double posInterRobotY;
JimmyAREM 5:3638d7e7c5c1 26
JimmyAREM 5:3638d7e7c5c1 27 bool evitementValider = false;
JimmyAREM 5:3638d7e7c5c1 28 Coordonnees pointIntermediaire()
JimmyAREM 5:3638d7e7c5c1 29 {
JimmyAREM 5:3638d7e7c5c1 30 Coordonnees P1;
JimmyAREM 5:3638d7e7c5c1 31 double x_init = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 32 double y_init = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 33 P1.x = posInterRobotX *100 - x_init;
JimmyAREM 5:3638d7e7c5c1 34 P1.y = posInterRobotY *100 - y_init;
JimmyAREM 5:3638d7e7c5c1 35 return P1;
JimmyAREM 5:3638d7e7c5c1 36 }
JimmyAREM 5:3638d7e7c5c1 37
JimmyAREM 5:3638d7e7c5c1 38 Coordonnees pointFinale()
JimmyAREM 5:3638d7e7c5c1 39 {
JimmyAREM 5:3638d7e7c5c1 40 Coordonnees P1;
JimmyAREM 5:3638d7e7c5c1 41 double x_init = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 42 double y_init = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 43 P1.x = posFinalRobotX *100 - x_init;
JimmyAREM 5:3638d7e7c5c1 44 P1.y = posFinalRobotY *100 - y_init;
JimmyAREM 5:3638d7e7c5c1 45 return P1;
JimmyAREM 5:3638d7e7c5c1 46 }
GuillaumeCH 2:3066e614372f 47
GuillaumeCH 2:3066e614372f 48 deplacement::deplacement(){
GuillaumeCH 2:3066e614372f 49 consigne_D = 0;
GuillaumeCH 2:3066e614372f 50 consigne_G = 0;
GuillaumeCH 2:3066e614372f 51 somme_erreur_D = 0;
GuillaumeCH 2:3066e614372f 52 somme_erreur_G = 0;
GuillaumeCH 2:3066e614372f 53 erreur_precedente_D = 0;
GuillaumeCH 2:3066e614372f 54 erreur_precedente_G = 0;
GuillaumeCH 2:3066e614372f 55 compteur_asser =0;
GuillaumeCH 2:3066e614372f 56 somme_y=0;
GuillaumeCH 2:3066e614372f 57
GuillaumeCH 2:3066e614372f 58 for (int k =0; k<5;k++){
GuillaumeCH 2:3066e614372f 59 erreur_glissee_D[k] = 0;
GuillaumeCH 2:3066e614372f 60 erreur_glissee_G[k] = 0;
GuillaumeCH 2:3066e614372f 61 }
GuillaumeCH 2:3066e614372f 62 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 63
GuillaumeCH 2:3066e614372f 64 Kp_D = 1.5;//1
GuillaumeCH 2:3066e614372f 65 Ki_D = 0.12;//0.15
GuillaumeCH 2:3066e614372f 66 Kd_D = 0.5;//1
GuillaumeCH 2:3066e614372f 67
GuillaumeCH 2:3066e614372f 68 Kp_G = 1;//1
GuillaumeCH 2:3066e614372f 69 Ki_G = 0.13;//0.15
GuillaumeCH 2:3066e614372f 70 Kd_G = 1.2;//1
GuillaumeCH 2:3066e614372f 71
GuillaumeCH 2:3066e614372f 72 tick_prec_D=0;
GuillaumeCH 2:3066e614372f 73 tick_prec_G = 0;
GuillaumeCH 2:3066e614372f 74 dix_ms = 0;
GuillaumeCH 2:3066e614372f 75 for (int k =0; k<TAILLE_TAB;k++){
GuillaumeCH 2:3066e614372f 76 tab_cmd_G[k]=0;
GuillaumeCH 2:3066e614372f 77 tab_cmd_D[k]=0;
GuillaumeCH 2:3066e614372f 78 vtab_G[k]=0;
GuillaumeCH 2:3066e614372f 79 vtab_D[k]=0;
GuillaumeCH 2:3066e614372f 80 c_D[k]=0;
GuillaumeCH 2:3066e614372f 81 c_G[k]=0;
GuillaumeCH 2:3066e614372f 82 }
GuillaumeCH 2:3066e614372f 83 consigne_tab[0][0]=0;
GuillaumeCH 2:3066e614372f 84 consigne_tab[0][1]=0;
GuillaumeCH 2:3066e614372f 85
GuillaumeCH 2:3066e614372f 86 consigne_tab[1][0]=10;
GuillaumeCH 2:3066e614372f 87 consigne_tab[1][1]=10;
GuillaumeCH 2:3066e614372f 88
GuillaumeCH 2:3066e614372f 89 consigne_tab[2][0]=20;
GuillaumeCH 2:3066e614372f 90 consigne_tab[2][1]=20;
GuillaumeCH 2:3066e614372f 91
GuillaumeCH 2:3066e614372f 92 consigne_tab[3][0]=30;
GuillaumeCH 2:3066e614372f 93 consigne_tab[3][1]=30;
GuillaumeCH 2:3066e614372f 94
GuillaumeCH 2:3066e614372f 95 consigne_tab[4][0]=40;
GuillaumeCH 2:3066e614372f 96 consigne_tab[4][1]=40;
GuillaumeCH 2:3066e614372f 97
GuillaumeCH 2:3066e614372f 98 /* consigne_tab[5][0]=3*5;
GuillaumeCH 2:3066e614372f 99 consigne_tab[5][1]=3*5;
GuillaumeCH 2:3066e614372f 100
GuillaumeCH 2:3066e614372f 101 consigne_tab[6][0]=3*6;
GuillaumeCH 2:3066e614372f 102 consigne_tab[6][1]=3*6;
GuillaumeCH 2:3066e614372f 103
GuillaumeCH 2:3066e614372f 104 consigne_tab[7][0]=3*7;
GuillaumeCH 2:3066e614372f 105 consigne_tab[7][1]=3*7;
GuillaumeCH 2:3066e614372f 106
GuillaumeCH 2:3066e614372f 107 consigne_tab[8][0]=3*8;
GuillaumeCH 2:3066e614372f 108 consigne_tab[8][1]=3*8;
GuillaumeCH 2:3066e614372f 109
GuillaumeCH 2:3066e614372f 110 consigne_tab[9][0]=3*9;
GuillaumeCH 2:3066e614372f 111 consigne_tab[9][1]=3*9;
GuillaumeCH 2:3066e614372f 112
GuillaumeCH 2:3066e614372f 113 consigne_tab[10][0]=3*10;
GuillaumeCH 2:3066e614372f 114 consigne_tab[10][1]=3*10;
GuillaumeCH 2:3066e614372f 115
GuillaumeCH 2:3066e614372f 116 consigne_tab[11][0]=3*11;
GuillaumeCH 2:3066e614372f 117 consigne_tab[11][1]=3*11;
GuillaumeCH 2:3066e614372f 118
GuillaumeCH 2:3066e614372f 119 consigne_tab[12][0]=3*12;
GuillaumeCH 2:3066e614372f 120 consigne_tab[12][1]=3*12;
GuillaumeCH 2:3066e614372f 121
GuillaumeCH 2:3066e614372f 122 consigne_tab[13][0]=3*13;
GuillaumeCH 2:3066e614372f 123 consigne_tab[13][1]=3*13;
GuillaumeCH 2:3066e614372f 124
GuillaumeCH 2:3066e614372f 125 consigne_tab[14][0]=3*14;
GuillaumeCH 2:3066e614372f 126 consigne_tab[14][1]=3*14;
GuillaumeCH 2:3066e614372f 127
GuillaumeCH 2:3066e614372f 128 consigne_tab[15][0]=0;
GuillaumeCH 2:3066e614372f 129 consigne_tab[15][1]=0;
GuillaumeCH 2:3066e614372f 130
GuillaumeCH 2:3066e614372f 131 consigne_tab[16][0]=0;
GuillaumeCH 2:3066e614372f 132 consigne_tab[16][1]=0;
GuillaumeCH 2:3066e614372f 133
GuillaumeCH 2:3066e614372f 134 consigne_tab[17][0]=0;
GuillaumeCH 2:3066e614372f 135 consigne_tab[17][1]=0;
GuillaumeCH 2:3066e614372f 136
GuillaumeCH 2:3066e614372f 137 consigne_tab[18][0]=0;
GuillaumeCH 2:3066e614372f 138 consigne_tab[18][1]=0;
GuillaumeCH 2:3066e614372f 139
GuillaumeCH 2:3066e614372f 140 consigne_tab[19][0]=0;
GuillaumeCH 2:3066e614372f 141 consigne_tab[19][1]=0;*/
GuillaumeCH 2:3066e614372f 142 }
GuillaumeCH 2:3066e614372f 143
JimmyAREM 6:e1585b8bd07d 144 void deplacement::arreterRobot()
JimmyAREM 6:e1585b8bd07d 145 {
JimmyAREM 6:e1585b8bd07d 146 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 147 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 148 }
JimmyAREM 6:e1585b8bd07d 149
GuillaumeCH 2:3066e614372f 150 void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
GuillaumeCH 2:3066e614372f 151
GuillaumeCH 2:3066e614372f 152 int sens_G=signe(vitesse_G);
GuillaumeCH 2:3066e614372f 153 int sens_D=signe(vitesse_D);
GuillaumeCH 2:3066e614372f 154 double vitesse_local_G=abs(vitesse_G);
GuillaumeCH 2:3066e614372f 155 double vitesse_local_D=abs(vitesse_D);
GuillaumeCH 2:3066e614372f 156
GuillaumeCH 2:3066e614372f 157 if(abs(vitesse_G) > 900){
GuillaumeCH 2:3066e614372f 158 vitesse_local_G=900;
GuillaumeCH 2:3066e614372f 159 }
GuillaumeCH 3:d38aa400d5e7 160 if(abs(vitesse_G)<10){
GuillaumeCH 3:d38aa400d5e7 161 vitesse_local_G=10;
GuillaumeCH 2:3066e614372f 162 }
GuillaumeCH 2:3066e614372f 163 if(abs(vitesse_D) > 900){
GuillaumeCH 2:3066e614372f 164 vitesse_local_D=900;
GuillaumeCH 2:3066e614372f 165 }
GuillaumeCH 3:d38aa400d5e7 166 if(abs(vitesse_D)< 10){
GuillaumeCH 3:d38aa400d5e7 167 vitesse_local_D=10;
GuillaumeCH 2:3066e614372f 168
GuillaumeCH 2:3066e614372f 169 }
GuillaumeCH 2:3066e614372f 170 ;
GuillaumeCH 2:3066e614372f 171 int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 172 int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 173 float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 174 float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 175 float centieme_D = (VD_f-VD_int)*1000;
GuillaumeCH 2:3066e614372f 176 float centieme_G = (VG_f-VG_int)*1000;
GuillaumeCH 2:3066e614372f 177 if ((rand()%1000) < centieme_G){
GuillaumeCH 2:3066e614372f 178 VG_int+=1;
GuillaumeCH 2:3066e614372f 179 }
GuillaumeCH 2:3066e614372f 180 if ((rand()%1000) < centieme_D){
GuillaumeCH 2:3066e614372f 181 VD_int+=1;
GuillaumeCH 2:3066e614372f 182 }
GuillaumeCH 2:3066e614372f 183 //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
GuillaumeCH 2:3066e614372f 184 set_PWM_moteur_G(VG_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
GuillaumeCH 2:3066e614372f 185 set_PWM_moteur_D(VD_int);//
GuillaumeCH 2:3066e614372f 186 }
GuillaumeCH 2:3066e614372f 187 void deplacement::vitesse_nulle_G(int zero){
GuillaumeCH 2:3066e614372f 188 if(zero == 0){
GuillaumeCH 2:3066e614372f 189 set_PWM_moteur_G(0);
GuillaumeCH 2:3066e614372f 190 }
GuillaumeCH 2:3066e614372f 191 }
GuillaumeCH 2:3066e614372f 192 void deplacement::vitesse_nulle_D(int zero){
GuillaumeCH 2:3066e614372f 193 if(zero == 0){
GuillaumeCH 2:3066e614372f 194 set_PWM_moteur_D(0);
GuillaumeCH 2:3066e614372f 195 }
GuillaumeCH 2:3066e614372f 196 }
GuillaumeCH 3:d38aa400d5e7 197 void deplacement::marche_arriere(int distance)
GuillaumeCH 2:3066e614372f 198 {
GuillaumeCH 2:3066e614372f 199 somme_y=0;
GuillaumeCH 2:3066e614372f 200 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 201 motors_on();
GuillaumeCH 2:3066e614372f 202 actualise_position();
GuillaumeCH 2:3066e614372f 203 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 204 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 205 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 206 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 207 double angle = get_angle();
GuillaumeCH 2:3066e614372f 208
GuillaumeCH 2:3066e614372f 209 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 210 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 211
GuillaumeCH 2:3066e614372f 212 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 213 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 214
GuillaumeCH 2:3066e614372f 215
GuillaumeCH 2:3066e614372f 216 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 217 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 218
GuillaumeCH 2:3066e614372f 219 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 220 float vitesse_G;
GuillaumeCH 2:3066e614372f 221 float vitesse_D;
GuillaumeCH 2:3066e614372f 222
GuillaumeCH 2:3066e614372f 223 angle = get_angle();
GuillaumeCH 2:3066e614372f 224 float Kip=0;
GuillaumeCH 2:3066e614372f 225 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 226 float Kdp= 10;
GuillaumeCH 2:3066e614372f 227 while (distance-x_local<0){
JimmyAREM 5:3638d7e7c5c1 228 if(detectionUltrasons)
JimmyAREM 5:3638d7e7c5c1 229 {
JimmyAREM 5:3638d7e7c5c1 230 detectionUltrasons = false;
JimmyAREM 5:3638d7e7c5c1 231 //writeCapteurs();
JimmyAREM 5:3638d7e7c5c1 232 LectureI2CCarteCapteur(*this);
JimmyAREM 5:3638d7e7c5c1 233 }
GuillaumeCH 2:3066e614372f 234 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 235 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 236 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 237 vitesse_G=400;
GuillaumeCH 2:3066e614372f 238 vitesse_D=400;
GuillaumeCH 2:3066e614372f 239 }
GuillaumeCH 2:3066e614372f 240 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 241 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 242 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 243 }
GuillaumeCH 2:3066e614372f 244
GuillaumeCH 2:3066e614372f 245 angle = get_angle();
GuillaumeCH 2:3066e614372f 246
GuillaumeCH 2:3066e614372f 247 vitesse_G = vitesse_G - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
GuillaumeCH 2:3066e614372f 248 vitesse_D = vitesse_D + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
GuillaumeCH 2:3066e614372f 249 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 250 //consigne_G = vitesse_G;
JimmyAREM 5:3638d7e7c5c1 251 if (stopCapteurs == true)
JimmyAREM 5:3638d7e7c5c1 252 {
JimmyAREM 5:3638d7e7c5c1 253 vitesse_nulle_D(0);
JimmyAREM 5:3638d7e7c5c1 254 vitesse_nulle_G(0);
JimmyAREM 5:3638d7e7c5c1 255 }
JimmyAREM 5:3638d7e7c5c1 256 else
JimmyAREM 5:3638d7e7c5c1 257 {
JimmyAREM 5:3638d7e7c5c1 258 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 259 }
GuillaumeCH 2:3066e614372f 260 actualise_position();
GuillaumeCH 2:3066e614372f 261 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 262 y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 263 somme_y+=y_actuel;
GuillaumeCH 2:3066e614372f 264 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 265 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 266 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 267 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 268 compteur_asser=0;
GuillaumeCH 2:3066e614372f 269 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 270 }
GuillaumeCH 2:3066e614372f 271 compteur_asser++;
GuillaumeCH 2:3066e614372f 272 //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 273 }
GuillaumeCH 2:3066e614372f 274 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 275 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 276 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 277 }
GuillaumeCH 2:3066e614372f 278
GuillaumeCH 2:3066e614372f 279
JimmyAREM 6:e1585b8bd07d 280 void deplacement::ligne_droite_basique(long int distance)
GuillaumeCH 2:3066e614372f 281 {
GuillaumeCH 2:3066e614372f 282 somme_y=0;
GuillaumeCH 2:3066e614372f 283 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 284 motors_on();
GuillaumeCH 2:3066e614372f 285 actualise_position();
GuillaumeCH 2:3066e614372f 286 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 287 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 288 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 289 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 290 double angle = get_angle();
GuillaumeCH 2:3066e614372f 291
GuillaumeCH 2:3066e614372f 292 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 293 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 294
GuillaumeCH 2:3066e614372f 295 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 296 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 297
GuillaumeCH 2:3066e614372f 298
GuillaumeCH 2:3066e614372f 299 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 300 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 301
GuillaumeCH 2:3066e614372f 302 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 303 float vitesse_G;
GuillaumeCH 2:3066e614372f 304 float vitesse_D;
GuillaumeCH 2:3066e614372f 305
GuillaumeCH 2:3066e614372f 306 angle = get_angle();
GuillaumeCH 2:3066e614372f 307 float Kip=0;
GuillaumeCH 2:3066e614372f 308 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 309 float Kdp= 10;
GuillaumeCH 2:3066e614372f 310 while (distance-x_local>0){
GuillaumeCH 2:3066e614372f 311
JimmyAREM 5:3638d7e7c5c1 312 if(detectionUltrasons)
JimmyAREM 5:3638d7e7c5c1 313 {
JimmyAREM 5:3638d7e7c5c1 314 detectionUltrasons = false;
JimmyAREM 5:3638d7e7c5c1 315 evitementValider = false;
JimmyAREM 5:3638d7e7c5c1 316 //writeCapteurs();
JimmyAREM 5:3638d7e7c5c1 317 LectureI2CCarteCapteur(*this);
JimmyAREM 5:3638d7e7c5c1 318 }
GuillaumeCH 2:3066e614372f 319 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 320 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 321 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 322 vitesse_G=400;
GuillaumeCH 2:3066e614372f 323 vitesse_D=400;
GuillaumeCH 2:3066e614372f 324 }
GuillaumeCH 2:3066e614372f 325 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 326 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 327 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 328 }
GuillaumeCH 2:3066e614372f 329
GuillaumeCH 2:3066e614372f 330 angle = get_angle();
GuillaumeCH 2:3066e614372f 331
GuillaumeCH 2:3066e614372f 332 vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
GuillaumeCH 2:3066e614372f 333 vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
GuillaumeCH 2:3066e614372f 334 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 335 //consigne_G = vitesse_G;
JimmyAREM 6:e1585b8bd07d 336 if (typeEvitement == ARRET)
JimmyAREM 5:3638d7e7c5c1 337 {
JimmyAREM 6:e1585b8bd07d 338 if(stopCapteurs == true)
JimmyAREM 5:3638d7e7c5c1 339 {
JimmyAREM 5:3638d7e7c5c1 340 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 341 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 342 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 343 }
JimmyAREM 6:e1585b8bd07d 344 else
JimmyAREM 6:e1585b8bd07d 345 {
JimmyAREM 6:e1585b8bd07d 346 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 347 }
JimmyAREM 6:e1585b8bd07d 348 }
JimmyAREM 6:e1585b8bd07d 349 actualise_position();
JimmyAREM 6:e1585b8bd07d 350 x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 351 y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 352 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 353 somme_y+=y_actuel;
JimmyAREM 6:e1585b8bd07d 354 //y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 355 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 356 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 357 if (compteur_asser==150){
JimmyAREM 6:e1585b8bd07d 358 compteur_asser=0;
JimmyAREM 6:e1585b8bd07d 359 //printf("%lf\n",get_y_actuel());
JimmyAREM 6:e1585b8bd07d 360 }
JimmyAREM 6:e1585b8bd07d 361 compteur_asser++;
JimmyAREM 6:e1585b8bd07d 362 //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));
JimmyAREM 6:e1585b8bd07d 363 }
JimmyAREM 6:e1585b8bd07d 364 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 365 rotation_abs(angle_vise_deg);
JimmyAREM 6:e1585b8bd07d 366 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 367 }
JimmyAREM 6:e1585b8bd07d 368
JimmyAREM 6:e1585b8bd07d 369
JimmyAREM 6:e1585b8bd07d 370 void deplacement::ligne_droite(long int distance, double x, double y, double cap)
JimmyAREM 6:e1585b8bd07d 371 {
JimmyAREM 6:e1585b8bd07d 372 somme_y=0;
JimmyAREM 6:e1585b8bd07d 373 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
JimmyAREM 6:e1585b8bd07d 374 motors_on();
JimmyAREM 6:e1585b8bd07d 375 actualise_position();
JimmyAREM 6:e1585b8bd07d 376 double x_ini = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 377 double y_ini = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 378 double angle_vise_deg = get_angle();
JimmyAREM 6:e1585b8bd07d 379 double angle_vise=angle_vise_deg*3.1416/180;
JimmyAREM 6:e1585b8bd07d 380 double angle = get_angle();
JimmyAREM 6:e1585b8bd07d 381
JimmyAREM 6:e1585b8bd07d 382 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 383 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 384
JimmyAREM 6:e1585b8bd07d 385 double x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 386 double y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 387
JimmyAREM 6:e1585b8bd07d 388
JimmyAREM 6:e1585b8bd07d 389 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 390 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 391
JimmyAREM 6:e1585b8bd07d 392 //long int y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 393 float vitesse_G;
JimmyAREM 6:e1585b8bd07d 394 float vitesse_D;
JimmyAREM 6:e1585b8bd07d 395
JimmyAREM 6:e1585b8bd07d 396 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 397 float Kip=0;
JimmyAREM 6:e1585b8bd07d 398 float Kpp= 0.05 ;
JimmyAREM 6:e1585b8bd07d 399 float Kdp= 10;
JimmyAREM 6:e1585b8bd07d 400 while (distance-x_local>0){
JimmyAREM 6:e1585b8bd07d 401
JimmyAREM 6:e1585b8bd07d 402 if(detectionUltrasons)
JimmyAREM 6:e1585b8bd07d 403 {
JimmyAREM 6:e1585b8bd07d 404 detectionUltrasons = false;
JimmyAREM 6:e1585b8bd07d 405 evitementValider = false;
JimmyAREM 6:e1585b8bd07d 406 //writeCapteurs();
JimmyAREM 6:e1585b8bd07d 407 LectureI2CCarteCapteur(*this);
JimmyAREM 6:e1585b8bd07d 408 }
JimmyAREM 6:e1585b8bd07d 409 vitesse_G = (distance-x_local)/70;
JimmyAREM 6:e1585b8bd07d 410 vitesse_D = vitesse_G;
JimmyAREM 6:e1585b8bd07d 411 if(vitesse_G >400){
JimmyAREM 6:e1585b8bd07d 412 vitesse_G=400;
JimmyAREM 6:e1585b8bd07d 413 vitesse_D=400;
JimmyAREM 6:e1585b8bd07d 414 }
JimmyAREM 6:e1585b8bd07d 415 if (vitesse_G<-400){
JimmyAREM 6:e1585b8bd07d 416 vitesse_G=-400;
JimmyAREM 6:e1585b8bd07d 417 vitesse_D=-400;
JimmyAREM 6:e1585b8bd07d 418 }
JimmyAREM 6:e1585b8bd07d 419
JimmyAREM 6:e1585b8bd07d 420 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 421
JimmyAREM 6:e1585b8bd07d 422 vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle) + Kip*somme_y;
JimmyAREM 6:e1585b8bd07d 423 vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle) - Kip*somme_y;
JimmyAREM 6:e1585b8bd07d 424 //consigne_D = vitesse_D;
JimmyAREM 6:e1585b8bd07d 425 //consigne_G = vitesse_G;
JimmyAREM 6:e1585b8bd07d 426 if (typeEvitement == ARRET)
JimmyAREM 6:e1585b8bd07d 427 {
JimmyAREM 6:e1585b8bd07d 428 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 429 {
JimmyAREM 6:e1585b8bd07d 430 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 431 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 432 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 433 }
JimmyAREM 6:e1585b8bd07d 434 else
JimmyAREM 6:e1585b8bd07d 435 {
JimmyAREM 6:e1585b8bd07d 436 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 437 }
JimmyAREM 6:e1585b8bd07d 438 }
JimmyAREM 6:e1585b8bd07d 439 else
JimmyAREM 6:e1585b8bd07d 440 {
JimmyAREM 6:e1585b8bd07d 441 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 442 {
JimmyAREM 6:e1585b8bd07d 443 evitement(x, y, cap);
JimmyAREM 6:e1585b8bd07d 444 return;
JimmyAREM 5:3638d7e7c5c1 445 }
JimmyAREM 5:3638d7e7c5c1 446 else
JimmyAREM 5:3638d7e7c5c1 447 {
JimmyAREM 6:e1585b8bd07d 448 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 449 }
JimmyAREM 5:3638d7e7c5c1 450 }
GuillaumeCH 2:3066e614372f 451 actualise_position();
GuillaumeCH 2:3066e614372f 452 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 453 y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 454 somme_y+=y_actuel;
GuillaumeCH 2:3066e614372f 455 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 456 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 457 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 458 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 459 compteur_asser=0;
GuillaumeCH 2:3066e614372f 460 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 461 }
GuillaumeCH 2:3066e614372f 462 compteur_asser++;
GuillaumeCH 2:3066e614372f 463 //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 464 }
GuillaumeCH 2:3066e614372f 465 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 466 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 467 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 468 }
GuillaumeCH 2:3066e614372f 469
GuillaumeCH 3:d38aa400d5e7 470 void deplacement::rotation_rel(double angle_vise)
GuillaumeCH 2:3066e614372f 471 {
GuillaumeCH 2:3066e614372f 472 // rotation de angle_vise
GuillaumeCH 2:3066e614372f 473 motors_on();
GuillaumeCH 2:3066e614372f 474 double vitesse=180;
GuillaumeCH 2:3066e614372f 475 int sens;
GuillaumeCH 2:3066e614372f 476 double angle = get_angle();
GuillaumeCH 2:3066e614372f 477 angle_vise+=angle;
GuillaumeCH 2:3066e614372f 478 borne_angle_d(angle_vise);
GuillaumeCH 2:3066e614372f 479 if (diff_angle(angle,angle_vise)<=0){
GuillaumeCH 2:3066e614372f 480 sens = -1;
GuillaumeCH 2:3066e614372f 481 //printf("negatif\n");
GuillaumeCH 2:3066e614372f 482 }
GuillaumeCH 2:3066e614372f 483 else{
GuillaumeCH 2:3066e614372f 484 sens = 1;
GuillaumeCH 2:3066e614372f 485
GuillaumeCH 2:3066e614372f 486 //printf("positif\n");
GuillaumeCH 2:3066e614372f 487 }
GuillaumeCH 2:3066e614372f 488 //printf("diff : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 489 while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
GuillaumeCH 2:3066e614372f 490 {
GuillaumeCH 2:3066e614372f 491 actualise_position();
GuillaumeCH 2:3066e614372f 492 angle = get_angle();
GuillaumeCH 2:3066e614372f 493 vitesse=1.5*sens*abs(diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 494
GuillaumeCH 2:3066e614372f 495 commande_vitesse(-vitesse,vitesse);
GuillaumeCH 2:3066e614372f 496 if (compteur_asser==150){
GuillaumeCH 2:3066e614372f 497 compteur_asser=0;
GuillaumeCH 2:3066e614372f 498 //printf("%lf\n",get_y_actuel());
GuillaumeCH 2:3066e614372f 499 }
GuillaumeCH 2:3066e614372f 500 compteur_asser++;
GuillaumeCH 2:3066e614372f 501 //printf("vitesse : %lf ", vitesse);
GuillaumeCH 2:3066e614372f 502 }
GuillaumeCH 2:3066e614372f 503 //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 504 //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 505 //consigne_D = 0;
GuillaumeCH 2:3066e614372f 506 //consigne_G = 0;
GuillaumeCH 2:3066e614372f 507 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 508 vitesse_nulle_D(0);
GuillaumeCH 3:d38aa400d5e7 509 wait(0.1);
GuillaumeCH 2:3066e614372f 510 motors_stop();
GuillaumeCH 2:3066e614372f 511 }
GuillaumeCH 2:3066e614372f 512
GuillaumeCH 2:3066e614372f 513
GuillaumeCH 3:d38aa400d5e7 514 void deplacement::rotation_abs(double angle_vise)
GuillaumeCH 2:3066e614372f 515 {
GuillaumeCH 2:3066e614372f 516 actualise_position();
GuillaumeCH 2:3066e614372f 517 //printf("bite");
GuillaumeCH 2:3066e614372f 518 double angle_rel = borne_angle_d(angle_vise-get_angle());
GuillaumeCH 3:d38aa400d5e7 519 rotation_rel(angle_rel);
GuillaumeCH 2:3066e614372f 520 }
GuillaumeCH 2:3066e614372f 521
GuillaumeCH 2:3066e614372f 522 void deplacement::asservissement(){
GuillaumeCH 2:3066e614372f 523 long int tick_D = get_nbr_tick_D();
GuillaumeCH 2:3066e614372f 524 long int tick_G = get_nbr_tick_G();
GuillaumeCH 2:3066e614372f 525
GuillaumeCH 2:3066e614372f 526 long int tick_D_passe = tick_D-tick_prec_D;
GuillaumeCH 2:3066e614372f 527 long int tick_G_passe = tick_G-tick_prec_G;
GuillaumeCH 2:3066e614372f 528
GuillaumeCH 2:3066e614372f 529 tick_prec_D=tick_D;
GuillaumeCH 2:3066e614372f 530 tick_prec_G=tick_G;
GuillaumeCH 2:3066e614372f 531
GuillaumeCH 2:3066e614372f 532 float vitesse_codeuse_D = tick_D_passe;
GuillaumeCH 2:3066e614372f 533 float vitesse_codeuse_G = tick_G_passe;
GuillaumeCH 2:3066e614372f 534
GuillaumeCH 2:3066e614372f 535 float erreur_D = (float) consigne_D - (float) vitesse_codeuse_D;
GuillaumeCH 2:3066e614372f 536 float erreur_G = (float) consigne_G - (float) vitesse_codeuse_G;
GuillaumeCH 2:3066e614372f 537
GuillaumeCH 2:3066e614372f 538 if (compteur_glisse == 5)
GuillaumeCH 2:3066e614372f 539 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 540
GuillaumeCH 2:3066e614372f 541 if (compteur_glisse == -1)
GuillaumeCH 2:3066e614372f 542 {
GuillaumeCH 2:3066e614372f 543 compteur_glisse = 0;
GuillaumeCH 2:3066e614372f 544 for (int i = 0; i<5; i++){
GuillaumeCH 2:3066e614372f 545 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 2:3066e614372f 546 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 2:3066e614372f 547 }
GuillaumeCH 2:3066e614372f 548 }
GuillaumeCH 2:3066e614372f 549
GuillaumeCH 2:3066e614372f 550 erreur_glissee_D[compteur_glisse] = erreur_D;
GuillaumeCH 2:3066e614372f 551 erreur_glissee_G[compteur_glisse] = erreur_G;
GuillaumeCH 2:3066e614372f 552 compteur_glisse++;
GuillaumeCH 2:3066e614372f 553
GuillaumeCH 2:3066e614372f 554 erreur_D = erreur_glissee_D[0];
GuillaumeCH 2:3066e614372f 555 erreur_G = erreur_glissee_G[0];
GuillaumeCH 2:3066e614372f 556 for (int i=1; i<5; i++)
GuillaumeCH 2:3066e614372f 557 {
GuillaumeCH 2:3066e614372f 558 erreur_D += erreur_glissee_D[i];
GuillaumeCH 2:3066e614372f 559 erreur_G += erreur_glissee_G[i];
GuillaumeCH 2:3066e614372f 560 }
GuillaumeCH 2:3066e614372f 561
GuillaumeCH 3:d38aa400d5e7 562 erreur_D = erreur_D/5.0f;
GuillaumeCH 3:d38aa400d5e7 563 erreur_G = erreur_G/5.0f; // erreur est maintenant la moyenne des 5 erreurs prec
GuillaumeCH 2:3066e614372f 564
GuillaumeCH 2:3066e614372f 565 somme_erreur_D += erreur_D;
GuillaumeCH 2:3066e614372f 566 somme_erreur_G += erreur_G;
GuillaumeCH 2:3066e614372f 567
GuillaumeCH 2:3066e614372f 568 float delta_erreur_D = erreur_D-erreur_precedente_D;
GuillaumeCH 2:3066e614372f 569 float delta_erreur_G = erreur_G-erreur_precedente_G;
GuillaumeCH 2:3066e614372f 570
GuillaumeCH 2:3066e614372f 571 erreur_precedente_G = erreur_G;
GuillaumeCH 2:3066e614372f 572 erreur_precedente_D = erreur_D;
GuillaumeCH 2:3066e614372f 573
GuillaumeCH 2:3066e614372f 574 float cmd_D = Kp_D*erreur_D+Ki_D*somme_erreur_D + Kd_D*delta_erreur_D;
GuillaumeCH 2:3066e614372f 575 float cmd_G = Kp_G*erreur_G+Ki_G*somme_erreur_G + Kd_G*delta_erreur_G;
GuillaumeCH 2:3066e614372f 576
GuillaumeCH 2:3066e614372f 577 if (cmd_G <0){
GuillaumeCH 2:3066e614372f 578 cmd_G = 0;
GuillaumeCH 2:3066e614372f 579 }
GuillaumeCH 2:3066e614372f 580 if (cmd_G > 500){
GuillaumeCH 2:3066e614372f 581 cmd_G = 500;
GuillaumeCH 2:3066e614372f 582 }
GuillaumeCH 2:3066e614372f 583 if (cmd_D <0){
GuillaumeCH 2:3066e614372f 584 cmd_D = 0;
GuillaumeCH 2:3066e614372f 585 }
GuillaumeCH 2:3066e614372f 586 if (cmd_D > 500){
GuillaumeCH 2:3066e614372f 587 cmd_D = 500;
GuillaumeCH 2:3066e614372f 588 }
GuillaumeCH 2:3066e614372f 589 c_D[dix_ms]=consigne_D;
GuillaumeCH 2:3066e614372f 590 c_G[dix_ms]=consigne_G;
GuillaumeCH 2:3066e614372f 591 //printf("%d\n",c[i]);
GuillaumeCH 2:3066e614372f 592 tab_cmd_D[dix_ms] = cmd_D;
GuillaumeCH 2:3066e614372f 593 tab_cmd_G[dix_ms] = cmd_G;
GuillaumeCH 2:3066e614372f 594 vtab_D[dix_ms] = vitesse_codeuse_D;
GuillaumeCH 2:3066e614372f 595 vtab_G[dix_ms] = vitesse_codeuse_G;
GuillaumeCH 2:3066e614372f 596 commande_vitesse(cmd_G,cmd_D);
GuillaumeCH 2:3066e614372f 597 dix_ms++;
GuillaumeCH 2:3066e614372f 598 //printf("%d\n",i);
GuillaumeCH 2:3066e614372f 599 //printf("tick : %ld cmd : %f,erreur : %f, somme_erreur : %f\n",tick_D_passe ,cmd_D,erreur_D, somme_erreur_D);
GuillaumeCH 2:3066e614372f 600 //printf("%f,%f\n",cmd_G,cmd_D);
GuillaumeCH 2:3066e614372f 601 //printf("oui");
GuillaumeCH 2:3066e614372f 602 }
GuillaumeCH 2:3066e614372f 603
GuillaumeCH 2:3066e614372f 604 void deplacement::printftab(){
GuillaumeCH 2:3066e614372f 605
GuillaumeCH 2:3066e614372f 606 for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 2:3066e614372f 607 if(j==500)
GuillaumeCH 2:3066e614372f 608 bouton();
GuillaumeCH 2:3066e614372f 609 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 610 }
GuillaumeCH 2:3066e614372f 611 /*if (j<5)
GuillaumeCH 2:3066e614372f 612 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 613 else
GuillaumeCH 2:3066e614372f 614 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 615 }*/
GuillaumeCH 2:3066e614372f 616
GuillaumeCH 2:3066e614372f 617 /*for (int j =0;j<TAILLE_TAB;j++){
GuillaumeCH 2:3066e614372f 618 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 619 }*/
GuillaumeCH 2:3066e614372f 620 }
GuillaumeCH 2:3066e614372f 621
GuillaumeCH 2:3066e614372f 622 void deplacement::test(){
GuillaumeCH 2:3066e614372f 623 Timer t;
GuillaumeCH 2:3066e614372f 624 t.start();
GuillaumeCH 2:3066e614372f 625 for (int i =0;i<5 ;i++){
GuillaumeCH 2:3066e614372f 626 changement_consigne(consigne_tab[i][0], consigne_tab[i][1]);
GuillaumeCH 3:d38aa400d5e7 627 while(t.read()<0.5f){
GuillaumeCH 2:3066e614372f 628 //actualise_positio n();
GuillaumeCH 2:3066e614372f 629 }
GuillaumeCH 2:3066e614372f 630 //printf("t.read() : %f\n",t.read());
GuillaumeCH 2:3066e614372f 631 //printf("consigne_D : %ld, consigne_G : %ld\n",consigne_D,consigne_G);
GuillaumeCH 2:3066e614372f 632 t.reset();
GuillaumeCH 2:3066e614372f 633 }
GuillaumeCH 2:3066e614372f 634 }
GuillaumeCH 2:3066e614372f 635
GuillaumeCH 2:3066e614372f 636 void deplacement::changement_consigne(int cons_D, int cons_G){
GuillaumeCH 2:3066e614372f 637 consigne_D = cons_D;
GuillaumeCH 2:3066e614372f 638 consigne_G = cons_G;
GuillaumeCH 2:3066e614372f 639 compteur_glisse = -1;
GuillaumeCH 2:3066e614372f 640 }
GuillaumeCH 2:3066e614372f 641
GuillaumeCH 3:d38aa400d5e7 642
JimmyAREM 5:3638d7e7c5c1 643 void deplacement::poussette(float temps){
GuillaumeCH 2:3066e614372f 644 motors_on();
JimmyAREM 5:3638d7e7c5c1 645 commande_vitesse(100,100);
JimmyAREM 5:3638d7e7c5c1 646 wait_ms(temps);
GuillaumeCH 2:3066e614372f 647 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 648 vitesse_nulle_D(0);
GuillaumeCH 2:3066e614372f 649 motors_stop();
GuillaumeCH 3:d38aa400d5e7 650 }
JimmyAREM 5:3638d7e7c5c1 651
GuillaumeCH 3:d38aa400d5e7 652 void deplacement::initialisation(){
GuillaumeCH 3:d38aa400d5e7 653 init_odometrie();
GuillaumeCH 3:d38aa400d5e7 654 init_hardware();
GuillaumeCH 3:d38aa400d5e7 655 srand(time(NULL));
GuillaumeCH 3:d38aa400d5e7 656 motors_on();
GuillaumeCH 4:eac6746544fb 657 }
JimmyAREM 6:e1585b8bd07d 658
JimmyAREM 7:6b15a1feed2d 659
JimmyAREM 6:e1585b8bd07d 660 void deplacement::arc(Coordonnees p1, Coordonnees p2, int sens){
GuillaumeCH 4:eac6746544fb 661 actualise_position();
GuillaumeCH 4:eac6746544fb 662 float vitesse_G;
GuillaumeCH 4:eac6746544fb 663 float vitesse_D;
GuillaumeCH 4:eac6746544fb 664 double x_ini = get_x_actuel();
GuillaumeCH 4:eac6746544fb 665 double y_ini = get_y_actuel();
GuillaumeCH 4:eac6746544fb 666 double angle_vise_deg = get_angle();
GuillaumeCH 4:eac6746544fb 667 double angle_vise=angle_vise_deg*3.1416/180.0;
GuillaumeCH 4:eac6746544fb 668 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 669
GuillaumeCH 4:eac6746544fb 670 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 671 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 672
GuillaumeCH 4:eac6746544fb 673 double x_actuel = get_x_actuel();
GuillaumeCH 4:eac6746544fb 674 double y_actuel = get_y_actuel();
GuillaumeCH 4:eac6746544fb 675
GuillaumeCH 4:eac6746544fb 676
GuillaumeCH 4:eac6746544fb 677 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 4:eac6746544fb 678 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 7:6b15a1feed2d 679 /*double p1x = p1.x*cos(angle_vise) + p1.y*sin(angle_vise)-x_local_ini;
JimmyAREM 7:6b15a1feed2d 680 double p2x = p2.x*cos(angle_vise) + p2.y*sin(angle_vise)-x_local_ini;
JimmyAREM 7:6b15a1feed2d 681 double p1y = p1.y*cos(angle_vise) - p1.x*sin(angle_vise)-y_local_ini;
JimmyAREM 7:6b15a1feed2d 682 double p2y = p2.y*cos(angle_vise) - p2.x*sin(angle_vise)-y_local_ini;*/
GuillaumeCH 4:eac6746544fb 683
GuillaumeCH 4:eac6746544fb 684 Coordonnees p0;
GuillaumeCH 4:eac6746544fb 685 p0.x = x_local;
GuillaumeCH 4:eac6746544fb 686 p0.y = y_local;
JimmyAREM 7:6b15a1feed2d 687 /*p1.x = p1x;
JimmyAREM 7:6b15a1feed2d 688 p2.x = p2x;
JimmyAREM 7:6b15a1feed2d 689 p1.y = p1y;
JimmyAREM 7:6b15a1feed2d 690 p2.y = p2y;*/
JimmyAREM 6:e1585b8bd07d 691
GuillaumeCH 4:eac6746544fb 692 cercle(p0,p1,p2);
JimmyAREM 5:3638d7e7c5c1 693 double xc = point[0];
JimmyAREM 5:3638d7e7c5c1 694 double yc = point[1];
JimmyAREM 5:3638d7e7c5c1 695 double Rc = point[2];
JimmyAREM 6:e1585b8bd07d 696 double angle_tan;
GuillaumeCH 4:eac6746544fb 697
JimmyAREM 6:e1585b8bd07d 698 double angle_a_parcourir = recup_angle_entre_trois_points_213(xc,yc,p0.x,p0.y,p2.x,p2.y);
JimmyAREM 7:6b15a1feed2d 699 printf("angle_a_parcourirrir : %lf\n",angle_a_parcourir);
JimmyAREM 6:e1585b8bd07d 700
JimmyAREM 6:e1585b8bd07d 701 if (angle_a_parcourir >0 && sens == A_DROITE){
JimmyAREM 6:e1585b8bd07d 702 angle_a_parcourir -=360.0 ;
JimmyAREM 5:3638d7e7c5c1 703 }
JimmyAREM 6:e1585b8bd07d 704 if (angle_a_parcourir <0 && sens == A_GAUCHE){
JimmyAREM 6:e1585b8bd07d 705 angle_a_parcourir +=360;
JimmyAREM 6:e1585b8bd07d 706 }
JimmyAREM 6:e1585b8bd07d 707
JimmyAREM 6:e1585b8bd07d 708 if (angle_a_parcourir >= 0)
JimmyAREM 6:e1585b8bd07d 709 {
JimmyAREM 6:e1585b8bd07d 710 if (p0.y != yc){
JimmyAREM 6:e1585b8bd07d 711 angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 7:6b15a1feed2d 712 if (xc<0){
JimmyAREM 7:6b15a1feed2d 713 angle_tan -=180.0;
JimmyAREM 7:6b15a1feed2d 714 }
JimmyAREM 5:3638d7e7c5c1 715 }
JimmyAREM 6:e1585b8bd07d 716 else{
JimmyAREM 6:e1585b8bd07d 717 angle_tan = 90;
JimmyAREM 7:6b15a1feed2d 718 if (xc<0){
JimmyAREM 7:6b15a1feed2d 719 angle_tan -=180.0;
JimmyAREM 7:6b15a1feed2d 720 }
GuillaumeCH 4:eac6746544fb 721 }
GuillaumeCH 4:eac6746544fb 722 }
JimmyAREM 6:e1585b8bd07d 723 else
JimmyAREM 6:e1585b8bd07d 724 {
JimmyAREM 6:e1585b8bd07d 725 if (p0.y != yc){
JimmyAREM 6:e1585b8bd07d 726 angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 7:6b15a1feed2d 727 if (xc<0){
JimmyAREM 7:6b15a1feed2d 728 angle_tan+=180.0;
JimmyAREM 7:6b15a1feed2d 729 }
JimmyAREM 6:e1585b8bd07d 730 }
JimmyAREM 6:e1585b8bd07d 731 else{
JimmyAREM 6:e1585b8bd07d 732 angle_tan = -90;
JimmyAREM 7:6b15a1feed2d 733 if (xc<0){
JimmyAREM 7:6b15a1feed2d 734 angle_tan+=180.0;
JimmyAREM 7:6b15a1feed2d 735 }
JimmyAREM 6:e1585b8bd07d 736 }
JimmyAREM 7:6b15a1feed2d 737
JimmyAREM 5:3638d7e7c5c1 738 }
JimmyAREM 6:e1585b8bd07d 739
JimmyAREM 6:e1585b8bd07d 740 //printf("angle_tan : %lf\n",angle_tan);
JimmyAREM 7:6b15a1feed2d 741 rotation_rel(angle_tan);
JimmyAREM 5:3638d7e7c5c1 742 actualise_position();
JimmyAREM 5:3638d7e7c5c1 743 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 744 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 745 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 746 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 747 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 748 //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 6:e1585b8bd07d 749 //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 750 motors_on();
JimmyAREM 6:e1585b8bd07d 751
JimmyAREM 6:e1585b8bd07d 752
JimmyAREM 5:3638d7e7c5c1 753 if (angle_a_parcourir < 0){
JimmyAREM 6:e1585b8bd07d 754 double mon_angle_ini = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 755 double mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 756 double mon_angle_prec;
JimmyAREM 6:e1585b8bd07d 757 if (mon_angle >= 0){
JimmyAREM 6:e1585b8bd07d 758 mon_angle -=360.0;
JimmyAREM 6:e1585b8bd07d 759 mon_angle_ini -=360.0;
JimmyAREM 6:e1585b8bd07d 760 }
JimmyAREM 6:e1585b8bd07d 761 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 762 while (!( mon_angle_prec + mon_angle < -359.9 && abs(mon_angle - mon_angle_prec) > 100)){
JimmyAREM 6:e1585b8bd07d 763 vitesse_D = abs(15 * mon_angle);
JimmyAREM 5:3638d7e7c5c1 764 if (vitesse_D>300){
JimmyAREM 5:3638d7e7c5c1 765 vitesse_D = 300;
JimmyAREM 5:3638d7e7c5c1 766 }
JimmyAREM 5:3638d7e7c5c1 767 vitesse_G = vitesse_D*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
JimmyAREM 6:e1585b8bd07d 768 double correction = int_ext_cercle(x_local,y_local);
JimmyAREM 7:6b15a1feed2d 769 double correction_en_cours = correction*0.035 - 3*diff_angle(borne_angle_d(angle_tan-angle_vise_deg),angle);
JimmyAREM 7:6b15a1feed2d 770 if (correction_en_cours>50){
JimmyAREM 7:6b15a1feed2d 771 correction_en_cours = 50;
JimmyAREM 7:6b15a1feed2d 772 }
JimmyAREM 7:6b15a1feed2d 773 if (correction_en_cours < -50){
JimmyAREM 7:6b15a1feed2d 774 correction_en_cours = -50;
JimmyAREM 7:6b15a1feed2d 775 }
JimmyAREM 7:6b15a1feed2d 776 vitesse_D= vitesse_D-correction_en_cours;
JimmyAREM 7:6b15a1feed2d 777 vitesse_G= vitesse_G+correction_en_cours;
JimmyAREM 7:6b15a1feed2d 778
JimmyAREM 7:6b15a1feed2d 779 //printf("angle _tan : %lf, angle : %lf , diff_angle : %lf , borne_angle : %lf\n",angle_tan, angle, diff_angle(borne_angle_d(angle_tan-angle_vise_deg),angle),borne_angle_d(angle_tan-angle_vise_deg));
JimmyAREM 7:6b15a1feed2d 780
JimmyAREM 6:e1585b8bd07d 781 if(detectionUltrasons)
JimmyAREM 6:e1585b8bd07d 782 {
JimmyAREM 6:e1585b8bd07d 783 detectionUltrasons = false;
JimmyAREM 6:e1585b8bd07d 784 evitementValider = false;
JimmyAREM 6:e1585b8bd07d 785 //writeCapteurs();
JimmyAREM 6:e1585b8bd07d 786 LectureI2CCarteCapteur(*this);
JimmyAREM 6:e1585b8bd07d 787 }
JimmyAREM 6:e1585b8bd07d 788 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 789 {
JimmyAREM 6:e1585b8bd07d 790 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 791 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 792 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 793 }
JimmyAREM 6:e1585b8bd07d 794 else
JimmyAREM 6:e1585b8bd07d 795 {
JimmyAREM 6:e1585b8bd07d 796 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 797 }
JimmyAREM 5:3638d7e7c5c1 798 actualise_position();
JimmyAREM 5:3638d7e7c5c1 799 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 800 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 801 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 802 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 803 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 804 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 805 mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 806
JimmyAREM 6:e1585b8bd07d 807
JimmyAREM 6:e1585b8bd07d 808 if (mon_angle >= 0){
JimmyAREM 6:e1585b8bd07d 809 mon_angle -=360.0;
JimmyAREM 5:3638d7e7c5c1 810 }
JimmyAREM 6:e1585b8bd07d 811
JimmyAREM 6:e1585b8bd07d 812 if (y_local-yc>=0){//gauche du cercle
JimmyAREM 6:e1585b8bd07d 813 if (y_local != yc){
JimmyAREM 6:e1585b8bd07d 814 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 815 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 + 180;
JimmyAREM 6:e1585b8bd07d 816 }
JimmyAREM 6:e1585b8bd07d 817 else{//haut droite
JimmyAREM 6:e1585b8bd07d 818 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 - 180;
JimmyAREM 6:e1585b8bd07d 819 }
JimmyAREM 6:e1585b8bd07d 820 }
JimmyAREM 6:e1585b8bd07d 821 else{
JimmyAREM 6:e1585b8bd07d 822 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 823 angle_tan = 90;
JimmyAREM 6:e1585b8bd07d 824 }
JimmyAREM 6:e1585b8bd07d 825 else{//haut droite
JimmyAREM 6:e1585b8bd07d 826 angle_tan = -90;
JimmyAREM 6:e1585b8bd07d 827 }
JimmyAREM 6:e1585b8bd07d 828 }
JimmyAREM 5:3638d7e7c5c1 829 }
JimmyAREM 6:e1585b8bd07d 830 else{//partie droite
JimmyAREM 6:e1585b8bd07d 831 if (x_local-xc>= 0){// haut droite
JimmyAREM 6:e1585b8bd07d 832 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 833 }
JimmyAREM 6:e1585b8bd07d 834 else{ //bas droite
JimmyAREM 6:e1585b8bd07d 835 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 836 }
JimmyAREM 6:e1585b8bd07d 837 }
JimmyAREM 7:6b15a1feed2d 838 if (xc<0){
JimmyAREM 7:6b15a1feed2d 839 angle_tan+=180.0;
JimmyAREM 7:6b15a1feed2d 840 }
JimmyAREM 5:3638d7e7c5c1 841 }
JimmyAREM 5:3638d7e7c5c1 842 }
JimmyAREM 6:e1585b8bd07d 843 if (angle_a_parcourir>= 0){
JimmyAREM 6:e1585b8bd07d 844 double mon_angle_ini = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 845 double mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 846 double mon_angle_prec;
JimmyAREM 6:e1585b8bd07d 847 if (mon_angle <= 0){
JimmyAREM 6:e1585b8bd07d 848 mon_angle +=360.0;
JimmyAREM 6:e1585b8bd07d 849 mon_angle_ini +=360.0;
JimmyAREM 6:e1585b8bd07d 850 }
JimmyAREM 6:e1585b8bd07d 851 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 852 //printf(" test :: %lf - %lf >< %lf ET %lf - %lf >< %lf, boolen : %d\n",mon_angle_ini,mon_angle,angle_a_parcourir,mon_angle_ini,mon_angle_prec,angle_a_parcourir,!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) <= angle_a_parcourir)));
JimmyAREM 6:e1585b8bd07d 853 //while(!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) >= angle_a_parcourir))){
JimmyAREM 6:e1585b8bd07d 854 while (!( mon_angle_prec + mon_angle > 359.9 && abs(mon_angle - mon_angle_prec) > 100)){
JimmyAREM 7:6b15a1feed2d 855
JimmyAREM 6:e1585b8bd07d 856 //printf(" test :: %lf - %lf >< %lf ET %lf + %lf >< %lf, boolen : %d\n",mon_angle_ini,mon_angle,angle_a_parcourir,mon_angle_ini,mon_angle_prec,angle_a_parcourir,!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) <= angle_a_parcourir)));
JimmyAREM 6:e1585b8bd07d 857 vitesse_G = 15 * mon_angle;
JimmyAREM 5:3638d7e7c5c1 858 if (abs(vitesse_G)>300){
JimmyAREM 5:3638d7e7c5c1 859 vitesse_G = 300;
JimmyAREM 5:3638d7e7c5c1 860 }
JimmyAREM 5:3638d7e7c5c1 861 vitesse_D = vitesse_G*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
JimmyAREM 5:3638d7e7c5c1 862 double correction = int_ext_cercle(x_local,y_local);
JimmyAREM 7:6b15a1feed2d 863 double correction_en_cours = correction*0.035 - 3*diff_angle(borne_angle_d(angle_tan+angle_vise_deg),angle);
JimmyAREM 7:6b15a1feed2d 864 if (correction_en_cours>50){
JimmyAREM 7:6b15a1feed2d 865 correction_en_cours = 50;
JimmyAREM 7:6b15a1feed2d 866 }
JimmyAREM 7:6b15a1feed2d 867 if (correction_en_cours < -50){
JimmyAREM 7:6b15a1feed2d 868 correction_en_cours = -50;
JimmyAREM 7:6b15a1feed2d 869 }
JimmyAREM 7:6b15a1feed2d 870 vitesse_D= vitesse_D+correction_en_cours;
JimmyAREM 7:6b15a1feed2d 871 vitesse_G= vitesse_G-correction_en_cours;
JimmyAREM 7:6b15a1feed2d 872 //printf("%lf , %lf ,%lf ,%lf, correction : %lf, vitesse_D : %f, vitesse_G : %f\n",angle_tan , angle,borne_angle_d(angle_tan+angle_vise_deg) ,diff_angle(borne_angle_d(angle_tan+angle_vise_deg),angle),correction,vitesse_D,vitesse_G);
JimmyAREM 6:e1585b8bd07d 873 if(detectionUltrasons)
JimmyAREM 6:e1585b8bd07d 874 {
JimmyAREM 6:e1585b8bd07d 875 detectionUltrasons = false;
JimmyAREM 6:e1585b8bd07d 876 evitementValider = false;
JimmyAREM 6:e1585b8bd07d 877 //writeCapteurs();
JimmyAREM 6:e1585b8bd07d 878 LectureI2CCarteCapteur(*this);
JimmyAREM 6:e1585b8bd07d 879 }
JimmyAREM 6:e1585b8bd07d 880 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 881 {
JimmyAREM 6:e1585b8bd07d 882 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 883 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 884 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 885 }
JimmyAREM 6:e1585b8bd07d 886 else
JimmyAREM 6:e1585b8bd07d 887 {
JimmyAREM 6:e1585b8bd07d 888 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 889 }
JimmyAREM 5:3638d7e7c5c1 890 actualise_position();
JimmyAREM 5:3638d7e7c5c1 891 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 892 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 893 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 894 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 895 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 896 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 897 mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 898 if (mon_angle <= 0){
JimmyAREM 6:e1585b8bd07d 899 mon_angle +=360.0;
JimmyAREM 5:3638d7e7c5c1 900 }
JimmyAREM 6:e1585b8bd07d 901 if (y_local-yc>=0){//gauche du cercle
JimmyAREM 6:e1585b8bd07d 902 if (y_local != yc){
JimmyAREM 6:e1585b8bd07d 903 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 904 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 905 }
JimmyAREM 6:e1585b8bd07d 906 else{//haut droite
JimmyAREM 6:e1585b8bd07d 907 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 908 }
JimmyAREM 6:e1585b8bd07d 909 }
JimmyAREM 6:e1585b8bd07d 910 else{
JimmyAREM 6:e1585b8bd07d 911 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 912 angle_tan = -90;
JimmyAREM 6:e1585b8bd07d 913 }
JimmyAREM 6:e1585b8bd07d 914 else{//haut droite
JimmyAREM 6:e1585b8bd07d 915 angle_tan = 90;
JimmyAREM 6:e1585b8bd07d 916 }
JimmyAREM 6:e1585b8bd07d 917 }
JimmyAREM 5:3638d7e7c5c1 918 }
JimmyAREM 6:e1585b8bd07d 919 else{//partie droite
JimmyAREM 6:e1585b8bd07d 920 if (x_local-xc>= 0){// haut droite
JimmyAREM 6:e1585b8bd07d 921 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 - 180.0 ;
JimmyAREM 6:e1585b8bd07d 922 }
JimmyAREM 6:e1585b8bd07d 923 else{ //bas droite
JimmyAREM 6:e1585b8bd07d 924 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 + 180.0 ;
JimmyAREM 6:e1585b8bd07d 925 }
JimmyAREM 6:e1585b8bd07d 926 }
JimmyAREM 7:6b15a1feed2d 927
JimmyAREM 7:6b15a1feed2d 928
JimmyAREM 6:e1585b8bd07d 929 }
JimmyAREM 6:e1585b8bd07d 930 //printf(" test :: %lf - %lf >< %lf ET %lf + %lf >< %lf, boolen : %d\n",mon_angle_ini,mon_angle,angle_a_parcourir,mon_angle_ini,mon_angle_prec,angle_a_parcourir,!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) <= angle_a_parcourir)));
GuillaumeCH 4:eac6746544fb 931
GuillaumeCH 4:eac6746544fb 932 }
JimmyAREM 6:e1585b8bd07d 933
JimmyAREM 5:3638d7e7c5c1 934 //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y));
JimmyAREM 5:3638d7e7c5c1 935 //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 936 vitesse_nulle_D(0);
GuillaumeCH 4:eac6746544fb 937 vitesse_nulle_G(0);
GuillaumeCH 4:eac6746544fb 938 wait(0.2);
GuillaumeCH 4:eac6746544fb 939 motors_stop();
GuillaumeCH 4:eac6746544fb 940
GuillaumeCH 4:eac6746544fb 941 }
GuillaumeCH 4:eac6746544fb 942
GuillaumeCH 4:eac6746544fb 943
GuillaumeCH 4:eac6746544fb 944 int deplacement::cercle(Coordonnees a,Coordonnees b, Coordonnees c){
GuillaumeCH 4:eac6746544fb 945 double ax2 = pow(a.x,2);
GuillaumeCH 4:eac6746544fb 946 double ay2 = pow(a.y,2);
GuillaumeCH 4:eac6746544fb 947 double bx2 = pow(b.x,2);
GuillaumeCH 4:eac6746544fb 948 double by2 = pow(b.y,2);
GuillaumeCH 4:eac6746544fb 949 double cx2 = pow(c.x,2);
GuillaumeCH 4:eac6746544fb 950 double cy2 = pow(c.y,2);
JimmyAREM 5:3638d7e7c5c1 951 double pente_a=0;
GuillaumeCH 4:eac6746544fb 952 double ord_b;
JimmyAREM 5:3638d7e7c5c1 953 double pente_A=0;
GuillaumeCH 4:eac6746544fb 954 double ord_B;
GuillaumeCH 4:eac6746544fb 955 double xc;
GuillaumeCH 4:eac6746544fb 956 double yc;
GuillaumeCH 4:eac6746544fb 957 double Rc;
GuillaumeCH 4:eac6746544fb 958 if (a.y == b.y && b.y == c.y){
GuillaumeCH 4:eac6746544fb 959 printf("Les points sont alignes1\n");
GuillaumeCH 4:eac6746544fb 960 return 0;
GuillaumeCH 4:eac6746544fb 961 }
GuillaumeCH 4:eac6746544fb 962 if (b.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 963 pente_a = -1*(b.x-a.x)/(b.y-a.y);
GuillaumeCH 4:eac6746544fb 964 printf("a : %lf ",pente_a);
GuillaumeCH 4:eac6746544fb 965 ord_b = (by2-ay2+bx2-ax2)/(2*(b.y-a.y));
GuillaumeCH 4:eac6746544fb 966 printf("b : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 967
GuillaumeCH 4:eac6746544fb 968 }
GuillaumeCH 4:eac6746544fb 969 else{
JimmyAREM 5:3638d7e7c5c1 970 cercle(c,a,b);
JimmyAREM 5:3638d7e7c5c1 971 return 0;
GuillaumeCH 4:eac6746544fb 972
GuillaumeCH 4:eac6746544fb 973 }
GuillaumeCH 4:eac6746544fb 974 if (c.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 975 pente_A = -1*(c.x-a.x)/(c.y-a.y);
GuillaumeCH 4:eac6746544fb 976 printf("A : %lf ",pente_A);
GuillaumeCH 4:eac6746544fb 977 ord_B = (cy2-ay2+cx2-ax2)/(2*(c.y-a.y));
GuillaumeCH 4:eac6746544fb 978 printf("B : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 979
GuillaumeCH 4:eac6746544fb 980 }
GuillaumeCH 4:eac6746544fb 981 else{
JimmyAREM 5:3638d7e7c5c1 982 cercle(b,c,a);
JimmyAREM 5:3638d7e7c5c1 983 return 0;
GuillaumeCH 4:eac6746544fb 984 }
GuillaumeCH 4:eac6746544fb 985
GuillaumeCH 4:eac6746544fb 986
GuillaumeCH 4:eac6746544fb 987 if (pente_a-pente_A!=0){
GuillaumeCH 4:eac6746544fb 988 xc = (ord_B-ord_b)/(pente_a-pente_A);
GuillaumeCH 4:eac6746544fb 989 yc = pente_a*xc+ord_b;
GuillaumeCH 4:eac6746544fb 990 Rc = pow((pow(xc-a.x,2)+pow(yc-a.y,2)),0.5);
GuillaumeCH 4:eac6746544fb 991 point[0] = xc;
GuillaumeCH 4:eac6746544fb 992 point[1] = yc;
GuillaumeCH 4:eac6746544fb 993 point[2] = Rc;
GuillaumeCH 4:eac6746544fb 994 printf("xc : %f, yc : %f, Rc : %f\n",xc,yc,Rc);
GuillaumeCH 4:eac6746544fb 995 }
GuillaumeCH 4:eac6746544fb 996 else{
GuillaumeCH 4:eac6746544fb 997 printf("Les points sont alignes2\n");
GuillaumeCH 4:eac6746544fb 998 return 0;
GuillaumeCH 4:eac6746544fb 999 }
GuillaumeCH 4:eac6746544fb 1000
GuillaumeCH 4:eac6746544fb 1001
GuillaumeCH 4:eac6746544fb 1002 return 0;
GuillaumeCH 4:eac6746544fb 1003 }
GuillaumeCH 4:eac6746544fb 1004 double deplacement::int_ext_cercle(double x, double y){
GuillaumeCH 4:eac6746544fb 1005 double xc= point[0];
GuillaumeCH 4:eac6746544fb 1006 double yc= point[1];
GuillaumeCH 4:eac6746544fb 1007 double Rc= point[2];
GuillaumeCH 4:eac6746544fb 1008 double rayon = pow((pow(xc-x,2)+pow(yc-y,2)),0.5);
GuillaumeCH 4:eac6746544fb 1009 return Rc-rayon;
GuillaumeCH 4:eac6746544fb 1010
GuillaumeCH 4:eac6746544fb 1011 }
GuillaumeCH 4:eac6746544fb 1012
GuillaumeCH 4:eac6746544fb 1013 void deplacement::va_au_point(double x,double y, double cap){
GuillaumeCH 4:eac6746544fb 1014 actualise_position();
GuillaumeCH 4:eac6746544fb 1015 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 1016 double x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 1017 double y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 1018 double x_projete = 10000.0*cos(angle*3.1416/180.0)+x_robot;
GuillaumeCH 4:eac6746544fb 1019 double y_projete = 10000.0*sin(angle*3.1416/180.0)+y_robot;
JimmyAREM 5:3638d7e7c5c1 1020 printf("angle ::: %lf\n",recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete));
JimmyAREM 5:3638d7e7c5c1 1021 rotation_rel(recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete));
GuillaumeCH 4:eac6746544fb 1022 //printf("oui\n");
GuillaumeCH 4:eac6746544fb 1023 actualise_position();
GuillaumeCH 4:eac6746544fb 1024 angle = get_angle();
GuillaumeCH 4:eac6746544fb 1025 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 1026 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 1027 double distance = pow(pow(x-x_robot,2)+pow(y-y_robot,2),0.5);
GuillaumeCH 4:eac6746544fb 1028 printf("distance : %lf\n",distance);
JimmyAREM 6:e1585b8bd07d 1029 ligne_droite(distance, x, y, cap);
GuillaumeCH 4:eac6746544fb 1030 actualise_position();
GuillaumeCH 4:eac6746544fb 1031 angle = get_angle();
GuillaumeCH 4:eac6746544fb 1032 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 1033 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 1034 rotation_abs(cap);
GuillaumeCH 4:eac6746544fb 1035
GuillaumeCH 4:eac6746544fb 1036
GuillaumeCH 4:eac6746544fb 1037 }
GuillaumeCH 4:eac6746544fb 1038
GuillaumeCH 4:eac6746544fb 1039 double deplacement::recup_angle_entre_trois_points_213(double x1,double y1,double x2,double y2,double x3,double y3){
JimmyAREM 5:3638d7e7c5c1 1040 double x13 = x3-x1;
JimmyAREM 5:3638d7e7c5c1 1041 double y13 = y3-y1;
GuillaumeCH 4:eac6746544fb 1042 double x12 = x2-x1;
GuillaumeCH 4:eac6746544fb 1043 double y12 = y2-y1;
GuillaumeCH 4:eac6746544fb 1044 double norme12 = pow(x12*x12+y12*y12,0.5);
GuillaumeCH 4:eac6746544fb 1045 double norme13 = pow(x13*x13+y13*y13,0.5);
JimmyAREM 5:3638d7e7c5c1 1046 double ux = x13/norme13;
JimmyAREM 5:3638d7e7c5c1 1047 double uy = y13/norme13;
JimmyAREM 5:3638d7e7c5c1 1048 double wx = x12/norme12;
JimmyAREM 5:3638d7e7c5c1 1049 double wy = y12/norme12;
JimmyAREM 5:3638d7e7c5c1 1050 //printf("u : %lf,%lf ,v : %lf,%lf ,w : %lf,%lf\n",ux,uy,-uy,ux,wx,wy);
GuillaumeCH 4:eac6746544fb 1051 double prod_scal = x12*x13+y12*y13;
JimmyAREM 5:3638d7e7c5c1 1052 double cos_angle = prod_scal/(norme12*norme13);
JimmyAREM 5:3638d7e7c5c1 1053 double sin_angle;
JimmyAREM 5:3638d7e7c5c1 1054 if (uy!=0){
JimmyAREM 5:3638d7e7c5c1 1055 sin_angle = -1*(wx - cos_angle*ux)/uy;
JimmyAREM 5:3638d7e7c5c1 1056 }
JimmyAREM 5:3638d7e7c5c1 1057 else{
JimmyAREM 5:3638d7e7c5c1 1058 sin_angle = (wy - cos_angle*uy)/ux;
JimmyAREM 5:3638d7e7c5c1 1059 }
JimmyAREM 5:3638d7e7c5c1 1060 //printf("cos : %lf sin : %lf\n",cos_angle,sin_angle);
JimmyAREM 5:3638d7e7c5c1 1061 if (sin_angle >=0){
JimmyAREM 5:3638d7e7c5c1 1062 return acos(prod_scal/(norme12*norme13))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 1063 }
JimmyAREM 5:3638d7e7c5c1 1064 else{
JimmyAREM 5:3638d7e7c5c1 1065 return -acos(prod_scal/(norme12*norme13))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 1066 }
GuillaumeCH 4:eac6746544fb 1067 }
JimmyAREM 6:e1585b8bd07d 1068
JimmyAREM 6:e1585b8bd07d 1069
JimmyAREM 6:e1585b8bd07d 1070 void deplacement::pente(long int distance, float vitesse, double angle_a_tourner)
JimmyAREM 6:e1585b8bd07d 1071 {
JimmyAREM 6:e1585b8bd07d 1072 Timer time;
JimmyAREM 6:e1585b8bd07d 1073 time.start();
JimmyAREM 6:e1585b8bd07d 1074 somme_y=0;
JimmyAREM 6:e1585b8bd07d 1075 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
JimmyAREM 6:e1585b8bd07d 1076 motors_on();
JimmyAREM 6:e1585b8bd07d 1077 actualise_position();
JimmyAREM 6:e1585b8bd07d 1078 double x_ini = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 1079 double y_ini = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 1080 double angle_vise_deg = get_angle();
JimmyAREM 6:e1585b8bd07d 1081 double angle_vise=angle_vise_deg*3.1416/180;
JimmyAREM 6:e1585b8bd07d 1082 double angle = get_angle();
JimmyAREM 6:e1585b8bd07d 1083
JimmyAREM 6:e1585b8bd07d 1084 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 1085 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 1086
JimmyAREM 6:e1585b8bd07d 1087 double x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 1088 double y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 1089
JimmyAREM 6:e1585b8bd07d 1090
JimmyAREM 6:e1585b8bd07d 1091 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 1092 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 1093
JimmyAREM 6:e1585b8bd07d 1094 //long int y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 1095 float vitesse_G;
JimmyAREM 6:e1585b8bd07d 1096 float vitesse_D;
JimmyAREM 6:e1585b8bd07d 1097
JimmyAREM 6:e1585b8bd07d 1098 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 1099 while (distance-x_local>0){
JimmyAREM 6:e1585b8bd07d 1100
JimmyAREM 6:e1585b8bd07d 1101 vitesse_G = vitesse+y_local*0.02;
JimmyAREM 6:e1585b8bd07d 1102 vitesse_D = vitesse-y_local*0.02;
JimmyAREM 6:e1585b8bd07d 1103
JimmyAREM 6:e1585b8bd07d 1104
JimmyAREM 6:e1585b8bd07d 1105 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 1106 actualise_position();
JimmyAREM 6:e1585b8bd07d 1107 x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 1108 y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 1109 somme_y+=y_actuel;
JimmyAREM 6:e1585b8bd07d 1110 //y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 1111 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 1112 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 1113
JimmyAREM 6:e1585b8bd07d 1114 //printf(" VG : %f VD : %f ; x_local : %lf, y_local : %lf, angle_vise : %f\n",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));
JimmyAREM 6:e1585b8bd07d 1115 }
JimmyAREM 6:e1585b8bd07d 1116 //printf("x : %lf, y : %lf, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 1117 rotation_abs_pente(angle_a_tourner);
JimmyAREM 6:e1585b8bd07d 1118 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 1119 }
JimmyAREM 6:e1585b8bd07d 1120
JimmyAREM 6:e1585b8bd07d 1121 void deplacement::rotation_rel_pente(double angle_vise)
JimmyAREM 6:e1585b8bd07d 1122 {
JimmyAREM 6:e1585b8bd07d 1123 // rotation de angle_vise
JimmyAREM 6:e1585b8bd07d 1124 motors_on();
JimmyAREM 6:e1585b8bd07d 1125 double vitesse=180;
JimmyAREM 6:e1585b8bd07d 1126 int sens;
JimmyAREM 6:e1585b8bd07d 1127 double angle = get_angle();
JimmyAREM 6:e1585b8bd07d 1128 angle_vise+=angle;
JimmyAREM 6:e1585b8bd07d 1129 borne_angle_d(angle_vise);
JimmyAREM 6:e1585b8bd07d 1130 if (diff_angle(angle,angle_vise)<=0){
JimmyAREM 6:e1585b8bd07d 1131 sens = -1;
JimmyAREM 6:e1585b8bd07d 1132 //printf("negatif\n");
JimmyAREM 6:e1585b8bd07d 1133 }
JimmyAREM 6:e1585b8bd07d 1134 else{
JimmyAREM 6:e1585b8bd07d 1135 sens = 1;
JimmyAREM 6:e1585b8bd07d 1136
JimmyAREM 6:e1585b8bd07d 1137 //printf("positif\n");
JimmyAREM 6:e1585b8bd07d 1138 }
JimmyAREM 6:e1585b8bd07d 1139 //printf("diff : %lf ",diff_angle(angle,angle_vise));
JimmyAREM 6:e1585b8bd07d 1140 while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
JimmyAREM 6:e1585b8bd07d 1141 {
JimmyAREM 6:e1585b8bd07d 1142 actualise_position();
JimmyAREM 6:e1585b8bd07d 1143 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 1144 vitesse=0.5*sens*abs(diff_angle(angle,angle_vise));
JimmyAREM 6:e1585b8bd07d 1145
JimmyAREM 6:e1585b8bd07d 1146 commande_vitesse(-vitesse,vitesse);
JimmyAREM 6:e1585b8bd07d 1147 if (compteur_asser==150){
JimmyAREM 6:e1585b8bd07d 1148 compteur_asser=0;
JimmyAREM 6:e1585b8bd07d 1149 //printf("%lf\n",get_y_actuel());
JimmyAREM 6:e1585b8bd07d 1150 }
JimmyAREM 6:e1585b8bd07d 1151 compteur_asser++;
JimmyAREM 6:e1585b8bd07d 1152 //printf("vitesse : %lf ", vitesse);
JimmyAREM 6:e1585b8bd07d 1153 }
JimmyAREM 6:e1585b8bd07d 1154 //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
JimmyAREM 6:e1585b8bd07d 1155 //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));
JimmyAREM 6:e1585b8bd07d 1156 //consigne_D = 0;
JimmyAREM 6:e1585b8bd07d 1157 //consigne_G = 0;
JimmyAREM 6:e1585b8bd07d 1158 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 1159 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 1160
JimmyAREM 6:e1585b8bd07d 1161 }
JimmyAREM 6:e1585b8bd07d 1162
JimmyAREM 6:e1585b8bd07d 1163 void deplacement::rotation_abs_pente(double angle_vise)
JimmyAREM 6:e1585b8bd07d 1164 {
JimmyAREM 6:e1585b8bd07d 1165 actualise_position();
JimmyAREM 6:e1585b8bd07d 1166 //printf("bite");
JimmyAREM 6:e1585b8bd07d 1167 double angle_rel = borne_angle_d(angle_vise-get_angle());
JimmyAREM 6:e1585b8bd07d 1168 rotation_rel_pente(angle_rel);
JimmyAREM 6:e1585b8bd07d 1169 }
JimmyAREM 6:e1585b8bd07d 1170
JimmyAREM 7:6b15a1feed2d 1171 void deplacement::pente_combo(BrasPousser brasPousserGauche, BrasPousser brasPousserDroit, Pompe pompe){
JimmyAREM 6:e1585b8bd07d 1172 activerDeuxBras(brasPousserGauche, brasPousserDroit);
JimmyAREM 6:e1585b8bd07d 1173 motors_on();
JimmyAREM 7:6b15a1feed2d 1174 pente(15000,50,0);
JimmyAREM 6:e1585b8bd07d 1175 pente(10000,30,0);
JimmyAREM 7:6b15a1feed2d 1176 pente(52500,100,0);
JimmyAREM 6:e1585b8bd07d 1177 desactiverDeuxBras(brasPousserGauche, brasPousserDroit);
JimmyAREM 6:e1585b8bd07d 1178 commande_vitesse(80,80);
JimmyAREM 7:6b15a1feed2d 1179 wait(1.5);
JimmyAREM 7:6b15a1feed2d 1180 vitesse_nulle_D(0);
JimmyAREM 7:6b15a1feed2d 1181 vitesse_nulle_G(0);
JimmyAREM 7:6b15a1feed2d 1182 //hardHiz();
JimmyAREM 7:6b15a1feed2d 1183 pompe.desactiver();
JimmyAREM 7:6b15a1feed2d 1184 wait(3);
JimmyAREM 7:6b15a1feed2d 1185 rotation_rel(-90);
JimmyAREM 6:e1585b8bd07d 1186 wait(4);
JimmyAREM 6:e1585b8bd07d 1187 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 1188 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 1189 }
JimmyAREM 6:e1585b8bd07d 1190
JimmyAREM 6:e1585b8bd07d 1191 void deplacement::evitement(double x, double y, double cap)
JimmyAREM 6:e1585b8bd07d 1192 {
JimmyAREM 6:e1585b8bd07d 1193 if((distanceUltrasonGauche == 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
JimmyAREM 6:e1585b8bd07d 1194 {
JimmyAREM 7:6b15a1feed2d 1195 if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 1196 {
JimmyAREM 6:e1585b8bd07d 1197 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1198 actualise_position();
JimmyAREM 6:e1585b8bd07d 1199 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1200 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1201 arc(P1,P2, A_GAUCHE);
JimmyAREM 6:e1585b8bd07d 1202 }
JimmyAREM 7:6b15a1feed2d 1203 else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 1204 {
JimmyAREM 6:e1585b8bd07d 1205 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1206 actualise_position();
JimmyAREM 6:e1585b8bd07d 1207 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1208 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1209 arc(P1,P2, A_DROITE);
JimmyAREM 6:e1585b8bd07d 1210 }
JimmyAREM 6:e1585b8bd07d 1211 }
JimmyAREM 6:e1585b8bd07d 1212 if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit == 1100) && (evitementValider == false))
JimmyAREM 6:e1585b8bd07d 1213 {
JimmyAREM 7:6b15a1feed2d 1214 if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 1215 {
JimmyAREM 6:e1585b8bd07d 1216 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1217 actualise_position();
JimmyAREM 6:e1585b8bd07d 1218 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1219 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1220 arc(P1,P2, A_DROITE);
JimmyAREM 6:e1585b8bd07d 1221 }
JimmyAREM 7:6b15a1feed2d 1222 else if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 1223 {
JimmyAREM 6:e1585b8bd07d 1224 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1225 actualise_position();
JimmyAREM 6:e1585b8bd07d 1226 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1227 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1228 arc(P1,P2, A_GAUCHE);
JimmyAREM 6:e1585b8bd07d 1229 }
JimmyAREM 6:e1585b8bd07d 1230 }
JimmyAREM 6:e1585b8bd07d 1231 if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
JimmyAREM 6:e1585b8bd07d 1232 {
JimmyAREM 7:6b15a1feed2d 1233 if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 1234 {
JimmyAREM 6:e1585b8bd07d 1235 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1236 actualise_position();
JimmyAREM 6:e1585b8bd07d 1237 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1238 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1239 arc(P1,P2, A_GAUCHE);
JimmyAREM 6:e1585b8bd07d 1240 }
JimmyAREM 7:6b15a1feed2d 1241 else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 1242 {
JimmyAREM 6:e1585b8bd07d 1243 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1244 actualise_position();
JimmyAREM 6:e1585b8bd07d 1245 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1246 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1247 arc(P1,P2, A_DROITE);
JimmyAREM 6:e1585b8bd07d 1248 }
JimmyAREM 6:e1585b8bd07d 1249 }
JimmyAREM 6:e1585b8bd07d 1250 else
JimmyAREM 6:e1585b8bd07d 1251 {
JimmyAREM 6:e1585b8bd07d 1252 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1253 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 1254 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 1255 }
JimmyAREM 6:e1585b8bd07d 1256 va_au_point(x,y,cap);
JimmyAREM 6:e1585b8bd07d 1257 }