direction coordinate modified(memory -> Decart)

.vscode/settings.json

@@ -0,0 +1,11 @@
+{
+    "files.associations": {
+        "deque": "cpp",
+        "algorithm": "cpp",
+        "vector": "cpp",
+        "xstring": "cpp",
+        "xutility": "cpp",
+        "iosfwd": "cpp",
+        "type_traits": "cpp"
+    }
+}

To do

@@ -0,0 +1,6 @@
+1. 범식이형한테 블루투스 모델 어캐 꼽는지 & 사용하는지 물어보기
+2. 각 버튼 무슨 버튼 인지
+3. 프로그램 실행 어캐 하는지 
+4. 배터리 충전
+5. JTAG연결 선꼽은다음에 돌릴 프로그램
+6. 사랑해요

firmware/src/maze/Maze.cc

@@ -251,6 +251,14 @@ void Maze::printMaze()
     mazeIO.printMaze();
 }
 
+/**
+ * @brief      print the current maze
+ */
+void Maze::printMazeWithPositions(std::vector<Position> &positions)
+{
+    mazeIO.printMazeWithPositions(positions);
+}
+
 /**
  * @brief      save the maze as file
  *

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<Position> &positions);
     void saveMazeFile(char* fileName);
 };
 

firmware/src/maze/MazeIO.cc

@@ -66,6 +66,44 @@ void MazeIO::printMaze(void)
     writeIOFromBuffer(printIO);
 }
 
+void MazeIO::printMazeWithPositions(std::vector<Position> &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) {

firmware/src/maze/MazeIO.hpp

@@ -54,6 +54,7 @@ class MazeIO
     bool positionIsDestination(Position pos);
     // Actual IO
     void printMaze(void);
+    void printMazeWithPositions(std::vector<Position> &positions);
     void loadMazeFromString(char* str);
     void loadMazeFromFile(char* fileName);
     void saveMazeToFile(char* fileName);

simulation/pathfinder/Makefile

@@ -2,7 +2,7 @@
 # Project build settings
 #######################################
 # Project name
-TARGET = pathfinder
+TARGET := pathfinder
 
 # Build path
 BUILD_DIR := build

simulation/pathfinder/main.cc

@@ -1,16 +1,34 @@
 #include "Position.hpp"
 #include "Queue.hpp"
+#include "Maze.hpp"
+#include "StdIO.hpp"
+
+#include <iostream>
 #include <vector>
 #include <stdio.h>
 #include <deque>
 
-std::vector<Position> 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<Position> 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<Position> 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<enum direction> path;
-std::deque<enum real_move> move;
+std::deque<enum direction> path; // position based path.
+
+std::deque<enum real_move> move; // smooth_path
+
+std::deque<enum real_move> diagonal_move; // diagonal path
+std::deque<enum real_move> temp_move;
+
+template<typename T>
+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;
 }