Guillaume Chauvon / AnalyseArcLib

lmlml

AnalyseArcLib.cpp@0:0d5755f210cb, 2019-05-20 (annotated)

Committer:
JimmyAREM
Date:
Mon May 20 23:06:51 2019 +0000
Revision:
0:0d5755f210cb
Child:
2:9fbf52f12cea
maj pour publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JimmyAREM 0:0d5755f210cb 1 #include "AnalyseArcLib.h"
JimmyAREM 0:0d5755f210cb 2
JimmyAREM 0:0d5755f210cb 3
JimmyAREM 0:0d5755f210cb 4 double posStartRobotX;
JimmyAREM 0:0d5755f210cb 5 double posStartRobotY;
JimmyAREM 0:0d5755f210cb 6 double capStartRobot;
JimmyAREM 0:0d5755f210cb 7
JimmyAREM 0:0d5755f210cb 8 double posFinalRobotX;
JimmyAREM 0:0d5755f210cb 9 double posFinalRobotY;
JimmyAREM 0:0d5755f210cb 10
JimmyAREM 0:0d5755f210cb 11 double posInterRobotX;
JimmyAREM 0:0d5755f210cb 12 double posInterRobotY;
JimmyAREM 0:0d5755f210cb 13
JimmyAREM 0:0d5755f210cb 14
JimmyAREM 0:0d5755f210cb 15
JimmyAREM 0:0d5755f210cb 16 bool evitement = AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 17
JimmyAREM 0:0d5755f210cb 18 extern bool couleur = 0; //0 Violet 1 Jaune
JimmyAREM 0:0d5755f210cb 19
JimmyAREM 0:0d5755f210cb 20 void actualiserPosRobot()
JimmyAREM 0:0d5755f210cb 21 {
JimmyAREM 0:0d5755f210cb 22 actualise_position();
JimmyAREM 0:0d5755f210cb 23 posStartRobotX = get_x_actuel()/100;
JimmyAREM 0:0d5755f210cb 24 posStartRobotY = get_y_actuel()/100;
JimmyAREM 0:0d5755f210cb 25 capStartRobot = get_angle();
JimmyAREM 0:0d5755f210cb 26 }
JimmyAREM 0:0d5755f210cb 27
JimmyAREM 0:0d5755f210cb 28 void calculPosFinalGauche(int distanceUltrasonGauche)
JimmyAREM 0:0d5755f210cb 29 {
JimmyAREM 0:0d5755f210cb 30 posFinalRobotX = posStartRobotX + (LONGUEUR_ROBOT + distanceUltrasonGauche + LONGUEUR_ROBOT_ADVERSE + MARGE) * cos((capStartRobot* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 31 posFinalRobotY = posStartRobotY + (LONGUEUR_ROBOT + distanceUltrasonGauche + LONGUEUR_ROBOT_ADVERSE + MARGE) * sin((capStartRobot* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 32 //printf("Position finale : (%lf, %lf) \n\r", posFinalRobotX, posFinalRobotY);
JimmyAREM 0:0d5755f210cb 33 }
JimmyAREM 0:0d5755f210cb 34
JimmyAREM 0:0d5755f210cb 35 void calculPosFinalDroite(int distanceUltrasonDroite)
JimmyAREM 0:0d5755f210cb 36 {
JimmyAREM 0:0d5755f210cb 37 posFinalRobotX = posStartRobotX + (LONGUEUR_ROBOT + distanceUltrasonDroite + LONGUEUR_ROBOT_ADVERSE + MARGE) * cos((capStartRobot* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 38 posFinalRobotY = posStartRobotY + (LONGUEUR_ROBOT + distanceUltrasonDroite + LONGUEUR_ROBOT_ADVERSE + MARGE ) * sin((capStartRobot* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 39 //printf("Position finale : (%lf, %lf) \n\r", posFinalRobotX, posFinalRobotY);
JimmyAREM 0:0d5755f210cb 40 }
JimmyAREM 0:0d5755f210cb 41
JimmyAREM 0:0d5755f210cb 42 void calculPosInterParLaGauche()
JimmyAREM 0:0d5755f210cb 43 {
JimmyAREM 0:0d5755f210cb 44 posInterRobotX = (posStartRobotX + posFinalRobotX)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * cos((capStartRobot* 3.1415)/180.0 + 1.57075);
JimmyAREM 0:0d5755f210cb 45 posInterRobotY = (posStartRobotY + posFinalRobotY)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * sin((capStartRobot* 3.1415)/180.0 + 1.57075);
JimmyAREM 0:0d5755f210cb 46 //printf("Position intermediaire gauche: (%lf, %lf) \n\r", posInterRobotX, posInterRobotY);
JimmyAREM 0:0d5755f210cb 47 }
JimmyAREM 0:0d5755f210cb 48
JimmyAREM 0:0d5755f210cb 49 void calculPosInterParLaDroite()
JimmyAREM 0:0d5755f210cb 50 {
JimmyAREM 0:0d5755f210cb 51 posInterRobotX = (posStartRobotX + posFinalRobotX)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * cos((capStartRobot* 3.1415)/180.0 - 1.57075);
JimmyAREM 0:0d5755f210cb 52 posInterRobotY = (posStartRobotY + posFinalRobotY)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * sin((capStartRobot* 3.1415)/180.0 - 1.57075);
JimmyAREM 0:0d5755f210cb 53 //printf("Position intermediaire droite: (%lf, %lf) \n\r", posInterRobotX, posInterRobotY);
JimmyAREM 0:0d5755f210cb 54 }
JimmyAREM 0:0d5755f210cb 55
JimmyAREM 0:0d5755f210cb 56 Robot setPosRobot(Point point, double capActuel)
JimmyAREM 0:0d5755f210cb 57 {
JimmyAREM 0:0d5755f210cb 58 struct Robot robot;
JimmyAREM 0:0d5755f210cb 59
JimmyAREM 0:0d5755f210cb 60 robot.Ax = point.X + DEMI_LONGUEUR_ROBOT_AVANT * cos((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 61 robot.Ay = point.Y + DEMI_LONGUEUR_ROBOT_AVANT * sin((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 62
JimmyAREM 0:0d5755f210cb 63 robot.Bx = point.X + DEMI_LONGUEUR_ROBOT_AVANT * cos((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 64 robot.By = point.Y + DEMI_LONGUEUR_ROBOT_AVANT * sin((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 65
JimmyAREM 0:0d5755f210cb 66 robot.Cx = point.X - DEMI_LONGUEUR_ROBOT_ARRIERE * cos((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 67 robot.Cy = point.Y - DEMI_LONGUEUR_ROBOT_ARRIERE * sin((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 68
JimmyAREM 0:0d5755f210cb 69 robot.Dx = point.X - DEMI_LONGUEUR_ROBOT_ARRIERE * cos((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 70 robot.Dy = point.Y - DEMI_LONGUEUR_ROBOT_ARRIERE * sin((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
JimmyAREM 0:0d5755f210cb 71
JimmyAREM 0:0d5755f210cb 72 return robot;
JimmyAREM 0:0d5755f210cb 73 }
JimmyAREM 0:0d5755f210cb 74
JimmyAREM 0:0d5755f210cb 75 double calculercapActuel(Point P1, Point P2)
JimmyAREM 0:0d5755f210cb 76 {
JimmyAREM 0:0d5755f210cb 77 double capActuel;
JimmyAREM 0:0d5755f210cb 78 double cste = 0;
JimmyAREM 0:0d5755f210cb 79 cste = pow((pow((P1.X - P2.X),2) + pow((P1.Y - P2.Y),2)),0.5);
JimmyAREM 0:0d5755f210cb 80 capActuel = acos(-(P2.Y - P1.Y) / cste);
JimmyAREM 0:0d5755f210cb 81 return capActuel;
JimmyAREM 0:0d5755f210cb 82 }
JimmyAREM 0:0d5755f210cb 83
JimmyAREM 0:0d5755f210cb 84 bool arcPossible()
JimmyAREM 0:0d5755f210cb 85 {
JimmyAREM 0:0d5755f210cb 86 double x1 = posStartRobotX;
JimmyAREM 0:0d5755f210cb 87 double x2 = posInterRobotX;
JimmyAREM 0:0d5755f210cb 88 double x3 = posFinalRobotX;
JimmyAREM 0:0d5755f210cb 89
JimmyAREM 0:0d5755f210cb 90 double y1 = posStartRobotY;
JimmyAREM 0:0d5755f210cb 91 double y2 = posInterRobotY;
JimmyAREM 0:0d5755f210cb 92 double y3 = posFinalRobotY;
JimmyAREM 0:0d5755f210cb 93
JimmyAREM 0:0d5755f210cb 94 double a = x1 - x2;
JimmyAREM 0:0d5755f210cb 95 double b = y1 - y2;
JimmyAREM 0:0d5755f210cb 96 double c = x1 - x3;
JimmyAREM 0:0d5755f210cb 97 double d = y1 - y3;
JimmyAREM 0:0d5755f210cb 98
JimmyAREM 0:0d5755f210cb 99 if ((b*c) - (a*d) == 0)
JimmyAREM 0:0d5755f210cb 100 {
JimmyAREM 0:0d5755f210cb 101 return false;
JimmyAREM 0:0d5755f210cb 102 }
JimmyAREM 0:0d5755f210cb 103 else
JimmyAREM 0:0d5755f210cb 104 {
JimmyAREM 0:0d5755f210cb 105 return true;
JimmyAREM 0:0d5755f210cb 106 }
JimmyAREM 0:0d5755f210cb 107 }
JimmyAREM 0:0d5755f210cb 108
JimmyAREM 0:0d5755f210cb 109 Arc trouverArc()
JimmyAREM 0:0d5755f210cb 110 {
JimmyAREM 0:0d5755f210cb 111 Arc arcTrajectoire;
JimmyAREM 0:0d5755f210cb 112 double cX;
JimmyAREM 0:0d5755f210cb 113 double cY;
JimmyAREM 0:0d5755f210cb 114 double R;
JimmyAREM 0:0d5755f210cb 115
JimmyAREM 0:0d5755f210cb 116 double x1 = posStartRobotX;
JimmyAREM 0:0d5755f210cb 117 double x2 = posInterRobotX;
JimmyAREM 0:0d5755f210cb 118 double x3 = posFinalRobotX;
JimmyAREM 0:0d5755f210cb 119
JimmyAREM 0:0d5755f210cb 120 double y1 = posStartRobotY;
JimmyAREM 0:0d5755f210cb 121 double y2 = posInterRobotY;
JimmyAREM 0:0d5755f210cb 122 double y3 = posFinalRobotY;
JimmyAREM 0:0d5755f210cb 123
JimmyAREM 0:0d5755f210cb 124 double a = x1 - x2;
JimmyAREM 0:0d5755f210cb 125 double b = y1 - y2;
JimmyAREM 0:0d5755f210cb 126 double c = x1 - x3;
JimmyAREM 0:0d5755f210cb 127 double d = y1 - y3;
JimmyAREM 0:0d5755f210cb 128 double e = ( pow(x1,2) - pow(x2,2) - ((pow(y2,2) - pow(y1,2))) ) / 2;
JimmyAREM 0:0d5755f210cb 129 double f = ( pow(x1,2) - pow(x3,2) - ((pow(y3,2) - pow(y1,2))) ) / 2;
JimmyAREM 0:0d5755f210cb 130
JimmyAREM 0:0d5755f210cb 131
JimmyAREM 0:0d5755f210cb 132 arcTrajectoire.intervaleXd = x1;
JimmyAREM 0:0d5755f210cb 133 arcTrajectoire.intervaleXf = x3;
JimmyAREM 0:0d5755f210cb 134 arcTrajectoire.intervaleYd = y1;
JimmyAREM 0:0d5755f210cb 135 arcTrajectoire.intervaleYf = y3;
JimmyAREM 0:0d5755f210cb 136
JimmyAREM 0:0d5755f210cb 137 cX = -(d*e - b*f)/(b*c - a*d);
JimmyAREM 0:0d5755f210cb 138 cY = -(a*f - c*e)/(b *c - a*d);
JimmyAREM 0:0d5755f210cb 139 R = pow(pow((x1 - cX),2) + pow((y1 - cY),2), 0.5);
JimmyAREM 0:0d5755f210cb 140
JimmyAREM 0:0d5755f210cb 141 arcTrajectoire.CentreX = cX;
JimmyAREM 0:0d5755f210cb 142 arcTrajectoire.CentreY = cY;
JimmyAREM 0:0d5755f210cb 143 arcTrajectoire.Rayon = R;
JimmyAREM 0:0d5755f210cb 144
JimmyAREM 0:0d5755f210cb 145 //printf("le cercle de centre(%lf,%lf) et de Rayon=%lf \n\r", arcTrajectoire.CentreX, arcTrajectoire.CentreY, arcTrajectoire.Rayon);
JimmyAREM 0:0d5755f210cb 146 return arcTrajectoire;
JimmyAREM 0:0d5755f210cb 147 }
JimmyAREM 0:0d5755f210cb 148
JimmyAREM 0:0d5755f210cb 149 Arc genererArcInterieur(Arc arc)
JimmyAREM 0:0d5755f210cb 150 {
JimmyAREM 0:0d5755f210cb 151 Arc arcInterieur;
JimmyAREM 0:0d5755f210cb 152 arcInterieur.CentreX = arc.CentreX;
JimmyAREM 0:0d5755f210cb 153 arcInterieur.CentreY = arc.CentreY;
JimmyAREM 0:0d5755f210cb 154 arcInterieur.Rayon = arc.Rayon - DEMI_LARGEUR_ROBOT;
JimmyAREM 0:0d5755f210cb 155
JimmyAREM 0:0d5755f210cb 156 arcInterieur.intervaleXd = arc.intervaleXd + DEMI_LARGEUR_ROBOT * cos((capStartRobot* 3.1415/180.0) + 1.5707);
JimmyAREM 0:0d5755f210cb 157 arcInterieur.intervaleXf = arc.intervaleXf + DEMI_LARGEUR_ROBOT * cos((capStartRobot* 3.1415/180.0) + 1.5707);
JimmyAREM 0:0d5755f210cb 158 arcInterieur.intervaleYd = arc.intervaleYd + DEMI_LARGEUR_ROBOT * sin((capStartRobot* 3.1415/180.0) + 1.5707);
JimmyAREM 0:0d5755f210cb 159 arcInterieur.intervaleYf = arc.intervaleYf;
JimmyAREM 0:0d5755f210cb 160 return arcInterieur;
JimmyAREM 0:0d5755f210cb 161 }
JimmyAREM 0:0d5755f210cb 162
JimmyAREM 0:0d5755f210cb 163
JimmyAREM 0:0d5755f210cb 164 Arc genererArcExterieur(Arc arc)
JimmyAREM 0:0d5755f210cb 165 {
JimmyAREM 0:0d5755f210cb 166 Arc arcExterieur;
JimmyAREM 0:0d5755f210cb 167 arcExterieur.CentreX = arc.CentreX;
JimmyAREM 0:0d5755f210cb 168 arcExterieur.CentreY = arc.CentreY;
JimmyAREM 0:0d5755f210cb 169 arcExterieur.Rayon = arc.Rayon + DEMI_LARGEUR_ROBOT;
JimmyAREM 0:0d5755f210cb 170 return arcExterieur;
JimmyAREM 0:0d5755f210cb 171 }
JimmyAREM 0:0d5755f210cb 172 double trouverAngle(Arc arc)
JimmyAREM 0:0d5755f210cb 173 {
JimmyAREM 0:0d5755f210cb 174 double angle;
JimmyAREM 0:0d5755f210cb 175 double a = arc.Rayon;
JimmyAREM 0:0d5755f210cb 176 double b = arc.Rayon;
JimmyAREM 0:0d5755f210cb 177 double c = pow((pow((arc.intervaleXd - arc.intervaleXf),2) + pow((arc.intervaleYd - arc.intervaleYf),2)),0.5) ;
JimmyAREM 0:0d5755f210cb 178 //printf("Distance entre le point de depart et le point final : %lf\n\r", c);
JimmyAREM 0:0d5755f210cb 179 angle = acos((a*a + b*b - c*c)/(2*a*b)) ;
JimmyAREM 0:0d5755f210cb 180 //printf("Angle en degree trouve : %lf \n\r", angle *180/3.1415);
JimmyAREM 0:0d5755f210cb 181 return angle;
JimmyAREM 0:0d5755f210cb 182 }
JimmyAREM 0:0d5755f210cb 183
JimmyAREM 0:0d5755f210cb 184 double trouverAngleInit(Arc arc)
JimmyAREM 0:0d5755f210cb 185 {
JimmyAREM 0:0d5755f210cb 186 double angle;
JimmyAREM 0:0d5755f210cb 187 double a = arc.Rayon;
JimmyAREM 0:0d5755f210cb 188 double b = arc.Rayon;
JimmyAREM 0:0d5755f210cb 189 double c = pow((pow((arc.CentreX + arc.Rayon - arc.intervaleXf),2) + pow((arc.CentreY - arc.intervaleYf),2)),0.5) ;
JimmyAREM 0:0d5755f210cb 190 angle = acos((a*a + b*b - c*c)/(2*a*b)) ;
JimmyAREM 0:0d5755f210cb 191 if(arc.CentreY >= arc.intervaleYf)
JimmyAREM 0:0d5755f210cb 192 {
JimmyAREM 0:0d5755f210cb 193 angle = -angle;
JimmyAREM 0:0d5755f210cb 194 }
JimmyAREM 0:0d5755f210cb 195 //printf("Angle initial en degree trouve : %lf \n\r", angle *180/3.1415);
JimmyAREM 0:0d5755f210cb 196 return angle;
JimmyAREM 0:0d5755f210cb 197 }
JimmyAREM 0:0d5755f210cb 198
JimmyAREM 0:0d5755f210cb 199 void recupererTableauPointArc(Arc arc, Point buffer[], bool type) //type = 0 => gauche / type = 1 => droit
JimmyAREM 0:0d5755f210cb 200 {
JimmyAREM 0:0d5755f210cb 201 double rPolaire = arc.Rayon;
JimmyAREM 0:0d5755f210cb 202 double tethaTotal;
JimmyAREM 0:0d5755f210cb 203 if (type == PARLAGAUCHE)
JimmyAREM 0:0d5755f210cb 204 {
JimmyAREM 0:0d5755f210cb 205 tethaTotal = trouverAngle(arc);
JimmyAREM 0:0d5755f210cb 206 //printf("Angle total a parcourir : %lf \n\r", tethaTotal*180/3.1415);
JimmyAREM 0:0d5755f210cb 207 }
JimmyAREM 0:0d5755f210cb 208 else
JimmyAREM 0:0d5755f210cb 209 {
JimmyAREM 0:0d5755f210cb 210 tethaTotal = trouverAngle(arc);
JimmyAREM 0:0d5755f210cb 211 //printf("Angle total a parcourir : %lf \n\r", tethaTotal*180/3.1415);
JimmyAREM 0:0d5755f210cb 212 }
JimmyAREM 0:0d5755f210cb 213 double tethaInit = trouverAngleInit(arc);
JimmyAREM 0:0d5755f210cb 214 //double longueurArc = rPolaire * tethaTotal;
JimmyAREM 0:0d5755f210cb 215 //printf("Angle : %lf et longueur : %lf\n\r", tethaTotal*180/3.1415, longueurArc);
JimmyAREM 0:0d5755f210cb 216 if (type == PARLAGAUCHE)
JimmyAREM 0:0d5755f210cb 217 {
JimmyAREM 0:0d5755f210cb 218 for(int i = 0; i<TAILLE_BUFFER_POINT; i++)
JimmyAREM 0:0d5755f210cb 219 {
JimmyAREM 0:0d5755f210cb 220 buffer[i].X = arc.CentreX + rPolaire * cos(tethaInit + i * tethaTotal/NB_POINT);
JimmyAREM 0:0d5755f210cb 221 buffer[i].Y = arc.CentreY + rPolaire * sin(tethaInit + i * tethaTotal/NB_POINT);
JimmyAREM 0:0d5755f210cb 222 //printf("Point %d : (%lf,%lf) \n\r", i, buffer[i].X, buffer[i].Y);
JimmyAREM 0:0d5755f210cb 223 }
JimmyAREM 0:0d5755f210cb 224 }
JimmyAREM 0:0d5755f210cb 225 else
JimmyAREM 0:0d5755f210cb 226 {
JimmyAREM 0:0d5755f210cb 227 for(int i = 0; i<TAILLE_BUFFER_POINT; i++)
JimmyAREM 0:0d5755f210cb 228 {
JimmyAREM 0:0d5755f210cb 229 buffer[i].X = arc.CentreX + rPolaire * cos(tethaInit - i * tethaTotal/NB_POINT);
JimmyAREM 0:0d5755f210cb 230 buffer[i].Y = arc.CentreY + rPolaire * sin(tethaInit - i * tethaTotal/NB_POINT);
JimmyAREM 0:0d5755f210cb 231 //printf("Point %d : (%lf,%lf) \n\r", i, buffer[i].X, buffer[i].Y);
JimmyAREM 0:0d5755f210cb 232 }
JimmyAREM 0:0d5755f210cb 233 }
JimmyAREM 0:0d5755f210cb 234 return;
JimmyAREM 0:0d5755f210cb 235 }
JimmyAREM 0:0d5755f210cb 236
JimmyAREM 0:0d5755f210cb 237 void afficherRobot(Robot robot)
JimmyAREM 0:0d5755f210cb 238 {
JimmyAREM 0:0d5755f210cb 239 printf("le robot est define par A(%lf,%lf), B(%lf,%lf), C(%lf,%lf) et D(%lf,%lf)\n\r", robot.Ax, robot.Ay, robot.Bx, robot.By, robot.Cx, robot.Cy, robot.Dx, robot.Dy );
JimmyAREM 0:0d5755f210cb 240 }
JimmyAREM 0:0d5755f210cb 241
JimmyAREM 0:0d5755f210cb 242
JimmyAREM 0:0d5755f210cb 243 void afficherArc(Arc arc)
JimmyAREM 0:0d5755f210cb 244 {
JimmyAREM 0:0d5755f210cb 245 printf("L'arc est define par son centre C(%lf,%lf)et son rayon R = %lf sur le domaine des abscisses [%lf; %lf] et sur le domaine des ordonnees [%lf; %lf] \n\r", arc.CentreX, arc.CentreY, arc.Rayon, arc.intervaleXd, arc.intervaleXf, arc.intervaleYd, arc.intervaleYf);
JimmyAREM 0:0d5755f210cb 246 }
JimmyAREM 0:0d5755f210cb 247
JimmyAREM 0:0d5755f210cb 248 void afficher(Point buffer[])
JimmyAREM 0:0d5755f210cb 249 {
JimmyAREM 0:0d5755f210cb 250 for(int i=0; i<TAILLE_BUFFER_POINT; i++)
JimmyAREM 0:0d5755f210cb 251 {
JimmyAREM 0:0d5755f210cb 252 printf("Coordonnee du point P indice %d : (%lf,%lf) \n\r", i, buffer[i].X, buffer[i].Y);
JimmyAREM 0:0d5755f210cb 253 }
JimmyAREM 0:0d5755f210cb 254 }
JimmyAREM 0:0d5755f210cb 255
JimmyAREM 0:0d5755f210cb 256
JimmyAREM 0:0d5755f210cb 257
JimmyAREM 0:0d5755f210cb 258 bool estUnArc(Point tableau[])
JimmyAREM 0:0d5755f210cb 259 {
JimmyAREM 0:0d5755f210cb 260 Robot robot;
JimmyAREM 0:0d5755f210cb 261
JimmyAREM 0:0d5755f210cb 262 for(int i=0; i<TAILLE_BUFFER_POINT; i++)
JimmyAREM 0:0d5755f210cb 263 { if(i != TAILLE_BUFFER_POINT -1)
JimmyAREM 0:0d5755f210cb 264 {
JimmyAREM 0:0d5755f210cb 265 robot = setPosRobot(tableau[i], calculercapActuel(tableau[i], tableau[i+1]));
JimmyAREM 0:0d5755f210cb 266 }
JimmyAREM 0:0d5755f210cb 267 else
JimmyAREM 0:0d5755f210cb 268 {
JimmyAREM 0:0d5755f210cb 269 robot = setPosRobot(tableau[i], calculercapActuel(tableau[i-1], tableau[i]));
JimmyAREM 0:0d5755f210cb 270 }
JimmyAREM 0:0d5755f210cb 271 //printf(" Point A, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 272 if ((robot.Ax >= AX0) && (robot.Ax <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 273 {
JimmyAREM 0:0d5755f210cb 274 if ((robot.Ay >= AY0) && (robot.Ay <= BY0))
JimmyAREM 0:0d5755f210cb 275 {
JimmyAREM 0:0d5755f210cb 276 //Point correct
JimmyAREM 0:0d5755f210cb 277 }
JimmyAREM 0:0d5755f210cb 278 else
JimmyAREM 0:0d5755f210cb 279 {
JimmyAREM 0:0d5755f210cb 280 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 281 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 282 }
JimmyAREM 0:0d5755f210cb 283 }
JimmyAREM 0:0d5755f210cb 284 else
JimmyAREM 0:0d5755f210cb 285 {
JimmyAREM 0:0d5755f210cb 286 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 287 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 288 }
JimmyAREM 0:0d5755f210cb 289 if ((robot.Ax >= AX1) && (robot.Ax <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 290 {
JimmyAREM 0:0d5755f210cb 291 if ((robot.Ay >= AY1) && (robot.Ay <= BY1))
JimmyAREM 0:0d5755f210cb 292 {
JimmyAREM 0:0d5755f210cb 293 printf("Dans l'accelerateur\n\r");
JimmyAREM 0:0d5755f210cb 294 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 295 }
JimmyAREM 0:0d5755f210cb 296 else
JimmyAREM 0:0d5755f210cb 297 {
JimmyAREM 0:0d5755f210cb 298 //Point correct
JimmyAREM 0:0d5755f210cb 299 }
JimmyAREM 0:0d5755f210cb 300 }
JimmyAREM 0:0d5755f210cb 301 else
JimmyAREM 0:0d5755f210cb 302 {
JimmyAREM 0:0d5755f210cb 303 //point correct
JimmyAREM 0:0d5755f210cb 304 }
JimmyAREM 0:0d5755f210cb 305 if ((robot.Ax >= AX2) && (robot.Ax <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 306 {
JimmyAREM 0:0d5755f210cb 307 if ((robot.Ay >= AY2) && (robot.Ay <= BY2))
JimmyAREM 0:0d5755f210cb 308 {
JimmyAREM 0:0d5755f210cb 309 printf("Dans la zone chaos violet \n\r");
JimmyAREM 0:0d5755f210cb 310 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 311 }
JimmyAREM 0:0d5755f210cb 312 else
JimmyAREM 0:0d5755f210cb 313 {
JimmyAREM 0:0d5755f210cb 314 //Point correct
JimmyAREM 0:0d5755f210cb 315 }
JimmyAREM 0:0d5755f210cb 316 }
JimmyAREM 0:0d5755f210cb 317 else
JimmyAREM 0:0d5755f210cb 318 {
JimmyAREM 0:0d5755f210cb 319 //Point correct
JimmyAREM 0:0d5755f210cb 320 }
JimmyAREM 0:0d5755f210cb 321 if ((robot.Ax >= AX3) && (robot.Ax <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 322 {
JimmyAREM 0:0d5755f210cb 323 if ((robot.Ay >= AY3) && (robot.Ay <= BY3))
JimmyAREM 0:0d5755f210cb 324 {
JimmyAREM 0:0d5755f210cb 325 printf("Dans la zone chaos jaune \n\r");
JimmyAREM 0:0d5755f210cb 326 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 327 }
JimmyAREM 0:0d5755f210cb 328 else
JimmyAREM 0:0d5755f210cb 329 {
JimmyAREM 0:0d5755f210cb 330 //Point correct
JimmyAREM 0:0d5755f210cb 331 }
JimmyAREM 0:0d5755f210cb 332 }
JimmyAREM 0:0d5755f210cb 333 else
JimmyAREM 0:0d5755f210cb 334 {
JimmyAREM 0:0d5755f210cb 335 //Point correct
JimmyAREM 0:0d5755f210cb 336 }
JimmyAREM 0:0d5755f210cb 337 if ((robot.Ax >= AX4) && (robot.Ax <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 338 {
JimmyAREM 0:0d5755f210cb 339 if ((robot.Ay >= AY4) && (robot.Ay <= BY4))
JimmyAREM 0:0d5755f210cb 340 {
JimmyAREM 0:0d5755f210cb 341 printf("Dans la zone distributeur violet \n\r");
JimmyAREM 0:0d5755f210cb 342 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 343 }
JimmyAREM 0:0d5755f210cb 344 else
JimmyAREM 0:0d5755f210cb 345 {
JimmyAREM 0:0d5755f210cb 346 //Point correct
JimmyAREM 0:0d5755f210cb 347 }
JimmyAREM 0:0d5755f210cb 348 }
JimmyAREM 0:0d5755f210cb 349 else
JimmyAREM 0:0d5755f210cb 350 {
JimmyAREM 0:0d5755f210cb 351 //Point correct
JimmyAREM 0:0d5755f210cb 352 }
JimmyAREM 0:0d5755f210cb 353 if ((robot.Ax >= AX5) && (robot.Ax <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 354 {
JimmyAREM 0:0d5755f210cb 355 if ((robot.Ay >= AY5) && (robot.Ay <= BY5))
JimmyAREM 0:0d5755f210cb 356 {
JimmyAREM 0:0d5755f210cb 357 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 358 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 359 }
JimmyAREM 0:0d5755f210cb 360 else
JimmyAREM 0:0d5755f210cb 361 {
JimmyAREM 0:0d5755f210cb 362 //Point correct
JimmyAREM 0:0d5755f210cb 363 }
JimmyAREM 0:0d5755f210cb 364 }
JimmyAREM 0:0d5755f210cb 365 else
JimmyAREM 0:0d5755f210cb 366 {
JimmyAREM 0:0d5755f210cb 367 //Point correct
JimmyAREM 0:0d5755f210cb 368 }
JimmyAREM 0:0d5755f210cb 369 if ((robot.Ax >= AX6) && (robot.Ax <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 370 {
JimmyAREM 0:0d5755f210cb 371 if ((robot.Ay >= AY6) && (robot.Ay <= BY6))
JimmyAREM 0:0d5755f210cb 372 {
JimmyAREM 0:0d5755f210cb 373 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 374 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 375 }
JimmyAREM 0:0d5755f210cb 376 else
JimmyAREM 0:0d5755f210cb 377 {
JimmyAREM 0:0d5755f210cb 378 //Point correct
JimmyAREM 0:0d5755f210cb 379 }
JimmyAREM 0:0d5755f210cb 380 }
JimmyAREM 0:0d5755f210cb 381 else
JimmyAREM 0:0d5755f210cb 382 {
JimmyAREM 0:0d5755f210cb 383 //Point correct
JimmyAREM 0:0d5755f210cb 384 }
JimmyAREM 0:0d5755f210cb 385 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 386 {
JimmyAREM 0:0d5755f210cb 387 if ((robot.Ax >= AX7) && (robot.Ax <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 388 {
JimmyAREM 0:0d5755f210cb 389 if ((robot.Ay >= AY7) && (robot.Ay <= BY7))
JimmyAREM 0:0d5755f210cb 390 {
JimmyAREM 0:0d5755f210cb 391 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 392 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 393 }
JimmyAREM 0:0d5755f210cb 394 else
JimmyAREM 0:0d5755f210cb 395 {
JimmyAREM 0:0d5755f210cb 396 //Point correct
JimmyAREM 0:0d5755f210cb 397 }
JimmyAREM 0:0d5755f210cb 398 }
JimmyAREM 0:0d5755f210cb 399 else
JimmyAREM 0:0d5755f210cb 400 {
JimmyAREM 0:0d5755f210cb 401 //Point correct
JimmyAREM 0:0d5755f210cb 402 }
JimmyAREM 0:0d5755f210cb 403 }
JimmyAREM 0:0d5755f210cb 404 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 405 {
JimmyAREM 0:0d5755f210cb 406 if ((robot.Ax >= AX8) && (robot.Ax <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 407 {
JimmyAREM 0:0d5755f210cb 408 if ((robot.Ay>= AY8) && (robot.Ay <= BY8))
JimmyAREM 0:0d5755f210cb 409 {
JimmyAREM 0:0d5755f210cb 410 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 411 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 412 }
JimmyAREM 0:0d5755f210cb 413 else
JimmyAREM 0:0d5755f210cb 414 {
JimmyAREM 0:0d5755f210cb 415 //Point correct
JimmyAREM 0:0d5755f210cb 416 }
JimmyAREM 0:0d5755f210cb 417 }
JimmyAREM 0:0d5755f210cb 418 else
JimmyAREM 0:0d5755f210cb 419 {
JimmyAREM 0:0d5755f210cb 420 //Point correct
JimmyAREM 0:0d5755f210cb 421 }
JimmyAREM 0:0d5755f210cb 422 }
JimmyAREM 0:0d5755f210cb 423 if ((robot.Ax >= AX9) && (robot.Ax <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 424 {
JimmyAREM 0:0d5755f210cb 425 if ((robot.Ay >= AY9) && (robot.Ay <= BY9))
JimmyAREM 0:0d5755f210cb 426 {
JimmyAREM 0:0d5755f210cb 427 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 428 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 429 }
JimmyAREM 0:0d5755f210cb 430 else
JimmyAREM 0:0d5755f210cb 431 {
JimmyAREM 0:0d5755f210cb 432 //Point correct
JimmyAREM 0:0d5755f210cb 433 }
JimmyAREM 0:0d5755f210cb 434 }
JimmyAREM 0:0d5755f210cb 435 else
JimmyAREM 0:0d5755f210cb 436 {
JimmyAREM 0:0d5755f210cb 437 //Point correct
JimmyAREM 0:0d5755f210cb 438 }
JimmyAREM 0:0d5755f210cb 439 //printf(" Point B, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 440 if ((robot.Bx >= AX0) && (robot.Bx <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 441 {
JimmyAREM 0:0d5755f210cb 442 if ((robot.By >= AY0) && (robot.By <= BY0))
JimmyAREM 0:0d5755f210cb 443 {
JimmyAREM 0:0d5755f210cb 444 //Point correct
JimmyAREM 0:0d5755f210cb 445 }
JimmyAREM 0:0d5755f210cb 446 else
JimmyAREM 0:0d5755f210cb 447 {
JimmyAREM 0:0d5755f210cb 448 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 449 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 450 }
JimmyAREM 0:0d5755f210cb 451 }
JimmyAREM 0:0d5755f210cb 452 else
JimmyAREM 0:0d5755f210cb 453 {
JimmyAREM 0:0d5755f210cb 454 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 455 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 456 }
JimmyAREM 0:0d5755f210cb 457 if ((robot.Bx >= AX1) && (robot.Bx <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 458 {
JimmyAREM 0:0d5755f210cb 459 if ((robot.By >= AY1) && (robot.By <= BY1))
JimmyAREM 0:0d5755f210cb 460 {
JimmyAREM 0:0d5755f210cb 461 printf("Dans l'accelerateur\n\r");
JimmyAREM 0:0d5755f210cb 462 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 463 }
JimmyAREM 0:0d5755f210cb 464 else
JimmyAREM 0:0d5755f210cb 465 {
JimmyAREM 0:0d5755f210cb 466 //Point correct
JimmyAREM 0:0d5755f210cb 467 }
JimmyAREM 0:0d5755f210cb 468 }
JimmyAREM 0:0d5755f210cb 469 else
JimmyAREM 0:0d5755f210cb 470 {
JimmyAREM 0:0d5755f210cb 471 //point correct
JimmyAREM 0:0d5755f210cb 472 }
JimmyAREM 0:0d5755f210cb 473 if ((robot.Bx >= AX2) && (robot.Bx <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 474 {
JimmyAREM 0:0d5755f210cb 475 if ((robot.By >= AY2) && (robot.By <= BY2))
JimmyAREM 0:0d5755f210cb 476 {
JimmyAREM 0:0d5755f210cb 477 printf("Dans la zone chaos violet \n\r");
JimmyAREM 0:0d5755f210cb 478 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 479 }
JimmyAREM 0:0d5755f210cb 480 else
JimmyAREM 0:0d5755f210cb 481 {
JimmyAREM 0:0d5755f210cb 482 //Point correct
JimmyAREM 0:0d5755f210cb 483 }
JimmyAREM 0:0d5755f210cb 484 }
JimmyAREM 0:0d5755f210cb 485 else
JimmyAREM 0:0d5755f210cb 486 {
JimmyAREM 0:0d5755f210cb 487 //Point correct
JimmyAREM 0:0d5755f210cb 488 }
JimmyAREM 0:0d5755f210cb 489 if ((robot.Bx >= AX3) && (robot.Bx <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 490 {
JimmyAREM 0:0d5755f210cb 491 if ((robot.By >= AY3) && (robot.By <= BY3))
JimmyAREM 0:0d5755f210cb 492 {
JimmyAREM 0:0d5755f210cb 493 printf("Dans la zone chaos jaune \n\r");
JimmyAREM 0:0d5755f210cb 494 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 495 }
JimmyAREM 0:0d5755f210cb 496 else
JimmyAREM 0:0d5755f210cb 497 {
JimmyAREM 0:0d5755f210cb 498 //Point correct
JimmyAREM 0:0d5755f210cb 499 }
JimmyAREM 0:0d5755f210cb 500 }
JimmyAREM 0:0d5755f210cb 501 else
JimmyAREM 0:0d5755f210cb 502 {
JimmyAREM 0:0d5755f210cb 503 //Point correct
JimmyAREM 0:0d5755f210cb 504 }
JimmyAREM 0:0d5755f210cb 505 if ((robot.Bx >= AX4) && (robot.Bx <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 506 {
JimmyAREM 0:0d5755f210cb 507 if ((robot.By >= AY4) && (robot.By <= BY4))
JimmyAREM 0:0d5755f210cb 508 {
JimmyAREM 0:0d5755f210cb 509 printf("Dans la zone distributeur violet \n\r");
JimmyAREM 0:0d5755f210cb 510 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 511 }
JimmyAREM 0:0d5755f210cb 512 else
JimmyAREM 0:0d5755f210cb 513 {
JimmyAREM 0:0d5755f210cb 514 //Point correct
JimmyAREM 0:0d5755f210cb 515 }
JimmyAREM 0:0d5755f210cb 516 }
JimmyAREM 0:0d5755f210cb 517 else
JimmyAREM 0:0d5755f210cb 518 {
JimmyAREM 0:0d5755f210cb 519 //Point correct
JimmyAREM 0:0d5755f210cb 520 }
JimmyAREM 0:0d5755f210cb 521 if ((robot.Bx >= AX5) && (robot.Bx <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 522 {
JimmyAREM 0:0d5755f210cb 523 if ((robot.By >= AY5) && (robot.By <= BY5))
JimmyAREM 0:0d5755f210cb 524 {
JimmyAREM 0:0d5755f210cb 525 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 526 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 527 }
JimmyAREM 0:0d5755f210cb 528 else
JimmyAREM 0:0d5755f210cb 529 {
JimmyAREM 0:0d5755f210cb 530 //Point correct
JimmyAREM 0:0d5755f210cb 531 }
JimmyAREM 0:0d5755f210cb 532 }
JimmyAREM 0:0d5755f210cb 533 else
JimmyAREM 0:0d5755f210cb 534 {
JimmyAREM 0:0d5755f210cb 535 //Point correct
JimmyAREM 0:0d5755f210cb 536 }
JimmyAREM 0:0d5755f210cb 537 if ((robot.Bx >= AX6) && (robot.Bx <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 538 {
JimmyAREM 0:0d5755f210cb 539 if ((robot.By >= AY6) && (robot.By <= BY6))
JimmyAREM 0:0d5755f210cb 540 {
JimmyAREM 0:0d5755f210cb 541 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 542 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 543 }
JimmyAREM 0:0d5755f210cb 544 else
JimmyAREM 0:0d5755f210cb 545 {
JimmyAREM 0:0d5755f210cb 546 //Point correct
JimmyAREM 0:0d5755f210cb 547 }
JimmyAREM 0:0d5755f210cb 548 }
JimmyAREM 0:0d5755f210cb 549 else
JimmyAREM 0:0d5755f210cb 550 {
JimmyAREM 0:0d5755f210cb 551 //Point correct
JimmyAREM 0:0d5755f210cb 552 }
JimmyAREM 0:0d5755f210cb 553 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 554 {
JimmyAREM 0:0d5755f210cb 555 if ((robot.Bx >= AX7) && (robot.Bx <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 556 {
JimmyAREM 0:0d5755f210cb 557 if ((robot.By >= AY7) && (robot.By <= BY7))
JimmyAREM 0:0d5755f210cb 558 {
JimmyAREM 0:0d5755f210cb 559 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 560 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 561 }
JimmyAREM 0:0d5755f210cb 562 else
JimmyAREM 0:0d5755f210cb 563 {
JimmyAREM 0:0d5755f210cb 564 //Point correct
JimmyAREM 0:0d5755f210cb 565 }
JimmyAREM 0:0d5755f210cb 566 }
JimmyAREM 0:0d5755f210cb 567 else
JimmyAREM 0:0d5755f210cb 568 {
JimmyAREM 0:0d5755f210cb 569 //Point correct
JimmyAREM 0:0d5755f210cb 570 }
JimmyAREM 0:0d5755f210cb 571 }
JimmyAREM 0:0d5755f210cb 572 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 573 {
JimmyAREM 0:0d5755f210cb 574 if ((robot.Bx >= AX8) && (robot.Bx <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 575 {
JimmyAREM 0:0d5755f210cb 576 if ((robot.By>= AY8) && (robot.By <= BY8))
JimmyAREM 0:0d5755f210cb 577 {
JimmyAREM 0:0d5755f210cb 578 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 579 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 580 }
JimmyAREM 0:0d5755f210cb 581 else
JimmyAREM 0:0d5755f210cb 582 {
JimmyAREM 0:0d5755f210cb 583 //Point correct
JimmyAREM 0:0d5755f210cb 584 }
JimmyAREM 0:0d5755f210cb 585 }
JimmyAREM 0:0d5755f210cb 586 else
JimmyAREM 0:0d5755f210cb 587 {
JimmyAREM 0:0d5755f210cb 588 //Point correct
JimmyAREM 0:0d5755f210cb 589 }
JimmyAREM 0:0d5755f210cb 590 }
JimmyAREM 0:0d5755f210cb 591 if ((robot.Bx >= AX9) && (robot.Bx <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 592 {
JimmyAREM 0:0d5755f210cb 593 if ((robot.By >= AY9) && (robot.By <= BY9))
JimmyAREM 0:0d5755f210cb 594 {
JimmyAREM 0:0d5755f210cb 595 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 596 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 597 }
JimmyAREM 0:0d5755f210cb 598 else
JimmyAREM 0:0d5755f210cb 599 {
JimmyAREM 0:0d5755f210cb 600 //Point correct
JimmyAREM 0:0d5755f210cb 601 }
JimmyAREM 0:0d5755f210cb 602 }
JimmyAREM 0:0d5755f210cb 603 else
JimmyAREM 0:0d5755f210cb 604 {
JimmyAREM 0:0d5755f210cb 605 //Point correct
JimmyAREM 0:0d5755f210cb 606 }
JimmyAREM 0:0d5755f210cb 607 //printf(" Point C, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 608 if ((robot.Cx >= AX0) && (robot.Cx <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 609 {
JimmyAREM 0:0d5755f210cb 610 if ((robot.Cy >= AY0) && (robot.Cy <= BY0))
JimmyAREM 0:0d5755f210cb 611 {
JimmyAREM 0:0d5755f210cb 612 //Point correct
JimmyAREM 0:0d5755f210cb 613 }
JimmyAREM 0:0d5755f210cb 614 else
JimmyAREM 0:0d5755f210cb 615 {
JimmyAREM 0:0d5755f210cb 616 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 617 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 618 }
JimmyAREM 0:0d5755f210cb 619 }
JimmyAREM 0:0d5755f210cb 620 else
JimmyAREM 0:0d5755f210cb 621 {
JimmyAREM 0:0d5755f210cb 622 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 623 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 624 }
JimmyAREM 0:0d5755f210cb 625 if ((robot.Cx >= AX1) && (robot.Cx <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 626 {
JimmyAREM 0:0d5755f210cb 627 if ((robot.Cy >= AY1) && (robot.Cy <= BY1))
JimmyAREM 0:0d5755f210cb 628 {
JimmyAREM 0:0d5755f210cb 629 printf("Dans l'accelerateur\n\r");
JimmyAREM 0:0d5755f210cb 630 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 631 }
JimmyAREM 0:0d5755f210cb 632 else
JimmyAREM 0:0d5755f210cb 633 {
JimmyAREM 0:0d5755f210cb 634 //Point correct
JimmyAREM 0:0d5755f210cb 635 }
JimmyAREM 0:0d5755f210cb 636 }
JimmyAREM 0:0d5755f210cb 637 else
JimmyAREM 0:0d5755f210cb 638 {
JimmyAREM 0:0d5755f210cb 639 //point correct
JimmyAREM 0:0d5755f210cb 640 }
JimmyAREM 0:0d5755f210cb 641 if ((robot.Cx >= AX2) && (robot.Cx <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 642 {
JimmyAREM 0:0d5755f210cb 643 if ((robot.Cy >= AY2) && (robot.Cy <= BY2))
JimmyAREM 0:0d5755f210cb 644 {
JimmyAREM 0:0d5755f210cb 645 printf("Dans la zone chaos violet \n\r");
JimmyAREM 0:0d5755f210cb 646 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 647 }
JimmyAREM 0:0d5755f210cb 648 else
JimmyAREM 0:0d5755f210cb 649 {
JimmyAREM 0:0d5755f210cb 650 //Point correct
JimmyAREM 0:0d5755f210cb 651 }
JimmyAREM 0:0d5755f210cb 652 }
JimmyAREM 0:0d5755f210cb 653 else
JimmyAREM 0:0d5755f210cb 654 {
JimmyAREM 0:0d5755f210cb 655 //Point correct
JimmyAREM 0:0d5755f210cb 656 }
JimmyAREM 0:0d5755f210cb 657 if ((robot.Cx >= AX3) && (robot.Cx <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 658 {
JimmyAREM 0:0d5755f210cb 659 if ((robot.Cy >= AY3) && (robot.Cy <= BY3))
JimmyAREM 0:0d5755f210cb 660 {
JimmyAREM 0:0d5755f210cb 661 printf("Dans la zone chaos jaune \n\r");
JimmyAREM 0:0d5755f210cb 662 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 663 }
JimmyAREM 0:0d5755f210cb 664 else
JimmyAREM 0:0d5755f210cb 665 {
JimmyAREM 0:0d5755f210cb 666 //Point correct
JimmyAREM 0:0d5755f210cb 667 }
JimmyAREM 0:0d5755f210cb 668 }
JimmyAREM 0:0d5755f210cb 669 else
JimmyAREM 0:0d5755f210cb 670 {
JimmyAREM 0:0d5755f210cb 671 //Point correct
JimmyAREM 0:0d5755f210cb 672 }
JimmyAREM 0:0d5755f210cb 673 if ((robot.Cx >= AX4) && (robot.Cx <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 674 {
JimmyAREM 0:0d5755f210cb 675 if ((robot.Cy >= AY4) && (robot.Cy <= BY4))
JimmyAREM 0:0d5755f210cb 676 {
JimmyAREM 0:0d5755f210cb 677 printf("Dans la zone distributeur violet \n\r");
JimmyAREM 0:0d5755f210cb 678 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 679 }
JimmyAREM 0:0d5755f210cb 680 else
JimmyAREM 0:0d5755f210cb 681 {
JimmyAREM 0:0d5755f210cb 682 //Point correct
JimmyAREM 0:0d5755f210cb 683 }
JimmyAREM 0:0d5755f210cb 684 }
JimmyAREM 0:0d5755f210cb 685 else
JimmyAREM 0:0d5755f210cb 686 {
JimmyAREM 0:0d5755f210cb 687 //Point correct
JimmyAREM 0:0d5755f210cb 688 }
JimmyAREM 0:0d5755f210cb 689 if ((robot.Cx >= AX5) && (robot.Cx <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 690 {
JimmyAREM 0:0d5755f210cb 691 if ((robot.Cy >= AY5) && (robot.Cy <= BY5))
JimmyAREM 0:0d5755f210cb 692 {
JimmyAREM 0:0d5755f210cb 693 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 694 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 695 }
JimmyAREM 0:0d5755f210cb 696 else
JimmyAREM 0:0d5755f210cb 697 {
JimmyAREM 0:0d5755f210cb 698 //Point correct
JimmyAREM 0:0d5755f210cb 699 }
JimmyAREM 0:0d5755f210cb 700 }
JimmyAREM 0:0d5755f210cb 701 else
JimmyAREM 0:0d5755f210cb 702 {
JimmyAREM 0:0d5755f210cb 703 //Point correct
JimmyAREM 0:0d5755f210cb 704 }
JimmyAREM 0:0d5755f210cb 705 if ((robot.Cx >= AX6) && (robot.Cx <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 706 {
JimmyAREM 0:0d5755f210cb 707 if ((robot.Cy >= AY6) && (robot.Cy <= BY6))
JimmyAREM 0:0d5755f210cb 708 {
JimmyAREM 0:0d5755f210cb 709 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 710 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 711 }
JimmyAREM 0:0d5755f210cb 712 else
JimmyAREM 0:0d5755f210cb 713 {
JimmyAREM 0:0d5755f210cb 714 //Point correct
JimmyAREM 0:0d5755f210cb 715 }
JimmyAREM 0:0d5755f210cb 716 }
JimmyAREM 0:0d5755f210cb 717 else
JimmyAREM 0:0d5755f210cb 718 {
JimmyAREM 0:0d5755f210cb 719 //Point correct
JimmyAREM 0:0d5755f210cb 720 }
JimmyAREM 0:0d5755f210cb 721 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 722 {
JimmyAREM 0:0d5755f210cb 723 if ((robot.Cx >= AX7) && (robot.Cx <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 724 {
JimmyAREM 0:0d5755f210cb 725 if ((robot.Cy >= AY7) && (robot.Cy <= BY7))
JimmyAREM 0:0d5755f210cb 726 {
JimmyAREM 0:0d5755f210cb 727 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 728 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 729 }
JimmyAREM 0:0d5755f210cb 730 else
JimmyAREM 0:0d5755f210cb 731 {
JimmyAREM 0:0d5755f210cb 732 //Point correct
JimmyAREM 0:0d5755f210cb 733 }
JimmyAREM 0:0d5755f210cb 734 }
JimmyAREM 0:0d5755f210cb 735 else
JimmyAREM 0:0d5755f210cb 736 {
JimmyAREM 0:0d5755f210cb 737 //Point correct
JimmyAREM 0:0d5755f210cb 738 }
JimmyAREM 0:0d5755f210cb 739 }
JimmyAREM 0:0d5755f210cb 740 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 741 {
JimmyAREM 0:0d5755f210cb 742 if ((robot.Cx >= AX8) && (robot.Cx <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 743 {
JimmyAREM 0:0d5755f210cb 744 if ((robot.Cy>= AY8) && (robot.Cy <= BY8))
JimmyAREM 0:0d5755f210cb 745 {
JimmyAREM 0:0d5755f210cb 746 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 747 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 748 }
JimmyAREM 0:0d5755f210cb 749 else
JimmyAREM 0:0d5755f210cb 750 {
JimmyAREM 0:0d5755f210cb 751 //Point correct
JimmyAREM 0:0d5755f210cb 752 }
JimmyAREM 0:0d5755f210cb 753 }
JimmyAREM 0:0d5755f210cb 754 else
JimmyAREM 0:0d5755f210cb 755 {
JimmyAREM 0:0d5755f210cb 756 //Point correct
JimmyAREM 0:0d5755f210cb 757 }
JimmyAREM 0:0d5755f210cb 758 }
JimmyAREM 0:0d5755f210cb 759 if ((robot.Cx >= AX9) && (robot.Cx <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 760 {
JimmyAREM 0:0d5755f210cb 761 if ((robot.Cy >= AY9) && (robot.Cy <= BY9))
JimmyAREM 0:0d5755f210cb 762 {
JimmyAREM 0:0d5755f210cb 763 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 764 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 765 }
JimmyAREM 0:0d5755f210cb 766 else
JimmyAREM 0:0d5755f210cb 767 {
JimmyAREM 0:0d5755f210cb 768 //Point correct
JimmyAREM 0:0d5755f210cb 769 }
JimmyAREM 0:0d5755f210cb 770 }
JimmyAREM 0:0d5755f210cb 771 else
JimmyAREM 0:0d5755f210cb 772 {
JimmyAREM 0:0d5755f210cb 773 //Point correct
JimmyAREM 0:0d5755f210cb 774 }
JimmyAREM 0:0d5755f210cb 775 //printf(" Point D, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 776 if ((robot.Dx >= AX0) && (robot.Dx <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 777 {
JimmyAREM 0:0d5755f210cb 778 if ((robot.Dy >= AY0) && (robot.Dy <= BY0))
JimmyAREM 0:0d5755f210cb 779 {
JimmyAREM 0:0d5755f210cb 780 //Point correct
JimmyAREM 0:0d5755f210cb 781 }
JimmyAREM 0:0d5755f210cb 782 else
JimmyAREM 0:0d5755f210cb 783 {
JimmyAREM 0:0d5755f210cb 784 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 785 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 786 }
JimmyAREM 0:0d5755f210cb 787 }
JimmyAREM 0:0d5755f210cb 788 else
JimmyAREM 0:0d5755f210cb 789 {
JimmyAREM 0:0d5755f210cb 790 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 791 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 792 }
JimmyAREM 0:0d5755f210cb 793 if ((robot.Dx >= AX1) && (robot.Dx <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 794 {
JimmyAREM 0:0d5755f210cb 795 if ((robot.Dy >= AY1) && (robot.Dy <= BY1))
JimmyAREM 0:0d5755f210cb 796 {
JimmyAREM 0:0d5755f210cb 797 printf("Dans l'accelerateur\n\r");
JimmyAREM 0:0d5755f210cb 798 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 799 }
JimmyAREM 0:0d5755f210cb 800 else
JimmyAREM 0:0d5755f210cb 801 {
JimmyAREM 0:0d5755f210cb 802 //Point correct
JimmyAREM 0:0d5755f210cb 803 }
JimmyAREM 0:0d5755f210cb 804 }
JimmyAREM 0:0d5755f210cb 805 else
JimmyAREM 0:0d5755f210cb 806 {
JimmyAREM 0:0d5755f210cb 807 //point correct
JimmyAREM 0:0d5755f210cb 808 }
JimmyAREM 0:0d5755f210cb 809 if ((robot.Dx >= AX2) && (robot.Dx <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 810 {
JimmyAREM 0:0d5755f210cb 811 if ((robot.Dy >= AY2) && (robot.Dy <= BY2))
JimmyAREM 0:0d5755f210cb 812 {
JimmyAREM 0:0d5755f210cb 813 printf("Dans la zone chaos violet \n\r");
JimmyAREM 0:0d5755f210cb 814 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 815 }
JimmyAREM 0:0d5755f210cb 816 else
JimmyAREM 0:0d5755f210cb 817 {
JimmyAREM 0:0d5755f210cb 818 //Point correct
JimmyAREM 0:0d5755f210cb 819 }
JimmyAREM 0:0d5755f210cb 820 }
JimmyAREM 0:0d5755f210cb 821 else
JimmyAREM 0:0d5755f210cb 822 {
JimmyAREM 0:0d5755f210cb 823 //Point correct
JimmyAREM 0:0d5755f210cb 824 }
JimmyAREM 0:0d5755f210cb 825 if ((robot.Dx >= AX3) && (robot.Dx <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 826 {
JimmyAREM 0:0d5755f210cb 827 if ((robot.Dy >= AY3) && (robot.Dy <= BY3))
JimmyAREM 0:0d5755f210cb 828 {
JimmyAREM 0:0d5755f210cb 829 printf("Dans la zone chaos jaune \n\r");
JimmyAREM 0:0d5755f210cb 830 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 831 }
JimmyAREM 0:0d5755f210cb 832 else
JimmyAREM 0:0d5755f210cb 833 {
JimmyAREM 0:0d5755f210cb 834 //Point correct
JimmyAREM 0:0d5755f210cb 835 }
JimmyAREM 0:0d5755f210cb 836 }
JimmyAREM 0:0d5755f210cb 837 else
JimmyAREM 0:0d5755f210cb 838 {
JimmyAREM 0:0d5755f210cb 839 //Point correct
JimmyAREM 0:0d5755f210cb 840 }
JimmyAREM 0:0d5755f210cb 841 if ((robot.Dx >= AX4) && (robot.Dx <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 842 {
JimmyAREM 0:0d5755f210cb 843 if ((robot.Dy >= AY4) && (robot.Dy <= BY4))
JimmyAREM 0:0d5755f210cb 844 {
JimmyAREM 0:0d5755f210cb 845 printf("Dans la zone distributeur violet \n\r");
JimmyAREM 0:0d5755f210cb 846 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 847 }
JimmyAREM 0:0d5755f210cb 848 else
JimmyAREM 0:0d5755f210cb 849 {
JimmyAREM 0:0d5755f210cb 850 //Point correct
JimmyAREM 0:0d5755f210cb 851 }
JimmyAREM 0:0d5755f210cb 852 }
JimmyAREM 0:0d5755f210cb 853 else
JimmyAREM 0:0d5755f210cb 854 {
JimmyAREM 0:0d5755f210cb 855 //Point correct
JimmyAREM 0:0d5755f210cb 856 }
JimmyAREM 0:0d5755f210cb 857 if ((robot.Dx >= AX5) && (robot.Dx <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 858 {
JimmyAREM 0:0d5755f210cb 859 if ((robot.Dy >= AY5) && (robot.Dy <= BY5))
JimmyAREM 0:0d5755f210cb 860 {
JimmyAREM 0:0d5755f210cb 861 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 862 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 863 }
JimmyAREM 0:0d5755f210cb 864 else
JimmyAREM 0:0d5755f210cb 865 {
JimmyAREM 0:0d5755f210cb 866 //Point correct
JimmyAREM 0:0d5755f210cb 867 }
JimmyAREM 0:0d5755f210cb 868 }
JimmyAREM 0:0d5755f210cb 869 else
JimmyAREM 0:0d5755f210cb 870 {
JimmyAREM 0:0d5755f210cb 871 //Point correct
JimmyAREM 0:0d5755f210cb 872 }
JimmyAREM 0:0d5755f210cb 873 if ((robot.Dx >= AX6) && (robot.Dx <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 874 {
JimmyAREM 0:0d5755f210cb 875 if ((robot.Dy >= AY6) && (robot.Dy <= BY6))
JimmyAREM 0:0d5755f210cb 876 {
JimmyAREM 0:0d5755f210cb 877 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 878 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 879 }
JimmyAREM 0:0d5755f210cb 880 else
JimmyAREM 0:0d5755f210cb 881 {
JimmyAREM 0:0d5755f210cb 882 //Point correct
JimmyAREM 0:0d5755f210cb 883 }
JimmyAREM 0:0d5755f210cb 884 }
JimmyAREM 0:0d5755f210cb 885 else
JimmyAREM 0:0d5755f210cb 886 {
JimmyAREM 0:0d5755f210cb 887 //Point correct
JimmyAREM 0:0d5755f210cb 888 }
JimmyAREM 0:0d5755f210cb 889 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 890 {
JimmyAREM 0:0d5755f210cb 891 if ((robot.Dx >= AX7) && (robot.Dx <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 892 {
JimmyAREM 0:0d5755f210cb 893 if ((robot.Dy >= AY7) && (robot.Dy <= BY7))
JimmyAREM 0:0d5755f210cb 894 {
JimmyAREM 0:0d5755f210cb 895 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 896 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 897 }
JimmyAREM 0:0d5755f210cb 898 else
JimmyAREM 0:0d5755f210cb 899 {
JimmyAREM 0:0d5755f210cb 900 //Point correct
JimmyAREM 0:0d5755f210cb 901 }
JimmyAREM 0:0d5755f210cb 902 }
JimmyAREM 0:0d5755f210cb 903 else
JimmyAREM 0:0d5755f210cb 904 {
JimmyAREM 0:0d5755f210cb 905 //Point correct
JimmyAREM 0:0d5755f210cb 906 }
JimmyAREM 0:0d5755f210cb 907 }
JimmyAREM 0:0d5755f210cb 908 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 909 {
JimmyAREM 0:0d5755f210cb 910 if ((robot.Dx >= AX8) && (robot.Dx <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 911 {
JimmyAREM 0:0d5755f210cb 912 if ((robot.Dy>= AY8) && (robot.Dy <= BY8))
JimmyAREM 0:0d5755f210cb 913 {
JimmyAREM 0:0d5755f210cb 914 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 915 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 916 }
JimmyAREM 0:0d5755f210cb 917 else
JimmyAREM 0:0d5755f210cb 918 {
JimmyAREM 0:0d5755f210cb 919 //Point correct
JimmyAREM 0:0d5755f210cb 920 }
JimmyAREM 0:0d5755f210cb 921 }
JimmyAREM 0:0d5755f210cb 922 else
JimmyAREM 0:0d5755f210cb 923 {
JimmyAREM 0:0d5755f210cb 924 //Point correct
JimmyAREM 0:0d5755f210cb 925 }
JimmyAREM 0:0d5755f210cb 926 }
JimmyAREM 0:0d5755f210cb 927 if ((robot.Dx >= AX9) && (robot.Dx <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 928 {
JimmyAREM 0:0d5755f210cb 929 if ((robot.Dy >= AY9) && (robot.Dy <= BY9))
JimmyAREM 0:0d5755f210cb 930 {
JimmyAREM 0:0d5755f210cb 931 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 932 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 933 }
JimmyAREM 0:0d5755f210cb 934 else
JimmyAREM 0:0d5755f210cb 935 {
JimmyAREM 0:0d5755f210cb 936 //Point correct
JimmyAREM 0:0d5755f210cb 937 }
JimmyAREM 0:0d5755f210cb 938 }
JimmyAREM 0:0d5755f210cb 939 else
JimmyAREM 0:0d5755f210cb 940 {
JimmyAREM 0:0d5755f210cb 941 //Point correct
JimmyAREM 0:0d5755f210cb 942 }
JimmyAREM 0:0d5755f210cb 943 }
JimmyAREM 0:0d5755f210cb 944 return SANS_OBSTACLE;
JimmyAREM 0:0d5755f210cb 945 }
JimmyAREM 0:0d5755f210cb 946
JimmyAREM 0:0d5755f210cb 947
JimmyAREM 0:0d5755f210cb 948 bool lancerUnEvitementParArcGauche(int distanceUltrasonGauche)
JimmyAREM 0:0d5755f210cb 949 {
JimmyAREM 0:0d5755f210cb 950 Arc arcCentre;
JimmyAREM 0:0d5755f210cb 951 Point bufferCentre[TAILLE_BUFFER_POINT];
JimmyAREM 0:0d5755f210cb 952
JimmyAREM 0:0d5755f210cb 953 actualiserPosRobot();
JimmyAREM 0:0d5755f210cb 954 calculPosFinalGauche(distanceUltrasonGauche);
JimmyAREM 0:0d5755f210cb 955 calculPosInterParLaDroite();
JimmyAREM 0:0d5755f210cb 956
JimmyAREM 0:0d5755f210cb 957
JimmyAREM 0:0d5755f210cb 958 if(arcPossible() == false)
JimmyAREM 0:0d5755f210cb 959 {
JimmyAREM 0:0d5755f210cb 960 //printf("Les points sont alignes \n\r");
JimmyAREM 0:0d5755f210cb 961 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 962 }
JimmyAREM 0:0d5755f210cb 963 else
JimmyAREM 0:0d5755f210cb 964 {
JimmyAREM 0:0d5755f210cb 965 arcCentre = trouverArc();
JimmyAREM 0:0d5755f210cb 966 recupererTableauPointArc(arcCentre, bufferCentre, PARLAGAUCHE);
JimmyAREM 0:0d5755f210cb 967 if (estUnArc(bufferCentre) == SANS_OBSTACLE)
JimmyAREM 0:0d5755f210cb 968 {
JimmyAREM 0:0d5755f210cb 969 //printf("Arc gauche possible ! \n\r");
JimmyAREM 0:0d5755f210cb 970 return SANS_OBSTACLE;
JimmyAREM 0:0d5755f210cb 971 }
JimmyAREM 0:0d5755f210cb 972 else
JimmyAREM 0:0d5755f210cb 973 {
JimmyAREM 0:0d5755f210cb 974 //printf("Arc gauche impossible ! \n\r");
JimmyAREM 0:0d5755f210cb 975 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 976 }
JimmyAREM 0:0d5755f210cb 977 }
JimmyAREM 0:0d5755f210cb 978 }
JimmyAREM 0:0d5755f210cb 979
JimmyAREM 0:0d5755f210cb 980 bool lancerUnEvitementParArcDroit(int distanceUltrasonDroite)
JimmyAREM 0:0d5755f210cb 981 {
JimmyAREM 0:0d5755f210cb 982 Arc arcCentre;
JimmyAREM 0:0d5755f210cb 983 Point bufferCentre[TAILLE_BUFFER_POINT];
JimmyAREM 0:0d5755f210cb 984
JimmyAREM 0:0d5755f210cb 985 actualiserPosRobot();
JimmyAREM 0:0d5755f210cb 986 calculPosFinalDroite(distanceUltrasonDroite);
JimmyAREM 0:0d5755f210cb 987 calculPosInterParLaGauche();
JimmyAREM 0:0d5755f210cb 988
JimmyAREM 0:0d5755f210cb 989 if(arcPossible() == false)
JimmyAREM 0:0d5755f210cb 990 {
JimmyAREM 0:0d5755f210cb 991 //printf("Les points sont alignes \n\r");
JimmyAREM 0:0d5755f210cb 992 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 993 }
JimmyAREM 0:0d5755f210cb 994 else
JimmyAREM 0:0d5755f210cb 995 {
JimmyAREM 0:0d5755f210cb 996 arcCentre = trouverArc();
JimmyAREM 0:0d5755f210cb 997 recupererTableauPointArc(arcCentre, bufferCentre, PARLADROITE);
JimmyAREM 0:0d5755f210cb 998 if (estUnArc(bufferCentre) == SANS_OBSTACLE)
JimmyAREM 0:0d5755f210cb 999 {
JimmyAREM 0:0d5755f210cb 1000 //printf("Arc droit possible ! \n\r");
JimmyAREM 0:0d5755f210cb 1001 return SANS_OBSTACLE;
JimmyAREM 0:0d5755f210cb 1002 }
JimmyAREM 0:0d5755f210cb 1003 else
JimmyAREM 0:0d5755f210cb 1004 {
JimmyAREM 0:0d5755f210cb 1005 //printf("Arc droit impossible ! \n\r");
JimmyAREM 0:0d5755f210cb 1006 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 1007 }
JimmyAREM 0:0d5755f210cb 1008 }
JimmyAREM 0:0d5755f210cb 1009 }