lmlml
AnalyseArcLib.h@3:0c279e84f694, 2019-05-29 (annotated)
- Committer:
- GuillaumeCH
- Date:
- Wed May 29 06:34:07 2019 +0000
- Revision:
- 3:0c279e84f694
- Parent:
- 2:9fbf52f12cea
mllmlmlml
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JimmyAREM | 0:0d5755f210cb | 1 | #ifndef ANALYSEARCLIB_H |
JimmyAREM | 0:0d5755f210cb | 2 | #define ANALYSEARCLIB_H |
JimmyAREM | 0:0d5755f210cb | 3 | /** |
JimmyAREM | 0:0d5755f210cb | 4 | **************************************************************************** |
JimmyAREM | 0:0d5755f210cb | 5 | * @file AnalyseArcLib.h |
JimmyAREM | 0:0d5755f210cb | 6 | * @author Jimmy MAINGAM |
JimmyAREM | 0:0d5755f210cb | 7 | * @version V0.0.1 |
JimmyAREM | 0:0d5755f210cb | 8 | * @date 15/05/2019 |
JimmyAREM | 0:0d5755f210cb | 9 | * @brief Implementation file to analyse the arc trajectory to avoid a robot |
JimmyAREM | 0:0d5755f210cb | 10 | **************************************************************************** |
JimmyAREM | 0:0d5755f210cb | 11 | *This software has been developed to be used in AREM's robot for the cdfr 2019 |
JimmyAREM | 0:0d5755f210cb | 12 | **/ |
JimmyAREM | 0:0d5755f210cb | 13 | |
JimmyAREM | 0:0d5755f210cb | 14 | /* Includes ------------------------------------------------------------------*/ |
JimmyAREM | 0:0d5755f210cb | 15 | |
JimmyAREM | 0:0d5755f210cb | 16 | #include "mbed.h" |
JimmyAREM | 0:0d5755f210cb | 17 | #include "odometrie.h" |
JimmyAREM | 0:0d5755f210cb | 18 | |
JimmyAREM | 0:0d5755f210cb | 19 | /* Constants -------------------------------------------------------------------*/ |
JimmyAREM | 0:0d5755f210cb | 20 | |
JimmyAREM | 0:0d5755f210cb | 21 | #define LONGUEUR_ROBOT_ADVERSE 300 |
JimmyAREM | 0:0d5755f210cb | 22 | #//define LARGEUR_ROBOT 230 |
JimmyAREM | 0:0d5755f210cb | 23 | #define LONGUEUR_ROBOT 150 |
JimmyAREM | 0:0d5755f210cb | 24 | #define DEMI_LARGEUR_ROBOT 115 |
JimmyAREM | 0:0d5755f210cb | 25 | #define DEMI_LONGUEUR_ROBOT_AVANT 120 |
JimmyAREM | 0:0d5755f210cb | 26 | #define DEMI_LONGUEUR_ROBOT_ARRIERE 30 |
GuillaumeCH | 3:0c279e84f694 | 27 | #define MARGE 350 |
JimmyAREM | 0:0d5755f210cb | 28 | |
JimmyAREM | 0:0d5755f210cb | 29 | #define NB_POINT 49 |
JimmyAREM | 0:0d5755f210cb | 30 | #define TAILLE_BUFFER_POINT 50 |
JimmyAREM | 0:0d5755f210cb | 31 | |
JimmyAREM | 0:0d5755f210cb | 32 | #define PARLAGAUCHE 0 |
JimmyAREM | 0:0d5755f210cb | 33 | #define PARLADROITE 1 |
JimmyAREM | 0:0d5755f210cb | 34 | |
JimmyAREM | 0:0d5755f210cb | 35 | //Accélérateur zone |
JimmyAREM | 0:0d5755f210cb | 36 | #define AX1 500 |
JimmyAREM | 0:0d5755f210cb | 37 | #define AY1 0 |
JimmyAREM | 0:0d5755f210cb | 38 | #define BX1 2500 |
JimmyAREM | 0:0d5755f210cb | 39 | #define BY1 50 |
JimmyAREM | 0:0d5755f210cb | 40 | |
JimmyAREM | 0:0d5755f210cb | 41 | //Chaos violet zone |
JimmyAREM | 0:0d5755f210cb | 42 | #define AX2 850 |
JimmyAREM | 0:0d5755f210cb | 43 | #define AY2 1050 |
JimmyAREM | 0:0d5755f210cb | 44 | #define BX2 1150 |
JimmyAREM | 0:0d5755f210cb | 45 | #define BY2 1350 |
JimmyAREM | 0:0d5755f210cb | 46 | |
JimmyAREM | 0:0d5755f210cb | 47 | //Chaos jaune zone |
JimmyAREM | 0:0d5755f210cb | 48 | #define AX3 1850 |
JimmyAREM | 0:0d5755f210cb | 49 | #define AY3 1050 |
JimmyAREM | 0:0d5755f210cb | 50 | #define BX3 2150 |
JimmyAREM | 0:0d5755f210cb | 51 | #define BY3 1350 |
JimmyAREM | 0:0d5755f210cb | 52 | |
JimmyAREM | 0:0d5755f210cb | 53 | //distributeur violet zone |
JimmyAREM | 0:0d5755f210cb | 54 | #define AX4 450 |
JimmyAREM | 0:0d5755f210cb | 55 | #define AY4 1543 |
JimmyAREM | 0:0d5755f210cb | 56 | #define BX4 1050 |
JimmyAREM | 0:0d5755f210cb | 57 | #define BY4 1600 |
JimmyAREM | 0:0d5755f210cb | 58 | |
JimmyAREM | 0:0d5755f210cb | 59 | //distributeur jaune zone |
JimmyAREM | 0:0d5755f210cb | 60 | #define AX5 1950 |
JimmyAREM | 0:0d5755f210cb | 61 | #define AY5 1543 |
JimmyAREM | 0:0d5755f210cb | 62 | #define BX5 2550 |
JimmyAREM | 0:0d5755f210cb | 63 | #define BY5 1600 |
JimmyAREM | 0:0d5755f210cb | 64 | |
JimmyAREM | 0:0d5755f210cb | 65 | //limite zone |
JimmyAREM | 0:0d5755f210cb | 66 | #define AX6 0 |
JimmyAREM | 0:0d5755f210cb | 67 | #define AY6 1600 |
JimmyAREM | 0:0d5755f210cb | 68 | #define BX6 3000 |
JimmyAREM | 0:0d5755f210cb | 69 | #define BY6 2000 |
JimmyAREM | 0:0d5755f210cb | 70 | |
JimmyAREM | 0:0d5755f210cb | 71 | //depart zone violet |
JimmyAREM | 0:0d5755f210cb | 72 | #define AX7 0 |
JimmyAREM | 0:0d5755f210cb | 73 | #define AY7 300 |
JimmyAREM | 0:0d5755f210cb | 74 | #define BX7 450 |
JimmyAREM | 0:0d5755f210cb | 75 | #define BY7 1200 |
JimmyAREM | 0:0d5755f210cb | 76 | |
JimmyAREM | 0:0d5755f210cb | 77 | //depart zone jaune |
JimmyAREM | 1:ca7bc5d9ff31 | 78 | #define AX8 2550 |
JimmyAREM | 0:0d5755f210cb | 79 | #define AY8 300 |
JimmyAREM | 1:ca7bc5d9ff31 | 80 | #define BX8 3000 |
JimmyAREM | 0:0d5755f210cb | 81 | #define BY8 1200 |
JimmyAREM | 0:0d5755f210cb | 82 | |
JimmyAREM | 0:0d5755f210cb | 83 | //truc relou milieu |
JimmyAREM | 0:0d5755f210cb | 84 | #define AX9 1460 |
JimmyAREM | 0:0d5755f210cb | 85 | #define AY9 1400 |
JimmyAREM | 0:0d5755f210cb | 86 | #define BX9 1540 |
JimmyAREM | 0:0d5755f210cb | 87 | #define BY9 1600 |
JimmyAREM | 0:0d5755f210cb | 88 | |
JimmyAREM | 0:0d5755f210cb | 89 | //hors terrain |
JimmyAREM | 0:0d5755f210cb | 90 | #define AX0 0 |
JimmyAREM | 0:0d5755f210cb | 91 | #define AY0 0 |
JimmyAREM | 0:0d5755f210cb | 92 | #define BX0 3000 |
JimmyAREM | 0:0d5755f210cb | 93 | #define BY0 2000 |
JimmyAREM | 0:0d5755f210cb | 94 | |
JimmyAREM | 0:0d5755f210cb | 95 | |
JimmyAREM | 0:0d5755f210cb | 96 | #define SANS_OBSTACLE 1 |
JimmyAREM | 0:0d5755f210cb | 97 | #define AVEC_OBSTACLE 0 |
JimmyAREM | 0:0d5755f210cb | 98 | |
JimmyAREM | 0:0d5755f210cb | 99 | |
JimmyAREM | 0:0d5755f210cb | 100 | /* Global Variables ---------------------------------------------------------*/ |
JimmyAREM | 0:0d5755f210cb | 101 | |
JimmyAREM | 0:0d5755f210cb | 102 | |
JimmyAREM | 0:0d5755f210cb | 103 | struct Robot |
JimmyAREM | 0:0d5755f210cb | 104 | { |
JimmyAREM | 0:0d5755f210cb | 105 | double Ax; |
JimmyAREM | 0:0d5755f210cb | 106 | double Ay; |
JimmyAREM | 0:0d5755f210cb | 107 | double Bx; |
JimmyAREM | 0:0d5755f210cb | 108 | double By; |
JimmyAREM | 0:0d5755f210cb | 109 | double Cx; |
JimmyAREM | 0:0d5755f210cb | 110 | double Cy; |
JimmyAREM | 0:0d5755f210cb | 111 | double Dx; |
JimmyAREM | 0:0d5755f210cb | 112 | double Dy; |
JimmyAREM | 0:0d5755f210cb | 113 | }; |
JimmyAREM | 0:0d5755f210cb | 114 | |
JimmyAREM | 0:0d5755f210cb | 115 | struct Point |
JimmyAREM | 0:0d5755f210cb | 116 | { |
JimmyAREM | 0:0d5755f210cb | 117 | double X; |
JimmyAREM | 0:0d5755f210cb | 118 | double Y; |
JimmyAREM | 0:0d5755f210cb | 119 | }; |
JimmyAREM | 0:0d5755f210cb | 120 | |
JimmyAREM | 0:0d5755f210cb | 121 | |
JimmyAREM | 0:0d5755f210cb | 122 | struct Arc |
JimmyAREM | 0:0d5755f210cb | 123 | { |
JimmyAREM | 0:0d5755f210cb | 124 | double CentreX; |
JimmyAREM | 0:0d5755f210cb | 125 | double CentreY; |
JimmyAREM | 0:0d5755f210cb | 126 | double Rayon; |
JimmyAREM | 0:0d5755f210cb | 127 | |
JimmyAREM | 0:0d5755f210cb | 128 | double intervaleXd; |
JimmyAREM | 0:0d5755f210cb | 129 | double intervaleXf; |
JimmyAREM | 0:0d5755f210cb | 130 | double intervaleYd; |
JimmyAREM | 0:0d5755f210cb | 131 | double intervaleYf; |
JimmyAREM | 0:0d5755f210cb | 132 | }; |
JimmyAREM | 0:0d5755f210cb | 133 | |
JimmyAREM | 0:0d5755f210cb | 134 | /* Public Functions ------------------------------------------------------------------*/ |
JimmyAREM | 0:0d5755f210cb | 135 | |
JimmyAREM | 0:0d5755f210cb | 136 | void actualiserPosRobot(); //a implémenter après intégration avec odometrie.h |
JimmyAREM | 0:0d5755f210cb | 137 | |
JimmyAREM | 0:0d5755f210cb | 138 | void calculPosFinalGauche(int distanceUltrasonGauche); //calcul de la position finale après l'évitement par la gauche |
JimmyAREM | 0:0d5755f210cb | 139 | void calculPosFinalDroite(int distanceUltrasonDroite); //calcul de la position finale après l'évitement par la droite |
JimmyAREM | 0:0d5755f210cb | 140 | |
JimmyAREM | 0:0d5755f210cb | 141 | void calculPosInterParLaGauche(); //calcul d'un point intermédiaire par la gauche pour le futur arc |
JimmyAREM | 0:0d5755f210cb | 142 | void calculPosInterParLaDroite(); //calcul d'un point intermédiaire par la droite pour le futur arc |
JimmyAREM | 0:0d5755f210cb | 143 | |
JimmyAREM | 0:0d5755f210cb | 144 | Robot setPosRobot(Point point, double capActuel); // calcul les coordonnées du robot dans l'expace sachant son point de départ |
JimmyAREM | 0:0d5755f210cb | 145 | double calculercapActuel(Point P1, Point P2); |
JimmyAREM | 0:0d5755f210cb | 146 | |
JimmyAREM | 0:0d5755f210cb | 147 | void afficherRobot(Robot robot); //fct d'affichage |
JimmyAREM | 0:0d5755f210cb | 148 | void afficherArc(Arc arc); //fct d'affichage |
JimmyAREM | 0:0d5755f210cb | 149 | void afficher(Point buffer[]); //fct d'affichage |
JimmyAREM | 0:0d5755f210cb | 150 | |
JimmyAREM | 0:0d5755f210cb | 151 | bool arcPossible(); // vérification qu'un arc existe |
JimmyAREM | 0:0d5755f210cb | 152 | Arc trouverArc(); //renvoie un arc (cercle) passant par les trois points |
JimmyAREM | 0:0d5755f210cb | 153 | |
JimmyAREM | 0:0d5755f210cb | 154 | double trouverAngle(Arc arc); // renvoie l'angle totale de l'arc |
JimmyAREM | 0:0d5755f210cb | 155 | double trouverAngleInit(Arc arc); //renvoie l'angle initial (attention ce n'est pas le cap) |
JimmyAREM | 0:0d5755f210cb | 156 | |
JimmyAREM | 0:0d5755f210cb | 157 | Arc genererArcInterieur(Arc arc); // génère un arc intérieur à partir de trouverArc() prenant en compte la surface du robot |
JimmyAREM | 0:0d5755f210cb | 158 | Arc genererArcExterieur(Arc arc); // génère un arc extérieur à partir de trouverArc() prenant en compte la surface du robot |
JimmyAREM | 0:0d5755f210cb | 159 | |
JimmyAREM | 2:9fbf52f12cea | 160 | int trouverIndicePointFinal(Point buffer[], double xF, double yF); |
JimmyAREM | 2:9fbf52f12cea | 161 | void modifierPointFinal(Point buffer[], int indice); |
JimmyAREM | 2:9fbf52f12cea | 162 | |
JimmyAREM | 0:0d5755f210cb | 163 | void recupererTableauPointArc(Arc arc, Point buffer[], bool type); //Discrétise les points d'un Arc en les stockant dans un buffer |
JimmyAREM | 0:0d5755f210cb | 164 | |
JimmyAREM | 2:9fbf52f12cea | 165 | bool estUnArc(Point tableau[], int indice); // a implémenter |
JimmyAREM | 2:9fbf52f12cea | 166 | bool lancerUnEvitementParArcGauche(int distanceUltrasonGauche, double xFinal, double yFinal); //fonction qui check si on peut faire un évitement par la gauche |
JimmyAREM | 2:9fbf52f12cea | 167 | bool lancerUnEvitementParArcDroit(int distanceUltrasonDroite, double xFinal, double yFinal); //fonction qui check si on peut faire un évitement par la gauche |
JimmyAREM | 0:0d5755f210cb | 168 | #endif |