288 lines
7.8 KiB
C++
288 lines
7.8 KiB
C++
#include "api.h"
|
|
#include "mms.h"
|
|
#include <queue>
|
|
#include <string>
|
|
#include <sys/types.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;
|
|
|
|
struct fill {
|
|
int x, y, z;
|
|
};
|
|
|
|
void flood(struct maze *, struct mmstats *, int coordinates[2]);
|
|
void follow(struct maze maze, class mmstats mms);
|
|
int *nextcell(int i, int j, int dir, int ndir[2]);
|
|
void updatepos(class mmstats *mms);
|
|
void locateWall(struct maze &maze);
|
|
|
|
void flood(struct maze *maze, struct mmstats *mmstats, int coordinates[2]) {
|
|
int i, j;
|
|
int prevNum;
|
|
int marked[16][16] = {0};
|
|
std::queue<fill> myqueue;
|
|
|
|
myqueue.push({coordinates[0], coordinates[1], -1});
|
|
|
|
while (myqueue.size()) {
|
|
i = myqueue.front().x;
|
|
j = myqueue.front().y;
|
|
prevNum = myqueue.front().z;
|
|
myqueue.pop();
|
|
|
|
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;
|
|
if (!maze->vwalls[i][j])
|
|
myqueue.push({i + 1, j, prevNum + 1});
|
|
|
|
if (!maze->vwalls[i - 1][j])
|
|
myqueue.push({i - 1, j, prevNum + 1});
|
|
|
|
if (!maze->hwalls[i][j])
|
|
myqueue.push({i, j + 1, prevNum + 1});
|
|
|
|
if (!maze->hwalls[i][j - 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 mht = maze.mhtn[mms->x][mms->y];
|
|
int nextmht = mht - 1;
|
|
int next[2];
|
|
int next_L[2];
|
|
int next_R[2];
|
|
int next_B[2];
|
|
nextcell(mms->x, mms->y, mms->dir, next);
|
|
nextcell(mms->x, mms->y, mms->head->prev->value, next_L);
|
|
nextcell(mms->x, mms->y, mms->head->next->value, next_R);
|
|
nextcell(mms->x, mms->y, mms->head->next->next->value, next_B);
|
|
if (maze.mhtn[next[0]][next[1]] == nextmht && !API::wallFront() ||
|
|
!API::wallFront() && API::wallLeft() && API::wallRight()) {
|
|
if (maze.mhtn[next_L[0]][next_L[1]] <= nextmht &&
|
|
!API::wallLeft()) { /* Detectar si hay atajos */
|
|
API::turnLeft();
|
|
mms->turn(left);
|
|
log("left");
|
|
return;
|
|
} else if (maze.mhtn[next_R[0]][next_R[1]] <= nextmht &&
|
|
!API::wallRight()) {
|
|
API::turnRight();
|
|
mms->turn(right);
|
|
log("right");
|
|
return;
|
|
} else {
|
|
log("forward");
|
|
return;
|
|
}
|
|
} else if (maze.mhtn[next_L[0]][next_L[1]] <= nextmht && !API::wallLeft()) {
|
|
API::turnLeft();
|
|
mms->turn(left);
|
|
log("left");
|
|
return;
|
|
} else if (maze.mhtn[next_R[0]][next_R[1]] <= nextmht &&
|
|
!API::wallRight()) {
|
|
API::turnRight();
|
|
mms->turn(right);
|
|
log("right");
|
|
return;
|
|
} else if (maze.mhtn[next_B[0]][next_B[1]] == nextmht ||
|
|
API::wallFront() && API::wallLeft() && API::wallRight()) {
|
|
API::turnRight();
|
|
mms->turn(right);
|
|
API::turnRight();
|
|
mms->turn(right);
|
|
log("180");
|
|
return;
|
|
} else if (maze.mhtn[next_R[0]][next_R[1]] <=
|
|
maze.mhtn[next_B[0]][next_B[1]] &&
|
|
!API::wallRight()) {
|
|
API::turnRight();
|
|
mms->turn(right);
|
|
log("bRight");
|
|
} else if (maze.mhtn[next_L[0]][next_L[1]] <=
|
|
maze.mhtn[next_B[0]][next_B[1]] &&
|
|
!API::wallLeft()) {
|
|
API::turnLeft();
|
|
mms->turn(left);
|
|
log("bLeft");
|
|
}
|
|
// log("error en la matri");
|
|
}
|
|
|
|
int *nextcell(int i, int j, int dir, int ndir[2]) {
|
|
switch (dir) {
|
|
case norte:
|
|
j++;
|
|
break;
|
|
case este:
|
|
i++;
|
|
break;
|
|
case sur:
|
|
j--;
|
|
break;
|
|
case oeste:
|
|
i--;
|
|
break;
|
|
}
|
|
ndir[0] = i;
|
|
ndir[1] = j;
|
|
return ndir;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
void locateWall(struct maze *maze, class mmstats *mms) {
|
|
switch (mms->dir) {
|
|
case norte:
|
|
if (API::wallFront() && mms->y != 15) {
|
|
maze->hwalls[mms->x][mms->y] = 1;
|
|
API::setWall(mms->x, mms->y, 'n');
|
|
}
|
|
if (API::wallLeft() && mms->x != 0) {
|
|
maze->vwalls[mms->x - 1][mms->y] = 1;
|
|
API::setWall(mms->x - 1, mms->y, 'e');
|
|
}
|
|
if (API::wallRight() && mms->x != 15) {
|
|
maze->vwalls[mms->x][mms->y] = 1;
|
|
API::setWall(mms->x, mms->y, 'e');
|
|
}
|
|
break;
|
|
case este:
|
|
if (API::wallFront() && mms->x != 15) {
|
|
maze->vwalls[mms->x][mms->y] = 1;
|
|
API::setWall(mms->x, mms->y, 'e');
|
|
}
|
|
if (API::wallLeft() && mms->y != 15) {
|
|
maze->hwalls[mms->x][mms->y] = 1;
|
|
API::setWall(mms->x, mms->y, 'n');
|
|
}
|
|
if (API::wallRight() && mms->y != 0) {
|
|
maze->hwalls[mms->x][mms->y - 1] = 1;
|
|
API::setWall(mms->x, mms->y - 1, 'n');
|
|
}
|
|
break;
|
|
case sur:
|
|
if (API::wallFront() && mms->y != 0) {
|
|
maze->hwalls[mms->x][mms->y - 1] = 1;
|
|
API::setWall(mms->x, mms->y - 1, 'n');
|
|
}
|
|
if (API::wallLeft() && mms->x != 15) {
|
|
maze->vwalls[mms->x][mms->y] = 1;
|
|
API::setWall(mms->x, mms->y, 'e');
|
|
}
|
|
if (API::wallRight() && mms->x != 0) {
|
|
maze->vwalls[mms->x - 1][mms->y] = 1;
|
|
API::setWall(mms->x - 1, mms->y, 'e');
|
|
}
|
|
break;
|
|
case oeste:
|
|
if (API::wallFront() && mms->x != 0) {
|
|
maze->vwalls[mms->x - 1][mms->y] = 1;
|
|
API::setWall(mms->x - 1, mms->y, 'e');
|
|
}
|
|
if (API::wallLeft() && mms->y != 0) {
|
|
maze->hwalls[mms->x][mms->y - 1] = 1;
|
|
API::setWall(mms->x, mms->y - 1, 'n');
|
|
}
|
|
if (API::wallRight() && mms->y != 15) {
|
|
maze->hwalls[mms->x][mms->y] = 1;
|
|
API::setWall(mms->x, mms->y, 'n');
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
int main(int argc, char *argv[]) {
|
|
log("Running...");
|
|
API::setColor(0, 0, 'G');
|
|
mmstats stats;
|
|
|
|
stats.x = 0;
|
|
stats.y = 0;
|
|
|
|
int next_pos[2] = {7, 7};
|
|
|
|
for (;;) {
|
|
locateWall(&maze, &stats);
|
|
flood(&maze, &stats, next_pos);
|
|
follow(maze, &stats);
|
|
if (maze.mhtn[stats.x][stats.y] == 0) {
|
|
log("Finish");
|
|
break;
|
|
}
|
|
// log(stats.x);
|
|
// log(stats.y);
|
|
API::moveForward();
|
|
updatepos(&stats);
|
|
// log(maze.hwalls[stats.x][stats.y]);
|
|
}
|
|
|
|
next_pos[0] = 0;
|
|
next_pos[1] = 0;
|
|
for (;;) {
|
|
locateWall(&maze, &stats);
|
|
flood(&maze, &stats, next_pos);
|
|
follow(maze, &stats);
|
|
if (stats.x == 0 && stats.y == 0) {
|
|
log("start");
|
|
break;
|
|
}
|
|
// log(stats.x);
|
|
// log(stats.y);
|
|
API::moveForward();
|
|
updatepos(&stats);
|
|
// log(maze.hwalls[stats.x][stats.y]);
|
|
}
|
|
|
|
next_pos[0] = 7;
|
|
next_pos[1] = 7;
|
|
for (;;) {
|
|
locateWall(&maze, &stats);
|
|
flood(&maze, &stats, next_pos);
|
|
follow(maze, &stats);
|
|
if (maze.mhtn[stats.x][stats.y] == 0) {
|
|
log("Finish");
|
|
break;
|
|
}
|
|
// log(stats.x);
|
|
// log(stats.y);
|
|
API::moveForward();
|
|
updatepos(&stats);
|
|
}
|
|
// log(maze.hwalls[stats.x][stats.y]);
|
|
return 0;
|
|
}
|