add intended direction solvoing

KenwoodFox · Nov 14, 2023 · 0d41d2a · 0d41d2a
1 parent 277da44
commit 0d41d2a
Show file tree

Hide file tree

Showing 3 changed files with 115 additions and 12 deletions.
diff --git a/Firmware/lib/algorithm/algorithm.cpp b/Firmware/lib/algorithm/algorithm.cpp
@@ -178,6 +178,14 @@ void Algorithm::setWall(Cardinal dir)
     maze[mapPoseX][mapPoseY] = maze[mapPoseX][mapPoseY] | dir;
 }
 
+bool Algorithm::getWall(Cardinal dir)
+{
+    // We're using dir as an AND mask here,
+    // if the specific bit is set in maze,
+    // it will evaulate to true.
+    return maze[mapPoseX][mapPoseY] & dir;
+}
+
 void Algorithm::setAbsWall(Cardinal dir, uint8_t _px, uint8_t _py)
 {
     maze[_px][_py] = maze[_px][_py] | dir;

diff --git a/Firmware/lib/algorithm/algorithm.h b/Firmware/lib/algorithm/algorithm.h
@@ -111,12 +111,21 @@ class Algorithm
     void forward();
 
     /**
-     * @brief Set wall in cardinal direction (relative position)
+     * @brief Set wall in cardinal direction (absolute position)
      *
      * @param dir NSEW
      */
     void setWall(Cardinal dir);
 
+    /**
+     * @brief Check if wall in direction is occupied or not
+     *
+     * @param dir    Direction to check
+     * @return true  Wall is set
+     * @return false Wall is unset
+     */
+    bool getWall(Cardinal dir);
+
     /**
      * @brief Set a wall (absolute position)
      *

diff --git a/Firmware/sim/main.cpp b/Firmware/sim/main.cpp
@@ -11,6 +11,28 @@ void log(const std::string &text)
     std::cerr << text << std::endl;
 }
 
+char toDir(Cardinal _dir)
+{
+    // Helper to convert raw dir to string
+    if (_dir == NORTH)
+    {
+        return 'N';
+    }
+    else if (_dir == SOUTH)
+    {
+        return 'S';
+    }
+    else if (_dir == EAST)
+    {
+        return 'E';
+    }
+    else if (_dir == WEST)
+    {
+        return 'W';
+    }
+    return '?';
+}
+
 int main(int argc, char *argv[])
 {
     log("Initializing algorithm");
@@ -22,7 +44,7 @@ int main(int argc, char *argv[])
 
     while (true)
     {
-        // Start by always scanning right and left
+        // Mark all walls in this square!
         if (API::wallLeft())
         {
             alg.markWall(LEFT);
@@ -38,23 +60,58 @@ int main(int argc, char *argv[])
             alg.markWall(FORWARD);
         }
 
-        if (!API::wallLeft())
+        // Pick a direction to go
+        // if (direction is clear)..
+        Cardinal intendedDir = alg.getDirection(alg.mapPoseX, alg.mapPoseY, 8, 8);
+        if (!alg.getWall(intendedDir)) // If the wall in the intended dir is clear
         {
-            alg.setWall(alg.mapDir);
-            alg.turnLeft();
-            API::turnLeft();
+            log("Intended dir is clear!");
+
+            if (alg.mapDir == intendedDir)
+            {
+                API::moveForward();
+                alg.forward();
+            }
+            else if (alg.rotate(LEFT) == intendedDir)
+            {
+                API::turnLeft();
+                alg.turnLeft();
+                API::moveForward();
+                alg.forward();
+            }
+            else if (alg.rotate(RIGHT) == intendedDir)
+            {
+                API::turnRight();
+                alg.turnRight();
+                API::moveForward();
+                alg.forward();
+            }
         }
-        while (API::wallFront())
+        else
         {
-            alg.setWall(alg.mapDir);
-            alg.turnRight();
             API::turnRight();
+            alg.turnRight();
+            API::moveForward();
+            alg.forward();
         }
-        API::moveForward();
-        alg.forward();
+
+        // if (!API::wallLeft())
+        // {
+        //     alg.setWall(alg.mapDir);
+        //     alg.turnLeft();
+        //     API::turnLeft();
+        // }
+        // while (API::wallFront())
+        // {
+        //     alg.setWall(alg.mapDir);
+        //     alg.turnRight();
+        //     API::turnRight();
+        // }
+        // API::moveForward();
+        // alg.forward();
 
         char _buf[20];
-        sprintf(_buf, "Intended dir: %d", alg.getDirection(alg.mapPoseX, alg.mapPoseY, 8, 8));
+        sprintf(_buf, "Intended dir: %c", toDir(alg.getDirection(alg.mapPoseX, alg.mapPoseY, 8, 8)));
         log(_buf);
 
         // Update the simulator with what the robot has in memory
@@ -64,6 +121,8 @@ int main(int argc, char *argv[])
             {
                 uint16_t _raw = alg.getRaw(x, y);
 
+                API::setColor(x, y, 'k');
+
                 // Process all cardinals (TODO: clean this up!)
                 if (_raw & (1 << 15))
                 {
@@ -96,5 +155,32 @@ int main(int argc, char *argv[])
                 }
             }
         }
+
+        // Mark along the intended direction
+        int startX = alg.mapPoseX;         // Current robot pose
+        int startY = alg.mapPoseY;         // Current robot pose
+        while (startX != 8 || startY != 8) // As long as we haven't walked to the center
+        {
+            if (alg.getDirection(startX, startY, 8, 8) == NORTH)
+            {
+                startY += 1;
+                API::setColor(startX, startY, 'g');
+            }
+            else if (alg.getDirection(startX, startY, 8, 8) == SOUTH)
+            {
+                startY -= 1;
+                API::setColor(startX, startY, 'g');
+            }
+            else if (alg.getDirection(startX, startY, 8, 8) == EAST)
+            {
+                startX += 1;
+                API::setColor(startX, startY, 'g');
+            }
+            else if (alg.getDirection(startX, startY, 8, 8) == WEST)
+            {
+                startY -= 1;
+                API::setColor(startX, startY, 'g');
+            }
+        }
     }
 }