ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Map/map.cpp@77:f19cc7f81f2a, 2016-05-05 (annotated)

Committer:
sype
Date:
Thu May 05 08:46:08 2016 +0000
Revision:
77:f19cc7f81f2a
Parent:
47:be4eebf40568
Child:
52:98f8a6ccb6ae
commit homologation

Who changed what in which revision?

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