Guillaume Chauvon
lmlml
Diff: AnalyseArcLib.cpp
- Revision:
- 0:0d5755f210cb
- Child:
- 2:9fbf52f12cea
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AnalyseArcLib.cpp Mon May 20 23:06:51 2019 +0000
@@ -0,0 +1,1009 @@
+#include "AnalyseArcLib.h"
+
+
+double posStartRobotX;
+double posStartRobotY;
+double capStartRobot;
+
+double posFinalRobotX;
+double posFinalRobotY;
+
+double posInterRobotX;
+double posInterRobotY;
+
+
+
+bool evitement = AVEC_OBSTACLE;
+
+extern bool couleur = 0; //0 Violet 1 Jaune
+
+void actualiserPosRobot()
+{
+ actualise_position();
+ posStartRobotX = get_x_actuel()/100;
+ posStartRobotY = get_y_actuel()/100;
+ capStartRobot = get_angle();
+}
+
+void calculPosFinalGauche(int distanceUltrasonGauche)
+{
+ posFinalRobotX = posStartRobotX + (LONGUEUR_ROBOT + distanceUltrasonGauche + LONGUEUR_ROBOT_ADVERSE + MARGE) * cos((capStartRobot* 3.1415)/180.0);
+ posFinalRobotY = posStartRobotY + (LONGUEUR_ROBOT + distanceUltrasonGauche + LONGUEUR_ROBOT_ADVERSE + MARGE) * sin((capStartRobot* 3.1415)/180.0);
+ //printf("Position finale : (%lf, %lf) \n\r", posFinalRobotX, posFinalRobotY);
+}
+
+void calculPosFinalDroite(int distanceUltrasonDroite)
+{
+ posFinalRobotX = posStartRobotX + (LONGUEUR_ROBOT + distanceUltrasonDroite + LONGUEUR_ROBOT_ADVERSE + MARGE) * cos((capStartRobot* 3.1415)/180.0);
+ posFinalRobotY = posStartRobotY + (LONGUEUR_ROBOT + distanceUltrasonDroite + LONGUEUR_ROBOT_ADVERSE + MARGE ) * sin((capStartRobot* 3.1415)/180.0);
+ //printf("Position finale : (%lf, %lf) \n\r", posFinalRobotX, posFinalRobotY);
+}
+
+void calculPosInterParLaGauche()
+{
+ posInterRobotX = (posStartRobotX + posFinalRobotX)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * cos((capStartRobot* 3.1415)/180.0 + 1.57075);
+ posInterRobotY = (posStartRobotY + posFinalRobotY)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * sin((capStartRobot* 3.1415)/180.0 + 1.57075);
+ //printf("Position intermediaire gauche: (%lf, %lf) \n\r", posInterRobotX, posInterRobotY);
+}
+
+void calculPosInterParLaDroite()
+{
+ posInterRobotX = (posStartRobotX + posFinalRobotX)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * cos((capStartRobot* 3.1415)/180.0 - 1.57075);
+ posInterRobotY = (posStartRobotY + posFinalRobotY)/2 + (LONGUEUR_ROBOT_ADVERSE + DEMI_LARGEUR_ROBOT + MARGE) * sin((capStartRobot* 3.1415)/180.0 - 1.57075);
+ //printf("Position intermediaire droite: (%lf, %lf) \n\r", posInterRobotX, posInterRobotY);
+}
+
+Robot setPosRobot(Point point, double capActuel)
+{
+ struct Robot robot;
+
+ robot.Ax = point.X + DEMI_LONGUEUR_ROBOT_AVANT * cos((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
+ robot.Ay = point.Y + DEMI_LONGUEUR_ROBOT_AVANT * sin((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
+
+ robot.Bx = point.X + DEMI_LONGUEUR_ROBOT_AVANT * cos((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
+ robot.By = point.Y + DEMI_LONGUEUR_ROBOT_AVANT * sin((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
+
+ robot.Cx = point.X - DEMI_LONGUEUR_ROBOT_ARRIERE * cos((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
+ robot.Cy = point.Y - DEMI_LONGUEUR_ROBOT_ARRIERE * sin((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
+
+ robot.Dx = point.X - DEMI_LONGUEUR_ROBOT_ARRIERE * cos((capActuel* 3.1415)/180.0) - DEMI_LARGEUR_ROBOT * sin((capActuel* 3.1415)/180.0);
+ robot.Dy = point.Y - DEMI_LONGUEUR_ROBOT_ARRIERE * sin((capActuel* 3.1415)/180.0) + DEMI_LARGEUR_ROBOT * cos((capActuel* 3.1415)/180.0);
+
+ return robot;
+}
+
+double calculercapActuel(Point P1, Point P2)
+{
+ double capActuel;
+ double cste = 0;
+ cste = pow((pow((P1.X - P2.X),2) + pow((P1.Y - P2.Y),2)),0.5);
+ capActuel = acos(-(P2.Y - P1.Y) / cste);
+ return capActuel;
+}
+
+bool arcPossible()
+{
+ double x1 = posStartRobotX;
+ double x2 = posInterRobotX;
+ double x3 = posFinalRobotX;
+
+ double y1 = posStartRobotY;
+ double y2 = posInterRobotY;
+ double y3 = posFinalRobotY;
+
+ double a = x1 - x2;
+ double b = y1 - y2;
+ double c = x1 - x3;
+ double d = y1 - y3;
+
+ if ((b*c) - (a*d) == 0)
+ {
+ return false;
+ }
+ else
+ {
+ return true;
+ }
+}
+
+Arc trouverArc()
+{
+ Arc arcTrajectoire;
+ double cX;
+ double cY;
+ double R;
+
+ double x1 = posStartRobotX;
+ double x2 = posInterRobotX;
+ double x3 = posFinalRobotX;
+
+ double y1 = posStartRobotY;
+ double y2 = posInterRobotY;
+ double y3 = posFinalRobotY;
+
+ double a = x1 - x2;
+ double b = y1 - y2;
+ double c = x1 - x3;
+ double d = y1 - y3;
+ double e = ( pow(x1,2) - pow(x2,2) - ((pow(y2,2) - pow(y1,2))) ) / 2;
+ double f = ( pow(x1,2) - pow(x3,2) - ((pow(y3,2) - pow(y1,2))) ) / 2;
+
+
+ arcTrajectoire.intervaleXd = x1;
+ arcTrajectoire.intervaleXf = x3;
+ arcTrajectoire.intervaleYd = y1;
+ arcTrajectoire.intervaleYf = y3;
+
+ cX = -(d*e - b*f)/(b*c - a*d);
+ cY = -(a*f - c*e)/(b *c - a*d);
+ R = pow(pow((x1 - cX),2) + pow((y1 - cY),2), 0.5);
+
+ arcTrajectoire.CentreX = cX;
+ arcTrajectoire.CentreY = cY;
+ arcTrajectoire.Rayon = R;
+
+ //printf("le cercle de centre(%lf,%lf) et de Rayon=%lf \n\r", arcTrajectoire.CentreX, arcTrajectoire.CentreY, arcTrajectoire.Rayon);
+ return arcTrajectoire;
+}
+
+Arc genererArcInterieur(Arc arc)
+{
+ Arc arcInterieur;
+ arcInterieur.CentreX = arc.CentreX;
+ arcInterieur.CentreY = arc.CentreY;
+ arcInterieur.Rayon = arc.Rayon - DEMI_LARGEUR_ROBOT;
+
+ arcInterieur.intervaleXd = arc.intervaleXd + DEMI_LARGEUR_ROBOT * cos((capStartRobot* 3.1415/180.0) + 1.5707);
+ arcInterieur.intervaleXf = arc.intervaleXf + DEMI_LARGEUR_ROBOT * cos((capStartRobot* 3.1415/180.0) + 1.5707);
+ arcInterieur.intervaleYd = arc.intervaleYd + DEMI_LARGEUR_ROBOT * sin((capStartRobot* 3.1415/180.0) + 1.5707);
+ arcInterieur.intervaleYf = arc.intervaleYf;
+ return arcInterieur;
+}
+
+
+Arc genererArcExterieur(Arc arc)
+{
+ Arc arcExterieur;
+ arcExterieur.CentreX = arc.CentreX;
+ arcExterieur.CentreY = arc.CentreY;
+ arcExterieur.Rayon = arc.Rayon + DEMI_LARGEUR_ROBOT;
+ return arcExterieur;
+}
+double trouverAngle(Arc arc)
+{
+ double angle;
+ double a = arc.Rayon;
+ double b = arc.Rayon;
+ double c = pow((pow((arc.intervaleXd - arc.intervaleXf),2) + pow((arc.intervaleYd - arc.intervaleYf),2)),0.5) ;
+ //printf("Distance entre le point de depart et le point final : %lf\n\r", c);
+ angle = acos((a*a + b*b - c*c)/(2*a*b)) ;
+ //printf("Angle en degree trouve : %lf \n\r", angle *180/3.1415);
+ return angle;
+}
+
+double trouverAngleInit(Arc arc)
+{
+ double angle;
+ double a = arc.Rayon;
+ double b = arc.Rayon;
+ double c = pow((pow((arc.CentreX + arc.Rayon - arc.intervaleXf),2) + pow((arc.CentreY - arc.intervaleYf),2)),0.5) ;
+ angle = acos((a*a + b*b - c*c)/(2*a*b)) ;
+ if(arc.CentreY >= arc.intervaleYf)
+ {
+ angle = -angle;
+ }
+ //printf("Angle initial en degree trouve : %lf \n\r", angle *180/3.1415);
+ return angle;
+}
+
+void recupererTableauPointArc(Arc arc, Point buffer[], bool type) //type = 0 => gauche / type = 1 => droit
+{
+ double rPolaire = arc.Rayon;
+ double tethaTotal;
+ if (type == PARLAGAUCHE)
+ {
+ tethaTotal = trouverAngle(arc);
+ //printf("Angle total a parcourir : %lf \n\r", tethaTotal*180/3.1415);
+ }
+ else
+ {
+ tethaTotal = trouverAngle(arc);
+ //printf("Angle total a parcourir : %lf \n\r", tethaTotal*180/3.1415);
+ }
+ double tethaInit = trouverAngleInit(arc);
+ //double longueurArc = rPolaire * tethaTotal;
+ //printf("Angle : %lf et longueur : %lf\n\r", tethaTotal*180/3.1415, longueurArc);
+ if (type == PARLAGAUCHE)
+ {
+ for(int i = 0; i<TAILLE_BUFFER_POINT; i++)
+ {
+ buffer[i].X = arc.CentreX + rPolaire * cos(tethaInit + i * tethaTotal/NB_POINT);
+ buffer[i].Y = arc.CentreY + rPolaire * sin(tethaInit + i * tethaTotal/NB_POINT);
+ //printf("Point %d : (%lf,%lf) \n\r", i, buffer[i].X, buffer[i].Y);
+ }
+ }
+ else
+ {
+ for(int i = 0; i<TAILLE_BUFFER_POINT; i++)
+ {
+ buffer[i].X = arc.CentreX + rPolaire * cos(tethaInit - i * tethaTotal/NB_POINT);
+ buffer[i].Y = arc.CentreY + rPolaire * sin(tethaInit - i * tethaTotal/NB_POINT);
+ //printf("Point %d : (%lf,%lf) \n\r", i, buffer[i].X, buffer[i].Y);
+ }
+ }
+ return;
+}
+
+void afficherRobot(Robot robot)
+{
+ 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 );
+}
+
+
+void afficherArc(Arc arc)
+{
+ 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);
+}
+
+void afficher(Point buffer[])
+{
+ for(int i=0; i<TAILLE_BUFFER_POINT; i++)
+ {
+ printf("Coordonnee du point P indice %d : (%lf,%lf) \n\r", i, buffer[i].X, buffer[i].Y);
+ }
+}
+
+
+
+bool estUnArc(Point tableau[])
+{
+ Robot robot;
+
+ for(int i=0; i<TAILLE_BUFFER_POINT; i++)
+ { if(i != TAILLE_BUFFER_POINT -1)
+ {
+ robot = setPosRobot(tableau[i], calculercapActuel(tableau[i], tableau[i+1]));
+ }
+ else
+ {
+ robot = setPosRobot(tableau[i], calculercapActuel(tableau[i-1], tableau[i]));
+ }
+ //printf(" Point A, a l'indice : %d\n\r", i);
+ if ((robot.Ax >= AX0) && (robot.Ax <= BX0)) // zone de jeu
+ {
+ if ((robot.Ay >= AY0) && (robot.Ay <= BY0))
+ {
+ //Point correct
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ if ((robot.Ax >= AX1) && (robot.Ax <= BX1)) //zone accélérateur
+ {
+ if ((robot.Ay >= AY1) && (robot.Ay <= BY1))
+ {
+ printf("Dans l'accelerateur\n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //point correct
+ }
+ if ((robot.Ax >= AX2) && (robot.Ax <= BX2)) //zone chaos violet
+ {
+ if ((robot.Ay >= AY2) && (robot.Ay <= BY2))
+ {
+ printf("Dans la zone chaos violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Ax >= AX3) && (robot.Ax <= BX3)) //zone chaos jaune
+ {
+ if ((robot.Ay >= AY3) && (robot.Ay <= BY3))
+ {
+ printf("Dans la zone chaos jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Ax >= AX4) && (robot.Ax <= BX4)) //zone distributeur grand violet
+ {
+ if ((robot.Ay >= AY4) && (robot.Ay <= BY4))
+ {
+ printf("Dans la zone distributeur violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Ax >= AX5) && (robot.Ax <= BX5)) //zone distributeur grand jaune
+ {
+ if ((robot.Ay >= AY5) && (robot.Ay <= BY5))
+ {
+ printf("Dans la zone distributeur jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Ax >= AX6) && (robot.Ax <= BX6)) //zone limite pente
+ {
+ if ((robot.Ay >= AY6) && (robot.Ay <= BY6))
+ {
+ printf("Dans la zone pente \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if( couleur == 1) //Jaune
+ {
+ if ((robot.Ax >= AX7) && (robot.Ax <= BX7)) //zone depart violet
+ {
+ if ((robot.Ay >= AY7) && (robot.Ay <= BY7))
+ {
+ printf("Dans la zone depart violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if( couleur == 0) //Violet
+ {
+ if ((robot.Ax >= AX8) && (robot.Ax <= BX8)) //zone depart Jaune
+ {
+ if ((robot.Ay>= AY8) && (robot.Ay <= BY8))
+ {
+ printf("Dans la zone depart jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if ((robot.Ax >= AX9) && (robot.Ax <= BX9)) //zone chelou mileu balance
+ {
+ if ((robot.Ay >= AY9) && (robot.Ay <= BY9))
+ {
+ printf("Dans le petit obstacle balance \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ //printf(" Point B, a l'indice : %d\n\r", i);
+ if ((robot.Bx >= AX0) && (robot.Bx <= BX0)) // zone de jeu
+ {
+ if ((robot.By >= AY0) && (robot.By <= BY0))
+ {
+ //Point correct
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ if ((robot.Bx >= AX1) && (robot.Bx <= BX1)) //zone accélérateur
+ {
+ if ((robot.By >= AY1) && (robot.By <= BY1))
+ {
+ printf("Dans l'accelerateur\n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //point correct
+ }
+ if ((robot.Bx >= AX2) && (robot.Bx <= BX2)) //zone chaos violet
+ {
+ if ((robot.By >= AY2) && (robot.By <= BY2))
+ {
+ printf("Dans la zone chaos violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Bx >= AX3) && (robot.Bx <= BX3)) //zone chaos jaune
+ {
+ if ((robot.By >= AY3) && (robot.By <= BY3))
+ {
+ printf("Dans la zone chaos jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Bx >= AX4) && (robot.Bx <= BX4)) //zone distributeur grand violet
+ {
+ if ((robot.By >= AY4) && (robot.By <= BY4))
+ {
+ printf("Dans la zone distributeur violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Bx >= AX5) && (robot.Bx <= BX5)) //zone distributeur grand jaune
+ {
+ if ((robot.By >= AY5) && (robot.By <= BY5))
+ {
+ printf("Dans la zone distributeur jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Bx >= AX6) && (robot.Bx <= BX6)) //zone limite pente
+ {
+ if ((robot.By >= AY6) && (robot.By <= BY6))
+ {
+ printf("Dans la zone pente \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if( couleur == 1) //Jaune
+ {
+ if ((robot.Bx >= AX7) && (robot.Bx <= BX7)) //zone depart violet
+ {
+ if ((robot.By >= AY7) && (robot.By <= BY7))
+ {
+ printf("Dans la zone depart violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if( couleur == 0) //Violet
+ {
+ if ((robot.Bx >= AX8) && (robot.Bx <= BX8)) //zone depart Jaune
+ {
+ if ((robot.By>= AY8) && (robot.By <= BY8))
+ {
+ printf("Dans la zone depart jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if ((robot.Bx >= AX9) && (robot.Bx <= BX9)) //zone chelou mileu balance
+ {
+ if ((robot.By >= AY9) && (robot.By <= BY9))
+ {
+ printf("Dans le petit obstacle balance \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ //printf(" Point C, a l'indice : %d\n\r", i);
+ if ((robot.Cx >= AX0) && (robot.Cx <= BX0)) // zone de jeu
+ {
+ if ((robot.Cy >= AY0) && (robot.Cy <= BY0))
+ {
+ //Point correct
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ if ((robot.Cx >= AX1) && (robot.Cx <= BX1)) //zone accélérateur
+ {
+ if ((robot.Cy >= AY1) && (robot.Cy <= BY1))
+ {
+ printf("Dans l'accelerateur\n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //point correct
+ }
+ if ((robot.Cx >= AX2) && (robot.Cx <= BX2)) //zone chaos violet
+ {
+ if ((robot.Cy >= AY2) && (robot.Cy <= BY2))
+ {
+ printf("Dans la zone chaos violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Cx >= AX3) && (robot.Cx <= BX3)) //zone chaos jaune
+ {
+ if ((robot.Cy >= AY3) && (robot.Cy <= BY3))
+ {
+ printf("Dans la zone chaos jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Cx >= AX4) && (robot.Cx <= BX4)) //zone distributeur grand violet
+ {
+ if ((robot.Cy >= AY4) && (robot.Cy <= BY4))
+ {
+ printf("Dans la zone distributeur violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Cx >= AX5) && (robot.Cx <= BX5)) //zone distributeur grand jaune
+ {
+ if ((robot.Cy >= AY5) && (robot.Cy <= BY5))
+ {
+ printf("Dans la zone distributeur jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Cx >= AX6) && (robot.Cx <= BX6)) //zone limite pente
+ {
+ if ((robot.Cy >= AY6) && (robot.Cy <= BY6))
+ {
+ printf("Dans la zone pente \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if( couleur == 1) //Jaune
+ {
+ if ((robot.Cx >= AX7) && (robot.Cx <= BX7)) //zone depart violet
+ {
+ if ((robot.Cy >= AY7) && (robot.Cy <= BY7))
+ {
+ printf("Dans la zone depart violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if( couleur == 0) //Violet
+ {
+ if ((robot.Cx >= AX8) && (robot.Cx <= BX8)) //zone depart Jaune
+ {
+ if ((robot.Cy>= AY8) && (robot.Cy <= BY8))
+ {
+ printf("Dans la zone depart jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if ((robot.Cx >= AX9) && (robot.Cx <= BX9)) //zone chelou mileu balance
+ {
+ if ((robot.Cy >= AY9) && (robot.Cy <= BY9))
+ {
+ printf("Dans le petit obstacle balance \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ //printf(" Point D, a l'indice : %d\n\r", i);
+ if ((robot.Dx >= AX0) && (robot.Dx <= BX0)) // zone de jeu
+ {
+ if ((robot.Dy >= AY0) && (robot.Dy <= BY0))
+ {
+ //Point correct
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ }
+ else
+ {
+ printf("Hors de la zone de jeu\n\r");
+ return AVEC_OBSTACLE;
+ }
+ if ((robot.Dx >= AX1) && (robot.Dx <= BX1)) //zone accélérateur
+ {
+ if ((robot.Dy >= AY1) && (robot.Dy <= BY1))
+ {
+ printf("Dans l'accelerateur\n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //point correct
+ }
+ if ((robot.Dx >= AX2) && (robot.Dx <= BX2)) //zone chaos violet
+ {
+ if ((robot.Dy >= AY2) && (robot.Dy <= BY2))
+ {
+ printf("Dans la zone chaos violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Dx >= AX3) && (robot.Dx <= BX3)) //zone chaos jaune
+ {
+ if ((robot.Dy >= AY3) && (robot.Dy <= BY3))
+ {
+ printf("Dans la zone chaos jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Dx >= AX4) && (robot.Dx <= BX4)) //zone distributeur grand violet
+ {
+ if ((robot.Dy >= AY4) && (robot.Dy <= BY4))
+ {
+ printf("Dans la zone distributeur violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Dx >= AX5) && (robot.Dx <= BX5)) //zone distributeur grand jaune
+ {
+ if ((robot.Dy >= AY5) && (robot.Dy <= BY5))
+ {
+ printf("Dans la zone distributeur jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if ((robot.Dx >= AX6) && (robot.Dx <= BX6)) //zone limite pente
+ {
+ if ((robot.Dy >= AY6) && (robot.Dy <= BY6))
+ {
+ printf("Dans la zone pente \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ if( couleur == 1) //Jaune
+ {
+ if ((robot.Dx >= AX7) && (robot.Dx <= BX7)) //zone depart violet
+ {
+ if ((robot.Dy >= AY7) && (robot.Dy <= BY7))
+ {
+ printf("Dans la zone depart violet \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if( couleur == 0) //Violet
+ {
+ if ((robot.Dx >= AX8) && (robot.Dx <= BX8)) //zone depart Jaune
+ {
+ if ((robot.Dy>= AY8) && (robot.Dy <= BY8))
+ {
+ printf("Dans la zone depart jaune \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ if ((robot.Dx >= AX9) && (robot.Dx <= BX9)) //zone chelou mileu balance
+ {
+ if ((robot.Dy >= AY9) && (robot.Dy <= BY9))
+ {
+ printf("Dans le petit obstacle balance \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ else
+ {
+ //Point correct
+ }
+ }
+ return SANS_OBSTACLE;
+}
+
+
+bool lancerUnEvitementParArcGauche(int distanceUltrasonGauche)
+{
+ Arc arcCentre;
+ Point bufferCentre[TAILLE_BUFFER_POINT];
+
+ actualiserPosRobot();
+ calculPosFinalGauche(distanceUltrasonGauche);
+ calculPosInterParLaDroite();
+
+
+ if(arcPossible() == false)
+ {
+ //printf("Les points sont alignes \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ arcCentre = trouverArc();
+ recupererTableauPointArc(arcCentre, bufferCentre, PARLAGAUCHE);
+ if (estUnArc(bufferCentre) == SANS_OBSTACLE)
+ {
+ //printf("Arc gauche possible ! \n\r");
+ return SANS_OBSTACLE;
+ }
+ else
+ {
+ //printf("Arc gauche impossible ! \n\r");
+ return AVEC_OBSTACLE;
+ }
+ }
+}
+
+bool lancerUnEvitementParArcDroit(int distanceUltrasonDroite)
+{
+ Arc arcCentre;
+ Point bufferCentre[TAILLE_BUFFER_POINT];
+
+ actualiserPosRobot();
+ calculPosFinalDroite(distanceUltrasonDroite);
+ calculPosInterParLaGauche();
+
+ if(arcPossible() == false)
+ {
+ //printf("Les points sont alignes \n\r");
+ return AVEC_OBSTACLE;
+ }
+ else
+ {
+ arcCentre = trouverArc();
+ recupererTableauPointArc(arcCentre, bufferCentre, PARLADROITE);
+ if (estUnArc(bufferCentre) == SANS_OBSTACLE)
+ {
+ //printf("Arc droit possible ! \n\r");
+ return SANS_OBSTACLE;
+ }
+ else
+ {
+ //printf("Arc droit impossible ! \n\r");
+ return AVEC_OBSTACLE;
+ }
+ }
+}
\ No newline at end of file