ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

Map/map.cpp@52:98f8a6ccb6ae, 2016-05-03 (annotated)

Committer:
IceTeam
Date:
Tue May 03 12:37:31 2016 +0000
Revision:
52:98f8a6ccb6ae
Parent:
47:be4eebf40568
Child:
54:be4ea8da9057
Debut ajout obstacle;

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 52:98f8a6ccb6ae 6 void map::Build (int start_type) {
IceTeam 52:98f8a6ccb6ae 7 m.addObs(obsCarr (800, 100, 100, 15));
IceTeam 52:98f8a6ccb6ae 8 m.addObs(obsCarr (2200, 100, 100, 15));
IceTeam 52:98f8a6ccb6ae 9
IceTeam 52:98f8a6ccb6ae 10 m.addObs(obsCarr (1500, 750, 1100, 15));
IceTeam 52:98f8a6ccb6ae 11 m.addObs(obsCarr (1500, 1050, 20, 300));
IceTeam 52:98f8a6ccb6ae 12
IceTeam 52:98f8a6ccb6ae 13 if (start_type == MAP_RIGHTSIDE) {
IceTeam 52:98f8a6ccb6ae 14 m.addObs(obsCarr (0, 2000, 250, 150)); // Coté haut droite
IceTeam 52:98f8a6ccb6ae 15 m.addObs(obsCarr (200, 2000, 200, 50));
IceTeam 52:98f8a6ccb6ae 16
IceTeam 52:98f8a6ccb6ae 17 m.addObs(obsCarr (3000, 2000, 250, 150)); // Coté bas droite
IceTeam 52:98f8a6ccb6ae 18 m.addObs(obsCarr (2800, 2000, 200, 50));
IceTeam 52:98f8a6ccb6ae 19
IceTeam 52:98f8a6ccb6ae 20 m.addObs(obsCarr (200, 2000-450, 40, 40)); // Coquillages du haut droit
IceTeam 52:98f8a6ccb6ae 21 m.addObs(obsCarr (200, 2000-750, 40, 40));
IceTeam 52:98f8a6ccb6ae 22
IceTeam 52:98f8a6ccb6ae 23 m.addObs(obsCarr (900, 2000-550, 40, 40));
IceTeam 52:98f8a6ccb6ae 24 m.addObs(obsCarr (1200, 2000-350, 40, 40)); // Coquillages du milieu/haut
IceTeam 52:98f8a6ccb6ae 25
IceTeam 52:98f8a6ccb6ae 26 m.addObs(obsCarr (1500, 2000-550, 40, 40));
IceTeam 52:98f8a6ccb6ae 27 m.addObs(obsCarr (1500, 2000-350, 40, 40));
IceTeam 52:98f8a6ccb6ae 28
IceTeam 52:98f8a6ccb6ae 29 m.addObs(obsCarr (3000-900, 2000-550, 40, 40));
IceTeam 52:98f8a6ccb6ae 30 m.addObs(obsCarr (3000-1200, 2000-350, 40, 40));
IceTeam 52:98f8a6ccb6ae 31
IceTeam 52:98f8a6ccb6ae 32 m.addObs(obsCarr (3000-200, 2000-450, 40, 40)); // Coquillages du bas droite
IceTeam 52:98f8a6ccb6ae 33 m.addObs(obsCarr (3000-200, 2000-750, 40, 40));
IceTeam 52:98f8a6ccb6ae 34 }
IceTeam 52:98f8a6ccb6ae 35 else {
IceTeam 52:98f8a6ccb6ae 36 m.addObs(obsCarr (1250, 1000, 220, 220));
IceTeam 52:98f8a6ccb6ae 37 m.addObs(obsCarr (1500, 750, 220, 220));
IceTeam 52:98f8a6ccb6ae 38 m.addObs(obsCarr (1500, 1250, 220, 220));
IceTeam 52:98f8a6ccb6ae 39 }
IceTeam 52:98f8a6ccb6ae 40 }
IceTeam 52:98f8a6ccb6ae 41
IceTeam 47:be4eebf40568 42 void map::addObs (obsCarr nobs) {
IceTeam 47:be4eebf40568 43 obs.push_back (nobs);
IceTeam 47:be4eebf40568 44 }
IceTeam 47:be4eebf40568 45
IceTeam 47:be4eebf40568 46 void map::FindWay (float depX, float depY, float arrX, float arrY) {
IceTeam 47:be4eebf40568 47 point depart(depX, depY);
IceTeam 47:be4eebf40568 48 point arrivee(arrX, arrY);
IceTeam 47:be4eebf40568 49 FindWay(depart, arrivee);
IceTeam 47:be4eebf40568 50 }
IceTeam 47:be4eebf40568 51
IceTeam 47:be4eebf40568 52 void map::FindWay (point dep, point arr) {
IceTeam 47:be4eebf40568 53 //logger.printf("On a cherche un chemin\n\r");
IceTeam 47:be4eebf40568 54 nVector<pointParcours> open;
IceTeam 47:be4eebf40568 55 nVector<pointParcours> close;
IceTeam 47:be4eebf40568 56 points4 tmp;
IceTeam 47:be4eebf40568 57 bool val[4] = {true,true,true,true};
IceTeam 47:be4eebf40568 58 int os = obs.size ();
IceTeam 47:be4eebf40568 59 int i, j;
IceTeam 47:be4eebf40568 60 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 61 endedParc = false;
IceTeam 47:be4eebf40568 62
IceTeam 47:be4eebf40568 63 path.clear();
IceTeam 47:be4eebf40568 64
IceTeam 47:be4eebf40568 65 pointParcours depP (dep, NULL, arr);
IceTeam 47:be4eebf40568 66 int indTMP1=0; // Le point actuel
IceTeam 47:be4eebf40568 67 int PointEnding = 0;
IceTeam 47:be4eebf40568 68 open.push_back (depP);
IceTeam 47:be4eebf40568 69
IceTeam 47:be4eebf40568 70 while (!ended && !open.empty ()) {
IceTeam 47:be4eebf40568 71 for (i = 0; i < open.size (); ++i) {
IceTeam 47:be4eebf40568 72 if (open[i].getP2 () < open[indTMP1].getP2 ())
IceTeam 47:be4eebf40568 73 indTMP1 = i;
IceTeam 47:be4eebf40568 74 }
IceTeam 47:be4eebf40568 75
IceTeam 47:be4eebf40568 76 close.push_first (open[indTMP1]);
IceTeam 47:be4eebf40568 77 open.erase (indTMP1);
IceTeam 47:be4eebf40568 78 indTMP1 = 0;
IceTeam 47:be4eebf40568 79
IceTeam 47:be4eebf40568 80 ended = true;
IceTeam 47:be4eebf40568 81 for (i = 0; i < os; ++i) {
IceTeam 47:be4eebf40568 82 if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
IceTeam 47:be4eebf40568 83 ended = false;
IceTeam 47:be4eebf40568 84 tmp = obs[i].getPoints ();
IceTeam 47:be4eebf40568 85
IceTeam 47:be4eebf40568 86 // On vérifie si le point croise un obstacle
IceTeam 47:be4eebf40568 87 for (j = 0; j < os; ++j)
IceTeam 47:be4eebf40568 88 if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
IceTeam 47:be4eebf40568 89 val[0] = false;
IceTeam 47:be4eebf40568 90 // On vérifie si le point existe déjà dans la liste ouverte
IceTeam 47:be4eebf40568 91 for (j = 0; j < open.size (); ++j) {
IceTeam 47:be4eebf40568 92 if (open[j] == tmp.p0)
IceTeam 47:be4eebf40568 93 val[0] = false;
IceTeam 47:be4eebf40568 94 }
IceTeam 47:be4eebf40568 95 // On vérifie si le point existe déjà dans la liste fermée
IceTeam 47:be4eebf40568 96 for (j = 0; j < close.size (); ++j) {
IceTeam 47:be4eebf40568 97 if (close[j] == tmp.p0)
IceTeam 47:be4eebf40568 98 val[0] = false;
IceTeam 47:be4eebf40568 99 }
IceTeam 47:be4eebf40568 100 if (val[0]) {
IceTeam 47:be4eebf40568 101 open.push_back (pointParcours (tmp.p0, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 102 }
IceTeam 47:be4eebf40568 103
IceTeam 47:be4eebf40568 104 // On repete l'operation pour le second point
IceTeam 47:be4eebf40568 105 for (j = 0; j < os; ++j)
IceTeam 47:be4eebf40568 106 if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
IceTeam 47:be4eebf40568 107 val[1] = false;
IceTeam 47:be4eebf40568 108 for (j = 0; j < open.size (); ++j) {
IceTeam 47:be4eebf40568 109 if (open[j] == tmp.p1)
IceTeam 47:be4eebf40568 110 val[1] = false;
IceTeam 47:be4eebf40568 111 }
IceTeam 47:be4eebf40568 112 for (j = 0; j < close.size (); ++j) {
IceTeam 47:be4eebf40568 113 if (close[j] == tmp.p1)
IceTeam 47:be4eebf40568 114 val[1] = false;
IceTeam 47:be4eebf40568 115 }
IceTeam 47:be4eebf40568 116 if (val[1]) {
IceTeam 47:be4eebf40568 117 open.push_back (pointParcours (tmp.p1, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 118 }
IceTeam 47:be4eebf40568 119
IceTeam 47:be4eebf40568 120 // On répète l'opération pour le troisième point
IceTeam 47:be4eebf40568 121 for (j = 0; j < os; ++j)
IceTeam 47:be4eebf40568 122 if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
IceTeam 47:be4eebf40568 123 val[2] = false;
IceTeam 47:be4eebf40568 124 for (j = 0; j < open.size (); ++j) {
IceTeam 47:be4eebf40568 125 if (open[j] == tmp.p2)
IceTeam 47:be4eebf40568 126 val[2] = false;
IceTeam 47:be4eebf40568 127 }
IceTeam 47:be4eebf40568 128 for (j = 0; j < close.size (); ++j) {
IceTeam 47:be4eebf40568 129 if (close[j] == tmp.p2)
IceTeam 47:be4eebf40568 130 val[2] = false;
IceTeam 47:be4eebf40568 131 }
IceTeam 47:be4eebf40568 132 if (val[2]) {
IceTeam 47:be4eebf40568 133 open.push_back (pointParcours (tmp.p2, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 134 }
IceTeam 47:be4eebf40568 135
IceTeam 47:be4eebf40568 136 // On répète l'opération pour le quatrieme point
IceTeam 47:be4eebf40568 137 for (j = 0; j < os; ++j)
IceTeam 47:be4eebf40568 138 if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
IceTeam 47:be4eebf40568 139 val[3] = false;
IceTeam 47:be4eebf40568 140 for (j = 0; j < open.size (); ++j) {
IceTeam 47:be4eebf40568 141 if (open[j] == tmp.p3)
IceTeam 47:be4eebf40568 142 val[3] = false;
IceTeam 47:be4eebf40568 143 }
IceTeam 47:be4eebf40568 144 for (j = 0; j < close.size (); ++j) {
IceTeam 47:be4eebf40568 145 if (close[j] == tmp.p3)
IceTeam 47:be4eebf40568 146 val[3] = false;
IceTeam 47:be4eebf40568 147 }
IceTeam 47:be4eebf40568 148 if (val[3]) {
IceTeam 47:be4eebf40568 149 open.push_back (pointParcours (tmp.p3, &close[indTMP1], arr));
IceTeam 47:be4eebf40568 150 }
IceTeam 47:be4eebf40568 151
IceTeam 47:be4eebf40568 152 val[0] = true;
IceTeam 47:be4eebf40568 153 val[1] = true;
IceTeam 47:be4eebf40568 154 val[2] = true;
IceTeam 47:be4eebf40568 155 val[3] = true;
IceTeam 47:be4eebf40568 156 }
IceTeam 47:be4eebf40568 157 }
IceTeam 47:be4eebf40568 158 }
IceTeam 47:be4eebf40568 159
IceTeam 47:be4eebf40568 160 /* 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 161 if (ended) {
IceTeam 47:be4eebf40568 162 pointParcours* pente;
IceTeam 47:be4eebf40568 163 pente = &close[0];
IceTeam 47:be4eebf40568 164 while (pente != NULL) {
IceTeam 47:be4eebf40568 165 path.push_first (*pente);
IceTeam 47:be4eebf40568 166 pente = pente->getPere ();
IceTeam 47:be4eebf40568 167 }
IceTeam 47:be4eebf40568 168 path.push_back (pointParcours(arr, NULL, arr));
IceTeam 47:be4eebf40568 169 path.erase(0);
IceTeam 47:be4eebf40568 170 endedParc = true;
IceTeam 47:be4eebf40568 171 /*
IceTeam 47:be4eebf40568 172 if (path.size() > 1)
IceTeam 47:be4eebf40568 173 path.erase(0);*/
IceTeam 47:be4eebf40568 174 }
IceTeam 47:be4eebf40568 175 }
IceTeam 47:be4eebf40568 176
IceTeam 47:be4eebf40568 177 void map::Execute(float XObjectif, float YObjectif) {
IceTeam 47:be4eebf40568 178 logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
IceTeam 47:be4eebf40568 179 FindWay (Codo->getX(), Codo->getY(), XObjectif, YObjectif);
IceTeam 47:be4eebf40568 180
IceTeam 47:be4eebf40568 181 if (endedParc) {
IceTeam 47:be4eebf40568 182 //logger.printf("\n\r");
IceTeam 47:be4eebf40568 183 for (int i = 0; i < path.size (); i++) {
IceTeam 47:be4eebf40568 184 logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
IceTeam 47:be4eebf40568 185 //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX()));
IceTeam 47:be4eebf40568 186 Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
IceTeam 47:be4eebf40568 187 logger.printf("Goto Fini\n\r");
IceTeam 47:be4eebf40568 188 }
IceTeam 47:be4eebf40568 189 //logger.printf("Chemin fini !\n\r");
IceTeam 47:be4eebf40568 190 }
IceTeam 47:be4eebf40568 191 else {
IceTeam 47:be4eebf40568 192 logger.printf("Chemin pas trouve ...\n\r");
IceTeam 47:be4eebf40568 193 }
IceTeam 47:be4eebf40568 194 endedParc = false;
IceTeam 47:be4eebf40568 195 }