diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..041af72 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,11 @@ +{ + "files.associations": { + "deque": "cpp", + "algorithm": "cpp", + "vector": "cpp", + "xstring": "cpp", + "xutility": "cpp", + "iosfwd": "cpp", + "type_traits": "cpp" + } +} \ No newline at end of file diff --git a/To do b/To do new file mode 100644 index 0000000..54d04cf --- /dev/null +++ b/To do @@ -0,0 +1,6 @@ +1. 범식이형한테 블루투스 모델 어캐 꼽는지 & 사용하는지 물어보기 +2. 각 버튼 무슨 버튼 인지 +3. 프로그램 실행 어캐 하는지 +4. 배터리 충전 +5. JTAG연결 선꼽은다음에 돌릴 프로그램 +6. 사랑해요 diff --git a/firmware/src/maze/Maze.cc b/firmware/src/maze/Maze.cc index f179e7d..cd24b73 100644 --- a/firmware/src/maze/Maze.cc +++ b/firmware/src/maze/Maze.cc @@ -251,6 +251,14 @@ void Maze::printMaze() mazeIO.printMaze(); } +/** + * @brief print the current maze + */ +void Maze::printMazeWithPositions(std::vector &positions) +{ + mazeIO.printMazeWithPositions(positions); +} + /** * @brief save the maze as file * diff --git a/firmware/src/maze/Maze.hpp b/firmware/src/maze/Maze.hpp index 6d39812..d79b392 100644 --- a/firmware/src/maze/Maze.hpp +++ b/firmware/src/maze/Maze.hpp @@ -75,7 +75,8 @@ class Maze /* IO Related */ void readMazeFromFile(char* fileName); void readMazeFromString(StringMaze *stringMaze); - virtual void printMaze(); + virtual void printMaze(void); + virtual void printMazeWithPositions(std::vector &positions); void saveMazeFile(char* fileName); }; diff --git a/firmware/src/maze/MazeIO.cc b/firmware/src/maze/MazeIO.cc index bb8f545..ab07a85 100644 --- a/firmware/src/maze/MazeIO.cc +++ b/firmware/src/maze/MazeIO.cc @@ -66,6 +66,44 @@ void MazeIO::printMaze(void) writeIOFromBuffer(printIO); } +void MazeIO::printMazeWithPositions(std::vector &positions) +{ + if (NULL == maze) { + printf("No maze object"); + return; + } + + char *ptr = buffer; + writeBufferFromMaze(true); + /** + * Edit buffers to print positions + * it's like: + * __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ + * | 1. 2. 3. 6. 7. . . . . . . . . . . | + * .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. + * | 0| . 4. 5. 8. 9.19.11.12.13. . . . . . | + * __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ + */ + for (int i = 0; i < positions.size(); i++) { + auto const& pos = positions[i]; + char *ptr = buffer + + ((pos.row * 2 + 1) * (CONFIG_MAX_COL_SIZE * 3 + 1 + 1)) + + (pos.col * 3 + 1); + char output; + // digit of 10's + if (i >= 10) { + output = '0' + ((i / 10) % 10); + *ptr++ = output; + } else { + ptr++; + } + // digit of 1's + output = '0' + (i % 10); + *ptr = output; + } + writeIOFromBuffer(printIO); +} + void MazeIO::loadMazeFromString(char* str) { if (NULL == str) { diff --git a/firmware/src/maze/MazeIO.hpp b/firmware/src/maze/MazeIO.hpp index ae63752..eec5ab5 100644 --- a/firmware/src/maze/MazeIO.hpp +++ b/firmware/src/maze/MazeIO.hpp @@ -54,6 +54,7 @@ class MazeIO bool positionIsDestination(Position pos); // Actual IO void printMaze(void); + void printMazeWithPositions(std::vector &positions); void loadMazeFromString(char* str); void loadMazeFromFile(char* fileName); void saveMazeToFile(char* fileName); diff --git a/simulation/pathfinder/Makefile b/simulation/pathfinder/Makefile index 32f4269..9d8d72f 100644 --- a/simulation/pathfinder/Makefile +++ b/simulation/pathfinder/Makefile @@ -2,7 +2,7 @@ # Project build settings ####################################### # Project name -TARGET = pathfinder +TARGET := pathfinder # Build path BUILD_DIR := build diff --git a/simulation/pathfinder/main.cc b/simulation/pathfinder/main.cc index a5678c3..d7527c7 100644 --- a/simulation/pathfinder/main.cc +++ b/simulation/pathfinder/main.cc @@ -1,16 +1,34 @@ #include "Position.hpp" #include "Queue.hpp" +#include "Maze.hpp" +#include "StdIO.hpp" + +#include #include #include #include -std::vector pathPos = { - Position{13, 1}, Position{13, 2}, Position{12, 2}, Position{12, 1}, Position{13, 1}, Position{14, 1}, - Position{14, 2}, Position{14, 3}, Position{14, 4}, Position{13, 4}, Position{12, 4}, Position{11, 4}, - Position{10, 4}, Position{9, 4}, Position{8, 4}, Position{7, 4}, Position{7, 5}, Position{7, 6} +#define MAZE_FILE "maze_base.txt" +std::vector pathPos = { + Position{15,0}, Position{14,0}, Position{13,0}, Position{12,0}, Position{11,0}, + Position{10,0}, Position{9,0}, Position{8,0}, Position{7,0}, Position{6,0}, + Position{5,0}, Position{4,0}, Position{3,0}, Position{2,0}, Position{1,0}, + Position{0,0}, Position{0,1}, Position{0,2}, Position{0,3}, Position{0,4}, + Position{0,5}, Position{0,6}, Position{0,7}, Position{0,8}, Position{0,9}, + Position{0,10}, Position{0,11}, Position{0,12}, Position{0,13}, Position{0,14}, + Position{0,15}, Position{1,15}, Position{2,15}, Position{3,15}, Position{4,15}, + Position{5,15}, Position{6,15}, Position{7,15}, Position{7,14}, Position{8,14}, + Position{8,13}, Position{9,13}, Position{9,12}, Position{9,11}, Position{9,10}, + Position{9,9}, Position{8,9}, Position{7,9}, Position{6,9}, Position{6,10}, + Position{6,11}, Position{5,11}, Position{5,10}, Position{5,9}, Position{5,8}, + Position{4,8}, Position{3,8}, Position{3,7}, Position{4,7}, Position{4,6}, + Position{3,6}, Position{3,5}, Position{4,5}, Position{5,5}, Position{6,5}, + Position{6,4}, Position{7,4}, Position{7,5}, Position{8,5}, Position{8,4}, + Position{9,4}, Position{10,4}, Position{11,4}, Position{12,4}, Position{13,4}, + Position{13,5}, Position{12,5}, Position{11,5}, Position{10,5}, Position{9,5}, + Position{9,6}, Position{8,6} }; - Queue queue; enum direction @@ -27,11 +45,50 @@ enum real_move F = 'F', H_F = 'h', smooth_R = 'R', - smooth_L = 'L' + smooth_L = 'L', + R_dia_turn = 'r', + L_dia_turn = 'l', + diagonal_forward = 'D' }; -std::deque path; -std::deque move; +std::deque path; // position based path. + +std::deque move; // smooth_path + +std::deque diagonal_move; // diagonal path +std::deque temp_move; + +template +void printPath(std::string name, T &path) { + // print name + std::cout << name << std::endl; + + char old_dir = path.front(); + int dup_count = 0; + // Print path with duplication counting + for (auto const& dir: path) { + char new_dir = (char) dir; + if (old_dir == new_dir) { + dup_count++; + } else { + if (dup_count == 1) { + std::cout << old_dir << " -> "; + } else { + std::cout << dup_count << old_dir << " -> "; + dup_count = 1; + } + } + old_dir = new_dir; + } + // print last one + if (dup_count == 1) { + std::cout << old_dir; + } else { + std::cout << dup_count << old_dir; + } + // double newline + std::cout << std::endl << std::endl; +} int main(void) { @@ -41,13 +98,32 @@ int main(void) Position last; direction path_direction; real_move move_direction; + real_move before_direction; + real_move d_start_direction; + real_move d_end_direction; + char current_status = V; // V : vertical, H : horizontal + int cnt = 0; + int d_cnt = 1; + int swap; init.row = -1; init.col = -1; queue.init(); + + // Create a maze object only for printing purpose + StdIO fileIO(true); + StdIO printIO(false); + Maze maze(MAZE_FILE, &fileIO, &printIO); + maze.printMazeWithPositions(pathPos); + for (int i = 0; i < pathPos.size(); i++) + { + swap = (15 - (pathPos[i].row)); + pathPos[i].row = (pathPos[i].col); + pathPos[i].col = swap; + } for (int i = 0; i < pathPos.size(); i++) { queue.pushToBack(pathPos[i]); @@ -58,7 +134,6 @@ int main(void) if (init.col == -1 && init.row == -1) { - init = queue.popFromFront(); last = queue.popFromFront(); } @@ -151,16 +226,8 @@ int main(void) } } - /* - pritnf("robot path \n") - while(!path.empty()) { - printf("%c -> ",path.front()); - path.pop_front(); - } - printf("\n"); - */ - - ///////////////////// I don't delete this for just incase we need it ///////////////////// + printPath("Non-smooth", path); + //smooth turn// while (!path.empty()) { @@ -191,14 +258,112 @@ int main(void) } } - printf(" robot moving direction \n"); + printPath("Smooth", move); + + // diagonal move// + + move.push_back(F); + move.push_back(H_F); + move.push_back(smooth_R); + move.push_back(smooth_L); + move.push_back(smooth_R); + move.push_back(smooth_L); + move.push_back(H_F); while (!move.empty()) { - printf("%c -> ", move.front()); - move.pop_front(); + if (move.front() == F) + { + diagonal_move.push_back(move.front()); + move.pop_front(); + } + else if (move.front() == H_F) + { + + diagonal_move.push_back(H_F); //insert H_F + move.pop_front(); + d_start_direction = move.front(); + // before_direction = move.front(); + // d_start_direction = before_direction; + while (move.front() == smooth_R || move.front() == smooth_L) // temp_move -> inserting all nearby Left/Right moving direction + { + temp_move.push_back(move.front()); + move.pop_front(); + cnt++; + } + + if (cnt == 1) // 100% smooth turn + { + diagonal_move.push_back(temp_move.front()); + temp_move.pop_front(); + //diagonal_move.push_back(H_F); + cnt--; + } + else if (cnt > 1) // various direction + { + while (cnt >= 1) + { + before_direction = temp_move.front(); + temp_move.pop_front(); + if (before_direction != temp_move.front()) // diagonal case counting + { + d_cnt++; + before_direction = temp_move.front(); + d_end_direction = before_direction; + temp_move.pop_front(); + cnt--; + } + else if (before_direction == temp_move.front() && d_cnt != 1) // diagonal case moving direction + { + diagonal_move.push_back((d_start_direction == smooth_R) ? R_dia_turn : L_dia_turn); + + while (d_cnt != 1) + { + diagonal_move.push_back(diagonal_forward); + d_cnt--; + } + diagonal_move.push_back(diagonal_forward); // as we start at d_cnt = 1 and ends at d_cnt = 1 + diagonal_move.push_back((d_end_direction == smooth_R) ? R_dia_turn : L_dia_turn); + } + else if (before_direction == temp_move.front()) + { // smooth turn case + if ((diagonal_move.back() != L_dia_turn) && (diagonal_move.back() != R_dia_turn)) + { + diagonal_move.push_back(temp_move.front()); + temp_move.pop_front(); //////////????????? + if (temp_move.empty()) + diagonal_move.push_back(H_F); //////////????????? + cnt--; + } + else if ((diagonal_move.back()) == (L_dia_turn) || (diagonal_move.back()) == (R_dia_turn)) + //diagonal case finish + { + //diagonal_move.push_back(temp_move.front()); + //temp_move.pop_front(); //////////????????? + //diagonal_move.push_back(H_F); //////////????????? + cnt--; + } + } + + else if (temp_move.empty() && d_cnt != 1) + { + diagonal_move.push_back((d_start_direction == smooth_R) ? R_dia_turn : L_dia_turn); + while (d_cnt != 1) + { + diagonal_move.push_back(diagonal_forward); + d_cnt--; + } + diagonal_move.push_back(diagonal_forward); // as we start at d_cnt = 1 and ends at d_cnt = 1 + diagonal_move.push_back((d_end_direction == smooth_R) ? R_dia_turn : L_dia_turn); + } + } + + cnt = 0; + d_cnt = 1; + } + } } - printf("\n"); + printPath("Diagonal", diagonal_move); return 0; } \ No newline at end of file diff --git a/simulation/pathfinder/maze_base.txt b/simulation/pathfinder/maze_base.txt new file mode 100644 index 0000000..8c21f0e --- /dev/null +++ b/simulation/pathfinder/maze_base.txt @@ -0,0 +1,33 @@ + __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . G. G. . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . G. G. . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| . . . . . . . . . . . . . . . | + .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. .. +| S| . . . . . . . . . . . . . . | + __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ __ diff --git a/wolfiemouse b/wolfiemouse new file mode 160000 index 0000000..a456618 --- /dev/null +++ b/wolfiemouse @@ -0,0 +1 @@ +Subproject commit a456618b3ddcc01a142f152c8bbe0482c48bcb61