Guillaume Chauvon / AnalyseArcLib

lmlml

AnalyseArcLib.cpp@2:9fbf52f12cea, 2019-05-26 (annotated)

Committer:
JimmyAREM
Date:
Sun May 26 14:58:08 2019 +0000
Revision:
2:9fbf52f12cea
Parent:
0:0d5755f210cb 
an

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 2:9fbf52f12cea 14 extern Serial pc;
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 2:9fbf52f12cea 222 pc.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 2:9fbf52f12cea 231 pc.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 2:9fbf52f12cea 237
JimmyAREM 2:9fbf52f12cea 238 int trouverIndicePointFinal(Point buffer[], double xF, double yF)
JimmyAREM 2:9fbf52f12cea 239 {
JimmyAREM 2:9fbf52f12cea 240 double xFlocal = xF/100.0 * cos(capStartRobot* 3.1415/180.0) + yF/100.0 * sin(capStartRobot* 3.1415/180.0);
JimmyAREM 2:9fbf52f12cea 241 double yFlocal = yF/100.0 * cos(capStartRobot* 3.1415/180.0) - xF/100.0 * sin(capStartRobot* 3.1415/180.0);
JimmyAREM 2:9fbf52f12cea 242 double min = 3000.0;
JimmyAREM 2:9fbf52f12cea 243 Point bufferLocal[TAILLE_BUFFER_POINT];
JimmyAREM 2:9fbf52f12cea 244 int indice = 0;
JimmyAREM 2:9fbf52f12cea 245 for(int i=0; i<TAILLE_BUFFER_POINT; i++)
JimmyAREM 2:9fbf52f12cea 246 {
JimmyAREM 2:9fbf52f12cea 247 bufferLocal[i].X = buffer[i].X * cos(capStartRobot* 3.1415/180.0) + buffer[i].Y * sin(capStartRobot* 3.1415/180.0);
JimmyAREM 2:9fbf52f12cea 248 pc.printf("la difference : %lf avec l'indice i : %d \n\r", abs(bufferLocal[i].X - xFlocal), i);
JimmyAREM 2:9fbf52f12cea 249 //bufferLocal.Y = buffer[i].Y * cos((capStartRobot* 3.1415/180.0) + buffer[i].X * sin((capStartRobot* 3.1415/180.0);
JimmyAREM 2:9fbf52f12cea 250 if(abs(bufferLocal[i].X - xFlocal) < min)
JimmyAREM 2:9fbf52f12cea 251 {
JimmyAREM 2:9fbf52f12cea 252 min = abs(bufferLocal[i].X - xFlocal);
JimmyAREM 2:9fbf52f12cea 253 indice = i;
JimmyAREM 2:9fbf52f12cea 254
JimmyAREM 2:9fbf52f12cea 255 }
JimmyAREM 2:9fbf52f12cea 256 }
JimmyAREM 2:9fbf52f12cea 257 pc.printf("indice du buffer : %d\n\r", indice);
JimmyAREM 2:9fbf52f12cea 258 return indice;
JimmyAREM 2:9fbf52f12cea 259 }
JimmyAREM 2:9fbf52f12cea 260
JimmyAREM 2:9fbf52f12cea 261 void modifierPointFinal(Point buffer[], int indice)
JimmyAREM 2:9fbf52f12cea 262 {
JimmyAREM 2:9fbf52f12cea 263 posFinalRobotX = buffer[indice].X;
JimmyAREM 2:9fbf52f12cea 264 posFinalRobotY = buffer[indice].Y;
JimmyAREM 2:9fbf52f12cea 265 pc.printf(" xf : %lf yf: %lf\n\r", posFinalRobotX, posFinalRobotY);
JimmyAREM 2:9fbf52f12cea 266 return;
JimmyAREM 2:9fbf52f12cea 267 }
JimmyAREM 2:9fbf52f12cea 268
JimmyAREM 0:0d5755f210cb 269 void afficherRobot(Robot robot)
JimmyAREM 0:0d5755f210cb 270 {
JimmyAREM 0:0d5755f210cb 271 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 272 }
JimmyAREM 0:0d5755f210cb 273
JimmyAREM 0:0d5755f210cb 274
JimmyAREM 0:0d5755f210cb 275 void afficherArc(Arc arc)
JimmyAREM 0:0d5755f210cb 276 {
JimmyAREM 0:0d5755f210cb 277 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 278 }
JimmyAREM 0:0d5755f210cb 279
JimmyAREM 0:0d5755f210cb 280 void afficher(Point buffer[])
JimmyAREM 0:0d5755f210cb 281 {
JimmyAREM 0:0d5755f210cb 282 for(int i=0; i<TAILLE_BUFFER_POINT; i++)
JimmyAREM 0:0d5755f210cb 283 {
JimmyAREM 0:0d5755f210cb 284 printf("Coordonnee du point P indice %d : (%lf,%lf) \n\r", i, buffer[i].X, buffer[i].Y);
JimmyAREM 0:0d5755f210cb 285 }
JimmyAREM 0:0d5755f210cb 286 }
JimmyAREM 0:0d5755f210cb 287
JimmyAREM 0:0d5755f210cb 288
JimmyAREM 0:0d5755f210cb 289
JimmyAREM 2:9fbf52f12cea 290 bool estUnArc(Point tableau[], int indice)
JimmyAREM 0:0d5755f210cb 291 {
JimmyAREM 0:0d5755f210cb 292 Robot robot;
JimmyAREM 0:0d5755f210cb 293
JimmyAREM 2:9fbf52f12cea 294 for(int i=indice; i<TAILLE_BUFFER_POINT; i++)
JimmyAREM 0:0d5755f210cb 295 { if(i != TAILLE_BUFFER_POINT -1)
JimmyAREM 0:0d5755f210cb 296 {
JimmyAREM 0:0d5755f210cb 297 robot = setPosRobot(tableau[i], calculercapActuel(tableau[i], tableau[i+1]));
JimmyAREM 0:0d5755f210cb 298 }
JimmyAREM 0:0d5755f210cb 299 else
JimmyAREM 0:0d5755f210cb 300 {
JimmyAREM 0:0d5755f210cb 301 robot = setPosRobot(tableau[i], calculercapActuel(tableau[i-1], tableau[i]));
JimmyAREM 0:0d5755f210cb 302 }
JimmyAREM 0:0d5755f210cb 303 //printf(" Point A, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 304 if ((robot.Ax >= AX0) && (robot.Ax <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 305 {
JimmyAREM 0:0d5755f210cb 306 if ((robot.Ay >= AY0) && (robot.Ay <= BY0))
JimmyAREM 0:0d5755f210cb 307 {
JimmyAREM 0:0d5755f210cb 308 //Point correct
JimmyAREM 0:0d5755f210cb 309 }
JimmyAREM 0:0d5755f210cb 310 else
JimmyAREM 0:0d5755f210cb 311 {
JimmyAREM 0:0d5755f210cb 312 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 313 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 314 }
JimmyAREM 0:0d5755f210cb 315 }
JimmyAREM 0:0d5755f210cb 316 else
JimmyAREM 0:0d5755f210cb 317 {
JimmyAREM 0:0d5755f210cb 318 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 319 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 320 }
JimmyAREM 0:0d5755f210cb 321 if ((robot.Ax >= AX1) && (robot.Ax <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 322 {
JimmyAREM 0:0d5755f210cb 323 if ((robot.Ay >= AY1) && (robot.Ay <= BY1))
JimmyAREM 0:0d5755f210cb 324 {
JimmyAREM 0:0d5755f210cb 325 printf("Dans l'accelerateur\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 >= AX2) && (robot.Ax <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 338 {
JimmyAREM 0:0d5755f210cb 339 if ((robot.Ay >= AY2) && (robot.Ay <= BY2))
JimmyAREM 0:0d5755f210cb 340 {
JimmyAREM 0:0d5755f210cb 341 printf("Dans la zone chaos 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 >= AX3) && (robot.Ax <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 354 {
JimmyAREM 0:0d5755f210cb 355 if ((robot.Ay >= AY3) && (robot.Ay <= BY3))
JimmyAREM 0:0d5755f210cb 356 {
JimmyAREM 0:0d5755f210cb 357 printf("Dans la zone chaos 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 >= AX4) && (robot.Ax <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 370 {
JimmyAREM 0:0d5755f210cb 371 if ((robot.Ay >= AY4) && (robot.Ay <= BY4))
JimmyAREM 0:0d5755f210cb 372 {
JimmyAREM 0:0d5755f210cb 373 printf("Dans la zone distributeur violet \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 ((robot.Ax >= AX5) && (robot.Ax <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 386 {
JimmyAREM 0:0d5755f210cb 387 if ((robot.Ay >= AY5) && (robot.Ay <= BY5))
JimmyAREM 0:0d5755f210cb 388 {
JimmyAREM 0:0d5755f210cb 389 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 390 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 391 }
JimmyAREM 0:0d5755f210cb 392 else
JimmyAREM 0:0d5755f210cb 393 {
JimmyAREM 0:0d5755f210cb 394 //Point correct
JimmyAREM 0:0d5755f210cb 395 }
JimmyAREM 0:0d5755f210cb 396 }
JimmyAREM 0:0d5755f210cb 397 else
JimmyAREM 0:0d5755f210cb 398 {
JimmyAREM 0:0d5755f210cb 399 //Point correct
JimmyAREM 0:0d5755f210cb 400 }
JimmyAREM 0:0d5755f210cb 401 if ((robot.Ax >= AX6) && (robot.Ax <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 402 {
JimmyAREM 0:0d5755f210cb 403 if ((robot.Ay >= AY6) && (robot.Ay <= BY6))
JimmyAREM 0:0d5755f210cb 404 {
JimmyAREM 0:0d5755f210cb 405 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 406 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 407 }
JimmyAREM 0:0d5755f210cb 408 else
JimmyAREM 0:0d5755f210cb 409 {
JimmyAREM 0:0d5755f210cb 410 //Point correct
JimmyAREM 0:0d5755f210cb 411 }
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 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 418 {
JimmyAREM 0:0d5755f210cb 419 if ((robot.Ax >= AX7) && (robot.Ax <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 420 {
JimmyAREM 0:0d5755f210cb 421 if ((robot.Ay >= AY7) && (robot.Ay <= BY7))
JimmyAREM 0:0d5755f210cb 422 {
JimmyAREM 0:0d5755f210cb 423 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 424 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 425 }
JimmyAREM 0:0d5755f210cb 426 else
JimmyAREM 0:0d5755f210cb 427 {
JimmyAREM 0:0d5755f210cb 428 //Point correct
JimmyAREM 0:0d5755f210cb 429 }
JimmyAREM 0:0d5755f210cb 430 }
JimmyAREM 0:0d5755f210cb 431 else
JimmyAREM 0:0d5755f210cb 432 {
JimmyAREM 0:0d5755f210cb 433 //Point correct
JimmyAREM 0:0d5755f210cb 434 }
JimmyAREM 0:0d5755f210cb 435 }
JimmyAREM 0:0d5755f210cb 436 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 437 {
JimmyAREM 0:0d5755f210cb 438 if ((robot.Ax >= AX8) && (robot.Ax <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 439 {
JimmyAREM 0:0d5755f210cb 440 if ((robot.Ay>= AY8) && (robot.Ay <= BY8))
JimmyAREM 0:0d5755f210cb 441 {
JimmyAREM 0:0d5755f210cb 442 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 443 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 444 }
JimmyAREM 0:0d5755f210cb 445 else
JimmyAREM 0:0d5755f210cb 446 {
JimmyAREM 0:0d5755f210cb 447 //Point correct
JimmyAREM 0:0d5755f210cb 448 }
JimmyAREM 0:0d5755f210cb 449 }
JimmyAREM 0:0d5755f210cb 450 else
JimmyAREM 0:0d5755f210cb 451 {
JimmyAREM 0:0d5755f210cb 452 //Point correct
JimmyAREM 0:0d5755f210cb 453 }
JimmyAREM 0:0d5755f210cb 454 }
JimmyAREM 0:0d5755f210cb 455 if ((robot.Ax >= AX9) && (robot.Ax <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 456 {
JimmyAREM 0:0d5755f210cb 457 if ((robot.Ay >= AY9) && (robot.Ay <= BY9))
JimmyAREM 0:0d5755f210cb 458 {
JimmyAREM 0:0d5755f210cb 459 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 460 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 461 }
JimmyAREM 0:0d5755f210cb 462 else
JimmyAREM 0:0d5755f210cb 463 {
JimmyAREM 0:0d5755f210cb 464 //Point correct
JimmyAREM 0:0d5755f210cb 465 }
JimmyAREM 0:0d5755f210cb 466 }
JimmyAREM 0:0d5755f210cb 467 else
JimmyAREM 0:0d5755f210cb 468 {
JimmyAREM 0:0d5755f210cb 469 //Point correct
JimmyAREM 0:0d5755f210cb 470 }
JimmyAREM 0:0d5755f210cb 471 //printf(" Point B, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 472 if ((robot.Bx >= AX0) && (robot.Bx <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 473 {
JimmyAREM 0:0d5755f210cb 474 if ((robot.By >= AY0) && (robot.By <= BY0))
JimmyAREM 0:0d5755f210cb 475 {
JimmyAREM 0:0d5755f210cb 476 //Point correct
JimmyAREM 0:0d5755f210cb 477 }
JimmyAREM 0:0d5755f210cb 478 else
JimmyAREM 0:0d5755f210cb 479 {
JimmyAREM 0:0d5755f210cb 480 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 481 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 482 }
JimmyAREM 0:0d5755f210cb 483 }
JimmyAREM 0:0d5755f210cb 484 else
JimmyAREM 0:0d5755f210cb 485 {
JimmyAREM 0:0d5755f210cb 486 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 487 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 488 }
JimmyAREM 0:0d5755f210cb 489 if ((robot.Bx >= AX1) && (robot.Bx <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 490 {
JimmyAREM 0:0d5755f210cb 491 if ((robot.By >= AY1) && (robot.By <= BY1))
JimmyAREM 0:0d5755f210cb 492 {
JimmyAREM 0:0d5755f210cb 493 printf("Dans l'accelerateur\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 >= AX2) && (robot.Bx <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 506 {
JimmyAREM 0:0d5755f210cb 507 if ((robot.By >= AY2) && (robot.By <= BY2))
JimmyAREM 0:0d5755f210cb 508 {
JimmyAREM 0:0d5755f210cb 509 printf("Dans la zone chaos 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 >= AX3) && (robot.Bx <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 522 {
JimmyAREM 0:0d5755f210cb 523 if ((robot.By >= AY3) && (robot.By <= BY3))
JimmyAREM 0:0d5755f210cb 524 {
JimmyAREM 0:0d5755f210cb 525 printf("Dans la zone chaos 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 >= AX4) && (robot.Bx <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 538 {
JimmyAREM 0:0d5755f210cb 539 if ((robot.By >= AY4) && (robot.By <= BY4))
JimmyAREM 0:0d5755f210cb 540 {
JimmyAREM 0:0d5755f210cb 541 printf("Dans la zone distributeur violet \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 ((robot.Bx >= AX5) && (robot.Bx <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 554 {
JimmyAREM 0:0d5755f210cb 555 if ((robot.By >= AY5) && (robot.By <= BY5))
JimmyAREM 0:0d5755f210cb 556 {
JimmyAREM 0:0d5755f210cb 557 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 558 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 559 }
JimmyAREM 0:0d5755f210cb 560 else
JimmyAREM 0:0d5755f210cb 561 {
JimmyAREM 0:0d5755f210cb 562 //Point correct
JimmyAREM 0:0d5755f210cb 563 }
JimmyAREM 0:0d5755f210cb 564 }
JimmyAREM 0:0d5755f210cb 565 else
JimmyAREM 0:0d5755f210cb 566 {
JimmyAREM 0:0d5755f210cb 567 //Point correct
JimmyAREM 0:0d5755f210cb 568 }
JimmyAREM 0:0d5755f210cb 569 if ((robot.Bx >= AX6) && (robot.Bx <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 570 {
JimmyAREM 0:0d5755f210cb 571 if ((robot.By >= AY6) && (robot.By <= BY6))
JimmyAREM 0:0d5755f210cb 572 {
JimmyAREM 0:0d5755f210cb 573 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 574 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 575 }
JimmyAREM 0:0d5755f210cb 576 else
JimmyAREM 0:0d5755f210cb 577 {
JimmyAREM 0:0d5755f210cb 578 //Point correct
JimmyAREM 0:0d5755f210cb 579 }
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 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 586 {
JimmyAREM 0:0d5755f210cb 587 if ((robot.Bx >= AX7) && (robot.Bx <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 588 {
JimmyAREM 0:0d5755f210cb 589 if ((robot.By >= AY7) && (robot.By <= BY7))
JimmyAREM 0:0d5755f210cb 590 {
JimmyAREM 0:0d5755f210cb 591 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 592 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 593 }
JimmyAREM 0:0d5755f210cb 594 else
JimmyAREM 0:0d5755f210cb 595 {
JimmyAREM 0:0d5755f210cb 596 //Point correct
JimmyAREM 0:0d5755f210cb 597 }
JimmyAREM 0:0d5755f210cb 598 }
JimmyAREM 0:0d5755f210cb 599 else
JimmyAREM 0:0d5755f210cb 600 {
JimmyAREM 0:0d5755f210cb 601 //Point correct
JimmyAREM 0:0d5755f210cb 602 }
JimmyAREM 0:0d5755f210cb 603 }
JimmyAREM 0:0d5755f210cb 604 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 605 {
JimmyAREM 0:0d5755f210cb 606 if ((robot.Bx >= AX8) && (robot.Bx <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 607 {
JimmyAREM 0:0d5755f210cb 608 if ((robot.By>= AY8) && (robot.By <= BY8))
JimmyAREM 0:0d5755f210cb 609 {
JimmyAREM 0:0d5755f210cb 610 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 611 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 612 }
JimmyAREM 0:0d5755f210cb 613 else
JimmyAREM 0:0d5755f210cb 614 {
JimmyAREM 0:0d5755f210cb 615 //Point correct
JimmyAREM 0:0d5755f210cb 616 }
JimmyAREM 0:0d5755f210cb 617 }
JimmyAREM 0:0d5755f210cb 618 else
JimmyAREM 0:0d5755f210cb 619 {
JimmyAREM 0:0d5755f210cb 620 //Point correct
JimmyAREM 0:0d5755f210cb 621 }
JimmyAREM 0:0d5755f210cb 622 }
JimmyAREM 0:0d5755f210cb 623 if ((robot.Bx >= AX9) && (robot.Bx <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 624 {
JimmyAREM 0:0d5755f210cb 625 if ((robot.By >= AY9) && (robot.By <= BY9))
JimmyAREM 0:0d5755f210cb 626 {
JimmyAREM 0:0d5755f210cb 627 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 628 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 629 }
JimmyAREM 0:0d5755f210cb 630 else
JimmyAREM 0:0d5755f210cb 631 {
JimmyAREM 0:0d5755f210cb 632 //Point correct
JimmyAREM 0:0d5755f210cb 633 }
JimmyAREM 0:0d5755f210cb 634 }
JimmyAREM 0:0d5755f210cb 635 else
JimmyAREM 0:0d5755f210cb 636 {
JimmyAREM 0:0d5755f210cb 637 //Point correct
JimmyAREM 0:0d5755f210cb 638 }
JimmyAREM 0:0d5755f210cb 639 //printf(" Point C, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 640 if ((robot.Cx >= AX0) && (robot.Cx <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 641 {
JimmyAREM 0:0d5755f210cb 642 if ((robot.Cy >= AY0) && (robot.Cy <= BY0))
JimmyAREM 0:0d5755f210cb 643 {
JimmyAREM 0:0d5755f210cb 644 //Point correct
JimmyAREM 0:0d5755f210cb 645 }
JimmyAREM 0:0d5755f210cb 646 else
JimmyAREM 0:0d5755f210cb 647 {
JimmyAREM 0:0d5755f210cb 648 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 649 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 650 }
JimmyAREM 0:0d5755f210cb 651 }
JimmyAREM 0:0d5755f210cb 652 else
JimmyAREM 0:0d5755f210cb 653 {
JimmyAREM 0:0d5755f210cb 654 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 655 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 656 }
JimmyAREM 0:0d5755f210cb 657 if ((robot.Cx >= AX1) && (robot.Cx <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 658 {
JimmyAREM 0:0d5755f210cb 659 if ((robot.Cy >= AY1) && (robot.Cy <= BY1))
JimmyAREM 0:0d5755f210cb 660 {
JimmyAREM 0:0d5755f210cb 661 printf("Dans l'accelerateur\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 >= AX2) && (robot.Cx <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 674 {
JimmyAREM 0:0d5755f210cb 675 if ((robot.Cy >= AY2) && (robot.Cy <= BY2))
JimmyAREM 0:0d5755f210cb 676 {
JimmyAREM 0:0d5755f210cb 677 printf("Dans la zone chaos 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 >= AX3) && (robot.Cx <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 690 {
JimmyAREM 0:0d5755f210cb 691 if ((robot.Cy >= AY3) && (robot.Cy <= BY3))
JimmyAREM 0:0d5755f210cb 692 {
JimmyAREM 0:0d5755f210cb 693 printf("Dans la zone chaos 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 >= AX4) && (robot.Cx <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 706 {
JimmyAREM 0:0d5755f210cb 707 if ((robot.Cy >= AY4) && (robot.Cy <= BY4))
JimmyAREM 0:0d5755f210cb 708 {
JimmyAREM 0:0d5755f210cb 709 printf("Dans la zone distributeur violet \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 ((robot.Cx >= AX5) && (robot.Cx <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 722 {
JimmyAREM 0:0d5755f210cb 723 if ((robot.Cy >= AY5) && (robot.Cy <= BY5))
JimmyAREM 0:0d5755f210cb 724 {
JimmyAREM 0:0d5755f210cb 725 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 726 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 727 }
JimmyAREM 0:0d5755f210cb 728 else
JimmyAREM 0:0d5755f210cb 729 {
JimmyAREM 0:0d5755f210cb 730 //Point correct
JimmyAREM 0:0d5755f210cb 731 }
JimmyAREM 0:0d5755f210cb 732 }
JimmyAREM 0:0d5755f210cb 733 else
JimmyAREM 0:0d5755f210cb 734 {
JimmyAREM 0:0d5755f210cb 735 //Point correct
JimmyAREM 0:0d5755f210cb 736 }
JimmyAREM 0:0d5755f210cb 737 if ((robot.Cx >= AX6) && (robot.Cx <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 738 {
JimmyAREM 0:0d5755f210cb 739 if ((robot.Cy >= AY6) && (robot.Cy <= BY6))
JimmyAREM 0:0d5755f210cb 740 {
JimmyAREM 0:0d5755f210cb 741 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 742 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 743 }
JimmyAREM 0:0d5755f210cb 744 else
JimmyAREM 0:0d5755f210cb 745 {
JimmyAREM 0:0d5755f210cb 746 //Point correct
JimmyAREM 0:0d5755f210cb 747 }
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 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 754 {
JimmyAREM 0:0d5755f210cb 755 if ((robot.Cx >= AX7) && (robot.Cx <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 756 {
JimmyAREM 0:0d5755f210cb 757 if ((robot.Cy >= AY7) && (robot.Cy <= BY7))
JimmyAREM 0:0d5755f210cb 758 {
JimmyAREM 0:0d5755f210cb 759 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 760 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 761 }
JimmyAREM 0:0d5755f210cb 762 else
JimmyAREM 0:0d5755f210cb 763 {
JimmyAREM 0:0d5755f210cb 764 //Point correct
JimmyAREM 0:0d5755f210cb 765 }
JimmyAREM 0:0d5755f210cb 766 }
JimmyAREM 0:0d5755f210cb 767 else
JimmyAREM 0:0d5755f210cb 768 {
JimmyAREM 0:0d5755f210cb 769 //Point correct
JimmyAREM 0:0d5755f210cb 770 }
JimmyAREM 0:0d5755f210cb 771 }
JimmyAREM 0:0d5755f210cb 772 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 773 {
JimmyAREM 0:0d5755f210cb 774 if ((robot.Cx >= AX8) && (robot.Cx <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 775 {
JimmyAREM 0:0d5755f210cb 776 if ((robot.Cy>= AY8) && (robot.Cy <= BY8))
JimmyAREM 0:0d5755f210cb 777 {
JimmyAREM 0:0d5755f210cb 778 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 779 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 780 }
JimmyAREM 0:0d5755f210cb 781 else
JimmyAREM 0:0d5755f210cb 782 {
JimmyAREM 0:0d5755f210cb 783 //Point correct
JimmyAREM 0:0d5755f210cb 784 }
JimmyAREM 0:0d5755f210cb 785 }
JimmyAREM 0:0d5755f210cb 786 else
JimmyAREM 0:0d5755f210cb 787 {
JimmyAREM 0:0d5755f210cb 788 //Point correct
JimmyAREM 0:0d5755f210cb 789 }
JimmyAREM 0:0d5755f210cb 790 }
JimmyAREM 0:0d5755f210cb 791 if ((robot.Cx >= AX9) && (robot.Cx <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 792 {
JimmyAREM 0:0d5755f210cb 793 if ((robot.Cy >= AY9) && (robot.Cy <= BY9))
JimmyAREM 0:0d5755f210cb 794 {
JimmyAREM 0:0d5755f210cb 795 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 796 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 797 }
JimmyAREM 0:0d5755f210cb 798 else
JimmyAREM 0:0d5755f210cb 799 {
JimmyAREM 0:0d5755f210cb 800 //Point correct
JimmyAREM 0:0d5755f210cb 801 }
JimmyAREM 0:0d5755f210cb 802 }
JimmyAREM 0:0d5755f210cb 803 else
JimmyAREM 0:0d5755f210cb 804 {
JimmyAREM 0:0d5755f210cb 805 //Point correct
JimmyAREM 0:0d5755f210cb 806 }
JimmyAREM 0:0d5755f210cb 807 //printf(" Point D, a l'indice : %d\n\r", i);
JimmyAREM 0:0d5755f210cb 808 if ((robot.Dx >= AX0) && (robot.Dx <= BX0)) // zone de jeu
JimmyAREM 0:0d5755f210cb 809 {
JimmyAREM 0:0d5755f210cb 810 if ((robot.Dy >= AY0) && (robot.Dy <= BY0))
JimmyAREM 0:0d5755f210cb 811 {
JimmyAREM 0:0d5755f210cb 812 //Point correct
JimmyAREM 0:0d5755f210cb 813 }
JimmyAREM 0:0d5755f210cb 814 else
JimmyAREM 0:0d5755f210cb 815 {
JimmyAREM 0:0d5755f210cb 816 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 817 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 818 }
JimmyAREM 0:0d5755f210cb 819 }
JimmyAREM 0:0d5755f210cb 820 else
JimmyAREM 0:0d5755f210cb 821 {
JimmyAREM 0:0d5755f210cb 822 printf("Hors de la zone de jeu\n\r");
JimmyAREM 0:0d5755f210cb 823 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 824 }
JimmyAREM 0:0d5755f210cb 825 if ((robot.Dx >= AX1) && (robot.Dx <= BX1)) //zone accélérateur
JimmyAREM 0:0d5755f210cb 826 {
JimmyAREM 0:0d5755f210cb 827 if ((robot.Dy >= AY1) && (robot.Dy <= BY1))
JimmyAREM 0:0d5755f210cb 828 {
JimmyAREM 0:0d5755f210cb 829 printf("Dans l'accelerateur\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 >= AX2) && (robot.Dx <= BX2)) //zone chaos violet
JimmyAREM 0:0d5755f210cb 842 {
JimmyAREM 0:0d5755f210cb 843 if ((robot.Dy >= AY2) && (robot.Dy <= BY2))
JimmyAREM 0:0d5755f210cb 844 {
JimmyAREM 0:0d5755f210cb 845 printf("Dans la zone chaos 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 >= AX3) && (robot.Dx <= BX3)) //zone chaos jaune
JimmyAREM 0:0d5755f210cb 858 {
JimmyAREM 0:0d5755f210cb 859 if ((robot.Dy >= AY3) && (robot.Dy <= BY3))
JimmyAREM 0:0d5755f210cb 860 {
JimmyAREM 0:0d5755f210cb 861 printf("Dans la zone chaos 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 >= AX4) && (robot.Dx <= BX4)) //zone distributeur grand violet
JimmyAREM 0:0d5755f210cb 874 {
JimmyAREM 0:0d5755f210cb 875 if ((robot.Dy >= AY4) && (robot.Dy <= BY4))
JimmyAREM 0:0d5755f210cb 876 {
JimmyAREM 0:0d5755f210cb 877 printf("Dans la zone distributeur violet \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 ((robot.Dx >= AX5) && (robot.Dx <= BX5)) //zone distributeur grand jaune
JimmyAREM 0:0d5755f210cb 890 {
JimmyAREM 0:0d5755f210cb 891 if ((robot.Dy >= AY5) && (robot.Dy <= BY5))
JimmyAREM 0:0d5755f210cb 892 {
JimmyAREM 0:0d5755f210cb 893 printf("Dans la zone distributeur jaune \n\r");
JimmyAREM 0:0d5755f210cb 894 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 895 }
JimmyAREM 0:0d5755f210cb 896 else
JimmyAREM 0:0d5755f210cb 897 {
JimmyAREM 0:0d5755f210cb 898 //Point correct
JimmyAREM 0:0d5755f210cb 899 }
JimmyAREM 0:0d5755f210cb 900 }
JimmyAREM 0:0d5755f210cb 901 else
JimmyAREM 0:0d5755f210cb 902 {
JimmyAREM 0:0d5755f210cb 903 //Point correct
JimmyAREM 0:0d5755f210cb 904 }
JimmyAREM 0:0d5755f210cb 905 if ((robot.Dx >= AX6) && (robot.Dx <= BX6)) //zone limite pente
JimmyAREM 0:0d5755f210cb 906 {
JimmyAREM 0:0d5755f210cb 907 if ((robot.Dy >= AY6) && (robot.Dy <= BY6))
JimmyAREM 0:0d5755f210cb 908 {
JimmyAREM 0:0d5755f210cb 909 printf("Dans la zone pente \n\r");
JimmyAREM 0:0d5755f210cb 910 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 911 }
JimmyAREM 0:0d5755f210cb 912 else
JimmyAREM 0:0d5755f210cb 913 {
JimmyAREM 0:0d5755f210cb 914 //Point correct
JimmyAREM 0:0d5755f210cb 915 }
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 if( couleur == 1) //Jaune
JimmyAREM 0:0d5755f210cb 922 {
JimmyAREM 0:0d5755f210cb 923 if ((robot.Dx >= AX7) && (robot.Dx <= BX7)) //zone depart violet
JimmyAREM 0:0d5755f210cb 924 {
JimmyAREM 0:0d5755f210cb 925 if ((robot.Dy >= AY7) && (robot.Dy <= BY7))
JimmyAREM 0:0d5755f210cb 926 {
JimmyAREM 0:0d5755f210cb 927 printf("Dans la zone depart violet \n\r");
JimmyAREM 0:0d5755f210cb 928 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 929 }
JimmyAREM 0:0d5755f210cb 930 else
JimmyAREM 0:0d5755f210cb 931 {
JimmyAREM 0:0d5755f210cb 932 //Point correct
JimmyAREM 0:0d5755f210cb 933 }
JimmyAREM 0:0d5755f210cb 934 }
JimmyAREM 0:0d5755f210cb 935 else
JimmyAREM 0:0d5755f210cb 936 {
JimmyAREM 0:0d5755f210cb 937 //Point correct
JimmyAREM 0:0d5755f210cb 938 }
JimmyAREM 0:0d5755f210cb 939 }
JimmyAREM 0:0d5755f210cb 940 if( couleur == 0) //Violet
JimmyAREM 0:0d5755f210cb 941 {
JimmyAREM 0:0d5755f210cb 942 if ((robot.Dx >= AX8) && (robot.Dx <= BX8)) //zone depart Jaune
JimmyAREM 0:0d5755f210cb 943 {
JimmyAREM 0:0d5755f210cb 944 if ((robot.Dy>= AY8) && (robot.Dy <= BY8))
JimmyAREM 0:0d5755f210cb 945 {
JimmyAREM 0:0d5755f210cb 946 printf("Dans la zone depart jaune \n\r");
JimmyAREM 0:0d5755f210cb 947 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 948 }
JimmyAREM 0:0d5755f210cb 949 else
JimmyAREM 0:0d5755f210cb 950 {
JimmyAREM 0:0d5755f210cb 951 //Point correct
JimmyAREM 0:0d5755f210cb 952 }
JimmyAREM 0:0d5755f210cb 953 }
JimmyAREM 0:0d5755f210cb 954 else
JimmyAREM 0:0d5755f210cb 955 {
JimmyAREM 0:0d5755f210cb 956 //Point correct
JimmyAREM 0:0d5755f210cb 957 }
JimmyAREM 0:0d5755f210cb 958 }
JimmyAREM 0:0d5755f210cb 959 if ((robot.Dx >= AX9) && (robot.Dx <= BX9)) //zone chelou mileu balance
JimmyAREM 0:0d5755f210cb 960 {
JimmyAREM 0:0d5755f210cb 961 if ((robot.Dy >= AY9) && (robot.Dy <= BY9))
JimmyAREM 0:0d5755f210cb 962 {
JimmyAREM 0:0d5755f210cb 963 printf("Dans le petit obstacle balance \n\r");
JimmyAREM 0:0d5755f210cb 964 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 965 }
JimmyAREM 0:0d5755f210cb 966 else
JimmyAREM 0:0d5755f210cb 967 {
JimmyAREM 0:0d5755f210cb 968 //Point correct
JimmyAREM 0:0d5755f210cb 969 }
JimmyAREM 0:0d5755f210cb 970 }
JimmyAREM 0:0d5755f210cb 971 else
JimmyAREM 0:0d5755f210cb 972 {
JimmyAREM 0:0d5755f210cb 973 //Point correct
JimmyAREM 0:0d5755f210cb 974 }
JimmyAREM 0:0d5755f210cb 975 }
JimmyAREM 0:0d5755f210cb 976 return SANS_OBSTACLE;
JimmyAREM 0:0d5755f210cb 977 }
JimmyAREM 0:0d5755f210cb 978
JimmyAREM 0:0d5755f210cb 979
JimmyAREM 2:9fbf52f12cea 980 bool lancerUnEvitementParArcGauche(int distanceUltrasonGauche, double xFinal, double yFinal)
JimmyAREM 0:0d5755f210cb 981 {
JimmyAREM 0:0d5755f210cb 982 Arc arcCentre;
JimmyAREM 0:0d5755f210cb 983 Point bufferCentre[TAILLE_BUFFER_POINT];
JimmyAREM 2:9fbf52f12cea 984 int indice = 0;
JimmyAREM 0:0d5755f210cb 985 actualiserPosRobot();
JimmyAREM 0:0d5755f210cb 986 calculPosFinalGauche(distanceUltrasonGauche);
JimmyAREM 0:0d5755f210cb 987 calculPosInterParLaDroite();
JimmyAREM 0:0d5755f210cb 988
JimmyAREM 0:0d5755f210cb 989
JimmyAREM 0:0d5755f210cb 990 if(arcPossible() == false)
JimmyAREM 0:0d5755f210cb 991 {
JimmyAREM 0:0d5755f210cb 992 //printf("Les points sont alignes \n\r");
JimmyAREM 0:0d5755f210cb 993 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 994 }
JimmyAREM 0:0d5755f210cb 995 else
JimmyAREM 0:0d5755f210cb 996 {
JimmyAREM 0:0d5755f210cb 997 arcCentre = trouverArc();
JimmyAREM 0:0d5755f210cb 998 recupererTableauPointArc(arcCentre, bufferCentre, PARLAGAUCHE);
JimmyAREM 2:9fbf52f12cea 999 indice = trouverIndicePointFinal(bufferCentre, xFinal, yFinal);
JimmyAREM 2:9fbf52f12cea 1000 modifierPointFinal(bufferCentre, indice);
JimmyAREM 2:9fbf52f12cea 1001 if (estUnArc(bufferCentre, indice) == SANS_OBSTACLE)
JimmyAREM 0:0d5755f210cb 1002 {
JimmyAREM 0:0d5755f210cb 1003 //printf("Arc gauche possible ! \n\r");
JimmyAREM 0:0d5755f210cb 1004 return SANS_OBSTACLE;
JimmyAREM 0:0d5755f210cb 1005 }
JimmyAREM 0:0d5755f210cb 1006 else
JimmyAREM 0:0d5755f210cb 1007 {
JimmyAREM 0:0d5755f210cb 1008 //printf("Arc gauche impossible ! \n\r");
JimmyAREM 0:0d5755f210cb 1009 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 1010 }
JimmyAREM 0:0d5755f210cb 1011 }
JimmyAREM 0:0d5755f210cb 1012 }
JimmyAREM 0:0d5755f210cb 1013
JimmyAREM 2:9fbf52f12cea 1014 bool lancerUnEvitementParArcDroit(int distanceUltrasonDroite, double xFinal, double yFinal)
JimmyAREM 0:0d5755f210cb 1015 {
JimmyAREM 0:0d5755f210cb 1016 Arc arcCentre;
JimmyAREM 0:0d5755f210cb 1017 Point bufferCentre[TAILLE_BUFFER_POINT];
JimmyAREM 2:9fbf52f12cea 1018 int indice = 0;
JimmyAREM 0:0d5755f210cb 1019 actualiserPosRobot();
JimmyAREM 0:0d5755f210cb 1020 calculPosFinalDroite(distanceUltrasonDroite);
JimmyAREM 0:0d5755f210cb 1021 calculPosInterParLaGauche();
JimmyAREM 0:0d5755f210cb 1022
JimmyAREM 0:0d5755f210cb 1023 if(arcPossible() == false)
JimmyAREM 0:0d5755f210cb 1024 {
JimmyAREM 0:0d5755f210cb 1025 //printf("Les points sont alignes \n\r");
JimmyAREM 0:0d5755f210cb 1026 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 1027 }
JimmyAREM 0:0d5755f210cb 1028 else
JimmyAREM 0:0d5755f210cb 1029 {
JimmyAREM 0:0d5755f210cb 1030 arcCentre = trouverArc();
JimmyAREM 0:0d5755f210cb 1031 recupererTableauPointArc(arcCentre, bufferCentre, PARLADROITE);
JimmyAREM 2:9fbf52f12cea 1032 indice = trouverIndicePointFinal(bufferCentre, xFinal, yFinal);
JimmyAREM 2:9fbf52f12cea 1033 modifierPointFinal(bufferCentre, indice);
JimmyAREM 2:9fbf52f12cea 1034 if (estUnArc(bufferCentre, indice) == SANS_OBSTACLE)
JimmyAREM 0:0d5755f210cb 1035 {
JimmyAREM 0:0d5755f210cb 1036 //printf("Arc droit possible ! \n\r");
JimmyAREM 0:0d5755f210cb 1037 return SANS_OBSTACLE;
JimmyAREM 0:0d5755f210cb 1038 }
JimmyAREM 0:0d5755f210cb 1039 else
JimmyAREM 0:0d5755f210cb 1040 {
JimmyAREM 0:0d5755f210cb 1041 //printf("Arc droit impossible ! \n\r");
JimmyAREM 0:0d5755f210cb 1042 return AVEC_OBSTACLE;
JimmyAREM 0:0d5755f210cb 1043 }
JimmyAREM 0:0d5755f210cb 1044 }
JimmyAREM 0:0d5755f210cb 1045 }