Casi resuelve el primer laberinto

main.cpp

@@ -1,7 +1,7 @@
 #include <string>
 #include <sys/types.h>
-#include "api.h"
 #include <queue>
+#include "api.h"
 #include "mms.h"
 
 #define W 15 /* ancho y alto del laberinto -1 */
@@ -11,12 +11,6 @@ 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 {
@@ -24,12 +18,18 @@ struct fill {
 };
 
 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});
@@ -39,11 +39,12 @@ void flood(struct maze* maze, struct mmstats* mmstats) {
     myqueue.push({9,8,0});
     myqueue.push({7,9,0});
     myqueue.push({8,9,0});
+    */
 
-    marked[7][7] = 1; /* las celdas del centro tienen */
+    myqueue.push({7,7,-1});
-    marked[7][8] = 1; /* un valor de cero porque son  */
+    myqueue.push({7,8,-1});
-    marked[8][7] = 1; /* la meta.                     */
+    myqueue.push({8,7,-1});
-    marked[8][8] = 1;
+    myqueue.push({8,8,-1});
 
     API::setText(7, 7, 0);
     API::setText(7, 8, 0);
@@ -56,11 +57,6 @@ void flood(struct maze* maze, struct mmstats* mmstats) {
         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;
@@ -69,34 +65,208 @@ void flood(struct maze* maze, struct mmstats* mmstats) {
             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()) {
+        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::wallFront() && API::wallLeft())){
+        API::turnRight();
+        mms->turn(right);
+    }
+    else if(maze.mhtn[next_L[0]][next_L[1]] <= maze.mhtn[next_B[0]][next_B[1]] && (API::wallFront() && API::wallRight())){
+        API::turnLeft();
+        mms->turn(left);
+    }
+    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 != 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");
-    API::setWall(1, 0, 'e');
     mmstats stats;
     stats.x = 0;
     stats.y = 0;
-    flood(&maze, &stats);
     
+    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("");
    }
+   */
 }

mms.h

@@ -10,7 +10,6 @@ struct dirnode {
 class mmstats {
 private:
     dirnode* tail;
-    dirnode* head;
     void addnode(int val) {
         dirnode* temp = new dirnode(val);
         if (head != nullptr) {
@@ -27,6 +26,7 @@ private:
         }
     }
 public:
+    dirnode* head;
     int y;
     int x;
     char dir;
@@ -42,6 +42,7 @@ public:
             head = head->next;
         else head = head->prev;
         dir = head->value;
+        /*
         switch (dir) {
         case norte: log("Norte");
                   break;
@@ -52,6 +53,7 @@ public:
         case oeste: log("Oeste");
                   break;
         }
+        */
     }
 
 };