Guillaume Chauvon / AnalyseArcLib

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?

UserRevisionLine numberNew 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