From 71a01558e674ba36f23a2d6f281ed89a9d779432 Mon Sep 17 00:00:00 2001 From: Nadeesha Jayamanne Date: Sat, 4 Oct 2025 09:56:14 +0530 Subject: [PATCH 1/4] SUccefully reach center --- .../mms src/Floodfill_Backtracking.cpp | 465 ++++++++++++++++ .../mms src/Floodfill_DoubleSearch.cpp | 504 ++++++++++++++++++ lib/Floodfill/mms src/Floodfill_SearchRun.cpp | 257 +++++++++ lib/Floodfill/src/Floodfill.h | 12 +- lib/Floodfill/src/Floodfill_SearchRun.cpp | 114 +++- lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h | 2 +- lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp | 116 ++-- src/src_main/main.cpp | 12 +- 8 files changed, 1406 insertions(+), 76 deletions(-) create mode 100644 lib/Floodfill/mms src/Floodfill_Backtracking.cpp create mode 100644 lib/Floodfill/mms src/Floodfill_SearchRun.cpp diff --git a/lib/Floodfill/mms src/Floodfill_Backtracking.cpp b/lib/Floodfill/mms src/Floodfill_Backtracking.cpp new file mode 100644 index 0000000..6d9e1b1 --- /dev/null +++ b/lib/Floodfill/mms src/Floodfill_Backtracking.cpp @@ -0,0 +1,465 @@ +// Micromouse floodfill maze-solving algorithm +// Compatible with MMS Simulator +// Written in C++ for robotics competitions +#include "API.h" +#include +#include +#include +#include +#include +#include "Floodfill.h" +#include + +using namespace std; +Floodfill floodfill; + +enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND}; + +int idx[] = {0,1,3,2}; +int curRow = 0, curCol = 0, curDir = 0; // 0=north,1=east,2=south,3=west +bool reachedCenter = false; + + +float getStat(const std::string& statName) { + std::cout << "getStat " << statName << std::endl; + std::string response; + std::cin >> response; + try { + return std::stof(response); // handles both int and float + } catch (...) { + std::cerr << "Failed to parse stat: " << statName << std::endl; + return -1; + } +} + +bool atGoal(int row, int col) { + return (row == 7 || row == 8) && (col == 7 || col == 8); +} + +void Floodfill::setWall(int row, int col, int direction) { + if(direction == 0) { + maze.vertical_walls[row][col].first = 1; // North wall + if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above + } + if(direction == 1) { + maze.horizontal_walls[row][col].second = 1; // East wall + if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right + } + if(direction == 2) { + maze.vertical_walls[row][col].second = 1; // South wall + if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below + } + if(direction == 3) { + maze.horizontal_walls[row][col].first = 1; // West wall + if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left + } + char cdir = "nesw"[direction]; + API::setWall(col, row, cdir); +} +void Floodfill::detectWalls(vector sensorDistances, int row, int col, int direction){ + if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds + if(API::wallFront()) { // North Wall + setWall(row, col, direction); + // Serial.print("Wall detected at "); + // Serial.print(direction); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[2]); + } + if(API::wallLeft()) { + setWall(row, col, (direction + 3) % 4); + // Serial.print("Wall detected at "); + // Serial.print((direction + 3) % 4); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[0]); + } + //if(sensorDistances[1] < wall_threshhold) setWall(row, col, direction); + if(API::wallRight()) { + setWall(row, col, (direction + 1) % 4); + // Serial.print("Wall detected at "); + // Serial.print((direction + 1) % 4); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[4]); + } +} +bool Floodfill::hasWall(int row, int col, int dir) { + if(dir == 0) return maze.vertical_walls[row][col].first; // North wall + if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall + if(dir == 2) return maze.vertical_walls[row][col].second; // South wall + if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall + return false; // No walls +} + +void Floodfill::floodfill() { + queue> q; + + for (auto& row : maze.manhattan_distances) + row.fill(255); // Re-initialize all distances to a large number + + maze.manhattan_distances[8][8] = 0; //row,column + maze.manhattan_distances[8][7] = 0; + maze.manhattan_distances[7][8] = 0; + maze.manhattan_distances[7][7] = 0; // Goal + + maze.visited[8][8] = true; + maze.visited[8][7] = true; + maze.visited[7][8] = true; + maze.visited[7][7] = true; + maze.visited[0][0] = true; + + + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + + array, 16> reached = {}; + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + + // BFS to fill the manhattan distances + while(!q.empty()){ + pair cell = q.front(); + q.pop(); + + int row = cell.first; + int col = cell.second; + + if (row < 0 || row > 15 || col < 0 || col > 15) continue; + + if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) { + maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col - 1}); + reached[row][col - 1] = true; + } + + // NORTH + if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) { + maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row + 1, col}); + reached[row + 1][col] = true; + } + + // EAST + if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) { + maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col + 1}); + reached[row][col + 1] = true; + } + + // SOUTH + if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) { + maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row - 1, col}); + reached[row - 1][col] = true; + } + } + + for (int r = 0; r < 16; ++r) { + for (int c = 0; c < 16; ++c) { + int dist = maze.manhattan_distances[r][c]; + if (dist != 255) { + API::setText(c, r, to_string(dist)); // NOTE: col=x, row=y + } + } + } +} + +void Floodfill::floodfillToStart() { + queue> q; + + for (auto& row : maze.reverse_manhattan_distances) + row.fill(255); // Reset distances + + // New goal is the start: bottom-left of the maze + maze.reverse_manhattan_distances[0][0] = 0; + q.push({0, 0}); + + array, 16> reached = {}; + reached[0][0] = true; + + while (!q.empty()) { + pair cell = q.front(); + q.pop(); + + int row = cell.first; + int col = cell.second; + + if (row < 0 || row > 15 || col < 0 || col > 15) continue; + + // WEST + if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) { + maze.reverse_manhattan_distances[row][col - 1] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row, col - 1}); + reached[row][col - 1] = true; + } + + // NORTH + if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) { + maze.reverse_manhattan_distances[row + 1][col] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row + 1, col}); + reached[row + 1][col] = true; + } + + // EAST + if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) { + maze.reverse_manhattan_distances[row][col + 1] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row, col + 1}); + reached[row][col + 1] = true; + } + + // SOUTH + if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) { + maze.reverse_manhattan_distances[row - 1][col] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row - 1, col}); + reached[row - 1][col] = true; + } + } +} +void Floodfill::final_floodfill() { + queue> q; + + for (auto& row : maze.manhattan_distances) + row.fill(255); // Re-initialize all distances to a large number + + maze.manhattan_distances[8][8] = 0; //row,column + maze.manhattan_distances[8][7] = 0; + maze.manhattan_distances[7][8] = 0; + maze.manhattan_distances[7][7] = 0; // Goal + + + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + + array, 16> reached = {}; + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + + // BFS to fill the manhattan distances + while(!q.empty()){ + pair cell = q.front(); + q.pop(); + + int row = cell.first; + int col = cell.second; + + if (row < 0 || row > 15 || col < 0 || col > 15) continue; + + // WEST + if (col > 0 && !maze.horizontal_walls[row][col].first && maze.visited[row][col - 1] && !reached[row][col - 1]) { + maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col - 1}); + reached[row][col - 1] = true; + } + + // NORTH + if (row < 15 && !maze.vertical_walls[row][col].first && maze.visited[row + 1][col] && !reached[row + 1][col]) { + maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row + 1, col}); + reached[row + 1][col] = true; + } + + // EAST + if (col < 15 && !maze.horizontal_walls[row][col].second && maze.visited[row][col + 1] && !reached[row][col + 1]) { + maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col + 1}); + reached[row][col + 1] = true; + } + + // SOUTH + if (row > 0 && !maze.vertical_walls[row][col].second && maze.visited[row - 1][col] && !reached[row - 1][col]) { + maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row - 1, col}); + reached[row - 1][col] = true; + } + + } + for (int r = 0; r < 16; ++r) { + for (int c = 0; c < 16; ++c) { + int dist = maze.manhattan_distances[r][c]; + if (dist != 255) { + API::setText(c, r, to_string(dist)); + } + } + } +} + +int Floodfill::getNextMove(int row, int col) { + int minDist = 255; + int bestDirection = -1; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !hasWall(row, col, 0)) r++; + else if (dir == 1 && !hasWall(row, col, 1)) c++; + else if (dir == 2 && !hasWall(row, col, 2)) r--; + else if (dir == 3 && !hasWall(row, col, 3)) c--; + else continue; // skip if there's a wall + + if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + + int dist = maze.manhattan_distances[r][c]; + + if (dist < minDist || (dist == minDist && dir == curDir)) { + minDist = dist; + bestDirection = dir; + } + } + + return bestDirection; +} +int Floodfill::reverse_getNextMove(int row, int col) { + int minDist = 255; + int bestDirection = -1; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !hasWall(row, col, 0)) r++; + else if (dir == 1 && !hasWall(row, col, 1)) c++; + else if (dir == 2 && !hasWall(row, col, 2)) r--; + else if (dir == 3 && !hasWall(row, col, 3)) c--; + else continue; // skip if there's a wall + + if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + + int dist = maze.reverse_manhattan_distances[r][c]; + + if (dist < minDist || (dist == minDist && dir == curDir)) { + minDist = dist; + bestDirection = dir; + } + } + + return bestDirection; +} +Action rotateTo(int newDir) { + int diff = (newDir - curDir + 4) % 4; + if (diff == 0) return FORWARD; + if (diff == 1) return RIGHT; + if (diff == 3) return LEFT; + if (diff == 2) return AROUND; // U-turn + return IDLE; +} +void moveForwardUpdatePos() { + if (curDir == 0) curRow++; + if (curDir == 1) curCol++; + if (curDir == 2) curRow--; + if (curDir == 3) curCol--; + +} +Action solver() { + floodfill.detectWalls({}, curRow, curCol, curDir); + if (!reachedCenter && ( + (curRow == 8 && curCol == 8) || + (curRow == 8 && curCol == 7) || + (curRow == 7 && curCol == 8) || + (curRow == 7 && curCol == 7))) { + reachedCenter = true; + std::cerr << "Reached center! Recomputing best path to start...\n"; + } + int bestDir=0; + if(reachedCenter){ + floodfill.floodfillToStart(); + floodfill.floodfill(); + bestDir = floodfill.reverse_getNextMove(curRow, curCol); + } + else { + floodfill.floodfill(); + bestDir = floodfill.getNextMove(curRow, curCol); + } + Action action = rotateTo(bestDir); + std::cerr << "Moved to: (" << curRow << ", " << curCol << ")" << std::endl; + //API::setColor(curRow, curCol, 'g'); + std::cerr << "Action decided: " << action << std::endl; + if (action == IDLE) { + std::cerr << "No valid move found, staying idle." << std::endl; + } + return action; +} + +void log(const string& text) { + cerr << text << endl; +} + +int main(int argc, char* argv[]) { + log("Running..."); + while (true) { + + if (reachedCenter && curRow == 0 && curCol == 0) { + std::cerr << "Returned to start!\n"; + log("Goal reached!"); + std::cerr << "\n==== MMS SIMULATOR STATS ====" << std::endl; + std::cerr << "Total Distance : " << getStat("total-distance") << std::endl; + std::cerr << "Total Turns : " << getStat("total-turns") << std::endl; + std::cerr << "Best Run Distance : " << getStat("best-run-distance") << std::endl; + std::cerr << "Best Run Turns : " << getStat("best-run-turns") << std::endl; + std::cerr << "Current Run Distance : " << getStat("current-run-distance") << std::endl; + std::cerr << "Current Run Turns : " << getStat("current-run-turns") << std::endl; + std::cerr << "Total Effective Distance : " << getStat("total-effective-distance") << std::endl; + std::cerr << "Best Run Effective Distance : " << getStat("best-run-effective-distance") << std::endl; + std::cerr << "Current Run Effective Dist : " << getStat("current-run-effective-distance") << std::endl; + std::cerr << "Final Score : " << 2000 - getStat("score") << std::endl; + std::cerr << "Highest Possible score is 2000 " << std::endl; + + API::turnRight(); + API::turnRight(); + curDir = (curDir + 2) % 4; + + // Highlight explored shortest path from start to center using known walls only + floodfill.final_floodfill(); + int row = 0, col = 0; + API::setColor(col, row, 'C'); // Highlight starting point + + while (!atGoal(row, col)) { + int currentDist = floodfill.maze.manhattan_distances[row][col]; + int nextRow = row, nextCol = col; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !floodfill.hasWall(row, col, 0)) r++; + else if (dir == 1 && !floodfill.hasWall(row, col, 1)) c++; + else if (dir == 2 && !floodfill.hasWall(row, col, 2)) r--; + else if (dir == 3 && !floodfill.hasWall(row, col, 3)) c--; + else continue; + + if (r >= 0 && r < 16 && c >= 0 && c < 16 && floodfill.maze.manhattan_distances[r][c] < currentDist) { + nextRow = r; + nextCol = c; + break; + } + } + + row = nextRow; + col = nextCol; + API::setColor(col, row, 'C'); // Color path step + } + + break; + } + if (!reachedCenter && atGoal(curRow, curCol)) { + log("Reached center. Reversing goal to return home..."); + reachedCenter = true; + } + + Action action = solver(); + + if (action == FORWARD) { + API::moveForward(); + API::setColor(curCol, curRow, 'c'); + floodfill.maze.visited[curRow][curCol] = true; + API::setText(curCol, curRow, to_string(floodfill.maze.manhattan_distances[curRow][curCol])); + moveForwardUpdatePos(); + + + + } else if (action == LEFT) { + API::turnLeft(); + curDir = (curDir + 3) % 4; + } else if (action == RIGHT) { + API::turnRight(); + curDir = (curDir + 1) % 4; + } else if (action == AROUND) { + API::turnRight(); + API::turnRight(); + curDir = (curDir + 2) % 4; + } + + } + return 0; +} diff --git a/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp b/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp index e69de29..d0e2f43 100644 --- a/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp +++ b/lib/Floodfill/mms src/Floodfill_DoubleSearch.cpp @@ -0,0 +1,504 @@ +// Micromouse floodfill maze-solving algorithm +// Compatible with MMS Simulator +// Written in C++ for robotics competitions +#include "API.h" +#include +#include +#include +#include +#include +#include "Floodfill.h" +#include + +using namespace std; +Floodfill floodfill; + +enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND}; + +int idx[] = {0,1,3,2}; +int curRow = 0, curCol = 0, curDir = 0; // 0=north,1=east,2=south,3=west +bool reachedCenter = false; +bool rerun = false; + +float getStat(const std::string& statName) { + std::cout << "getStat " << statName << std::endl; + std::string response; + std::cin >> response; + try { + return std::stof(response); // handles both int and float + } catch (...) { + std::cerr << "Failed to parse stat: " << statName << std::endl; + return -1; + } +} + +bool atGoal(int row, int col) { + return (row == 7 || row == 8) && (col == 7 || col == 8); +} + +void Floodfill::setWall(int row, int col, int direction) { + if(direction == 0) { + maze.vertical_walls[row][col].first = 1; // North wall + if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above + } + if(direction == 1) { + maze.horizontal_walls[row][col].second = 1; // East wall + if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right + } + if(direction == 2) { + maze.vertical_walls[row][col].second = 1; // South wall + if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below + } + if(direction == 3) { + maze.horizontal_walls[row][col].first = 1; // West wall + if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left + } + char cdir = "nesw"[direction]; + API::setWall(col, row, cdir); +} +void Floodfill::detectWalls(vector sensorDistances, int row, int col, int direction){ + if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds + if(API::wallFront()) { // North Wall + setWall(row, col, direction); + // Serial.print("Wall detected at "); + // Serial.print(direction); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[2]); + } + if(API::wallLeft()) { + setWall(row, col, (direction + 3) % 4); + // Serial.print("Wall detected at "); + // Serial.print((direction + 3) % 4); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[0]); + } + //if(sensorDistances[1] < wall_threshhold) setWall(row, col, direction); + if(API::wallRight()) { + setWall(row, col, (direction + 1) % 4); + // Serial.print("Wall detected at "); + // Serial.print((direction + 1) % 4); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[4]); + } +} +bool Floodfill::hasWall(int row, int col, int dir) { + if(dir == 0) return maze.vertical_walls[row][col].first; // North wall + if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall + if(dir == 2) return maze.vertical_walls[row][col].second; // South wall + if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall + return false; // No walls +} + +void Floodfill::floodfill() { + queue> q; + + for (auto& row : maze.manhattan_distances) + row.fill(255); // Re-initialize all distances to a large number + + maze.manhattan_distances[8][8] = 0; //row,column + maze.manhattan_distances[8][7] = 0; + maze.manhattan_distances[7][8] = 0; + maze.manhattan_distances[7][7] = 0; // Goal + + maze.visited[8][8] = true; + maze.visited[8][7] = true; + maze.visited[7][8] = true; + maze.visited[7][7] = true; + maze.visited[0][0] = true; + + + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + + array, 16> reached = {}; + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + + // BFS to fill the manhattan distances + while(!q.empty()){ + pair cell = q.front(); + q.pop(); + + int row = cell.first; + int col = cell.second; + + if (row < 0 || row > 15 || col < 0 || col > 15) continue; + + if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) { + maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col - 1}); + reached[row][col - 1] = true; + } + + // NORTH + if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) { + maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row + 1, col}); + reached[row + 1][col] = true; + } + + // EAST + if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) { + maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col + 1}); + reached[row][col + 1] = true; + } + + // SOUTH + if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) { + maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row - 1, col}); + reached[row - 1][col] = true; + } + } + + for (int r = 0; r < 16; ++r) { + for (int c = 0; c < 16; ++c) { + int dist = maze.manhattan_distances[r][c]; + if (dist != 255) { + API::setText(c, r, to_string(dist)); // NOTE: col=x, row=y + } + } + } +} + +void Floodfill::floodfillToStart() { + queue> q; + + for (auto& row : maze.reverse_manhattan_distances) + row.fill(255); // Reset distances + + // New goal is the start: bottom-left of the maze + maze.reverse_manhattan_distances[0][0] = 0; + q.push({0, 0}); + + array, 16> reached = {}; + reached[0][0] = true; + + while (!q.empty()) { + pair cell = q.front(); + q.pop(); + + int row = cell.first; + int col = cell.second; + + if (row < 0 || row > 15 || col < 0 || col > 15) continue; + + // WEST + if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) { + maze.reverse_manhattan_distances[row][col - 1] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row, col - 1}); + reached[row][col - 1] = true; + } + + // NORTH + if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) { + maze.reverse_manhattan_distances[row + 1][col] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row + 1, col}); + reached[row + 1][col] = true; + } + + // EAST + if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) { + maze.reverse_manhattan_distances[row][col + 1] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row, col + 1}); + reached[row][col + 1] = true; + } + + // SOUTH + if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) { + maze.reverse_manhattan_distances[row - 1][col] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row - 1, col}); + reached[row - 1][col] = true; + } + } +} +void Floodfill::final_floodfill() { + queue> q; + + for (auto& row : maze.manhattan_distances) + row.fill(255); // Re-initialize all distances to a large number + + maze.manhattan_distances[8][8] = 0; //row,column + maze.manhattan_distances[8][7] = 0; + maze.manhattan_distances[7][8] = 0; + maze.manhattan_distances[7][7] = 0; // Goal + + + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + + array, 16> reached = {}; + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + + // BFS to fill the manhattan distances + while(!q.empty()){ + pair cell = q.front(); + q.pop(); + + int row = cell.first; + int col = cell.second; + + if (row < 0 || row > 15 || col < 0 || col > 15) continue; + + // WEST + if (col > 0 && !maze.horizontal_walls[row][col].first && maze.visited[row][col - 1] && !reached[row][col - 1]) { + maze.manhattan_distances[row][col - 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col - 1}); + reached[row][col - 1] = true; + } + + // NORTH + if (row < 15 && !maze.vertical_walls[row][col].first && maze.visited[row + 1][col] && !reached[row + 1][col]) { + maze.manhattan_distances[row + 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row + 1, col}); + reached[row + 1][col] = true; + } + + // EAST + if (col < 15 && !maze.horizontal_walls[row][col].second && maze.visited[row][col + 1] && !reached[row][col + 1]) { + maze.manhattan_distances[row][col + 1] = maze.manhattan_distances[row][col] + 1; + q.push({row, col + 1}); + reached[row][col + 1] = true; + } + + // SOUTH + if (row > 0 && !maze.vertical_walls[row][col].second && maze.visited[row - 1][col] && !reached[row - 1][col]) { + maze.manhattan_distances[row - 1][col] = maze.manhattan_distances[row][col] + 1; + q.push({row - 1, col}); + reached[row - 1][col] = true; + } + + } + for (int r = 0; r < 16; ++r) { + for (int c = 0; c < 16; ++c) { + int dist = maze.manhattan_distances[r][c]; + if (dist != 255) { + API::setText(c, r, to_string(dist)); + } + } + } +} + +int Floodfill::getNextMove(int row, int col) { + int minDist = 255; + int bestDirection = -1; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !hasWall(row, col, 0)) r++; + else if (dir == 1 && !hasWall(row, col, 1)) c++; + else if (dir == 2 && !hasWall(row, col, 2)) r--; + else if (dir == 3 && !hasWall(row, col, 3)) c--; + else continue; // skip if there's a wall + + if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + + int dist = maze.manhattan_distances[r][c]; + + if (dist < minDist || (dist == minDist && dir == curDir)) { + minDist = dist; + bestDirection = dir; + } + } + + return bestDirection; +} +int Floodfill::reverse_getNextMove(int row, int col) { + int minDist = 255; + int bestDirection = -1; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !hasWall(row, col, 0)) r++; + else if (dir == 1 && !hasWall(row, col, 1)) c++; + else if (dir == 2 && !hasWall(row, col, 2)) r--; + else if (dir == 3 && !hasWall(row, col, 3)) c--; + else continue; // skip if there's a wall + + if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + + int dist = maze.reverse_manhattan_distances[r][c]; + + if (dist < minDist || (dist == minDist && dir == curDir)) { + minDist = dist; + bestDirection = dir; + } + } + + return bestDirection; +} +Action rotateTo(int newDir) { + int diff = (newDir - curDir + 4) % 4; + if (diff == 0) return FORWARD; + if (diff == 1) return RIGHT; + if (diff == 3) return LEFT; + if (diff == 2) return AROUND; // U-turn + return IDLE; +} +void moveForwardUpdatePos() { + if (curDir == 0) curRow++; + if (curDir == 1) curCol++; + if (curDir == 2) curRow--; + if (curDir == 3) curCol--; + +} +Action solver() { + floodfill.detectWalls({}, curRow, curCol, curDir); + if (!reachedCenter && ( + (curRow == 8 && curCol == 8) || + (curRow == 8 && curCol == 7) || + (curRow == 7 && curCol == 8) || + (curRow == 7 && curCol == 7))) { + reachedCenter = true; + std::cerr << "Reached center! Recomputing best path to start...\n"; + } + int bestDir=0; + if(reachedCenter){ + floodfill.floodfillToStart(); + floodfill.floodfill(); + bestDir = floodfill.reverse_getNextMove(curRow, curCol); + } + else { + floodfill.floodfill(); + bestDir = floodfill.getNextMove(curRow, curCol); + } + Action action = rotateTo(bestDir); + std::cerr << "Moved to: (" << curRow << ", " << curCol << ")" << std::endl; + //API::setColor(curRow, curCol, 'g'); + std::cerr << "Action decided: " << action << std::endl; + if (action == IDLE) { + std::cerr << "No valid move found, staying idle." << std::endl; + } + return action; +} + +void log(const string& text) { + cerr << text << endl; +} + +int main(int argc, char* argv[]) { + log("Running..."); + while (true) { + + if (reachedCenter && curRow == 0 && curCol == 0) { + std::cerr << "Returned to start!\n"; + log("Goal reached!"); + std::cerr << "\n==== MMS SIMULATOR STATS ====" << std::endl; + std::cerr << "Total Distance : " << getStat("total-distance") << std::endl; + std::cerr << "Total Turns : " << getStat("total-turns") << std::endl; + std::cerr << "Best Run Distance : " << getStat("best-run-distance") << std::endl; + std::cerr << "Best Run Turns : " << getStat("best-run-turns") << std::endl; + std::cerr << "Current Run Distance : " << getStat("current-run-distance") << std::endl; + std::cerr << "Current Run Turns : " << getStat("current-run-turns") << std::endl; + std::cerr << "Total Effective Distance : " << getStat("total-effective-distance") << std::endl; + std::cerr << "Best Run Effective Distance : " << getStat("best-run-effective-distance") << std::endl; + std::cerr << "Current Run Effective Dist : " << getStat("current-run-effective-distance") << std::endl; + std::cerr << "Final Score : " << 2000 - getStat("score") << std::endl; + std::cerr << "Highest Possible score is 2000 " << std::endl; + + API::turnRight(); + API::turnRight(); + curDir = (curDir + 2) % 4; + + // Highlight explored shortest path from start to center using known walls only + //floodfill.final_floodfill(); + int row = 0, col = 0; + API::setColor(col, row, 'C'); // Highlight starting point + + while (!atGoal(row, col)) { + int currentDist = floodfill.maze.manhattan_distances[row][col]; + int nextRow = row, nextCol = col; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !floodfill.hasWall(row, col, 0)) r++; + else if (dir == 1 && !floodfill.hasWall(row, col, 1)) c++; + else if (dir == 2 && !floodfill.hasWall(row, col, 2)) r--; + else if (dir == 3 && !floodfill.hasWall(row, col, 3)) c--; + else continue; + + if (r >= 0 && r < 16 && c >= 0 && c < 16 && floodfill.maze.visited[r][c] && floodfill.maze.manhattan_distances[r][c] < currentDist) { + nextRow = r; + nextCol = c; + break; + } + } + // If after checking all directions, we didn't move (still at same cell), path is blocked + if (row == nextRow && col == nextCol) { + log("Path to goal is blocked or incomplete. Re-run the floodfill..."); + rerun = true; //rerun the robot through the calculated shorter path + // Try to find a new path if the current one is blocked + reachedCenter = false; + goto path_blocked; + } + + row = nextRow; + col = nextCol; + API::setColor(col, row, 'C'); // Color path step + } + break; + } + path_blocked: + if (!reachedCenter && atGoal(curRow, curCol)) { + log("Reached center. Reversing goal to return home..."); + reachedCenter = true; + } + if(rerun && atGoal(curRow, curCol)) { + // Highlight explored shortest path from start to center using known walls only + floodfill.final_floodfill(); + int row = 0, col = 0; + API::setColor(col, row, 'C'); // Highlight starting point + while (!atGoal(row, col)) { + int currentDist = floodfill.maze.manhattan_distances[row][col]; + int nextRow = row, nextCol = col; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !floodfill.hasWall(row, col, 0)) r++; + else if (dir == 1 && !floodfill.hasWall(row, col, 1)) c++; + else if (dir == 2 && !floodfill.hasWall(row, col, 2)) r--; + else if (dir == 3 && !floodfill.hasWall(row, col, 3)) c--; + else continue; + + if (r >= 0 && r < 16 && c >= 0 && c < 16 && floodfill.maze.manhattan_distances[r][c] < currentDist) { + nextRow = r; + nextCol = c; + break; + } + } + + row = nextRow; + col = nextCol; + API::setColor(col, row, 'C'); // Color path step + } + break; + } + + Action action = solver(); + + if (action == FORWARD) { + API::moveForward(); + API::setColor(curCol, curRow, 'c'); + floodfill.maze.visited[curRow][curCol] = true; + API::setText(curCol, curRow, to_string(floodfill.maze.manhattan_distances[curRow][curCol])); + moveForwardUpdatePos(); + + + + } else if (action == LEFT) { + API::turnLeft(); + curDir = (curDir + 3) % 4; + } else if (action == RIGHT) { + API::turnRight(); + curDir = (curDir + 1) % 4; + } else if (action == AROUND) { + API::turnRight(); + API::turnRight(); + curDir = (curDir + 2) % 4; + } + + } + return 0; +} diff --git a/lib/Floodfill/mms src/Floodfill_SearchRun.cpp b/lib/Floodfill/mms src/Floodfill_SearchRun.cpp new file mode 100644 index 0000000..95c8632 --- /dev/null +++ b/lib/Floodfill/mms src/Floodfill_SearchRun.cpp @@ -0,0 +1,257 @@ +// Micromouse floodfill maze-solving algorithm +// Compatible with MMS Simulator +// Written in C++ for robotics competitions +#include "API.h" +#include +#include +#include +#include +#include +#include "Floodfill.h" +#include + +using namespace std; +Floodfill floodfill; + +enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND}; + +const int SIZE = 16; + +int curRow = 0, curCol = 0, curDir = 0; // 0=north,1=east,2=south,3=west + +float getStat(const std::string& statName) { + std::cout << "getStat " << statName << std::endl; + std::string response; + std::cin >> response; + try { + return std::stof(response); // handles both int and float + } catch (...) { + std::cerr << "Failed to parse stat: " << statName << std::endl; + return -1; + } +} + +bool atGoal(int row, int col) { + return (row == 7 || row == 8) && (col == 7 || col == 8); +} + +void Floodfill::setWall(int row, int col, int direction) { + if(direction == 0) { + maze.vertical_walls[row][col].first = 1; // North wall + if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above + } + if(direction == 1) { + maze.horizontal_walls[row][col].second = 1; // East wall + if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right + } + if(direction == 2) { + maze.vertical_walls[row][col].second = 1; // South wall + if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below + } + if(direction == 3) { + maze.horizontal_walls[row][col].first = 1; // West wall + if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left + } + char cdir = "nesw"[direction]; + API::setWall(col, row, cdir); +} +void Floodfill::detectWalls(vector sensorDistances, int row, int col, int direction){ + if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds + if(API::wallFront()) { // North Wall + setWall(row, col, direction); + // Serial.print("Wall detected at "); + // Serial.print(direction); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[2]); + } + if(API::wallLeft()) { + setWall(row, col, (direction + 3) % 4); + // Serial.print("Wall detected at "); + // Serial.print((direction + 3) % 4); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[0]); + } + //if(sensorDistances[1] < wall_threshhold) setWall(row, col, direction); + if(API::wallRight()) { + setWall(row, col, (direction + 1) % 4); + // Serial.print("Wall detected at "); + // Serial.print((direction + 1) % 4); + // Serial.print(" with distance: "); + // Serial.println(sensorDistances[4]); + } +} +bool Floodfill::hasWall(int row, int col, int dir) { + if(dir == 0) return maze.vertical_walls[row][col].first; // North wall + if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall + if(dir == 2) return maze.vertical_walls[row][col].second; // South wall + if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall + return false; // No walls +} + +void Floodfill::floodfill() { + queue> q; + + for (auto& row : maze.manhattan_distances) + row.fill(255); // Re-initialize all distances to a large number + + maze.manhattan_distances[8][8] = 0; //row,column + maze.manhattan_distances[8][7] = 0; + maze.manhattan_distances[7][8] = 0; + maze.manhattan_distances[7][7] = 0; // Goal + + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + + array, 16> reached = {}; + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + + // BFS to fill the manhattan distances + while(!q.empty()){ + pair cell = q.front(); + q.pop(); + + if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds + + if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left + maze.manhattan_distances[cell.first][cell.second - 1] = maze.manhattan_distances[cell.first][cell.second] + 1; + q.push({cell.first, cell.second - 1}); + reached[cell.first][cell.second - 1] = true; // Marking the cell as reached + } + if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above + maze.manhattan_distances[cell.first + 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1; + q.push({cell.first + 1, cell.second}); + reached[cell.first + 1][cell.second] = true; // Marking the cell as reached + } + if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right + maze.manhattan_distances[cell.first][cell.second + 1] = maze.manhattan_distances[cell.first][cell.second] + 1; + q.push({cell.first, cell.second + 1}); + reached[cell.first][cell.second + 1] = true; // Marking the cell as reached + } + if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below + maze.manhattan_distances[cell.first - 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1; + q.push({cell.first - 1, cell.second}); + reached[cell.first - 1][cell.second] = true; // Marking the cell as reached + } + } + + for (int r = 0; r < 16; ++r) { + for (int c = 0; c < 16; ++c) { + int dist = maze.manhattan_distances[r][c]; + if (dist != 255) { + API::setText(c, r, to_string(dist)); // NOTE: col=x, row=y + } + } + } +} + +int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) { + int minDist = 255; + int bestDirection = -1; + + // int nextRow = row; + // int nextCol = col; + + if(!hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) { + minDist = maze.manhattan_distances[row + 1][col]; + bestDirection = 0; + } + if(!hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) { + minDist = maze.manhattan_distances[row][col + 1]; + bestDirection = 1; + } + if(!hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) { + minDist = maze.manhattan_distances[row - 1][col]; + bestDirection = 2; + } + if(!hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) { + minDist = maze.manhattan_distances[row][col - 1]; + bestDirection = 3; + } + + // Serial.print("Let's move to the direction coded as "); + // Serial.println(bestDirection); + cout << "Let's move to the direction coded as " << bestDirection << endl; + + return bestDirection; // Return the best direction to move based on the minimum distance +} + + +Action rotateTo(int newDir) { + int diff = (newDir - curDir + 4) % 4; + if (diff == 0) return FORWARD; + if (diff == 1) return RIGHT; + if (diff == 3) return LEFT; + if (diff == 2) return AROUND; // U-turn + return IDLE; +} + + +void moveForwardUpdatePos() { + if (curDir == 0) curRow++; + if (curDir == 1) curCol++; + if (curDir == 2) curRow--; + if (curDir == 3) curCol--; + +} + + +Action solver() { + floodfill.detectWalls({}, curRow, curCol, curDir); + floodfill.floodfill(); + int bestDir = floodfill.getNextMove(curRow, curCol); + + Action action = rotateTo(bestDir); + std::cerr << "Moved to: (" << curRow << ", " << curCol << ")" << std::endl; + //API::setColor(curRow, curCol, 'g'); + std::cerr << "Action decided: " << action << std::endl; + if (action == IDLE) { + std::cerr << "No valid move found, staying idle." << std::endl; + } + return action; +} + +void log(const string& text) { + cerr << text << endl; +} +int main(int argc, char* argv[]) { + log("Running..."); + while (true) { + + if (atGoal(curRow, curCol)) { + log("Goal reached!"); + std::cerr << "\n==== MMS SIMULATOR STATS ====" << std::endl; + std::cerr << "Total Distance : " << getStat("total-distance") << std::endl; + std::cerr << "Total Turns : " << getStat("total-turns") << std::endl; + std::cerr << "Best Run Distance : " << getStat("best-run-distance") << std::endl; + std::cerr << "Best Run Turns : " << getStat("best-run-turns") << std::endl; + std::cerr << "Current Run Distance : " << getStat("current-run-distance") << std::endl; + std::cerr << "Current Run Turns : " << getStat("current-run-turns") << std::endl; + std::cerr << "Total Effective Distance : " << getStat("total-effective-distance") << std::endl; + std::cerr << "Best Run Effective Distance : " << getStat("best-run-effective-distance") << std::endl; + std::cerr << "Current Run Effective Dist : " << getStat("current-run-effective-distance") << std::endl; + std::cerr << "Final Score : " << 2000 - getStat("score") << std::endl; + std::cerr << "Highest Possible score is 2000 " << std::endl; + break; + } + + Action action = solver(); + + if (action == FORWARD) { + API::moveForward(); + API::setColor(curCol, curRow, 'c'); + API::setText(curCol, curRow, to_string(floodfill.maze.manhattan_distances[curRow][curCol])); + moveForwardUpdatePos(); + } else if (action == LEFT) { + API::turnLeft(); + curDir = (curDir + 3) % 4; + } else if (action == RIGHT) { + API::turnRight(); + curDir = (curDir + 1) % 4; + } else if (action == AROUND) { + API::turnRight(); + API::turnRight(); + curDir = (curDir + 2) % 4; + } + + } + return 0; +} diff --git a/lib/Floodfill/src/Floodfill.h b/lib/Floodfill/src/Floodfill.h index 336a106..e492baa 100644 --- a/lib/Floodfill/src/Floodfill.h +++ b/lib/Floodfill/src/Floodfill.h @@ -10,6 +10,8 @@ struct grid { array, 16>, 16> horizontal_walls; // (left,right) array, 16>, 16> vertical_walls; // (up,down) array, 16> manhattan_distances = {}; // assigned false at the beginning + array, 16> reverse_manhattan_distances = {}; // assigned false at the beginning + array, 16> visited{}; }; class Floodfill { @@ -18,16 +20,22 @@ class Floodfill { int wall_threshhold = 120; int direction; int row, col; - bool front, left, right; + bool front, left, right; + public: grid maze; // void setThreshhold(int threshhold) {wall_threshhold = threshhold;} + bool RobotDone = false; + void setWall(int row, int col, int direction); void detectWalls(vector sensorDistances, int row, int col, int direction); - void floodfill(); + void floodfill(std::array, 16> &manhattan_distances, int goalRow, int goalCol); + void floodfillToStart(); bool hasWall(int row, int col, int dir); int getNextMove(int row, int col); //next row column will also be automatically update from this function + int reverse_getNextMove(int row, int col); //next row column will also be automatically update from this function bool atGoal(int row, int col); //check if robot came to the goal + bool isRobotDone(); }; //both detectWall and updateWall do the same thing but with different parameters #endif diff --git a/lib/Floodfill/src/Floodfill_SearchRun.cpp b/lib/Floodfill/src/Floodfill_SearchRun.cpp index c3c52d1..2debb98 100644 --- a/lib/Floodfill/src/Floodfill_SearchRun.cpp +++ b/lib/Floodfill/src/Floodfill_SearchRun.cpp @@ -20,6 +20,7 @@ extern std::vector getDistances(); // returns VL6180X distances: [left,..., enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND }; static int curRow = 0, curCol = 0, curDir = 0; // 0=N,1=E,2=S,3=W static bool reachedCenter = false; + static Floodfill floodfill; // solver instance bool Floodfill::atGoal(int row, int col) { @@ -67,16 +68,24 @@ bool Floodfill::hasWall(int row, int col, int dir) { return false; // No walls } -void Floodfill::floodfill() { +void Floodfill::floodfill(array, 16> &dist, int goalRow, int goalCol) { queue> q; - for (auto& row : maze.manhattan_distances) - row.fill(255); // Re-initialize all distances to a large number + // maze.manhattan_distances[8][8] = 0; //row,column + // maze.manhattan_distances[8][7] = 0; + // maze.manhattan_distances[7][8] = 0; + // maze.manhattan_distances[7][7] = 0; // Goal + + dist[goalRow][goalCol] = 0; + dist[goalRow][goalCol+1] = 0; + dist[goalRow+1][goalCol] = 0; + dist[goalRow+1][goalCol+1] = 0; - maze.manhattan_distances[8][8] = 0; //row,column - maze.manhattan_distances[8][7] = 0; - maze.manhattan_distances[7][8] = 0; - maze.manhattan_distances[7][7] = 0; // Goal + maze.visited[8][8] = true; + maze.visited[8][7] = true; + maze.visited[7][8] = true; + maze.visited[7][7] = true; + maze.visited[0][0] = true; q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); @@ -91,30 +100,73 @@ void Floodfill::floodfill() { if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left - maze.manhattan_distances[cell.first][cell.second - 1] = maze.manhattan_distances[cell.first][cell.second] + 1; + dist[cell.first][cell.second - 1] = dist[cell.first][cell.second] + 1; q.push({cell.first, cell.second - 1}); reached[cell.first][cell.second - 1] = true; // Marking the cell as reached } if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above - maze.manhattan_distances[cell.first + 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1; + dist[cell.first + 1][cell.second] = dist[cell.first][cell.second] + 1; q.push({cell.first + 1, cell.second}); reached[cell.first + 1][cell.second] = true; // Marking the cell as reached } if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right - maze.manhattan_distances[cell.first][cell.second + 1] = maze.manhattan_distances[cell.first][cell.second] + 1; + dist[cell.first][cell.second + 1] = dist[cell.first][cell.second] + 1; q.push({cell.first, cell.second + 1}); reached[cell.first][cell.second + 1] = true; // Marking the cell as reached } if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below - maze.manhattan_distances[cell.first - 1][cell.second] = maze.manhattan_distances[cell.first][cell.second] + 1; + dist[cell.first - 1][cell.second] = dist[cell.first][cell.second] + 1; q.push({cell.first - 1, cell.second}); reached[cell.first - 1][cell.second] = true; // Marking the cell as reached } } +} + +void Floodfill::floodfillToStart() { + queue> q; + + // New goal is the start: bottom-left of the maze + maze.reverse_manhattan_distances[0][0] = 0; + q.push({0, 0}); + + array, 16> reached = {}; + reached[0][0] = true; + + while (!q.empty()) { + pair cell = q.front(); + q.pop(); + + int row = cell.first; + int col = cell.second; + + if (row < 0 || row > 15 || col < 0 || col > 15) continue; + + // WEST + if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) { + maze.reverse_manhattan_distances[row][col - 1] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row, col - 1}); + reached[row][col - 1] = true; + } + + // NORTH + if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) { + maze.reverse_manhattan_distances[row + 1][col] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row + 1, col}); + reached[row + 1][col] = true; + } + + // EAST + if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) { + maze.reverse_manhattan_distances[row][col + 1] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row, col + 1}); + reached[row][col + 1] = true; + } - for (int r = 0; r < 16; ++r) { - for (int c = 0; c < 16; ++c) { - int dist = maze.manhattan_distances[r][c]; + // SOUTH + if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) { + maze.reverse_manhattan_distances[row - 1][col] = maze.reverse_manhattan_distances[row][col] + 1; + q.push({row - 1, col}); + reached[row - 1][col] = true; } } } @@ -145,7 +197,31 @@ int Floodfill::getNextMove(int row, int col /*int direction //for optimize movem return bestDirection; // Return the best direction to move based on the minimum distance } +int Floodfill::reverse_getNextMove(int row, int col) { + int minDist = 255; + int bestDirection = -1; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !hasWall(row, col, 0)) r++; + else if (dir == 1 && !hasWall(row, col, 1)) c++; + else if (dir == 2 && !hasWall(row, col, 2)) r--; + else if (dir == 3 && !hasWall(row, col, 3)) c--; + else continue; // skip if there's a wall + if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + + int dist = maze.reverse_manhattan_distances[r][c]; + + if (dist < minDist || (dist == minDist && dir == curDir)) { + minDist = dist; + bestDirection = dir; + } + } + + return bestDirection; +} // --- Solver helpers and main loop (preserve your structure) --- static Action rotateTo(int newDir) { @@ -173,10 +249,15 @@ static Action solver() { // If you want double-search (return to start after center), toggle reachedCenter here if (!reachedCenter && floodfill.atGoal(curRow, curCol)) { reachedCenter = true; // center reached (optional behavior) + while(1); + } + else if( reachedCenter && floodfill.atGoal(curRow, curCol)) { + // Reached start again; stop moving + floodfill.RobotDone = true; } // Compute floodfill and choose next direction - floodfill.floodfill(); + floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); int bestDir = floodfill.getNextMove(curRow, curCol); return rotateTo(bestDir); } @@ -206,3 +287,6 @@ void runFloodfillStep() { return; } } +bool isRobotDone() { + return floodfill.RobotDone; + } \ No newline at end of file diff --git a/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h b/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h index d01bfa1..8980822 100644 --- a/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h +++ b/lib/MotorPIDbyNJ/src/MotorPIDbyNJ.h @@ -22,7 +22,7 @@ class MotorPIDbyNJ { bool isDone; long target; int tolerance = 5; // 5mm Tolerance for PID control, can be adjusted - long metricConverter = 27; // Conversion factor for encoder value, can be adjusted (27.6 encoder = 1 mm) + long metricConverter = 24.4; // Conversion factor for encoder value, can be adjusted (24.4 encoder = 1 mm) public: MotorPIDbyNJ(int pin1, int pin2, int encoderPin1, int encoderPin2); diff --git a/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp b/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp index e468ca4..11dc163 100644 --- a/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp +++ b/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp @@ -29,6 +29,10 @@ int RobotNavigatorV2::calculateWallPID() { Serial.println("Error: Not enough sensor data"); return 0; // or handle error } + int leftSensor = sensorDistances[0]; + int rightSensor = sensorDistances[4]; + if(leftSensor > 80) leftSensor = 45; + if(rightSensor > 80) rightSensor = 45; // float wallError = (constrain(sensorDistances[0], 0, 100) - constrain(sensorDistances[4], 0, 100)); // long currentTime = micros(); @@ -48,8 +52,8 @@ int RobotNavigatorV2::calculateWallPID() { // previousSensorError = wallError; // previousSensorTime = currentTime; // return WallPID; - int difference = constrain(sensorDistances[0]-sensorDistances[4], -80, 80); - return difference; + + return leftSensor - rightSensor; } void RobotNavigatorV2::moveForward() { @@ -76,8 +80,8 @@ void RobotNavigatorV2::moveForward() { moving = false; } else{ - speedL = constrain((leftEncoderPID - currentWallPID/5),-255,255); - speedR = constrain((rightEncoderPID + currentWallPID/5),-255,255); + speedL = constrain((leftEncoderPID - currentWallPID/2),-255,255); + speedR = constrain((rightEncoderPID + currentWallPID/2),-255,255); leftMotor->runMotor(speedL); rightMotor->runMotor(speedR); } @@ -93,20 +97,20 @@ void RobotNavigatorV2::moveForward() { void RobotNavigatorV2::turnLeft() { unsigned long moveStartTime = millis(); - if(!moving) { - Serial.println("Turning Left"); - centerInCell(); - resetEncoders(); - - float yaw = imu->getYaw(); - while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw - delay(10); - yaw = imu->getYaw(); - } - imu->setTargetYaw(yaw - 82); // Adjust target yaw for left turn edited for 82 degrees - moving = true; - cellDone = false; + //if(!moving) { + Serial.println("Turning Left"); + centerInCell(); + resetEncoders(); + + float yaw = imu->getYaw(); + while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw + delay(10); + yaw = imu->getYaw(); } + imu->setTargetYaw(yaw - 86);// Adjust target yaw for left turn edited for 82 degrees + moving = true; + cellDone = false; + //} while(moving){ //centerInCell(); int currentAnglePID = imu->calculateAnglePID(); @@ -130,20 +134,20 @@ void RobotNavigatorV2::turnLeft() { } void RobotNavigatorV2::turnRight() { unsigned long moveStartTime = millis(); - if(!moving) { - Serial.println("Turning Right"); - - centerInCell(); - resetEncoders(); - float yaw = imu->getYaw(); - while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw - delay(10); - yaw = imu->getYaw(); - } - imu->setTargetYaw(yaw + 82); // Adjust target yaw for right turn - moving = true; - cellDone = false; + + Serial.println("Turning Right"); + + centerInCell(); + resetEncoders(); + float yaw = imu->getYaw(); + while (abs(yaw) < 0.1) { // or another threshold for "invalid" yaw + delay(10); + yaw = imu->getYaw(); } + imu->setTargetYaw(yaw + 86); // Adjust target yaw for right turn + moving = true; + cellDone = false; + while(moving){ //centerInCell(); @@ -167,49 +171,49 @@ void RobotNavigatorV2::turnRight() { } } -int RobotNavigatorV2::centeringPID() { - sensorDistances = sensorGroup->readAll(); - if (sensorDistances.size() < 5) { - Serial.println("Error: Not enough sensor data"); - return 0; // or handle error - } - //float wallError = (constrain(sensorDistances[0], 0, 100) - constrain(sensorDistances[4], 0, 100)); - float wallError = sensorDistances[4]; - - long currentTime = millis(); - float deltaTime = ((float)(currentTime - previousSensorTime)) / 1000.0; +// int RobotNavigatorV2::centeringPID() { +// sensorDistances = sensorGroup->readAll(); +// if (sensorDistances.size() < 5) { +// Serial.println("Error: Not enough sensor data"); +// return 0; // or handle error +// } +// //float wallError = (constrain(sensorDistances[0], 0, 100) - constrain(sensorDistances[4], 0, 100)); +// float wallError = sensorDistances[4]; + +// long currentTime = millis(); +// float deltaTime = ((float)(currentTime - previousSensorTime)) / 1000.0; - // Calculate derivative - float derivative = (wallError - previousSensorError) / deltaTime; +// // Calculate derivative +// float derivative = (wallError - previousSensorError) / deltaTime; - // Calculate integral - integralWallError += wallError * deltaTime; - integralWallError = constrain(integralWallError, -300, 300); // Clamp integral +// // Calculate integral +// integralWallError += wallError * deltaTime; +// integralWallError = constrain(integralWallError, -300, 300); // Clamp integral - // Calculate PID output - float wallPID = wallKp * wallError + wallKi * integralWallError + wallKd * derivative; +// // Calculate PID output +// float wallPID = wallKp * wallError + wallKi * integralWallError + wallKd * derivative; - previousSensorError = wallError; - previousSensorTime = currentTime; - return wallPID; -} +// previousSensorError = wallError; +// previousSensorTime = currentTime; +// return wallPID; +// } void RobotNavigatorV2::centerInCell() { // Only center if there is a wall on at least one side sensorDistances = sensorGroup->readAll(); //bool leftWall = sensorDistances[0] < 80; // adjust threshold as needed //bool rightWall = sensorDistances[4] < 80; // adjust threshold as needed - bool frontWall = sensorDistances[2] < 100; // adjust threshold as needed + bool frontWall = sensorDistances[2] < 120; // adjust threshold as needed Serial.println("Front Wall Distance: " + String(sensorDistances[2])); if (!frontWall) return; // No wall to reference Serial.println("Front Wall is in: " + String(sensorDistances[2])+" mm"); - if(sensorDistances[2] > 30 && sensorDistances[2] < 50) return; + if(sensorDistances[2] > 25 && sensorDistances[2] < 40) return; Serial.println("Centering in cell..."); resetEncoders(); unsigned long startTime = millis(); - while (millis() - startTime < 1600 && (sensorDistances[2] < 30 || sensorDistances[2] > 50)) { // Center for up to 1.6s, adjust as needed - int speed = (sensorDistances[2] - 40)*2; // target distance is 40mm, adjust as needed + while (millis() - startTime < 1600 && (sensorDistances[2] < 25 || sensorDistances[2] > 40)) { // Center for up to 1.6s, adjust as needed + int speed = (sensorDistances[2] - 33)*2; // target distance is 33mm, adjust as needed speed = constrain(speed, -150, 150); // limit speed leftMotor->runMotor(speed); rightMotor->runMotor(speed); diff --git a/src/src_main/main.cpp b/src/src_main/main.cpp index ecae1fa..f8b4a87 100644 --- a/src/src_main/main.cpp +++ b/src/src_main/main.cpp @@ -8,6 +8,9 @@ // Forward-declare the floodfill step function defined in the Floodfill module void runFloodfillStep(); +bool isRobotDone(); // defined in Floodfill module + +// Floodfill solver instance // Hardware instances int xshutPins[] = {32, 17, 16, 15, 4}; @@ -50,6 +53,11 @@ void setup() { void loop() { // Run one solver step each tick so loop stays responsive - runFloodfillStep(); - delay(20); + if (!isRobotDone()) { + runFloodfillStep(); + delay(20); + } } + + +//hi \ No newline at end of file From 65edeebcc599b53f7fc3e4ed5b799a834f568d56 Mon Sep 17 00:00:00 2001 From: Nadeesha Jayamanne Date: Sat, 4 Oct 2025 18:29:46 +0530 Subject: [PATCH 2/4] Backtracking --- lib/Floodfill/src/Floodfill.h | 6 +- lib/Floodfill/src/Floodfill_SearchRun.cpp | 102 +++---- lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp | 50 ++-- note.cpp | 262 ++++++++++++++++++ note2.cpp | 262 ++++++++++++++++++ src/src_main/main.cpp | 11 +- src/src_test/main.cpp | 36 ++- 7 files changed, 616 insertions(+), 113 deletions(-) create mode 100644 note.cpp create mode 100644 note2.cpp diff --git a/lib/Floodfill/src/Floodfill.h b/lib/Floodfill/src/Floodfill.h index e492baa..2a2bdd5 100644 --- a/lib/Floodfill/src/Floodfill.h +++ b/lib/Floodfill/src/Floodfill.h @@ -26,16 +26,16 @@ class Floodfill { grid maze; // void setThreshhold(int threshhold) {wall_threshhold = threshhold;} bool RobotDone = false; - + void setWall(int row, int col, int direction); void detectWalls(vector sensorDistances, int row, int col, int direction); void floodfill(std::array, 16> &manhattan_distances, int goalRow, int goalCol); - void floodfillToStart(); bool hasWall(int row, int col, int dir); int getNextMove(int row, int col); //next row column will also be automatically update from this function int reverse_getNextMove(int row, int col); //next row column will also be automatically update from this function bool atGoal(int row, int col); //check if robot came to the goal - bool isRobotDone(); + + void floodfillToStart(); }; //both detectWall and updateWall do the same thing but with different parameters #endif diff --git a/lib/Floodfill/src/Floodfill_SearchRun.cpp b/lib/Floodfill/src/Floodfill_SearchRun.cpp index 2debb98..6551b1b 100644 --- a/lib/Floodfill/src/Floodfill_SearchRun.cpp +++ b/lib/Floodfill/src/Floodfill_SearchRun.cpp @@ -1,7 +1,3 @@ -// Micromouse floodfill maze-solving algorithm (on-robot variant) -// Removes MMS Simulator API usage and main(); integrates with robot by: -// 1) Detecting walls from sensor distances (threshold-based) -// 2) Providing floodfill() and getNextMove() you can call from your code #include #include #include @@ -70,16 +66,26 @@ bool Floodfill::hasWall(int row, int col, int dir) { void Floodfill::floodfill(array, 16> &dist, int goalRow, int goalCol) { queue> q; - + for (auto &row : dist) { + row.fill(200); + } // maze.manhattan_distances[8][8] = 0; //row,column // maze.manhattan_distances[8][7] = 0; // maze.manhattan_distances[7][8] = 0; // maze.manhattan_distances[7][7] = 0; // Goal - - dist[goalRow][goalCol] = 0; - dist[goalRow][goalCol+1] = 0; - dist[goalRow+1][goalCol] = 0; - dist[goalRow+1][goalCol+1] = 0; + array, 16> reached = {}; + if(!reachedCenter){ + dist[7][7] = 0; + dist[7][8] = 0; + dist[8][7] = 0; + dist[8][8] = 0; + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; + } else { + dist[0][0] = 0; + q.push({0, 0}); + reached[0][0] = true; + } maze.visited[8][8] = true; maze.visited[8][7] = true; @@ -87,10 +93,10 @@ void Floodfill::floodfill(array, 16> &dist, int goalRow, int goal maze.visited[7][7] = true; maze.visited[0][0] = true; - q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + // q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); - array, 16> reached = {}; - reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + // array, 16> reached = {}; + // reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached // BFS to fill the manhattan distances while(!q.empty()){ @@ -122,55 +128,6 @@ void Floodfill::floodfill(array, 16> &dist, int goalRow, int goal } } -void Floodfill::floodfillToStart() { - queue> q; - - // New goal is the start: bottom-left of the maze - maze.reverse_manhattan_distances[0][0] = 0; - q.push({0, 0}); - - array, 16> reached = {}; - reached[0][0] = true; - - while (!q.empty()) { - pair cell = q.front(); - q.pop(); - - int row = cell.first; - int col = cell.second; - - if (row < 0 || row > 15 || col < 0 || col > 15) continue; - - // WEST - if (col > 0 && !maze.horizontal_walls[row][col].first && !reached[row][col - 1]) { - maze.reverse_manhattan_distances[row][col - 1] = maze.reverse_manhattan_distances[row][col] + 1; - q.push({row, col - 1}); - reached[row][col - 1] = true; - } - - // NORTH - if (row < 15 && !maze.vertical_walls[row][col].first && !reached[row + 1][col]) { - maze.reverse_manhattan_distances[row + 1][col] = maze.reverse_manhattan_distances[row][col] + 1; - q.push({row + 1, col}); - reached[row + 1][col] = true; - } - - // EAST - if (col < 15 && !maze.horizontal_walls[row][col].second && !reached[row][col + 1]) { - maze.reverse_manhattan_distances[row][col + 1] = maze.reverse_manhattan_distances[row][col] + 1; - q.push({row, col + 1}); - reached[row][col + 1] = true; - } - - // SOUTH - if (row > 0 && !maze.vertical_walls[row][col].second && !reached[row - 1][col]) { - maze.reverse_manhattan_distances[row - 1][col] = maze.reverse_manhattan_distances[row][col] + 1; - q.push({row - 1, col}); - reached[row - 1][col] = true; - } - } -} - int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) { int minDist = 255; int bestDirection = -1; @@ -249,17 +206,26 @@ static Action solver() { // If you want double-search (return to start after center), toggle reachedCenter here if (!reachedCenter && floodfill.atGoal(curRow, curCol)) { reachedCenter = true; // center reached (optional behavior) - while(1); + return IDLE; } - else if( reachedCenter && floodfill.atGoal(curRow, curCol)) { - // Reached start again; stop moving + else if(reachedCenter && curRow == 0 && curCol == 0) { floodfill.RobotDone = true; + return IDLE; } // Compute floodfill and choose next direction - floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); - int bestDir = floodfill.getNextMove(curRow, curCol); - return rotateTo(bestDir); + if (!reachedCenter) { + // search phase → goal is center + floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); + int bestDir = floodfill.getNextMove(curRow, curCol); + return rotateTo(bestDir); + } else { + // return phase → goal is start + floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0); + int bestDir = floodfill.reverse_getNextMove(curRow, curCol); + return rotateTo(bestDir); + } + } // Perform a single floodfill decision + action; call repeatedly from loop() diff --git a/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp b/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp index 11dc163..ed528a6 100644 --- a/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp +++ b/lib/RobotNavigatorV2/src/RobotNavigatorV2.cpp @@ -224,29 +224,29 @@ void RobotNavigatorV2::centerInCell() { Serial.println("Centered."); } -void RobotNavigatorV2::turnAround() { - if(!moving) { - Serial.println("Turning Around"); - resetEncoders(); - imu->setTargetYaw(imu->getYaw() - 180); // Adjust target yaw for left turn - moving = true; - cellDone = false; - } - while(moving){ +// void RobotNavigatorV2::turnAround() { +// if(!moving) { +// Serial.println("Turning Around"); +// resetEncoders(); +// imu->setTargetYaw(imu->getYaw() - 180); // Adjust target yaw for left turn +// moving = true; +// cellDone = false; +// } +// while(moving){ - int currentAnglePID = imu->calculateAnglePID(); - Serial.println("Current Angle PID: " + String(currentAnglePID)); - - if(imu->checkDone() && imu->checkDone()) { - Serial.println("Turn Done"); - cellDone = true; - moving = false; - } - else{ - speedL = constrain((0 - currentAnglePID),-255,255); - speedR = constrain((0 + currentAnglePID),-255,255); - leftMotor->runMotor(speedL); - rightMotor->runMotor(speedR); - } - } -} \ No newline at end of file +// int currentAnglePID = imu->calculateAnglePID(); +// Serial.println("Current Angle PID: " + String(currentAnglePID)); + +// if(imu->checkDone() && imu->checkDone()) { +// Serial.println("Turn Done"); +// cellDone = true; +// moving = false; +// } +// else{ +// speedL = constrain((0 - currentAnglePID),-255,255); +// speedR = constrain((0 + currentAnglePID),-255,255); +// leftMotor->runMotor(speedL); +// rightMotor->runMotor(speedR); +// } +// } +// } \ No newline at end of file diff --git a/note.cpp b/note.cpp new file mode 100644 index 0000000..9fc295a --- /dev/null +++ b/note.cpp @@ -0,0 +1,262 @@ +// Micromouse floodfill maze-solving algorithm (on-robot variant) +// Removes MMS Simulator API usage and main(); integrates with robot by: +// 1) Detecting walls from sensor distances (threshold-based) +// 2) Providing floodfill() and getNextMove() you can call from your code +#include +#include +#include +#include +#include "Floodfill.h" + +using namespace std; + +// Extern primitives to be implemented in main.cpp +extern void moveForward(); +extern void turnLeft(); +extern void turnRight(); +extern std::vector getDistances(); // returns VL6180X distances: [left,...,front,...,right] + +// Action and robot pose/state (kept here so main.cpp stays minimal) +enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND }; +static int curRow = 0, curCol = 0, curDir = 0; // 0=N,1=E,2=S,3=W +static bool reachedCenter = false; + +static Floodfill floodfill; // solver instance + +bool Floodfill::atGoal(int row, int col) { + return (row == 7 || row == 8) && (col == 7 || col == 8); +} + +void Floodfill::setWall(int row, int col, int direction) { + if(direction == 0) { + maze.vertical_walls[row][col].first = 1; // North wall + if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above + } + if(direction == 1) { + maze.horizontal_walls[row][col].second = 1; // East wall + if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right + } + if(direction == 2) { + maze.vertical_walls[row][col].second = 1; // South wall + if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below + } + if(direction == 3) { + maze.horizontal_walls[row][col].first = 1; // West wall + if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left + } + // On-robot: no external API call here +} +void Floodfill::detectWalls(vector sensorDistances, int row, int col, int direction){ + if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds + if (sensorDistances[2] >= 0 && sensorDistances[2] < wall_threshhold) { //front + setWall(row, col, direction); + } + if (sensorDistances[0] >= 0 && sensorDistances[0] < wall_threshhold) { //left + setWall(row, col, (direction + 3) % 4); + } + if (sensorDistances[4] >= 0 && sensorDistances[4] < wall_threshhold) { //right + setWall(row, col, (direction + 1) % 4); + } +} + + +bool Floodfill::hasWall(int row, int col, int dir) { + if(dir == 0) return maze.vertical_walls[row][col].first; // North wall + if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall + if(dir == 2) return maze.vertical_walls[row][col].second; // South wall + if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall + return false; // No walls +} + +void Floodfill::floodfill(array, 16> &dist) { + queue> q; + for (auto &row : dist) { + row.fill(200); + } + // maze.manhattan_distances[8][8] = 0; //row,column + // maze.manhattan_distances[8][7] = 0; + // maze.manhattan_distances[7][8] = 0; + // maze.manhattan_distances[7][7] = 0; // Goal + array, 16> reached = {}; + if(!reachedCenter){ + dist[7][7] = 0; + dist[7][8] = 0; + dist[8][7] = 0; + dist[8][8] = 0; + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; + } else { + dist[0][0] = 0; + q.push({0, 0}); + reached[0][0] = true; + } + + maze.visited[8][8] = true; + maze.visited[8][7] = true; + maze.visited[7][8] = true; + maze.visited[7][7] = true; + maze.visited[0][0] = true; + + //q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + + + //reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + + // BFS to fill the manhattan distances + while(!q.empty()){ + pair cell = q.front(); + q.pop(); + + if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds + + if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left + dist[cell.first][cell.second - 1] = dist[cell.first][cell.second] + 1; + q.push({cell.first, cell.second - 1}); + reached[cell.first][cell.second - 1] = true; // Marking the cell as reached + } + if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above + dist[cell.first + 1][cell.second] = dist[cell.first][cell.second] + 1; + q.push({cell.first + 1, cell.second}); + reached[cell.first + 1][cell.second] = true; // Marking the cell as reached + } + if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right + dist[cell.first][cell.second + 1] = dist[cell.first][cell.second] + 1; + q.push({cell.first, cell.second + 1}); + reached[cell.first][cell.second + 1] = true; // Marking the cell as reached + } + if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below + dist[cell.first - 1][cell.second] = dist[cell.first][cell.second] + 1; + q.push({cell.first - 1, cell.second}); + reached[cell.first - 1][cell.second] = true; // Marking the cell as reached + } + } +} + +int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) { + int minDist = 255; + int bestDirection = -1; + + // int nextRow = row; + // int nextCol = col; + + if(row < 15 && !hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) { + minDist = maze.manhattan_distances[row + 1][col]; + bestDirection = 0; + } + if(col < 15 && !hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) { + minDist = maze.manhattan_distances[row][col + 1]; + bestDirection = 1; + } + if(row > 0 && !hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) { + minDist = maze.manhattan_distances[row - 1][col]; + bestDirection = 2; + } + if(col > 0 && !hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) { + minDist = maze.manhattan_distances[row][col - 1]; + bestDirection = 3; + } + + return bestDirection; // Return the best direction to move based on the minimum distance +} +int Floodfill::reverse_getNextMove(int row, int col) { + int minDist = 255; + int bestDirection = -1; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !hasWall(row, col, 0)) r++; + else if (dir == 1 && !hasWall(row, col, 1)) c++; + else if (dir == 2 && !hasWall(row, col, 2)) r--; + else if (dir == 3 && !hasWall(row, col, 3)) c--; + else continue; // skip if there's a wall + + if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + + int dist = maze.reverse_manhattan_distances[r][c]; + + if (dist < minDist || (dist == minDist && dir == curDir)) { + minDist = dist; + bestDirection = dir; + } + } + + return bestDirection; +} + +// --- Solver helpers and main loop (preserve your structure) --- +static Action rotateTo(int newDir) { + int diff = (newDir - curDir + 4) % 4; + if (diff == 0) return FORWARD; + if (diff == 1) return RIGHT; + if (diff == 3) return LEFT; + if (diff == 2) return AROUND; + return IDLE; +} + +void moveForwardUpdatePos() { + if (curDir == 0) curRow++; + if (curDir == 1) curCol++; + if (curDir == 2) curRow--; + if (curDir == 3) curCol--; + +} + +static Action solver() { + // Read sensors from main and update walls + std::vector distances = getDistances(); + floodfill.detectWalls(distances, curRow, curCol, curDir); + int bestDir = -1; + + // If you want double-search (return to start after center), toggle reachedCenter here + if (!reachedCenter && floodfill.atGoal(curRow, curCol)) { + reachedCenter = true; // center reached (optional behavior) + //while(1); + } + // else if(reachedCenter) { + // floodfill.floodfill(floodfill.maze.reverse_manhattan_distances); + // //floodfill.floodfill(floodfill.maze.manhattan_distances); + // bestDir = floodfill.reverse_getNextMove(curRow, curCol); + // } + else{ + // Compute floodfill and choose next direction + floodfill.floodfill(floodfill.maze.manhattan_distances); + int bestDir = floodfill.getNextMove(curRow, curCol); + + + // Reached start again; stop moving + //floodfill.RobotDone = true; + } + + + return rotateTo(bestDir); +} + +// Perform a single floodfill decision + action; call repeatedly from loop() +void runFloodfillStep() { + // if (floodfill.atGoal(curRow, curCol)) return; // optional early exit + + Action action = solver(); + + if (action == FORWARD) { + moveForward(); + moveForwardUpdatePos(); // Update pose used by solver + } else if (action == LEFT) { + turnLeft(); + curDir = (curDir + 3) % 4; + } else if (action == RIGHT) { + turnRight(); + curDir = (curDir + 1) % 4; + } else if (action == AROUND) { + // keep structure identical to your sample + turnRight(); + turnRight(); + curDir = (curDir + 2) % 4; + } else { + // IDLE or no valid move; nothing to do this tick + return; + } +} +bool isRobotDone() { + return floodfill.RobotDone; + } \ No newline at end of file diff --git a/note2.cpp b/note2.cpp new file mode 100644 index 0000000..0515b1a --- /dev/null +++ b/note2.cpp @@ -0,0 +1,262 @@ +// Micromouse floodfill maze-solving algorithm (on-robot variant) +// Removes MMS Simulator API usage and main(); integrates with robot by: +// 1) Detecting walls from sensor distances (threshold-based) +// 2) Providing floodfill() and getNextMove() you can call from your code +#include +#include +#include +#include +#include "Floodfill.h" + +using namespace std; + +// Extern primitives to be implemented in main.cpp +extern void moveForward(); +extern void turnLeft(); +extern void turnRight(); +extern std::vector getDistances(); // returns VL6180X distances: [left,...,front,...,right] + +// Action and robot pose/state (kept here so main.cpp stays minimal) +enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND }; +static int curRow = 0, curCol = 0, curDir = 0; // 0=N,1=E,2=S,3=W +static bool reachedCenter = false; + +static Floodfill floodfill; // solver instance + +bool Floodfill::atGoal(int row, int col) { + return (row == 7 || row == 8) && (col == 7 || col == 8); +} + +void Floodfill::setWall(int row, int col, int direction) { + if(direction == 0) { + maze.vertical_walls[row][col].first = 1; // North wall + if(row+1 < 16) maze.vertical_walls[row + 1][col].second = 1; // South wall of the cell above + } + if(direction == 1) { + maze.horizontal_walls[row][col].second = 1; // East wall + if(col + 1 < 16) maze.horizontal_walls[row][col + 1].first = 1; // West wall of the cell to the right + } + if(direction == 2) { + maze.vertical_walls[row][col].second = 1; // South wall + if(row - 1 >= 0) maze.vertical_walls[row - 1][col].first = 1; // North wall of the cell below + } + if(direction == 3) { + maze.horizontal_walls[row][col].first = 1; // West wall + if(col - 1 >= 0) maze.horizontal_walls[row][col - 1].second = 1; // East wall of the cell to the left + } + // On-robot: no external API call here +} +void Floodfill::detectWalls(vector sensorDistances, int row, int col, int direction){ + if(row < 0 || row >= 16 || col < 0 || col >= 16) return; // out of bounds + if (sensorDistances[2] >= 0 && sensorDistances[2] < wall_threshhold) { //front + setWall(row, col, direction); + } + if (sensorDistances[0] >= 0 && sensorDistances[0] < wall_threshhold) { //left + setWall(row, col, (direction + 3) % 4); + } + if (sensorDistances[4] >= 0 && sensorDistances[4] < wall_threshhold) { //right + setWall(row, col, (direction + 1) % 4); + } +} + + +bool Floodfill::hasWall(int row, int col, int dir) { + if(dir == 0) return maze.vertical_walls[row][col].first; // North wall + if(dir == 1) return maze.horizontal_walls[row][col].second; // East wall + if(dir == 2) return maze.vertical_walls[row][col].second; // South wall + if(dir == 3) return maze.horizontal_walls[row][col].first; // West wall + return false; // No walls +} + +void Floodfill::floodfill(array, 16> &dist, int goalRow, int goalCol) { + queue> q; + for (auto &row : dist) { + row.fill(200); + } + // maze.manhattan_distances[8][8] = 0; //row,column + // maze.manhattan_distances[8][7] = 0; + // maze.manhattan_distances[7][8] = 0; + // maze.manhattan_distances[7][7] = 0; // Goal + array, 16> reached = {}; + if(!reachedCenter){ + dist[7][7] = 0; + dist[7][8] = 0; + dist[8][7] = 0; + dist[8][8] = 0; + q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; + } else { + dist[0][0] = 0; + q.push({0, 0}); + reached[0][0] = true; + } + + maze.visited[8][8] = true; + maze.visited[8][7] = true; + maze.visited[7][8] = true; + maze.visited[7][7] = true; + maze.visited[0][0] = true; + + // q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + + // array, 16> reached = {}; + // reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + + // BFS to fill the manhattan distances + while(!q.empty()){ + pair cell = q.front(); + q.pop(); + + if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds + + if(cell.second > 0 && !maze.horizontal_walls[cell.first][cell.second].first && !reached[cell.first][cell.second - 1]) {// No wall to the left + dist[cell.first][cell.second - 1] = dist[cell.first][cell.second] + 1; + q.push({cell.first, cell.second - 1}); + reached[cell.first][cell.second - 1] = true; // Marking the cell as reached + } + if(cell.first < 15 && !maze.vertical_walls[cell.first][cell.second].first && !reached[cell.first + 1][cell.second]) {// No wall above + dist[cell.first + 1][cell.second] = dist[cell.first][cell.second] + 1; + q.push({cell.first + 1, cell.second}); + reached[cell.first + 1][cell.second] = true; // Marking the cell as reached + } + if(cell.second < 15 && !maze.horizontal_walls[cell.first][cell.second].second && !reached[cell.first][cell.second + 1]) {// No wall to the right + dist[cell.first][cell.second + 1] = dist[cell.first][cell.second] + 1; + q.push({cell.first, cell.second + 1}); + reached[cell.first][cell.second + 1] = true; // Marking the cell as reached + } + if(cell.first > 0 && !maze.vertical_walls[cell.first][cell.second].second && !reached[cell.first - 1][cell.second]) {// No wall below + dist[cell.first - 1][cell.second] = dist[cell.first][cell.second] + 1; + q.push({cell.first - 1, cell.second}); + reached[cell.first - 1][cell.second] = true; // Marking the cell as reached + } + } +} + +int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) { + int minDist = 255; + int bestDirection = -1; + + // int nextRow = row; + // int nextCol = col; + + if(row < 15 && !hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) { + minDist = maze.manhattan_distances[row + 1][col]; + bestDirection = 0; + } + if(col < 15 && !hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) { + minDist = maze.manhattan_distances[row][col + 1]; + bestDirection = 1; + } + if(row > 0 && !hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) { + minDist = maze.manhattan_distances[row - 1][col]; + bestDirection = 2; + } + if(col > 0 && !hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) { + minDist = maze.manhattan_distances[row][col - 1]; + bestDirection = 3; + } + + return bestDirection; // Return the best direction to move based on the minimum distance +} +int Floodfill::reverse_getNextMove(int row, int col) { + int minDist = 255; + int bestDirection = -1; + + for (int dir = 0; dir < 4; ++dir) { + int r = row, c = col; + + if (dir == 0 && !hasWall(row, col, 0)) r++; + else if (dir == 1 && !hasWall(row, col, 1)) c++; + else if (dir == 2 && !hasWall(row, col, 2)) r--; + else if (dir == 3 && !hasWall(row, col, 3)) c--; + else continue; // skip if there's a wall + + if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + + int dist = maze.reverse_manhattan_distances[r][c]; + + if (dist < minDist || (dist == minDist && dir == curDir)) { + minDist = dist; + bestDirection = dir; + } + } + + return bestDirection; +} + +// --- Solver helpers and main loop (preserve your structure) --- +static Action rotateTo(int newDir) { + int diff = (newDir - curDir + 4) % 4; + if (diff == 0) return FORWARD; + if (diff == 1) return RIGHT; + if (diff == 3) return LEFT; + if (diff == 2) return AROUND; + return IDLE; +} + +void moveForwardUpdatePos() { + if (curDir == 0) curRow++; + if (curDir == 1) curCol++; + if (curDir == 2) curRow--; + if (curDir == 3) curCol--; + +} + +static Action solver() { + // Read sensors from main and update walls + std::vector distances = getDistances(); + floodfill.detectWalls(distances, curRow, curCol, curDir); + + // If you want double-search (return to start after center), toggle reachedCenter here + if (!reachedCenter && floodfill.atGoal(curRow, curCol)) { + reachedCenter = true; // center reached (optional behavior) + while(1); + } + else if( reachedCenter && floodfill.atGoal(curRow, curCol)) { + // Reached start again; stop moving + floodfill.RobotDone = true; + } + + // Compute floodfill and choose next direction + if (!reachedCenter) { + // search phase → goal is center + floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); + int bestDir = floodfill.getNextMove(curRow, curCol); + return rotateTo(bestDir); + } else { + // return phase → goal is start + floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0); + int bestDir = floodfill.reverse_getNextMove(curRow, curCol); + return rotateTo(bestDir); + } + +} + +// Perform a single floodfill decision + action; call repeatedly from loop() +void runFloodfillStep() { + // if (floodfill.atGoal(curRow, curCol)) return; // optional early exit + + Action action = solver(); + + if (action == FORWARD) { + moveForward(); + moveForwardUpdatePos(); // Update pose used by solver + } else if (action == LEFT) { + turnLeft(); + curDir = (curDir + 3) % 4; + } else if (action == RIGHT) { + turnRight(); + curDir = (curDir + 1) % 4; + } else if (action == AROUND) { + // keep structure identical to your sample + turnRight(); + turnRight(); + curDir = (curDir + 2) % 4; + } else { + // IDLE or no valid move; nothing to do this tick + return; + } +} +bool isRobotDone() { + return floodfill.RobotDone; + } \ No newline at end of file diff --git a/src/src_main/main.cpp b/src/src_main/main.cpp index f8b4a87..c16e88c 100644 --- a/src/src_main/main.cpp +++ b/src/src_main/main.cpp @@ -14,7 +14,7 @@ bool isRobotDone(); // defined in Floodfill module // Hardware instances int xshutPins[] = {32, 17, 16, 15, 4}; -int sensorCorrections[] = { 6, 16, 0, 43, 26}; // mm to subtract from each sensor +int sensorCorrections[] = { 0, 16, 0, 43, 26}; // mm to subtract from each sensor VL6180XManagerV2 sensorGroup(xshutPins, 5, sensorCorrections); MotorPIDbyNJ leftMotor(25, 26, 18, 5); @@ -53,11 +53,12 @@ void setup() { void loop() { // Run one solver step each tick so loop stays responsive - if (!isRobotDone()) { + // if (!isRobotDone()) { + // runFloodfillStep(); + // delay(20); + // } + runFloodfillStep(); delay(20); - } } - -//hi \ No newline at end of file diff --git a/src/src_test/main.cpp b/src/src_test/main.cpp index b9db8fd..4ceb7f0 100644 --- a/src/src_test/main.cpp +++ b/src/src_test/main.cpp @@ -6,7 +6,7 @@ #include int xshutPins[] = {32, 17, 16, 15, 4}; -int sensorCorrections[] = { 6, 16, 0, 43, 26}; // mm to subtract from each sensor +int sensorCorrections[] = { 0, 16, 0, 43, 26}; // mm to subtract from each sensor VL6180XManagerV2 sensorGroup(xshutPins, 5, sensorCorrections); //Floodfill solveMaze; @@ -53,21 +53,33 @@ void loop() { // runFloodfillRobot(); // testMoveDone = true; // } - if (!testMoveDone) { + // sensorGroup.readAll(); + // Serial.println("Trying Serial.."); + // Serial.println("Sensor 1: " + String(sensorGroup.readAll()[0])); + // Serial.println("Sensor 2: " + String(sensorGroup.readAll()[1])); + // Serial.println("Sensor 3: " + String(sensorGroup.readAll()[2])); + // Serial.println("Sensor 4: " + String(sensorGroup.readAll()[3])); + // Serial.println("Sensor 5: " + String(sensorGroup.readAll()[4])); + delay(400); - //Motors.turnLeft(); + gyro.getYaw(); + Serial.println("Yaw: " + String(gyro.getYaw())); - //Motors.moveForward(); - //Motors.moveForward(); - Motors.turnRight(); - Motors.moveForward(); - Motors.turnRight(); + // if (!testMoveDone) { + + // //Motors.turnLeft(); + + // //Motors.moveForward(); + // //Motors.moveForward(); + // Motors.turnRight(); Motors.moveForward(); - Motors.turnRight(); + // Motors.turnRight(); + // Motors.moveForward(); + // Motors.turnRight(); - testMoveDone = true; - Serial.println("Exited"); - } + // testMoveDone = true; + // Serial.println("Exited"); + // } // ...existing code or idle... } From c6048bdf2c2684cb4449858e4a32716584a243cc Mon Sep 17 00:00:00 2001 From: Nadeesha Jayamanne Date: Sat, 4 Oct 2025 19:21:56 +0530 Subject: [PATCH 3/4] fully functioning robot goes from start to goal and back to start in a new path --- lib/Floodfill/src/Floodfill.h | 2 +- lib/Floodfill/src/Floodfill_SearchRun.cpp | 88 ++++++++++++++--------- src/src_main/main.cpp | 7 -- 3 files changed, 57 insertions(+), 40 deletions(-) diff --git a/lib/Floodfill/src/Floodfill.h b/lib/Floodfill/src/Floodfill.h index 2a2bdd5..57da0e5 100644 --- a/lib/Floodfill/src/Floodfill.h +++ b/lib/Floodfill/src/Floodfill.h @@ -31,7 +31,7 @@ class Floodfill { void detectWalls(vector sensorDistances, int row, int col, int direction); void floodfill(std::array, 16> &manhattan_distances, int goalRow, int goalCol); bool hasWall(int row, int col, int dir); - int getNextMove(int row, int col); //next row column will also be automatically update from this function + int getNextMove(array, 16> &dist,int row, int col); //next row column will also be automatically update from this function int reverse_getNextMove(int row, int col); //next row column will also be automatically update from this function bool atGoal(int row, int col); //check if robot came to the goal diff --git a/lib/Floodfill/src/Floodfill_SearchRun.cpp b/lib/Floodfill/src/Floodfill_SearchRun.cpp index 6551b1b..d1d2c2d 100644 --- a/lib/Floodfill/src/Floodfill_SearchRun.cpp +++ b/lib/Floodfill/src/Floodfill_SearchRun.cpp @@ -128,57 +128,80 @@ void Floodfill::floodfill(array, 16> &dist, int goalRow, int goal } } -int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) { +int Floodfill::getNextMove(array, 16> &dist,int row, int col /*int direction //for optimize movement(for later improvement)*/) { int minDist = 255; int bestDirection = -1; // int nextRow = row; // int nextCol = col; - if(row < 15 && !hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) { - minDist = maze.manhattan_distances[row + 1][col]; + if(row < 15 && !hasWall(row, col, 0) && dist[row + 1][col] < minDist) { + minDist = dist[row + 1][col]; bestDirection = 0; } - if(col < 15 && !hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) { - minDist = maze.manhattan_distances[row][col + 1]; + if(col < 15 && !hasWall(row, col, 1) && dist[row][col + 1] < minDist) { + minDist = dist[row][col + 1]; bestDirection = 1; } - if(row > 0 && !hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) { - minDist = maze.manhattan_distances[row - 1][col]; + if(row > 0 && !hasWall(row, col, 2) && dist[row - 1][col] < minDist) { + minDist = dist[row - 1][col]; bestDirection = 2; } - if(col > 0 && !hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) { - minDist = maze.manhattan_distances[row][col - 1]; + if(col > 0 && !hasWall(row, col, 3) && dist[row][col - 1] < minDist) { + minDist = dist[row][col - 1]; bestDirection = 3; } return bestDirection; // Return the best direction to move based on the minimum distance -} -int Floodfill::reverse_getNextMove(int row, int col) { - int minDist = 255; - int bestDirection = -1; + // int minDist = 255; + // int bestDirection = -1; - for (int dir = 0; dir < 4; ++dir) { - int r = row, c = col; + // for (int dir = 0; dir < 4; ++dir) { + // int r = row, c = col; - if (dir == 0 && !hasWall(row, col, 0)) r++; - else if (dir == 1 && !hasWall(row, col, 1)) c++; - else if (dir == 2 && !hasWall(row, col, 2)) r--; - else if (dir == 3 && !hasWall(row, col, 3)) c--; - else continue; // skip if there's a wall + // if (dir == 0 && !hasWall(row, col, 0)) r++; + // else if (dir == 1 && !hasWall(row, col, 1)) c++; + // else if (dir == 2 && !hasWall(row, col, 2)) r--; + // else if (dir == 3 && !hasWall(row, col, 3)) c--; + // else continue; // skip if there's a wall - if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + // if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; - int dist = maze.reverse_manhattan_distances[r][c]; + // int dist = maze.manhattan_distances[r][c]; - if (dist < minDist || (dist == minDist && dir == curDir)) { - minDist = dist; - bestDirection = dir; - } - } + // if (dist < minDist || (dist == minDist && dir == curDir)) { + // minDist = dist; + // bestDirection = dir; + // } + // } - return bestDirection; + // return bestDirection; } +// int Floodfill::reverse_getNextMove(int row, int col) { +// int minDist = 255; +// int bestDirection = -1; + +// for (int dir = 0; dir < 4; ++dir) { +// int r = row, c = col; + +// if (dir == 0 && !hasWall(row, col, 0)) r++; +// else if (dir == 1 && !hasWall(row, col, 1)) c++; +// else if (dir == 2 && !hasWall(row, col, 2)) r--; +// else if (dir == 3 && !hasWall(row, col, 3)) c--; +// else continue; // skip if there's a wall + +// if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + +// int dist = maze.reverse_manhattan_distances[r][c]; + +// if (dist < minDist || (dist == minDist && dir == curDir)) { +// minDist = dist; +// bestDirection = dir; +// } +// } + +// return bestDirection; +// } // --- Solver helpers and main loop (preserve your structure) --- static Action rotateTo(int newDir) { @@ -216,16 +239,17 @@ static Action solver() { // Compute floodfill and choose next direction if (!reachedCenter) { // search phase → goal is center - floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); - int bestDir = floodfill.getNextMove(curRow, curCol); - return rotateTo(bestDir); + floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); + int bestDir = floodfill.getNextMove(floodfill.maze.manhattan_distances, curRow, curCol); + return rotateTo(bestDir); } else { // return phase → goal is start floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0); - int bestDir = floodfill.reverse_getNextMove(curRow, curCol); + int bestDir = floodfill.getNextMove(floodfill.maze.reverse_manhattan_distances, curRow, curCol); return rotateTo(bestDir); } + } // Perform a single floodfill decision + action; call repeatedly from loop() diff --git a/src/src_main/main.cpp b/src/src_main/main.cpp index c16e88c..99151ce 100644 --- a/src/src_main/main.cpp +++ b/src/src_main/main.cpp @@ -30,7 +30,6 @@ void updateRightEncoder() { rightMotor.updateEncoder(); } void moveForward() { Motors.moveForward(); } void turnLeft() { Motors.turnLeft(); } void turnRight() { Motors.turnRight(); } -void turnAround() { Motors.turnAround(); } void moveForwardUpdatePos(); // defined in Floodfill module now does pose update std::vector getDistances() { return sensorGroup.readAll(); } @@ -52,12 +51,6 @@ void setup() { } void loop() { - // Run one solver step each tick so loop stays responsive - // if (!isRobotDone()) { - // runFloodfillStep(); - // delay(20); - // } - runFloodfillStep(); delay(20); } From 461d2ac534a9dae0ad98359837e90780431a86e4 Mon Sep 17 00:00:00 2001 From: Nadeesha Jayamanne Date: Sat, 4 Oct 2025 22:37:52 +0530 Subject: [PATCH 4/4] minor changes --- lib/Floodfill/src/Floodfill.h | 1 + lib/Floodfill/src/Floodfill_SearchRun.cpp | 38 +++++-- note.cpp | 128 +++++++++++++--------- src/src_main/main.cpp | 19 +++- 4 files changed, 122 insertions(+), 64 deletions(-) diff --git a/lib/Floodfill/src/Floodfill.h b/lib/Floodfill/src/Floodfill.h index 57da0e5..641d0cd 100644 --- a/lib/Floodfill/src/Floodfill.h +++ b/lib/Floodfill/src/Floodfill.h @@ -26,6 +26,7 @@ class Floodfill { grid maze; // void setThreshhold(int threshhold) {wall_threshhold = threshhold;} bool RobotDone = false; + bool LastRun = false; void setWall(int row, int col, int direction); void detectWalls(vector sensorDistances, int row, int col, int direction); diff --git a/lib/Floodfill/src/Floodfill_SearchRun.cpp b/lib/Floodfill/src/Floodfill_SearchRun.cpp index d1d2c2d..8b9a67d 100644 --- a/lib/Floodfill/src/Floodfill_SearchRun.cpp +++ b/lib/Floodfill/src/Floodfill_SearchRun.cpp @@ -15,7 +15,9 @@ extern std::vector getDistances(); // returns VL6180X distances: [left,..., // Action and robot pose/state (kept here so main.cpp stays minimal) enum Action { FORWARD, LEFT, RIGHT, IDLE, AROUND }; static int curRow = 0, curCol = 0, curDir = 0; // 0=N,1=E,2=S,3=W -static bool reachedCenter = false; +bool reachedCenter = false; +bool FinalRun = false; +bool readyToFinalRun = false; static Floodfill floodfill; // solver instance @@ -228,34 +230,46 @@ static Action solver() { // If you want double-search (return to start after center), toggle reachedCenter here if (!reachedCenter && floodfill.atGoal(curRow, curCol)) { - reachedCenter = true; // center reached (optional behavior) + reachedCenter = true; // center reached (optional behavior) return IDLE; } else if(reachedCenter && curRow == 0 && curCol == 0) { floodfill.RobotDone = true; + floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); + reachedCenter = false; // ensure we are in return mode + readyToFinalRun = true; return IDLE; } // Compute floodfill and choose next direction - if (!reachedCenter) { + if (!reachedCenter && !readyToFinalRun) { // search phase → goal is center floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); int bestDir = floodfill.getNextMove(floodfill.maze.manhattan_distances, curRow, curCol); return rotateTo(bestDir); - } else { + } else if(!FinalRun){ // return phase → goal is start - floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0); + floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0); int bestDir = floodfill.getNextMove(floodfill.maze.reverse_manhattan_distances, curRow, curCol); return rotateTo(bestDir); } - + else{ + //Floodfill is being calculated once when the robot came back to the start(in that if nest) + //Final Run phase → goal is center + int bestDir = floodfill.getNextMove(floodfill.maze.manhattan_distances, curRow, curCol); + + if (floodfill.atGoal(curRow, curCol)) { + floodfill.LastRun = true; // finished final run + return IDLE; + } + } } // Perform a single floodfill decision + action; call repeatedly from loop() void runFloodfillStep() { // if (floodfill.atGoal(curRow, curCol)) return; // optional early exit - + Action action = solver(); if (action == FORWARD) { @@ -279,4 +293,14 @@ void runFloodfillStep() { } bool isRobotDone() { return floodfill.RobotDone; + } +bool BeginFinalRun() { + if(!FinalRun) { + FinalRun = true; + curRow = curCol = 0; // reset robot position + curDir = 0; + reachedCenter = false; // ensure we are in return mode + return true; + } + return false; } \ No newline at end of file diff --git a/note.cpp b/note.cpp index 9fc295a..3ecfa01 100644 --- a/note.cpp +++ b/note.cpp @@ -1,7 +1,3 @@ -// Micromouse floodfill maze-solving algorithm (on-robot variant) -// Removes MMS Simulator API usage and main(); integrates with robot by: -// 1) Detecting walls from sensor distances (threshold-based) -// 2) Providing floodfill() and getNextMove() you can call from your code #include #include #include @@ -68,7 +64,7 @@ bool Floodfill::hasWall(int row, int col, int dir) { return false; // No walls } -void Floodfill::floodfill(array, 16> &dist) { +void Floodfill::floodfill(array, 16> &dist, int goalRow, int goalCol) { queue> q; for (auto &row : dist) { row.fill(200); @@ -90,22 +86,22 @@ void Floodfill::floodfill(array, 16> &dist) { q.push({0, 0}); reached[0][0] = true; } - + maze.visited[8][8] = true; maze.visited[8][7] = true; maze.visited[7][8] = true; maze.visited[7][7] = true; maze.visited[0][0] = true; - //q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); + // q.push({8, 8}); q.push({8, 7}); q.push({7, 8}); q.push({7, 7}); - - //reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached + // array, 16> reached = {}; + // reached[8][8] = reached[8][7] = reached[7][8] = reached[7][7] = true; // Marking the goal cells as reached // BFS to fill the manhattan distances while(!q.empty()){ pair cell = q.front(); - q.pop(); + q.pop(); if(cell.first > 15 || cell.second > 15 || cell.first < 0 || cell.second < 0) continue; // out of bounds @@ -132,57 +128,80 @@ void Floodfill::floodfill(array, 16> &dist) { } } -int Floodfill::getNextMove(int row, int col /*int direction //for optimize movement(for later improvement)*/) { +int Floodfill::getNextMove(array, 16> &dist,int row, int col /*int direction //for optimize movement(for later improvement)*/) { int minDist = 255; int bestDirection = -1; // int nextRow = row; // int nextCol = col; - if(row < 15 && !hasWall(row, col, 0) && maze.manhattan_distances[row + 1][col] < minDist) { - minDist = maze.manhattan_distances[row + 1][col]; + if(row < 15 && !hasWall(row, col, 0) && dist[row + 1][col] < minDist) { + minDist = dist[row + 1][col]; bestDirection = 0; } - if(col < 15 && !hasWall(row, col, 1) && maze.manhattan_distances[row][col + 1] < minDist) { - minDist = maze.manhattan_distances[row][col + 1]; + if(col < 15 && !hasWall(row, col, 1) && dist[row][col + 1] < minDist) { + minDist = dist[row][col + 1]; bestDirection = 1; } - if(row > 0 && !hasWall(row, col, 2) && maze.manhattan_distances[row - 1][col] < minDist) { - minDist = maze.manhattan_distances[row - 1][col]; + if(row > 0 && !hasWall(row, col, 2) && dist[row - 1][col] < minDist) { + minDist = dist[row - 1][col]; bestDirection = 2; } - if(col > 0 && !hasWall(row, col, 3) && maze.manhattan_distances[row][col - 1] < minDist) { - minDist = maze.manhattan_distances[row][col - 1]; + if(col > 0 && !hasWall(row, col, 3) && dist[row][col - 1] < minDist) { + minDist = dist[row][col - 1]; bestDirection = 3; } return bestDirection; // Return the best direction to move based on the minimum distance -} -int Floodfill::reverse_getNextMove(int row, int col) { - int minDist = 255; - int bestDirection = -1; + // int minDist = 255; + // int bestDirection = -1; - for (int dir = 0; dir < 4; ++dir) { - int r = row, c = col; + // for (int dir = 0; dir < 4; ++dir) { + // int r = row, c = col; - if (dir == 0 && !hasWall(row, col, 0)) r++; - else if (dir == 1 && !hasWall(row, col, 1)) c++; - else if (dir == 2 && !hasWall(row, col, 2)) r--; - else if (dir == 3 && !hasWall(row, col, 3)) c--; - else continue; // skip if there's a wall + // if (dir == 0 && !hasWall(row, col, 0)) r++; + // else if (dir == 1 && !hasWall(row, col, 1)) c++; + // else if (dir == 2 && !hasWall(row, col, 2)) r--; + // else if (dir == 3 && !hasWall(row, col, 3)) c--; + // else continue; // skip if there's a wall - if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + // if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; - int dist = maze.reverse_manhattan_distances[r][c]; + // int dist = maze.manhattan_distances[r][c]; - if (dist < minDist || (dist == minDist && dir == curDir)) { - minDist = dist; - bestDirection = dir; - } - } + // if (dist < minDist || (dist == minDist && dir == curDir)) { + // minDist = dist; + // bestDirection = dir; + // } + // } - return bestDirection; + // return bestDirection; } +// int Floodfill::reverse_getNextMove(int row, int col) { +// int minDist = 255; +// int bestDirection = -1; + +// for (int dir = 0; dir < 4; ++dir) { +// int r = row, c = col; + +// if (dir == 0 && !hasWall(row, col, 0)) r++; +// else if (dir == 1 && !hasWall(row, col, 1)) c++; +// else if (dir == 2 && !hasWall(row, col, 2)) r--; +// else if (dir == 3 && !hasWall(row, col, 3)) c--; +// else continue; // skip if there's a wall + +// if (r < 0 || r >= 16 || c < 0 || c >= 16) continue; + +// int dist = maze.reverse_manhattan_distances[r][c]; + +// if (dist < minDist || (dist == minDist && dir == curDir)) { +// minDist = dist; +// bestDirection = dir; +// } +// } + +// return bestDirection; +// } // --- Solver helpers and main loop (preserve your structure) --- static Action rotateTo(int newDir) { @@ -206,30 +225,31 @@ static Action solver() { // Read sensors from main and update walls std::vector distances = getDistances(); floodfill.detectWalls(distances, curRow, curCol, curDir); - int bestDir = -1; // If you want double-search (return to start after center), toggle reachedCenter here if (!reachedCenter && floodfill.atGoal(curRow, curCol)) { reachedCenter = true; // center reached (optional behavior) - //while(1); + return IDLE; } - // else if(reachedCenter) { - // floodfill.floodfill(floodfill.maze.reverse_manhattan_distances); - // //floodfill.floodfill(floodfill.maze.manhattan_distances); - // bestDir = floodfill.reverse_getNextMove(curRow, curCol); - // } - else{ - // Compute floodfill and choose next direction - floodfill.floodfill(floodfill.maze.manhattan_distances); - int bestDir = floodfill.getNextMove(curRow, curCol); - - - // Reached start again; stop moving - //floodfill.RobotDone = true; + else if(reachedCenter && curRow == 0 && curCol == 0) { + floodfill.RobotDone = true; + return IDLE; } + // Compute floodfill and choose next direction + if (!reachedCenter) { + // search phase → goal is center + floodfill.floodfill(floodfill.maze.manhattan_distances, 7, 7); + int bestDir = floodfill.getNextMove(floodfill.maze.manhattan_distances, curRow, curCol); + return rotateTo(bestDir); + } else{ + // return phase → goal is start + floodfill.floodfill(floodfill.maze.reverse_manhattan_distances, 0, 0); + int bestDir = floodfill.getNextMove(floodfill.maze.reverse_manhattan_distances, curRow, curCol); + return rotateTo(bestDir); + } + - return rotateTo(bestDir); } // Perform a single floodfill decision + action; call repeatedly from loop() diff --git a/src/src_main/main.cpp b/src/src_main/main.cpp index 99151ce..04bcc56 100644 --- a/src/src_main/main.cpp +++ b/src/src_main/main.cpp @@ -6,10 +6,12 @@ #include #include "Floodfill.h" +#define FINAL_RUN_BUTTON 2 // GPIO pin for the final run button + // Forward-declare the floodfill step function defined in the Floodfill module void runFloodfillStep(); bool isRobotDone(); // defined in Floodfill module - +bool BeginFinalRun(); // Floodfill solver instance // Hardware instances @@ -48,10 +50,21 @@ void setup() { // PID values for encoder values leftMotor.setPID(3, 0.05, 0.4, 8); rightMotor.setPID(3, 0.05, 0.4, 8); + + pinMode(FINAL_RUN_BUTTON, INPUT_PULLDOWN); + pinMode(33, OUTPUT); // onboard buzzer } void loop() { - runFloodfillStep(); - delay(20); + + if (digitalRead(FINAL_RUN_BUTTON) == HIGH) { // assuming active HIGH + delay(1000); + BeginFinalRun(); + digitalWrite(33, HIGH); // turn on buzzer + delay(500); + // reset heading to North + } + runFloodfillStep(); + delay(20); }