274 lines
7.3 KiB
C++
274 lines
7.3 KiB
C++
#include <string>
|
|
#include <sys/types.h>
|
|
#include <queue>
|
|
#include "api.h"
|
|
#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 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 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");*/
|
|
API::setText(i, j, prevNum+1);
|
|
maze->mhtn[i][j] = prevNum+1;
|
|
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;
|
|
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::wallLeft() && API::wallRight()) {
|
|
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::turnRight();
|
|
mms->turn(right);
|
|
API::turnRight();
|
|
mms->turn(right);
|
|
log("180");
|
|
return;
|
|
}
|
|
}
|
|
|
|
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->vwalls[mms->x][mms->y-1] = 1;
|
|
API::setWall(mms->x, mms->y-1, 'n');
|
|
}
|
|
if(API::wallLeft() && mms->x != 15){
|
|
maze->hwalls[mms->x][mms->y] = 1;
|
|
API::setWall(mms->x, mms->y, 'e');
|
|
}
|
|
if(API::wallRight() && mms->x != 0){
|
|
maze->hwalls[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 != 0){
|
|
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');
|
|
// API::setText(0, 0, "abc");
|
|
mmstats stats;
|
|
stats.x = 0;
|
|
stats.y = 0;
|
|
|
|
while (true) {
|
|
locateWall(&maze, &stats);
|
|
flood(&maze, &stats);
|
|
follow(maze, &stats);
|
|
if(maze.mhtn[stats.x][stats.y] == 0){
|
|
log("Finish");
|
|
return 0;
|
|
}
|
|
//log(stats.x);
|
|
//log(stats.y);
|
|
API::moveForward();
|
|
updatepos(&stats);
|
|
//log(maze.hwalls[stats.x][stats.y]);
|
|
}
|
|
/*
|
|
while (true) {
|
|
if (!API::wallLeft()) {
|
|
locateWall(&maze, &stats);
|
|
API::turnLeft();
|
|
stats.turn(left);
|
|
flood(&maze, &stats);
|
|
}
|
|
while (API::wallFront()) {
|
|
locateWall(&maze, &stats);
|
|
API::turnRight();
|
|
stats.turn(right);
|
|
flood(&maze, &stats);
|
|
}
|
|
locateWall(&maze, &stats);
|
|
API::moveForward();
|
|
updatepos(&stats);
|
|
flood(&maze, &stats);
|
|
log(stats.x);
|
|
log(stats.y);
|
|
log("");
|
|
}
|
|
*/
|
|
}
|