![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
l
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?
User | Revision | Line number | New 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 | } |