133 lines
3.1 KiB
C++
133 lines
3.1 KiB
C++
#include <string>
|
|
#include <sys/types.h>
|
|
#include "api.h"
|
|
#include <queue>
|
|
#include "mms.h"
|
|
|
|
#define W 15 /* ancho y alto del laberinto -1 */
|
|
|
|
|
|
struct maze { /* Distancias de Manhattan */
|
|
int vwalls[15][15];
|
|
int hwalls[15][15];
|
|
int mhtn[16][16];
|
|
maze(){
|
|
mhtn[7][7] = 0; /* las celdas del centro tienen */
|
|
mhtn[7][8] = 0; /* un valor de cero porque son */
|
|
mhtn[8][7] = 0; /* la meta. */
|
|
mhtn[8][8] = 0;
|
|
}
|
|
}maze;
|
|
|
|
struct fill {
|
|
int x, y, z;
|
|
};
|
|
|
|
void flood(struct maze*, struct mmstats*);
|
|
void flood(struct maze* maze, struct mmstats* mmstats) {
|
|
int i, j;
|
|
int prevNum;
|
|
int marked[16][16];
|
|
std::queue<fill> myqueue;
|
|
|
|
myqueue.push({6,7,0});
|
|
myqueue.push({6,8,0});
|
|
myqueue.push({7,6,0});
|
|
myqueue.push({8,6,0});
|
|
|
|
myqueue.push({9,7,0});
|
|
myqueue.push({9,8,0});
|
|
myqueue.push({7,9,0});
|
|
myqueue.push({8,9,0});
|
|
|
|
marked[7][7] = 1; /* las celdas del centro tienen */
|
|
marked[7][8] = 1; /* un valor de cero porque son */
|
|
marked[8][7] = 1; /* la meta. */
|
|
marked[8][8] = 1;
|
|
|
|
API::setText(7, 7, 0);
|
|
API::setText(7, 8, 0);
|
|
API::setText(8, 7, 0);
|
|
API::setText(8, 8, 0);
|
|
|
|
while (myqueue.size()) {
|
|
i = myqueue.front().x;
|
|
j = myqueue.front().y;
|
|
prevNum = myqueue.front().z;
|
|
myqueue.pop();
|
|
|
|
if (mmstats->x == i && mmstats->y == j) {
|
|
log("flood end");
|
|
break;
|
|
}
|
|
|
|
if (i < 0 || i > W || j < 0 || j > W
|
|
|| marked[i][j]==1) {
|
|
continue;
|
|
}
|
|
else {
|
|
API::setText(i, j, prevNum+1);
|
|
maze->mhtn[i][j] = prevNum+1;
|
|
marked[i][j] = 1;
|
|
myqueue.push({i+1, j, prevNum +1});
|
|
myqueue.push({i-1, j, prevNum +1});
|
|
myqueue.push({i, j+1, prevNum +1});
|
|
myqueue.push({i, j-1, prevNum +1});
|
|
}
|
|
}
|
|
}
|
|
|
|
void follow(struct maze maze, class mmstats mms) {
|
|
/* para que el micromouse siga el camino descendente
|
|
* se tienen que dar 16 estados, esta funcion lo reduce a 4
|
|
*/
|
|
int x = mms.x;
|
|
int y = mms.y;
|
|
int mht = maze.mhtn[x][y];
|
|
|
|
}
|
|
|
|
void updatepos(class mmstats* mms) {
|
|
/* cada que se ejecuta moveForward la funcion actualiza
|
|
* la posicion del micromouse en el valor correspondiente
|
|
*/
|
|
switch (mms->dir) {
|
|
case norte: mms->y++;
|
|
break;
|
|
case este: mms->x++;
|
|
break;
|
|
case sur: mms->y--;
|
|
break;
|
|
case oeste: mms->x--;
|
|
break;
|
|
}
|
|
log(mms->x);
|
|
log(mms->y);
|
|
log("");
|
|
}
|
|
|
|
|
|
int main(int argc, char* argv[]) {
|
|
log("Running...");
|
|
API::setColor(0, 0, 'G');
|
|
// API::setText(0, 0, "abc");
|
|
API::setWall(1, 0, 'e');
|
|
mmstats stats;
|
|
stats.x = 0;
|
|
stats.y = 0;
|
|
flood(&maze, &stats);
|
|
|
|
while (true) {
|
|
if (!API::wallLeft()) {
|
|
API::turnLeft();
|
|
stats.turn(left);
|
|
}
|
|
while (API::wallFront()) {
|
|
API::turnRight();
|
|
stats.turn(right);
|
|
}
|
|
API::moveForward();
|
|
updatepos(&stats);
|
|
}
|
|
}
|