Romain Ame / Mbed 2 deprecated Robot2016_2-0_STATIC

Voili voilou

Dependencies:   RoboClaw StepperMotor mbed

Fork of Robot2016_2-0 by ARES

Map/map.cpp@46:8eae88c45a78, 2016-04-20 (annotated)

Committer:
IceTeam
Date:
Wed Apr 20 13:13:37 2016 +0000
Revision:
46:8eae88c45a78
Mise en static;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 46:8eae88c45a78 1 #include "map.h"
IceTeam 46:8eae88c45a78 2
IceTeam 46:8eae88c45a78 3 map::map (Odometry* nodo) : Codo(nodo) {
IceTeam 46:8eae88c45a78 4 }
IceTeam 46:8eae88c45a78 5
IceTeam 46:8eae88c45a78 6 void map::addObs (obsCarr nobs) {
IceTeam 46:8eae88c45a78 7 obs.push_back (nobs);
IceTeam 46:8eae88c45a78 8 }
IceTeam 46:8eae88c45a78 9
IceTeam 46:8eae88c45a78 10 void map::FindWay (float depX, float depY, float arrX, float arrY) {
IceTeam 46:8eae88c45a78 11 point depart(depX, depY);
IceTeam 46:8eae88c45a78 12 point arrivee(arrX, arrY);
IceTeam 46:8eae88c45a78 13 FindWay(depart, arrivee);
IceTeam 46:8eae88c45a78 14 }
IceTeam 46:8eae88c45a78 15
IceTeam 46:8eae88c45a78 16 void map::FindWay (point dep, point arr) {
IceTeam 46:8eae88c45a78 17 //logger.printf("On a cherche un chemin\n\r");
IceTeam 46:8eae88c45a78 18 nVector<pointParcours> open;
IceTeam 46:8eae88c45a78 19 nVector<pointParcours> close;
IceTeam 46:8eae88c45a78 20 points4 tmp;
IceTeam 46:8eae88c45a78 21 bool val[4] = {true,true,true,true};
IceTeam 46:8eae88c45a78 22 int os = obs.size ();
IceTeam 46:8eae88c45a78 23 int i, j;
IceTeam 46:8eae88c45a78 24 bool ended=false; // On teste tous les points ajoutes dans l'open list pour savoir s'il y a intersection avec un obstacle. Ended passe à true quand aucun ne coupe un obstacle.
IceTeam 46:8eae88c45a78 25 endedParc = false;
IceTeam 46:8eae88c45a78 26
IceTeam 46:8eae88c45a78 27 path.clear();
IceTeam 46:8eae88c45a78 28
IceTeam 46:8eae88c45a78 29 pointParcours depP (dep, NULL, arr);
IceTeam 46:8eae88c45a78 30 int indTMP1=0; // Le point actuel
IceTeam 46:8eae88c45a78 31 int PointEnding = 0;
IceTeam 46:8eae88c45a78 32 open.push_back (depP);
IceTeam 46:8eae88c45a78 33
IceTeam 46:8eae88c45a78 34 while (!ended && !open.empty ()) {
IceTeam 46:8eae88c45a78 35 for (i = 0; i < open.size (); ++i) {
IceTeam 46:8eae88c45a78 36 if (open[i].getP2 () < open[indTMP1].getP2 ())
IceTeam 46:8eae88c45a78 37 indTMP1 = i;
IceTeam 46:8eae88c45a78 38 }
IceTeam 46:8eae88c45a78 39
IceTeam 46:8eae88c45a78 40 close.push_first (open[indTMP1]);
IceTeam 46:8eae88c45a78 41 open.erase (indTMP1);
IceTeam 46:8eae88c45a78 42 indTMP1 = 0;
IceTeam 46:8eae88c45a78 43
IceTeam 46:8eae88c45a78 44 ended = true;
IceTeam 46:8eae88c45a78 45 for (i = 0; i < os; ++i) {
IceTeam 46:8eae88c45a78 46 if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
IceTeam 46:8eae88c45a78 47 ended = false;
IceTeam 46:8eae88c45a78 48 tmp = obs[i].getPoints ();
IceTeam 46:8eae88c45a78 49
IceTeam 46:8eae88c45a78 50 // On vérifie si le point croise un obstacle
IceTeam 46:8eae88c45a78 51 for (j = 0; j < os; ++j)
IceTeam 46:8eae88c45a78 52 if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
IceTeam 46:8eae88c45a78 53 val[0] = false;
IceTeam 46:8eae88c45a78 54 // On vérifie si le point existe déjà dans la liste ouverte
IceTeam 46:8eae88c45a78 55 for (j = 0; j < open.size (); ++j) {
IceTeam 46:8eae88c45a78 56 if (open[j] == tmp.p0)
IceTeam 46:8eae88c45a78 57 val[0] = false;
IceTeam 46:8eae88c45a78 58 }
IceTeam 46:8eae88c45a78 59 // On vérifie si le point existe déjà dans la liste fermée
IceTeam 46:8eae88c45a78 60 for (j = 0; j < close.size (); ++j) {
IceTeam 46:8eae88c45a78 61 if (close[j] == tmp.p0)
IceTeam 46:8eae88c45a78 62 val[0] = false;
IceTeam 46:8eae88c45a78 63 }
IceTeam 46:8eae88c45a78 64 if (val[0]) {
IceTeam 46:8eae88c45a78 65 open.push_back (pointParcours (tmp.p0, &close[indTMP1], arr));
IceTeam 46:8eae88c45a78 66 }
IceTeam 46:8eae88c45a78 67
IceTeam 46:8eae88c45a78 68 // On repete l'operation pour le second point
IceTeam 46:8eae88c45a78 69 for (j = 0; j < os; ++j)
IceTeam 46:8eae88c45a78 70 if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
IceTeam 46:8eae88c45a78 71 val[1] = false;
IceTeam 46:8eae88c45a78 72 for (j = 0; j < open.size (); ++j) {
IceTeam 46:8eae88c45a78 73 if (open[j] == tmp.p1)
IceTeam 46:8eae88c45a78 74 val[1] = false;
IceTeam 46:8eae88c45a78 75 }
IceTeam 46:8eae88c45a78 76 for (j = 0; j < close.size (); ++j) {
IceTeam 46:8eae88c45a78 77 if (close[j] == tmp.p1)
IceTeam 46:8eae88c45a78 78 val[1] = false;
IceTeam 46:8eae88c45a78 79 }
IceTeam 46:8eae88c45a78 80 if (val[1]) {
IceTeam 46:8eae88c45a78 81 open.push_back (pointParcours (tmp.p1, &close[indTMP1], arr));
IceTeam 46:8eae88c45a78 82 }
IceTeam 46:8eae88c45a78 83
IceTeam 46:8eae88c45a78 84 // On répète l'opération pour le troisième point
IceTeam 46:8eae88c45a78 85 for (j = 0; j < os; ++j)
IceTeam 46:8eae88c45a78 86 if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
IceTeam 46:8eae88c45a78 87 val[2] = false;
IceTeam 46:8eae88c45a78 88 for (j = 0; j < open.size (); ++j) {
IceTeam 46:8eae88c45a78 89 if (open[j] == tmp.p2)
IceTeam 46:8eae88c45a78 90 val[2] = false;
IceTeam 46:8eae88c45a78 91 }
IceTeam 46:8eae88c45a78 92 for (j = 0; j < close.size (); ++j) {
IceTeam 46:8eae88c45a78 93 if (close[j] == tmp.p2)
IceTeam 46:8eae88c45a78 94 val[2] = false;
IceTeam 46:8eae88c45a78 95 }
IceTeam 46:8eae88c45a78 96 if (val[2]) {
IceTeam 46:8eae88c45a78 97 open.push_back (pointParcours (tmp.p2, &close[indTMP1], arr));
IceTeam 46:8eae88c45a78 98 }
IceTeam 46:8eae88c45a78 99
IceTeam 46:8eae88c45a78 100 // On répète l'opération pour le quatrieme point
IceTeam 46:8eae88c45a78 101 for (j = 0; j < os; ++j)
IceTeam 46:8eae88c45a78 102 if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
IceTeam 46:8eae88c45a78 103 val[3] = false;
IceTeam 46:8eae88c45a78 104 for (j = 0; j < open.size (); ++j) {
IceTeam 46:8eae88c45a78 105 if (open[j] == tmp.p3)
IceTeam 46:8eae88c45a78 106 val[3] = false;
IceTeam 46:8eae88c45a78 107 }
IceTeam 46:8eae88c45a78 108 for (j = 0; j < close.size (); ++j) {
IceTeam 46:8eae88c45a78 109 if (close[j] == tmp.p3)
IceTeam 46:8eae88c45a78 110 val[3] = false;
IceTeam 46:8eae88c45a78 111 }
IceTeam 46:8eae88c45a78 112 if (val[3]) {
IceTeam 46:8eae88c45a78 113 open.push_back (pointParcours (tmp.p3, &close[indTMP1], arr));
IceTeam 46:8eae88c45a78 114 }
IceTeam 46:8eae88c45a78 115
IceTeam 46:8eae88c45a78 116 val[0] = true;
IceTeam 46:8eae88c45a78 117 val[1] = true;
IceTeam 46:8eae88c45a78 118 val[2] = true;
IceTeam 46:8eae88c45a78 119 val[3] = true;
IceTeam 46:8eae88c45a78 120 }
IceTeam 46:8eae88c45a78 121 }
IceTeam 46:8eae88c45a78 122 }
IceTeam 46:8eae88c45a78 123
IceTeam 46:8eae88c45a78 124 /* L'algorithme n'est pas bon. Je devrais prendre ici le plus court chemin vers l'arrivée pour ceux qui ne sont pas bloqués, et pas un aléatoire ... */
IceTeam 46:8eae88c45a78 125 if (ended) {
IceTeam 46:8eae88c45a78 126 pointParcours* pente;
IceTeam 46:8eae88c45a78 127 pente = &close[0];
IceTeam 46:8eae88c45a78 128 while (pente != NULL) {
IceTeam 46:8eae88c45a78 129 path.push_first (*pente);
IceTeam 46:8eae88c45a78 130 pente = pente->getPere ();
IceTeam 46:8eae88c45a78 131 }
IceTeam 46:8eae88c45a78 132 path.push_back (pointParcours(arr, NULL, arr));
IceTeam 46:8eae88c45a78 133 path.erase(0);
IceTeam 46:8eae88c45a78 134 endedParc = true;
IceTeam 46:8eae88c45a78 135 /*
IceTeam 46:8eae88c45a78 136 if (path.size() > 1)
IceTeam 46:8eae88c45a78 137 path.erase(0);*/
IceTeam 46:8eae88c45a78 138 }
IceTeam 46:8eae88c45a78 139 }
IceTeam 46:8eae88c45a78 140
IceTeam 46:8eae88c45a78 141 void map::Execute(float XObjectif, float YObjectif) {
IceTeam 46:8eae88c45a78 142 logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
IceTeam 46:8eae88c45a78 143 FindWay (Codo->getX(), Codo->getY(), XObjectif, YObjectif);
IceTeam 46:8eae88c45a78 144
IceTeam 46:8eae88c45a78 145 if (endedParc) {
IceTeam 46:8eae88c45a78 146 //logger.printf("\n\r");
IceTeam 46:8eae88c45a78 147 for (int i = 0; i < path.size (); i++) {
IceTeam 46:8eae88c45a78 148 logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
IceTeam 46:8eae88c45a78 149 //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX()));
IceTeam 46:8eae88c45a78 150 Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
IceTeam 46:8eae88c45a78 151 logger.printf("Goto Fini\n\r");
IceTeam 46:8eae88c45a78 152 }
IceTeam 46:8eae88c45a78 153 //logger.printf("Chemin fini !\n\r");
IceTeam 46:8eae88c45a78 154 }
IceTeam 46:8eae88c45a78 155 else {
IceTeam 46:8eae88c45a78 156 logger.printf("Chemin pas trouve ...\n\r");
IceTeam 46:8eae88c45a78 157 }
IceTeam 46:8eae88c45a78 158 endedParc = false;
IceTeam 46:8eae88c45a78 159 }