Guillaume Chauvon / Mbed 2 deprecated Asservissment_robot2_v16_05

l

Dependencies:   mbed Asser2

deplacement.cpp@8:f2425e4302fc, 2019-05-29 (annotated)

Committer:
GuillaumeCH
Date:
Wed May 29 06:34:17 2019 +0000
Revision:
8:f2425e4302fc
Parent:
7:6b15a1feed2d 
mlmlml

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 8:f2425e4302fc 49 point[0]=0;
GuillaumeCH 8:f2425e4302fc 50 point[1]=0;
GuillaumeCH 8:f2425e4302fc 51 point[2]=0;
GuillaumeCH 2:3066e614372f 52 }
GuillaumeCH 2:3066e614372f 53
JimmyAREM 6:e1585b8bd07d 54 void deplacement::arreterRobot()
JimmyAREM 6:e1585b8bd07d 55 {
JimmyAREM 6:e1585b8bd07d 56 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 57 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 58 }
JimmyAREM 6:e1585b8bd07d 59
GuillaumeCH 2:3066e614372f 60 void deplacement::commande_vitesse(float vitesse_G,float vitesse_D){ //fonction pour commander les moteurs sans avoir à utiliser set_PWM
GuillaumeCH 2:3066e614372f 61
GuillaumeCH 2:3066e614372f 62 int sens_G=signe(vitesse_G);
GuillaumeCH 2:3066e614372f 63 int sens_D=signe(vitesse_D);
GuillaumeCH 2:3066e614372f 64 double vitesse_local_G=abs(vitesse_G);
GuillaumeCH 2:3066e614372f 65 double vitesse_local_D=abs(vitesse_D);
GuillaumeCH 2:3066e614372f 66
GuillaumeCH 2:3066e614372f 67 if(abs(vitesse_G) > 900){
GuillaumeCH 2:3066e614372f 68 vitesse_local_G=900;
GuillaumeCH 2:3066e614372f 69 }
GuillaumeCH 3:d38aa400d5e7 70 if(abs(vitesse_G)<10){
GuillaumeCH 3:d38aa400d5e7 71 vitesse_local_G=10;
GuillaumeCH 2:3066e614372f 72 }
GuillaumeCH 2:3066e614372f 73 if(abs(vitesse_D) > 900){
GuillaumeCH 2:3066e614372f 74 vitesse_local_D=900;
GuillaumeCH 2:3066e614372f 75 }
GuillaumeCH 3:d38aa400d5e7 76 if(abs(vitesse_D)< 10){
GuillaumeCH 3:d38aa400d5e7 77 vitesse_local_D=10;
GuillaumeCH 2:3066e614372f 78
GuillaumeCH 2:3066e614372f 79 }
GuillaumeCH 2:3066e614372f 80 ;
GuillaumeCH 2:3066e614372f 81 int VG_int = (int) vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 82 int VD_int = (int) vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 83 float VG_f = vitesse_local_G*sens_G*COEFF_MOTEUR_G;
GuillaumeCH 2:3066e614372f 84 float VD_f = vitesse_local_D*sens_D*COEFF_MOTEUR_D;
GuillaumeCH 2:3066e614372f 85 float centieme_D = (VD_f-VD_int)*1000;
GuillaumeCH 2:3066e614372f 86 float centieme_G = (VG_f-VG_int)*1000;
GuillaumeCH 2:3066e614372f 87 if ((rand()%1000) < centieme_G){
GuillaumeCH 2:3066e614372f 88 VG_int+=1;
GuillaumeCH 2:3066e614372f 89 }
GuillaumeCH 2:3066e614372f 90 if ((rand()%1000) < centieme_D){
GuillaumeCH 2:3066e614372f 91 VD_int+=1;
GuillaumeCH 2:3066e614372f 92 }
GuillaumeCH 2:3066e614372f 93 //printf("vitesseG : %f, vitesseD : %f, %d, %d", VG_f, VD_f, VG_int, VD_int);
GuillaumeCH 2:3066e614372f 94 set_PWM_moteur_G(VG_int);//le branchements des moteurs est à vérifier ( fonctionne dans l'état actuel du robots
GuillaumeCH 2:3066e614372f 95 set_PWM_moteur_D(VD_int);//
GuillaumeCH 2:3066e614372f 96 }
GuillaumeCH 2:3066e614372f 97 void deplacement::vitesse_nulle_G(int zero){
GuillaumeCH 2:3066e614372f 98 if(zero == 0){
GuillaumeCH 2:3066e614372f 99 set_PWM_moteur_G(0);
GuillaumeCH 2:3066e614372f 100 }
GuillaumeCH 2:3066e614372f 101 }
GuillaumeCH 2:3066e614372f 102 void deplacement::vitesse_nulle_D(int zero){
GuillaumeCH 2:3066e614372f 103 if(zero == 0){
GuillaumeCH 2:3066e614372f 104 set_PWM_moteur_D(0);
GuillaumeCH 2:3066e614372f 105 }
GuillaumeCH 2:3066e614372f 106 }
GuillaumeCH 3:d38aa400d5e7 107 void deplacement::marche_arriere(int distance)
GuillaumeCH 2:3066e614372f 108 {
GuillaumeCH 8:f2425e4302fc 109
GuillaumeCH 2:3066e614372f 110 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 111 motors_on();
GuillaumeCH 2:3066e614372f 112 actualise_position();
GuillaumeCH 2:3066e614372f 113 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 114 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 115 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 116 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 117 double angle = get_angle();
GuillaumeCH 2:3066e614372f 118
GuillaumeCH 2:3066e614372f 119 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 120 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 121
GuillaumeCH 2:3066e614372f 122 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 123 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 124
GuillaumeCH 2:3066e614372f 125
GuillaumeCH 2:3066e614372f 126 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 127 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 128
GuillaumeCH 2:3066e614372f 129 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 130 float vitesse_G;
GuillaumeCH 2:3066e614372f 131 float vitesse_D;
GuillaumeCH 2:3066e614372f 132
GuillaumeCH 2:3066e614372f 133 angle = get_angle();
GuillaumeCH 2:3066e614372f 134 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 135 float Kdp= 10;
GuillaumeCH 2:3066e614372f 136 while (distance-x_local<0){
JimmyAREM 5:3638d7e7c5c1 137 if(detectionUltrasons)
JimmyAREM 5:3638d7e7c5c1 138 {
JimmyAREM 5:3638d7e7c5c1 139 detectionUltrasons = false;
JimmyAREM 5:3638d7e7c5c1 140 //writeCapteurs();
JimmyAREM 5:3638d7e7c5c1 141 LectureI2CCarteCapteur(*this);
JimmyAREM 5:3638d7e7c5c1 142 }
GuillaumeCH 2:3066e614372f 143 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 144 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 145 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 146 vitesse_G=400;
GuillaumeCH 2:3066e614372f 147 vitesse_D=400;
GuillaumeCH 2:3066e614372f 148 }
GuillaumeCH 2:3066e614372f 149 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 150 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 151 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 152 }
GuillaumeCH 2:3066e614372f 153
GuillaumeCH 2:3066e614372f 154 angle = get_angle();
GuillaumeCH 2:3066e614372f 155
GuillaumeCH 8:f2425e4302fc 156 vitesse_G = vitesse_G - Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle);
GuillaumeCH 8:f2425e4302fc 157 vitesse_D = vitesse_D + Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle);
GuillaumeCH 2:3066e614372f 158 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 159 //consigne_G = vitesse_G;
JimmyAREM 5:3638d7e7c5c1 160 if (stopCapteurs == true)
JimmyAREM 5:3638d7e7c5c1 161 {
JimmyAREM 5:3638d7e7c5c1 162 vitesse_nulle_D(0);
JimmyAREM 5:3638d7e7c5c1 163 vitesse_nulle_G(0);
JimmyAREM 5:3638d7e7c5c1 164 }
JimmyAREM 5:3638d7e7c5c1 165 else
JimmyAREM 5:3638d7e7c5c1 166 {
JimmyAREM 5:3638d7e7c5c1 167 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 168 }
GuillaumeCH 2:3066e614372f 169 actualise_position();
GuillaumeCH 2:3066e614372f 170 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 171 y_actuel = get_y_actuel();
GuillaumeCH 8:f2425e4302fc 172
GuillaumeCH 2:3066e614372f 173 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 174 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 175 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 176 }
GuillaumeCH 2:3066e614372f 177 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 178 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 179 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 180 }
GuillaumeCH 2:3066e614372f 181
GuillaumeCH 2:3066e614372f 182
JimmyAREM 6:e1585b8bd07d 183 void deplacement::ligne_droite_basique(long int distance)
GuillaumeCH 2:3066e614372f 184 {
GuillaumeCH 2:3066e614372f 185 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
GuillaumeCH 2:3066e614372f 186 motors_on();
GuillaumeCH 2:3066e614372f 187 actualise_position();
GuillaumeCH 2:3066e614372f 188 double x_ini = get_x_actuel();
GuillaumeCH 2:3066e614372f 189 double y_ini = get_y_actuel();
GuillaumeCH 2:3066e614372f 190 double angle_vise_deg = get_angle();
GuillaumeCH 2:3066e614372f 191 double angle_vise=angle_vise_deg*3.1416/180;
GuillaumeCH 2:3066e614372f 192 double angle = get_angle();
GuillaumeCH 2:3066e614372f 193
GuillaumeCH 2:3066e614372f 194 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 195 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 2:3066e614372f 196
GuillaumeCH 2:3066e614372f 197 double x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 198 double y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 199
GuillaumeCH 2:3066e614372f 200
GuillaumeCH 2:3066e614372f 201 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 202 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 203
GuillaumeCH 2:3066e614372f 204 //long int y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 205 float vitesse_G;
GuillaumeCH 2:3066e614372f 206 float vitesse_D;
GuillaumeCH 2:3066e614372f 207
GuillaumeCH 2:3066e614372f 208 angle = get_angle();
GuillaumeCH 2:3066e614372f 209 float Kpp= 0.05 ;
GuillaumeCH 2:3066e614372f 210 float Kdp= 10;
GuillaumeCH 2:3066e614372f 211 while (distance-x_local>0){
GuillaumeCH 2:3066e614372f 212
JimmyAREM 5:3638d7e7c5c1 213 if(detectionUltrasons)
JimmyAREM 5:3638d7e7c5c1 214 {
JimmyAREM 5:3638d7e7c5c1 215 detectionUltrasons = false;
JimmyAREM 5:3638d7e7c5c1 216 evitementValider = false;
JimmyAREM 5:3638d7e7c5c1 217 //writeCapteurs();
JimmyAREM 5:3638d7e7c5c1 218 LectureI2CCarteCapteur(*this);
JimmyAREM 5:3638d7e7c5c1 219 }
GuillaumeCH 2:3066e614372f 220 vitesse_G = (distance-x_local)/70;
GuillaumeCH 2:3066e614372f 221 vitesse_D = vitesse_G;
GuillaumeCH 2:3066e614372f 222 if(vitesse_G >400){
GuillaumeCH 2:3066e614372f 223 vitesse_G=400;
GuillaumeCH 2:3066e614372f 224 vitesse_D=400;
GuillaumeCH 2:3066e614372f 225 }
GuillaumeCH 2:3066e614372f 226 if (vitesse_G<-400){
GuillaumeCH 2:3066e614372f 227 vitesse_G=-400;
GuillaumeCH 2:3066e614372f 228 vitesse_D=-400;
GuillaumeCH 2:3066e614372f 229 }
GuillaumeCH 2:3066e614372f 230
GuillaumeCH 2:3066e614372f 231 angle = get_angle();
GuillaumeCH 2:3066e614372f 232
GuillaumeCH 8:f2425e4302fc 233 vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle);
GuillaumeCH 8:f2425e4302fc 234 vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle);
GuillaumeCH 2:3066e614372f 235 //consigne_D = vitesse_D;
GuillaumeCH 2:3066e614372f 236 //consigne_G = vitesse_G;
JimmyAREM 6:e1585b8bd07d 237 if (typeEvitement == ARRET)
JimmyAREM 5:3638d7e7c5c1 238 {
JimmyAREM 6:e1585b8bd07d 239 if(stopCapteurs == true)
JimmyAREM 5:3638d7e7c5c1 240 {
JimmyAREM 5:3638d7e7c5c1 241 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 242 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 243 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 244 }
JimmyAREM 6:e1585b8bd07d 245 else
JimmyAREM 6:e1585b8bd07d 246 {
JimmyAREM 6:e1585b8bd07d 247 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 248 }
JimmyAREM 6:e1585b8bd07d 249 }
JimmyAREM 6:e1585b8bd07d 250 actualise_position();
JimmyAREM 6:e1585b8bd07d 251 x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 252 y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 253 commande_vitesse(vitesse_G,vitesse_D);
GuillaumeCH 8:f2425e4302fc 254
JimmyAREM 6:e1585b8bd07d 255 //y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 256 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 257 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 258 }
JimmyAREM 6:e1585b8bd07d 259 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 260 rotation_abs(angle_vise_deg);
JimmyAREM 6:e1585b8bd07d 261 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 262 }
JimmyAREM 6:e1585b8bd07d 263
JimmyAREM 6:e1585b8bd07d 264
GuillaumeCH 8:f2425e4302fc 265 void deplacement::ligne_droite(long int distance, double x, double y, double cap){
JimmyAREM 6:e1585b8bd07d 266 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
JimmyAREM 6:e1585b8bd07d 267 motors_on();
JimmyAREM 6:e1585b8bd07d 268 actualise_position();
JimmyAREM 6:e1585b8bd07d 269 double x_ini = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 270 double y_ini = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 271 double angle_vise_deg = get_angle();
JimmyAREM 6:e1585b8bd07d 272 double angle_vise=angle_vise_deg*3.1416/180;
JimmyAREM 6:e1585b8bd07d 273 double angle = get_angle();
JimmyAREM 6:e1585b8bd07d 274
JimmyAREM 6:e1585b8bd07d 275 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 276 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 277
JimmyAREM 6:e1585b8bd07d 278 double x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 279 double y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 280
JimmyAREM 6:e1585b8bd07d 281
JimmyAREM 6:e1585b8bd07d 282 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 283 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 284
JimmyAREM 6:e1585b8bd07d 285 //long int y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 286 float vitesse_G;
JimmyAREM 6:e1585b8bd07d 287 float vitesse_D;
JimmyAREM 6:e1585b8bd07d 288
JimmyAREM 6:e1585b8bd07d 289 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 290 float Kpp= 0.05 ;
JimmyAREM 6:e1585b8bd07d 291 float Kdp= 10;
JimmyAREM 6:e1585b8bd07d 292 while (distance-x_local>0){
JimmyAREM 6:e1585b8bd07d 293
JimmyAREM 6:e1585b8bd07d 294 if(detectionUltrasons)
JimmyAREM 6:e1585b8bd07d 295 {
JimmyAREM 6:e1585b8bd07d 296 detectionUltrasons = false;
JimmyAREM 6:e1585b8bd07d 297 evitementValider = false;
JimmyAREM 6:e1585b8bd07d 298 //writeCapteurs();
JimmyAREM 6:e1585b8bd07d 299 LectureI2CCarteCapteur(*this);
JimmyAREM 6:e1585b8bd07d 300 }
JimmyAREM 6:e1585b8bd07d 301 vitesse_G = (distance-x_local)/70;
JimmyAREM 6:e1585b8bd07d 302 vitesse_D = vitesse_G;
JimmyAREM 6:e1585b8bd07d 303 if(vitesse_G >400){
JimmyAREM 6:e1585b8bd07d 304 vitesse_G=400;
JimmyAREM 6:e1585b8bd07d 305 vitesse_D=400;
JimmyAREM 6:e1585b8bd07d 306 }
JimmyAREM 6:e1585b8bd07d 307 if (vitesse_G<-400){
JimmyAREM 6:e1585b8bd07d 308 vitesse_G=-400;
JimmyAREM 6:e1585b8bd07d 309 vitesse_D=-400;
JimmyAREM 6:e1585b8bd07d 310 }
JimmyAREM 6:e1585b8bd07d 311
JimmyAREM 6:e1585b8bd07d 312 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 313
GuillaumeCH 8:f2425e4302fc 314 vitesse_G = vitesse_G + Kpp*y_local + Kdp * diff_angle(angle_vise_deg, angle);
GuillaumeCH 8:f2425e4302fc 315 vitesse_D = vitesse_D - Kpp*y_local - Kdp * diff_angle(angle_vise_deg, angle);
JimmyAREM 6:e1585b8bd07d 316 //consigne_D = vitesse_D;
JimmyAREM 6:e1585b8bd07d 317 //consigne_G = vitesse_G;
JimmyAREM 6:e1585b8bd07d 318 if (typeEvitement == ARRET)
JimmyAREM 6:e1585b8bd07d 319 {
JimmyAREM 6:e1585b8bd07d 320 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 321 {
JimmyAREM 6:e1585b8bd07d 322 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 323 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 324 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 325 }
JimmyAREM 6:e1585b8bd07d 326 else
JimmyAREM 6:e1585b8bd07d 327 {
JimmyAREM 6:e1585b8bd07d 328 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 329 }
JimmyAREM 6:e1585b8bd07d 330 }
JimmyAREM 6:e1585b8bd07d 331 else
JimmyAREM 6:e1585b8bd07d 332 {
JimmyAREM 6:e1585b8bd07d 333 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 334 {
JimmyAREM 6:e1585b8bd07d 335 evitement(x, y, cap);
JimmyAREM 6:e1585b8bd07d 336 return;
JimmyAREM 5:3638d7e7c5c1 337 }
JimmyAREM 5:3638d7e7c5c1 338 else
JimmyAREM 5:3638d7e7c5c1 339 {
JimmyAREM 6:e1585b8bd07d 340 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 5:3638d7e7c5c1 341 }
JimmyAREM 5:3638d7e7c5c1 342 }
GuillaumeCH 2:3066e614372f 343 actualise_position();
GuillaumeCH 2:3066e614372f 344 x_actuel = get_x_actuel();
GuillaumeCH 2:3066e614372f 345 y_actuel = get_y_actuel();
GuillaumeCH 2:3066e614372f 346 //y_local_prec = y_local;
GuillaumeCH 2:3066e614372f 347 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 2:3066e614372f 348 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
GuillaumeCH 2:3066e614372f 349 }
GuillaumeCH 2:3066e614372f 350 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 3:d38aa400d5e7 351 rotation_abs(angle_vise_deg);
GuillaumeCH 2:3066e614372f 352 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
GuillaumeCH 2:3066e614372f 353 }
GuillaumeCH 2:3066e614372f 354
GuillaumeCH 3:d38aa400d5e7 355 void deplacement::rotation_rel(double angle_vise)
GuillaumeCH 2:3066e614372f 356 {
GuillaumeCH 2:3066e614372f 357 // rotation de angle_vise
GuillaumeCH 2:3066e614372f 358 motors_on();
GuillaumeCH 2:3066e614372f 359 double vitesse=180;
GuillaumeCH 2:3066e614372f 360 int sens;
GuillaumeCH 2:3066e614372f 361 double angle = get_angle();
GuillaumeCH 2:3066e614372f 362 angle_vise+=angle;
GuillaumeCH 2:3066e614372f 363 borne_angle_d(angle_vise);
GuillaumeCH 2:3066e614372f 364 if (diff_angle(angle,angle_vise)<=0){
GuillaumeCH 2:3066e614372f 365 sens = -1;
GuillaumeCH 2:3066e614372f 366 //printf("negatif\n");
GuillaumeCH 2:3066e614372f 367 }
GuillaumeCH 2:3066e614372f 368 else{
GuillaumeCH 2:3066e614372f 369 sens = 1;
GuillaumeCH 2:3066e614372f 370
GuillaumeCH 2:3066e614372f 371 //printf("positif\n");
GuillaumeCH 2:3066e614372f 372 }
GuillaumeCH 2:3066e614372f 373 //printf("diff : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 374 while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
GuillaumeCH 2:3066e614372f 375 {
GuillaumeCH 2:3066e614372f 376 actualise_position();
GuillaumeCH 2:3066e614372f 377 angle = get_angle();
GuillaumeCH 8:f2425e4302fc 378 vitesse=3*sens*abs(diff_angle(angle,angle_vise));
GuillaumeCH 8:f2425e4302fc 379 if (vitesse > 90){
GuillaumeCH 8:f2425e4302fc 380 vitesse = 90;
GuillaumeCH 8:f2425e4302fc 381 }
GuillaumeCH 8:f2425e4302fc 382 if (vitesse < -90){
GuillaumeCH 8:f2425e4302fc 383 vitesse = -90;
GuillaumeCH 8:f2425e4302fc 384 }
GuillaumeCH 8:f2425e4302fc 385
GuillaumeCH 2:3066e614372f 386
GuillaumeCH 2:3066e614372f 387 commande_vitesse(-vitesse,vitesse);
GuillaumeCH 8:f2425e4302fc 388
GuillaumeCH 2:3066e614372f 389 //printf("vitesse : %lf ", vitesse);
GuillaumeCH 2:3066e614372f 390 }
GuillaumeCH 2:3066e614372f 391 //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
GuillaumeCH 2:3066e614372f 392 //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 393 //consigne_D = 0;
GuillaumeCH 2:3066e614372f 394 //consigne_G = 0;
GuillaumeCH 2:3066e614372f 395 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 396 vitesse_nulle_D(0);
GuillaumeCH 3:d38aa400d5e7 397 wait(0.1);
GuillaumeCH 2:3066e614372f 398 motors_stop();
GuillaumeCH 2:3066e614372f 399 }
GuillaumeCH 2:3066e614372f 400
GuillaumeCH 2:3066e614372f 401
GuillaumeCH 3:d38aa400d5e7 402 void deplacement::rotation_abs(double angle_vise)
GuillaumeCH 2:3066e614372f 403 {
GuillaumeCH 2:3066e614372f 404 actualise_position();
GuillaumeCH 2:3066e614372f 405 //printf("bite");
GuillaumeCH 2:3066e614372f 406 double angle_rel = borne_angle_d(angle_vise-get_angle());
GuillaumeCH 3:d38aa400d5e7 407 rotation_rel(angle_rel);
GuillaumeCH 2:3066e614372f 408 }
GuillaumeCH 2:3066e614372f 409
GuillaumeCH 2:3066e614372f 410
GuillaumeCH 2:3066e614372f 411
GuillaumeCH 3:d38aa400d5e7 412
JimmyAREM 5:3638d7e7c5c1 413 void deplacement::poussette(float temps){
GuillaumeCH 2:3066e614372f 414 motors_on();
JimmyAREM 5:3638d7e7c5c1 415 commande_vitesse(100,100);
JimmyAREM 5:3638d7e7c5c1 416 wait_ms(temps);
GuillaumeCH 2:3066e614372f 417 vitesse_nulle_G(0);
GuillaumeCH 2:3066e614372f 418 vitesse_nulle_D(0);
GuillaumeCH 2:3066e614372f 419 motors_stop();
GuillaumeCH 3:d38aa400d5e7 420 }
JimmyAREM 5:3638d7e7c5c1 421
GuillaumeCH 3:d38aa400d5e7 422 void deplacement::initialisation(){
GuillaumeCH 3:d38aa400d5e7 423 init_odometrie();
GuillaumeCH 3:d38aa400d5e7 424 init_hardware();
GuillaumeCH 3:d38aa400d5e7 425 srand(time(NULL));
GuillaumeCH 3:d38aa400d5e7 426 motors_on();
GuillaumeCH 4:eac6746544fb 427 }
JimmyAREM 6:e1585b8bd07d 428
JimmyAREM 7:6b15a1feed2d 429
JimmyAREM 6:e1585b8bd07d 430 void deplacement::arc(Coordonnees p1, Coordonnees p2, int sens){
GuillaumeCH 4:eac6746544fb 431 actualise_position();
GuillaumeCH 4:eac6746544fb 432 float vitesse_G;
GuillaumeCH 4:eac6746544fb 433 float vitesse_D;
GuillaumeCH 4:eac6746544fb 434 double x_ini = get_x_actuel();
GuillaumeCH 4:eac6746544fb 435 double y_ini = get_y_actuel();
GuillaumeCH 4:eac6746544fb 436 double angle_vise_deg = get_angle();
GuillaumeCH 4:eac6746544fb 437 double angle_vise=angle_vise_deg*3.1416/180.0;
GuillaumeCH 4:eac6746544fb 438 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 439
GuillaumeCH 4:eac6746544fb 440 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 441 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
GuillaumeCH 4:eac6746544fb 442
GuillaumeCH 4:eac6746544fb 443 double x_actuel = get_x_actuel();
GuillaumeCH 4:eac6746544fb 444 double y_actuel = get_y_actuel();
GuillaumeCH 4:eac6746544fb 445
GuillaumeCH 4:eac6746544fb 446
GuillaumeCH 4:eac6746544fb 447 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
GuillaumeCH 4:eac6746544fb 448 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 7:6b15a1feed2d 449 /*double p1x = p1.x*cos(angle_vise) + p1.y*sin(angle_vise)-x_local_ini;
JimmyAREM 7:6b15a1feed2d 450 double p2x = p2.x*cos(angle_vise) + p2.y*sin(angle_vise)-x_local_ini;
JimmyAREM 7:6b15a1feed2d 451 double p1y = p1.y*cos(angle_vise) - p1.x*sin(angle_vise)-y_local_ini;
JimmyAREM 7:6b15a1feed2d 452 double p2y = p2.y*cos(angle_vise) - p2.x*sin(angle_vise)-y_local_ini;*/
GuillaumeCH 4:eac6746544fb 453
GuillaumeCH 4:eac6746544fb 454 Coordonnees p0;
GuillaumeCH 4:eac6746544fb 455 p0.x = x_local;
GuillaumeCH 4:eac6746544fb 456 p0.y = y_local;
GuillaumeCH 8:f2425e4302fc 457 //printf("xo: %lf yo:%lf \n", p0.x,p0.y);
JimmyAREM 7:6b15a1feed2d 458 /*p1.x = p1x;
JimmyAREM 7:6b15a1feed2d 459 p2.x = p2x;
JimmyAREM 7:6b15a1feed2d 460 p1.y = p1y;
JimmyAREM 7:6b15a1feed2d 461 p2.y = p2y;*/
JimmyAREM 6:e1585b8bd07d 462
GuillaumeCH 4:eac6746544fb 463 cercle(p0,p1,p2);
JimmyAREM 5:3638d7e7c5c1 464 double xc = point[0];
JimmyAREM 5:3638d7e7c5c1 465 double yc = point[1];
JimmyAREM 5:3638d7e7c5c1 466 double Rc = point[2];
JimmyAREM 6:e1585b8bd07d 467 double angle_tan;
GuillaumeCH 4:eac6746544fb 468
JimmyAREM 6:e1585b8bd07d 469 double angle_a_parcourir = recup_angle_entre_trois_points_213(xc,yc,p0.x,p0.y,p2.x,p2.y);
GuillaumeCH 8:f2425e4302fc 470 //printf("angle_a_parcourirrir : %lf\n",angle_a_parcourir);
JimmyAREM 6:e1585b8bd07d 471
JimmyAREM 6:e1585b8bd07d 472 if (angle_a_parcourir >0 && sens == A_DROITE){
JimmyAREM 6:e1585b8bd07d 473 angle_a_parcourir -=360.0 ;
JimmyAREM 5:3638d7e7c5c1 474 }
JimmyAREM 6:e1585b8bd07d 475 if (angle_a_parcourir <0 && sens == A_GAUCHE){
JimmyAREM 6:e1585b8bd07d 476 angle_a_parcourir +=360;
JimmyAREM 6:e1585b8bd07d 477 }
JimmyAREM 6:e1585b8bd07d 478
JimmyAREM 6:e1585b8bd07d 479 if (angle_a_parcourir >= 0)
JimmyAREM 6:e1585b8bd07d 480 {
JimmyAREM 6:e1585b8bd07d 481 if (p0.y != yc){
JimmyAREM 6:e1585b8bd07d 482 angle_tan = -atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 7:6b15a1feed2d 483 if (xc<0){
JimmyAREM 7:6b15a1feed2d 484 angle_tan -=180.0;
JimmyAREM 7:6b15a1feed2d 485 }
JimmyAREM 5:3638d7e7c5c1 486 }
JimmyAREM 6:e1585b8bd07d 487 else{
JimmyAREM 6:e1585b8bd07d 488 angle_tan = 90;
JimmyAREM 7:6b15a1feed2d 489 if (xc<0){
JimmyAREM 7:6b15a1feed2d 490 angle_tan -=180.0;
JimmyAREM 7:6b15a1feed2d 491 }
GuillaumeCH 4:eac6746544fb 492 }
GuillaumeCH 4:eac6746544fb 493 }
JimmyAREM 6:e1585b8bd07d 494 else
JimmyAREM 6:e1585b8bd07d 495 {
JimmyAREM 6:e1585b8bd07d 496 if (p0.y != yc){
JimmyAREM 6:e1585b8bd07d 497 angle_tan = atan((p0.x-xc)/(p0.y-yc))*180.0/3.1416;
JimmyAREM 7:6b15a1feed2d 498 if (xc<0){
JimmyAREM 7:6b15a1feed2d 499 angle_tan+=180.0;
JimmyAREM 7:6b15a1feed2d 500 }
JimmyAREM 6:e1585b8bd07d 501 }
JimmyAREM 6:e1585b8bd07d 502 else{
JimmyAREM 6:e1585b8bd07d 503 angle_tan = -90;
JimmyAREM 7:6b15a1feed2d 504 if (xc<0){
JimmyAREM 7:6b15a1feed2d 505 angle_tan+=180.0;
JimmyAREM 7:6b15a1feed2d 506 }
JimmyAREM 6:e1585b8bd07d 507 }
JimmyAREM 7:6b15a1feed2d 508
JimmyAREM 5:3638d7e7c5c1 509 }
JimmyAREM 6:e1585b8bd07d 510
JimmyAREM 6:e1585b8bd07d 511 //printf("angle_tan : %lf\n",angle_tan);
JimmyAREM 7:6b15a1feed2d 512 rotation_rel(angle_tan);
JimmyAREM 5:3638d7e7c5c1 513 actualise_position();
JimmyAREM 5:3638d7e7c5c1 514 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 515 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 516 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 517 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 518 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 519 //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 520 //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 521 motors_on();
JimmyAREM 6:e1585b8bd07d 522
JimmyAREM 6:e1585b8bd07d 523
JimmyAREM 5:3638d7e7c5c1 524 if (angle_a_parcourir < 0){
JimmyAREM 6:e1585b8bd07d 525 double mon_angle_ini = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 526 double mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 527 double mon_angle_prec;
JimmyAREM 6:e1585b8bd07d 528 if (mon_angle >= 0){
JimmyAREM 6:e1585b8bd07d 529 mon_angle -=360.0;
JimmyAREM 6:e1585b8bd07d 530 mon_angle_ini -=360.0;
JimmyAREM 6:e1585b8bd07d 531 }
JimmyAREM 6:e1585b8bd07d 532 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 533 while (!( mon_angle_prec + mon_angle < -359.9 && abs(mon_angle - mon_angle_prec) > 100)){
JimmyAREM 6:e1585b8bd07d 534 vitesse_D = abs(15 * mon_angle);
JimmyAREM 5:3638d7e7c5c1 535 if (vitesse_D>300){
JimmyAREM 5:3638d7e7c5c1 536 vitesse_D = 300;
JimmyAREM 5:3638d7e7c5c1 537 }
JimmyAREM 5:3638d7e7c5c1 538 vitesse_G = vitesse_D*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
JimmyAREM 6:e1585b8bd07d 539 double correction = int_ext_cercle(x_local,y_local);
JimmyAREM 7:6b15a1feed2d 540 double correction_en_cours = correction*0.035 - 3*diff_angle(borne_angle_d(angle_tan-angle_vise_deg),angle);
GuillaumeCH 8:f2425e4302fc 541 if (correction_en_cours>30){
GuillaumeCH 8:f2425e4302fc 542 correction_en_cours = 30;
JimmyAREM 7:6b15a1feed2d 543 }
GuillaumeCH 8:f2425e4302fc 544 if (correction_en_cours < -30){
GuillaumeCH 8:f2425e4302fc 545 correction_en_cours = -30;
JimmyAREM 7:6b15a1feed2d 546 }
JimmyAREM 7:6b15a1feed2d 547 vitesse_D= vitesse_D-correction_en_cours;
JimmyAREM 7:6b15a1feed2d 548 vitesse_G= vitesse_G+correction_en_cours;
JimmyAREM 7:6b15a1feed2d 549
JimmyAREM 7:6b15a1feed2d 550 //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 551
JimmyAREM 6:e1585b8bd07d 552 if(detectionUltrasons)
JimmyAREM 6:e1585b8bd07d 553 {
JimmyAREM 6:e1585b8bd07d 554 detectionUltrasons = false;
JimmyAREM 6:e1585b8bd07d 555 evitementValider = false;
JimmyAREM 6:e1585b8bd07d 556 //writeCapteurs();
JimmyAREM 6:e1585b8bd07d 557 LectureI2CCarteCapteur(*this);
JimmyAREM 6:e1585b8bd07d 558 }
JimmyAREM 6:e1585b8bd07d 559 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 560 {
JimmyAREM 6:e1585b8bd07d 561 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 562 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 563 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 564 }
JimmyAREM 6:e1585b8bd07d 565 else
JimmyAREM 6:e1585b8bd07d 566 {
JimmyAREM 6:e1585b8bd07d 567 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 568 }
JimmyAREM 5:3638d7e7c5c1 569 actualise_position();
JimmyAREM 5:3638d7e7c5c1 570 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 571 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 572 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 573 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 574 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 575 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 576 mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 577
JimmyAREM 6:e1585b8bd07d 578
JimmyAREM 6:e1585b8bd07d 579 if (mon_angle >= 0){
JimmyAREM 6:e1585b8bd07d 580 mon_angle -=360.0;
JimmyAREM 5:3638d7e7c5c1 581 }
JimmyAREM 6:e1585b8bd07d 582
JimmyAREM 6:e1585b8bd07d 583 if (y_local-yc>=0){//gauche du cercle
JimmyAREM 6:e1585b8bd07d 584 if (y_local != yc){
JimmyAREM 6:e1585b8bd07d 585 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 586 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 + 180;
JimmyAREM 6:e1585b8bd07d 587 }
JimmyAREM 6:e1585b8bd07d 588 else{//haut droite
JimmyAREM 6:e1585b8bd07d 589 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 - 180;
JimmyAREM 6:e1585b8bd07d 590 }
JimmyAREM 6:e1585b8bd07d 591 }
JimmyAREM 6:e1585b8bd07d 592 else{
JimmyAREM 6:e1585b8bd07d 593 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 594 angle_tan = 90;
JimmyAREM 6:e1585b8bd07d 595 }
JimmyAREM 6:e1585b8bd07d 596 else{//haut droite
JimmyAREM 6:e1585b8bd07d 597 angle_tan = -90;
JimmyAREM 6:e1585b8bd07d 598 }
JimmyAREM 6:e1585b8bd07d 599 }
JimmyAREM 5:3638d7e7c5c1 600 }
JimmyAREM 6:e1585b8bd07d 601 else{//partie droite
JimmyAREM 6:e1585b8bd07d 602 if (x_local-xc>= 0){// haut droite
JimmyAREM 6:e1585b8bd07d 603 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 604 }
JimmyAREM 6:e1585b8bd07d 605 else{ //bas droite
JimmyAREM 6:e1585b8bd07d 606 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 607 }
JimmyAREM 6:e1585b8bd07d 608 }
JimmyAREM 7:6b15a1feed2d 609 if (xc<0){
JimmyAREM 7:6b15a1feed2d 610 angle_tan+=180.0;
JimmyAREM 7:6b15a1feed2d 611 }
JimmyAREM 5:3638d7e7c5c1 612 }
JimmyAREM 5:3638d7e7c5c1 613 }
JimmyAREM 6:e1585b8bd07d 614 if (angle_a_parcourir>= 0){
JimmyAREM 6:e1585b8bd07d 615 double mon_angle_ini = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 616 double mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 617 double mon_angle_prec;
JimmyAREM 6:e1585b8bd07d 618 if (mon_angle <= 0){
JimmyAREM 6:e1585b8bd07d 619 mon_angle +=360.0;
JimmyAREM 6:e1585b8bd07d 620 mon_angle_ini +=360.0;
JimmyAREM 6:e1585b8bd07d 621 }
JimmyAREM 6:e1585b8bd07d 622 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 623 //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 624 //while(!(((mon_angle_ini - mon_angle) > angle_a_parcourir) && ((mon_angle_ini - mon_angle_prec) >= angle_a_parcourir))){
JimmyAREM 6:e1585b8bd07d 625 while (!( mon_angle_prec + mon_angle > 359.9 && abs(mon_angle - mon_angle_prec) > 100)){
JimmyAREM 7:6b15a1feed2d 626
JimmyAREM 6:e1585b8bd07d 627 //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 628 vitesse_G = 15 * mon_angle;
JimmyAREM 5:3638d7e7c5c1 629 if (abs(vitesse_G)>300){
JimmyAREM 5:3638d7e7c5c1 630 vitesse_G = 300;
JimmyAREM 5:3638d7e7c5c1 631 }
JimmyAREM 5:3638d7e7c5c1 632 vitesse_D = vitesse_G*(Rc-ECART_ROUE/2.0)/(Rc + ECART_ROUE/2.0);
JimmyAREM 5:3638d7e7c5c1 633 double correction = int_ext_cercle(x_local,y_local);
JimmyAREM 7:6b15a1feed2d 634 double correction_en_cours = correction*0.035 - 3*diff_angle(borne_angle_d(angle_tan+angle_vise_deg),angle);
GuillaumeCH 8:f2425e4302fc 635 if (correction_en_cours>30){
GuillaumeCH 8:f2425e4302fc 636 correction_en_cours = 30;
JimmyAREM 7:6b15a1feed2d 637 }
GuillaumeCH 8:f2425e4302fc 638 if (correction_en_cours < -30){
GuillaumeCH 8:f2425e4302fc 639 correction_en_cours = -30;
JimmyAREM 7:6b15a1feed2d 640 }
JimmyAREM 7:6b15a1feed2d 641 vitesse_D= vitesse_D+correction_en_cours;
JimmyAREM 7:6b15a1feed2d 642 vitesse_G= vitesse_G-correction_en_cours;
JimmyAREM 7:6b15a1feed2d 643 //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 644 if(detectionUltrasons)
JimmyAREM 6:e1585b8bd07d 645 {
JimmyAREM 6:e1585b8bd07d 646 detectionUltrasons = false;
JimmyAREM 6:e1585b8bd07d 647 evitementValider = false;
JimmyAREM 6:e1585b8bd07d 648 //writeCapteurs();
JimmyAREM 6:e1585b8bd07d 649 LectureI2CCarteCapteur(*this);
JimmyAREM 6:e1585b8bd07d 650 }
JimmyAREM 6:e1585b8bd07d 651 if(stopCapteurs == true)
JimmyAREM 6:e1585b8bd07d 652 {
JimmyAREM 6:e1585b8bd07d 653 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 654 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 655 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 656 }
JimmyAREM 6:e1585b8bd07d 657 else
JimmyAREM 6:e1585b8bd07d 658 {
JimmyAREM 6:e1585b8bd07d 659 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 660 }
JimmyAREM 5:3638d7e7c5c1 661 actualise_position();
JimmyAREM 5:3638d7e7c5c1 662 x_actuel = get_x_actuel();
JimmyAREM 5:3638d7e7c5c1 663 y_actuel = get_y_actuel();
JimmyAREM 5:3638d7e7c5c1 664 angle = get_angle();
JimmyAREM 5:3638d7e7c5c1 665 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 5:3638d7e7c5c1 666 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 667 mon_angle_prec = mon_angle;
JimmyAREM 6:e1585b8bd07d 668 mon_angle = recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y);
JimmyAREM 6:e1585b8bd07d 669 if (mon_angle <= 0){
JimmyAREM 6:e1585b8bd07d 670 mon_angle +=360.0;
JimmyAREM 5:3638d7e7c5c1 671 }
JimmyAREM 6:e1585b8bd07d 672 if (y_local-yc>=0){//gauche du cercle
JimmyAREM 6:e1585b8bd07d 673 if (y_local != yc){
JimmyAREM 6:e1585b8bd07d 674 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 675 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 676 }
JimmyAREM 6:e1585b8bd07d 677 else{//haut droite
JimmyAREM 6:e1585b8bd07d 678 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416;
JimmyAREM 6:e1585b8bd07d 679 }
JimmyAREM 6:e1585b8bd07d 680 }
JimmyAREM 6:e1585b8bd07d 681 else{
JimmyAREM 6:e1585b8bd07d 682 if (x_local-xc>= 0){//haut gauche
JimmyAREM 6:e1585b8bd07d 683 angle_tan = -90;
JimmyAREM 6:e1585b8bd07d 684 }
JimmyAREM 6:e1585b8bd07d 685 else{//haut droite
JimmyAREM 6:e1585b8bd07d 686 angle_tan = 90;
JimmyAREM 6:e1585b8bd07d 687 }
JimmyAREM 6:e1585b8bd07d 688 }
JimmyAREM 5:3638d7e7c5c1 689 }
JimmyAREM 6:e1585b8bd07d 690 else{//partie droite
JimmyAREM 6:e1585b8bd07d 691 if (x_local-xc>= 0){// haut droite
JimmyAREM 6:e1585b8bd07d 692 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 - 180.0 ;
JimmyAREM 6:e1585b8bd07d 693 }
JimmyAREM 6:e1585b8bd07d 694 else{ //bas droite
JimmyAREM 6:e1585b8bd07d 695 angle_tan = -atan((x_local-xc)/(y_local-yc))*180.0/3.1416 + 180.0 ;
JimmyAREM 6:e1585b8bd07d 696 }
JimmyAREM 6:e1585b8bd07d 697 }
JimmyAREM 7:6b15a1feed2d 698
JimmyAREM 7:6b15a1feed2d 699
JimmyAREM 6:e1585b8bd07d 700 }
JimmyAREM 6:e1585b8bd07d 701 //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 702
GuillaumeCH 4:eac6746544fb 703 }
JimmyAREM 6:e1585b8bd07d 704
JimmyAREM 5:3638d7e7c5c1 705 //printf("distance : %lf ",recup_angle_entre_trois_points_213(xc,yc,x_local,y_local,p2.x,p2.y));
JimmyAREM 5:3638d7e7c5c1 706 //printf("x_loc : %lf, y_loc : %lf\n",x_local,y_local);
GuillaumeCH 4:eac6746544fb 707 vitesse_nulle_D(0);
GuillaumeCH 4:eac6746544fb 708 vitesse_nulle_G(0);
GuillaumeCH 4:eac6746544fb 709 wait(0.2);
GuillaumeCH 4:eac6746544fb 710 motors_stop();
GuillaumeCH 4:eac6746544fb 711
GuillaumeCH 4:eac6746544fb 712 }
GuillaumeCH 4:eac6746544fb 713
GuillaumeCH 4:eac6746544fb 714
GuillaumeCH 4:eac6746544fb 715 int deplacement::cercle(Coordonnees a,Coordonnees b, Coordonnees c){
GuillaumeCH 4:eac6746544fb 716 double ax2 = pow(a.x,2);
GuillaumeCH 4:eac6746544fb 717 double ay2 = pow(a.y,2);
GuillaumeCH 4:eac6746544fb 718 double bx2 = pow(b.x,2);
GuillaumeCH 4:eac6746544fb 719 double by2 = pow(b.y,2);
GuillaumeCH 4:eac6746544fb 720 double cx2 = pow(c.x,2);
GuillaumeCH 4:eac6746544fb 721 double cy2 = pow(c.y,2);
JimmyAREM 5:3638d7e7c5c1 722 double pente_a=0;
GuillaumeCH 4:eac6746544fb 723 double ord_b;
JimmyAREM 5:3638d7e7c5c1 724 double pente_A=0;
GuillaumeCH 4:eac6746544fb 725 double ord_B;
GuillaumeCH 4:eac6746544fb 726 double xc;
GuillaumeCH 4:eac6746544fb 727 double yc;
GuillaumeCH 4:eac6746544fb 728 double Rc;
GuillaumeCH 4:eac6746544fb 729 if (a.y == b.y && b.y == c.y){
GuillaumeCH 4:eac6746544fb 730 printf("Les points sont alignes1\n");
GuillaumeCH 4:eac6746544fb 731 return 0;
GuillaumeCH 4:eac6746544fb 732 }
GuillaumeCH 4:eac6746544fb 733 if (b.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 734 pente_a = -1*(b.x-a.x)/(b.y-a.y);
GuillaumeCH 4:eac6746544fb 735 printf("a : %lf ",pente_a);
GuillaumeCH 4:eac6746544fb 736 ord_b = (by2-ay2+bx2-ax2)/(2*(b.y-a.y));
GuillaumeCH 4:eac6746544fb 737 printf("b : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 738
GuillaumeCH 4:eac6746544fb 739 }
GuillaumeCH 4:eac6746544fb 740 else{
JimmyAREM 5:3638d7e7c5c1 741 cercle(c,a,b);
JimmyAREM 5:3638d7e7c5c1 742 return 0;
GuillaumeCH 4:eac6746544fb 743
GuillaumeCH 4:eac6746544fb 744 }
GuillaumeCH 4:eac6746544fb 745 if (c.y-a.y !=0){
GuillaumeCH 4:eac6746544fb 746 pente_A = -1*(c.x-a.x)/(c.y-a.y);
GuillaumeCH 4:eac6746544fb 747 printf("A : %lf ",pente_A);
GuillaumeCH 4:eac6746544fb 748 ord_B = (cy2-ay2+cx2-ax2)/(2*(c.y-a.y));
GuillaumeCH 4:eac6746544fb 749 printf("B : %lf ",ord_B);
GuillaumeCH 4:eac6746544fb 750
GuillaumeCH 4:eac6746544fb 751 }
GuillaumeCH 4:eac6746544fb 752 else{
JimmyAREM 5:3638d7e7c5c1 753 cercle(b,c,a);
JimmyAREM 5:3638d7e7c5c1 754 return 0;
GuillaumeCH 4:eac6746544fb 755 }
GuillaumeCH 4:eac6746544fb 756
GuillaumeCH 4:eac6746544fb 757
GuillaumeCH 4:eac6746544fb 758 if (pente_a-pente_A!=0){
GuillaumeCH 4:eac6746544fb 759 xc = (ord_B-ord_b)/(pente_a-pente_A);
GuillaumeCH 4:eac6746544fb 760 yc = pente_a*xc+ord_b;
GuillaumeCH 4:eac6746544fb 761 Rc = pow((pow(xc-a.x,2)+pow(yc-a.y,2)),0.5);
GuillaumeCH 4:eac6746544fb 762 point[0] = xc;
GuillaumeCH 4:eac6746544fb 763 point[1] = yc;
GuillaumeCH 4:eac6746544fb 764 point[2] = Rc;
GuillaumeCH 4:eac6746544fb 765 printf("xc : %f, yc : %f, Rc : %f\n",xc,yc,Rc);
GuillaumeCH 4:eac6746544fb 766 }
GuillaumeCH 4:eac6746544fb 767 else{
GuillaumeCH 4:eac6746544fb 768 printf("Les points sont alignes2\n");
GuillaumeCH 4:eac6746544fb 769 return 0;
GuillaumeCH 4:eac6746544fb 770 }
GuillaumeCH 4:eac6746544fb 771
GuillaumeCH 4:eac6746544fb 772
GuillaumeCH 4:eac6746544fb 773 return 0;
GuillaumeCH 4:eac6746544fb 774 }
GuillaumeCH 4:eac6746544fb 775 double deplacement::int_ext_cercle(double x, double y){
GuillaumeCH 4:eac6746544fb 776 double xc= point[0];
GuillaumeCH 4:eac6746544fb 777 double yc= point[1];
GuillaumeCH 4:eac6746544fb 778 double Rc= point[2];
GuillaumeCH 4:eac6746544fb 779 double rayon = pow((pow(xc-x,2)+pow(yc-y,2)),0.5);
GuillaumeCH 4:eac6746544fb 780 return Rc-rayon;
GuillaumeCH 4:eac6746544fb 781
GuillaumeCH 4:eac6746544fb 782 }
GuillaumeCH 4:eac6746544fb 783
GuillaumeCH 4:eac6746544fb 784 void deplacement::va_au_point(double x,double y, double cap){
GuillaumeCH 4:eac6746544fb 785 actualise_position();
GuillaumeCH 4:eac6746544fb 786 double angle = get_angle();
GuillaumeCH 4:eac6746544fb 787 double x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 788 double y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 789 double x_projete = 10000.0*cos(angle*3.1416/180.0)+x_robot;
GuillaumeCH 4:eac6746544fb 790 double y_projete = 10000.0*sin(angle*3.1416/180.0)+y_robot;
JimmyAREM 5:3638d7e7c5c1 791 printf("angle ::: %lf\n",recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete));
JimmyAREM 5:3638d7e7c5c1 792 rotation_rel(recup_angle_entre_trois_points_213(x_robot,y_robot,x,y,x_projete,y_projete));
GuillaumeCH 4:eac6746544fb 793 //printf("oui\n");
GuillaumeCH 4:eac6746544fb 794 actualise_position();
GuillaumeCH 4:eac6746544fb 795 angle = get_angle();
GuillaumeCH 4:eac6746544fb 796 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 797 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 798 double distance = pow(pow(x-x_robot,2)+pow(y-y_robot,2),0.5);
GuillaumeCH 4:eac6746544fb 799 printf("distance : %lf\n",distance);
JimmyAREM 6:e1585b8bd07d 800 ligne_droite(distance, x, y, cap);
GuillaumeCH 4:eac6746544fb 801 actualise_position();
GuillaumeCH 4:eac6746544fb 802 angle = get_angle();
GuillaumeCH 4:eac6746544fb 803 x_robot = get_x_actuel();
GuillaumeCH 4:eac6746544fb 804 y_robot = get_y_actuel();
GuillaumeCH 4:eac6746544fb 805 rotation_abs(cap);
GuillaumeCH 4:eac6746544fb 806
GuillaumeCH 4:eac6746544fb 807
GuillaumeCH 4:eac6746544fb 808 }
GuillaumeCH 4:eac6746544fb 809
GuillaumeCH 4:eac6746544fb 810 double deplacement::recup_angle_entre_trois_points_213(double x1,double y1,double x2,double y2,double x3,double y3){
JimmyAREM 5:3638d7e7c5c1 811 double x13 = x3-x1;
JimmyAREM 5:3638d7e7c5c1 812 double y13 = y3-y1;
GuillaumeCH 4:eac6746544fb 813 double x12 = x2-x1;
GuillaumeCH 4:eac6746544fb 814 double y12 = y2-y1;
GuillaumeCH 4:eac6746544fb 815 double norme12 = pow(x12*x12+y12*y12,0.5);
GuillaumeCH 4:eac6746544fb 816 double norme13 = pow(x13*x13+y13*y13,0.5);
JimmyAREM 5:3638d7e7c5c1 817 double ux = x13/norme13;
JimmyAREM 5:3638d7e7c5c1 818 double uy = y13/norme13;
JimmyAREM 5:3638d7e7c5c1 819 double wx = x12/norme12;
JimmyAREM 5:3638d7e7c5c1 820 double wy = y12/norme12;
JimmyAREM 5:3638d7e7c5c1 821 //printf("u : %lf,%lf ,v : %lf,%lf ,w : %lf,%lf\n",ux,uy,-uy,ux,wx,wy);
GuillaumeCH 4:eac6746544fb 822 double prod_scal = x12*x13+y12*y13;
JimmyAREM 5:3638d7e7c5c1 823 double cos_angle = prod_scal/(norme12*norme13);
JimmyAREM 5:3638d7e7c5c1 824 double sin_angle;
JimmyAREM 5:3638d7e7c5c1 825 if (uy!=0){
JimmyAREM 5:3638d7e7c5c1 826 sin_angle = -1*(wx - cos_angle*ux)/uy;
JimmyAREM 5:3638d7e7c5c1 827 }
JimmyAREM 5:3638d7e7c5c1 828 else{
JimmyAREM 5:3638d7e7c5c1 829 sin_angle = (wy - cos_angle*uy)/ux;
JimmyAREM 5:3638d7e7c5c1 830 }
JimmyAREM 5:3638d7e7c5c1 831 //printf("cos : %lf sin : %lf\n",cos_angle,sin_angle);
JimmyAREM 5:3638d7e7c5c1 832 if (sin_angle >=0){
JimmyAREM 5:3638d7e7c5c1 833 return acos(prod_scal/(norme12*norme13))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 834 }
JimmyAREM 5:3638d7e7c5c1 835 else{
JimmyAREM 5:3638d7e7c5c1 836 return -acos(prod_scal/(norme12*norme13))*180.0/3.1416;
JimmyAREM 5:3638d7e7c5c1 837 }
GuillaumeCH 4:eac6746544fb 838 }
JimmyAREM 6:e1585b8bd07d 839
JimmyAREM 6:e1585b8bd07d 840
JimmyAREM 6:e1585b8bd07d 841 void deplacement::pente(long int distance, float vitesse, double angle_a_tourner)
JimmyAREM 6:e1585b8bd07d 842 {
JimmyAREM 6:e1585b8bd07d 843 Timer time;
JimmyAREM 6:e1585b8bd07d 844 time.start();
JimmyAREM 6:e1585b8bd07d 845 // le robot avance en ligne droite sur une distance donnée, à la vitesse voulue (entre 0 et 900)
JimmyAREM 6:e1585b8bd07d 846 motors_on();
JimmyAREM 6:e1585b8bd07d 847 actualise_position();
JimmyAREM 6:e1585b8bd07d 848 double x_ini = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 849 double y_ini = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 850 double angle_vise_deg = get_angle();
JimmyAREM 6:e1585b8bd07d 851 double angle_vise=angle_vise_deg*3.1416/180;
JimmyAREM 6:e1585b8bd07d 852 double angle = get_angle();
JimmyAREM 6:e1585b8bd07d 853
JimmyAREM 6:e1585b8bd07d 854 double x_local_ini = x_ini*cos(angle_vise) + y_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 855 double y_local_ini = y_ini*cos(angle_vise) - x_ini*sin(angle_vise);
JimmyAREM 6:e1585b8bd07d 856
JimmyAREM 6:e1585b8bd07d 857 double x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 858 double y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 859
JimmyAREM 6:e1585b8bd07d 860
JimmyAREM 6:e1585b8bd07d 861 double x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 862 double y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 863
JimmyAREM 6:e1585b8bd07d 864 //long int y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 865 float vitesse_G;
JimmyAREM 6:e1585b8bd07d 866 float vitesse_D;
JimmyAREM 6:e1585b8bd07d 867
JimmyAREM 6:e1585b8bd07d 868 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 869 while (distance-x_local>0){
JimmyAREM 6:e1585b8bd07d 870
GuillaumeCH 8:f2425e4302fc 871 vitesse_G = vitesse+y_local*0.02 + 5 * diff_angle(angle_vise_deg, angle);;
GuillaumeCH 8:f2425e4302fc 872 vitesse_D = vitesse-y_local*0.02 - 5 * diff_angle(angle_vise_deg, angle);;
JimmyAREM 6:e1585b8bd07d 873
JimmyAREM 6:e1585b8bd07d 874
JimmyAREM 6:e1585b8bd07d 875 commande_vitesse(vitesse_G,vitesse_D);
JimmyAREM 6:e1585b8bd07d 876 actualise_position();
JimmyAREM 6:e1585b8bd07d 877 x_actuel = get_x_actuel();
JimmyAREM 6:e1585b8bd07d 878 y_actuel = get_y_actuel();
JimmyAREM 6:e1585b8bd07d 879 //y_local_prec = y_local;
JimmyAREM 6:e1585b8bd07d 880 x_local = x_actuel*cos(angle_vise) + y_actuel*sin(angle_vise)-x_local_ini;
JimmyAREM 6:e1585b8bd07d 881 y_local = y_actuel*cos(angle_vise) - x_actuel*sin(angle_vise)-y_local_ini;
JimmyAREM 6:e1585b8bd07d 882
JimmyAREM 6:e1585b8bd07d 883 //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 884 }
JimmyAREM 6:e1585b8bd07d 885 //printf("x : %lf, y : %lf, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 886 rotation_abs_pente(angle_a_tourner);
JimmyAREM 6:e1585b8bd07d 887 //printf("x : %d, y : %d, angle : %f\n", get_x_actuel(), get_y_actuel(),get_angle());
JimmyAREM 6:e1585b8bd07d 888 }
JimmyAREM 6:e1585b8bd07d 889
JimmyAREM 6:e1585b8bd07d 890 void deplacement::rotation_rel_pente(double angle_vise)
JimmyAREM 6:e1585b8bd07d 891 {
JimmyAREM 6:e1585b8bd07d 892 // rotation de angle_vise
JimmyAREM 6:e1585b8bd07d 893 motors_on();
JimmyAREM 6:e1585b8bd07d 894 double vitesse=180;
JimmyAREM 6:e1585b8bd07d 895 int sens;
JimmyAREM 6:e1585b8bd07d 896 double angle = get_angle();
JimmyAREM 6:e1585b8bd07d 897 angle_vise+=angle;
JimmyAREM 6:e1585b8bd07d 898 borne_angle_d(angle_vise);
JimmyAREM 6:e1585b8bd07d 899 if (diff_angle(angle,angle_vise)<=0){
JimmyAREM 6:e1585b8bd07d 900 sens = -1;
JimmyAREM 6:e1585b8bd07d 901 //printf("negatif\n");
JimmyAREM 6:e1585b8bd07d 902 }
JimmyAREM 6:e1585b8bd07d 903 else{
JimmyAREM 6:e1585b8bd07d 904 sens = 1;
JimmyAREM 6:e1585b8bd07d 905
JimmyAREM 6:e1585b8bd07d 906 //printf("positif\n");
JimmyAREM 6:e1585b8bd07d 907 }
JimmyAREM 6:e1585b8bd07d 908 //printf("diff : %lf ",diff_angle(angle,angle_vise));
JimmyAREM 6:e1585b8bd07d 909 while ((sens*diff_angle(angle,angle_vise)>0) || abs(diff_angle(angle,angle_vise))>100)
JimmyAREM 6:e1585b8bd07d 910 {
JimmyAREM 6:e1585b8bd07d 911 actualise_position();
JimmyAREM 6:e1585b8bd07d 912 angle = get_angle();
JimmyAREM 6:e1585b8bd07d 913 vitesse=0.5*sens*abs(diff_angle(angle,angle_vise));
JimmyAREM 6:e1585b8bd07d 914
JimmyAREM 6:e1585b8bd07d 915 commande_vitesse(-vitesse,vitesse);
GuillaumeCH 8:f2425e4302fc 916 }
JimmyAREM 6:e1585b8bd07d 917 //printf("\ndiff2 : %lf ",diff_angle(angle,angle_vise));
JimmyAREM 6:e1585b8bd07d 918 //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 919 //consigne_D = 0;
JimmyAREM 6:e1585b8bd07d 920 //consigne_G = 0;
JimmyAREM 6:e1585b8bd07d 921 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 922 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 923
JimmyAREM 6:e1585b8bd07d 924 }
JimmyAREM 6:e1585b8bd07d 925
JimmyAREM 6:e1585b8bd07d 926 void deplacement::rotation_abs_pente(double angle_vise)
JimmyAREM 6:e1585b8bd07d 927 {
JimmyAREM 6:e1585b8bd07d 928 actualise_position();
JimmyAREM 6:e1585b8bd07d 929 //printf("bite");
JimmyAREM 6:e1585b8bd07d 930 double angle_rel = borne_angle_d(angle_vise-get_angle());
JimmyAREM 6:e1585b8bd07d 931 rotation_rel_pente(angle_rel);
JimmyAREM 6:e1585b8bd07d 932 }
JimmyAREM 6:e1585b8bd07d 933
GuillaumeCH 8:f2425e4302fc 934 void deplacement::pente_combo(double angle_pente, BrasPousser brasPousserGauche, BrasPousser brasPousserDroit, Pompe pompe){
JimmyAREM 6:e1585b8bd07d 935 activerDeuxBras(brasPousserGauche, brasPousserDroit);
JimmyAREM 6:e1585b8bd07d 936 motors_on();
GuillaumeCH 8:f2425e4302fc 937 pente(15000,50,angle_pente);
GuillaumeCH 8:f2425e4302fc 938 pente(10000,20,angle_pente);
GuillaumeCH 8:f2425e4302fc 939 pente(54500,80,angle_pente);
JimmyAREM 6:e1585b8bd07d 940 desactiverDeuxBras(brasPousserGauche, brasPousserDroit);
JimmyAREM 6:e1585b8bd07d 941 commande_vitesse(80,80);
JimmyAREM 7:6b15a1feed2d 942 wait(1.5);
JimmyAREM 7:6b15a1feed2d 943 vitesse_nulle_D(0);
JimmyAREM 7:6b15a1feed2d 944 vitesse_nulle_G(0);
JimmyAREM 7:6b15a1feed2d 945 //hardHiz();
JimmyAREM 7:6b15a1feed2d 946 pompe.desactiver();
JimmyAREM 7:6b15a1feed2d 947 wait(3);
JimmyAREM 7:6b15a1feed2d 948 rotation_rel(-90);
JimmyAREM 6:e1585b8bd07d 949 wait(4);
JimmyAREM 6:e1585b8bd07d 950 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 951 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 952 }
JimmyAREM 6:e1585b8bd07d 953
JimmyAREM 6:e1585b8bd07d 954 void deplacement::evitement(double x, double y, double cap)
JimmyAREM 6:e1585b8bd07d 955 {
JimmyAREM 6:e1585b8bd07d 956 if((distanceUltrasonGauche == 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
JimmyAREM 6:e1585b8bd07d 957 {
JimmyAREM 7:6b15a1feed2d 958 if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 959 {
JimmyAREM 6:e1585b8bd07d 960 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 961 actualise_position();
JimmyAREM 6:e1585b8bd07d 962 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 963 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 964 arc(P1,P2, A_GAUCHE);
JimmyAREM 6:e1585b8bd07d 965 }
JimmyAREM 7:6b15a1feed2d 966 else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 967 {
JimmyAREM 6:e1585b8bd07d 968 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 969 actualise_position();
JimmyAREM 6:e1585b8bd07d 970 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 971 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 972 arc(P1,P2, A_DROITE);
JimmyAREM 6:e1585b8bd07d 973 }
JimmyAREM 6:e1585b8bd07d 974 }
JimmyAREM 6:e1585b8bd07d 975 if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit == 1100) && (evitementValider == false))
JimmyAREM 6:e1585b8bd07d 976 {
JimmyAREM 7:6b15a1feed2d 977 if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 978 {
JimmyAREM 6:e1585b8bd07d 979 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 980 actualise_position();
JimmyAREM 6:e1585b8bd07d 981 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 982 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 983 arc(P1,P2, A_DROITE);
JimmyAREM 6:e1585b8bd07d 984 }
JimmyAREM 7:6b15a1feed2d 985 else if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 986 {
JimmyAREM 6:e1585b8bd07d 987 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 988 actualise_position();
JimmyAREM 6:e1585b8bd07d 989 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 990 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 991 arc(P1,P2, A_GAUCHE);
JimmyAREM 6:e1585b8bd07d 992 }
JimmyAREM 6:e1585b8bd07d 993 }
JimmyAREM 6:e1585b8bd07d 994 if((distanceUltrasonGauche != 1100) && (distanceUltrasonDroit != 1100) && (evitementValider == false))
JimmyAREM 6:e1585b8bd07d 995 {
JimmyAREM 7:6b15a1feed2d 996 if(lancerUnEvitementParArcGauche(distanceUltrasonGauche,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 997 {
JimmyAREM 6:e1585b8bd07d 998 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 999 actualise_position();
JimmyAREM 6:e1585b8bd07d 1000 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1001 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1002 arc(P1,P2, A_GAUCHE);
JimmyAREM 6:e1585b8bd07d 1003 }
JimmyAREM 7:6b15a1feed2d 1004 else if(lancerUnEvitementParArcDroit(distanceUltrasonDroit,x,y) == SANS_OBSTACLE)
JimmyAREM 6:e1585b8bd07d 1005 {
JimmyAREM 6:e1585b8bd07d 1006 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1007 actualise_position();
JimmyAREM 6:e1585b8bd07d 1008 Coordonnees P1 = pointIntermediaire();
JimmyAREM 6:e1585b8bd07d 1009 Coordonnees P2 = pointFinale();
JimmyAREM 6:e1585b8bd07d 1010 arc(P1,P2, A_DROITE);
JimmyAREM 6:e1585b8bd07d 1011 }
JimmyAREM 6:e1585b8bd07d 1012 }
JimmyAREM 6:e1585b8bd07d 1013 else
JimmyAREM 6:e1585b8bd07d 1014 {
JimmyAREM 6:e1585b8bd07d 1015 evitementValider = true;
JimmyAREM 6:e1585b8bd07d 1016 vitesse_nulle_D(0);
JimmyAREM 6:e1585b8bd07d 1017 vitesse_nulle_G(0);
JimmyAREM 6:e1585b8bd07d 1018 }
JimmyAREM 6:e1585b8bd07d 1019 va_au_point(x,y,cap);
JimmyAREM 6:e1585b8bd07d 1020 }