sort of working merge and code seperation

9 files changed, 123 insertions, 168 deletions

diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 27e17d9..a871b69 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -1,8 +1,8 @@
 #include "mode_chrg.h"
+#include "mode_grid.h"
 #include "orangutan_shim.h"
 #include "movement.h"
 #include "modes.h"
-#include "transition.h"
 
 int g_w2_charged_status;
 
@@ -46,13 +46,12 @@ void w2_charge_cross_walk() {
 				set_motors(0, 0);
 
 				g_w2_transition	= 0;
-				// g_w2_maze_status	= 1;
 				w2_modes_swap(W2_M_MAZE);
 				break;
 			}
 		} else {
 			g_w2_transition = 0;
-			w2_full_rotation();
+			w2_maze_rotation_full();
 		}
 	}
 }
@@ -62,7 +61,6 @@ void w2_mode_chrg() {
 	unsigned int last_proportional = 0;
 	long integral				   = 0;
 	// initialize();
-	g_w2_charged_status = 0;
 	clear();
 	print("CHARGE");
 
@@ -110,17 +108,17 @@ void w2_mode_chrg() {
 				 g_w2_charged_status == 0) {
 			w2_home();
 			delay(200);
-			w2_full_rotation();
+			w2_maze_rotation_full();
 			w2_short_drive();
 		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
 			clear();
-			w2_half_rotation_left();
+			w2_maze_rotation_half_left();
 		}
 
 		else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && g_w2_sensors[3] >= 250 &&
 				 g_w2_sensors[4] >= 500) {
 			clear();
-			w2_crossway_detection();
+			w2_maze_rotation_half_left();
 		} else {
 			if (power_difference < 0 &&
 				(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 927d4bc..c6ebef3 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -2,22 +2,36 @@
 #include "orangutan_shim.h"
 #include "modes.h"
 #include "movement.h"
-#include "transition.h"
 
 int g_w2_order_number;
 
 int g_w2_maze_status = 0;
 
-w2_s_grid_coordinate g_w2_order[4];
+w2_s_grid_coordinate g_w2_order[4] = {
+	{0, 0},
+	{1, 1},
+	{2, 2},
+	{3, 3},
+};
 w2_s_grid_coordinate g_w2_location;
 w2_s_grid_coordinate g_w2_destination;
 w2_e_orientation g_w2_direction;
 
+void w2_location_message() {
+	clear();
+	print_long(g_w2_location.x);
+	print(",");
+	print_long(g_w2_location.y);
+	delay(200); 
+}
+
 int g_w2_detection = 0;
+int g_w2_transition;
 char g_w2_x_location = 0;
 char g_w2_y_location = 0;
 
-void w2_crosswalk_detection() {
+void w2_crosswalk_stroll() {
+	print("hoi");
 	while (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && g_w2_sensors[3] < 100 &&
 		   g_w2_sensors[4] < 100) {
 		set_motors(15, 15);
@@ -33,37 +47,17 @@ void w2_crosswalk_detection() {
 				delay(600);
 				g_w2_transition	= 0;
 				w2_modes_swap(W2_M_GRID);
+				return;
 			}
 		}
 
 		else {
 			g_w2_transition = 0;
-			w2_full_rotation();
+			w2_maze_rotation_full();
 		}
 	}
 }
 
-void w2_grid_rotation_full() {
-	set_motors(60, -60);
-	delay_ms(540);
-	set_motors(10, 10);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-}
-
-void w2_grid_rotation_left() {
-	set_motors(-30, 30);
-	delay_ms(600);
-	set_motors(10, 10);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-}
-
-void w2_grid_rotation_right() {
-	set_motors(30, -30);
-	delay_ms(600);
-	set_motors(10, 10);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-}
-
 void w2_grid_crossway_detection() {
 	set_motors(50, 50);
 	delay_ms(150);
@@ -72,40 +66,41 @@ void w2_grid_crossway_detection() {
 }
 
 void w2_grid_follow_line() {
-	static unsigned int last_proportional = 0;
-	static long integral				   = 0;
+	unsigned int last_proportional=0;
+	long integral=0;
 
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
-	int proportional = ((int)g_w2_position) - 2000;
-	int derivative = proportional - last_proportional;
-	integral += proportional;
-	last_proportional = proportional;
-	int power_difference = proportional / 20 + integral / 10000 + derivative * 3 / 2;
-
-	const int max = 60;
-	if (power_difference > max) power_difference = max;
-	if (power_difference < -max) power_difference = -max;
-
-	if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 && g_w2_sensors[3] >= 250 &&
-		g_w2_sensors[4] >= 500) {
-		return;
-	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
-		return;
-	} else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 &&
-			   g_w2_sensors[0] < 100) { // for the south and west borders of the grid
-		return;
-	} else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[2] < 100 &&
-			   g_w2_sensors[0] < 100) { // sharp right corners
-		return;
-	}
+	while(1) {
+		g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+		int proportional = ((int)g_w2_position) - 2000;
+		int derivative = proportional - last_proportional;
+		integral += proportional;
+		last_proportional = proportional;
+		int power_difference = proportional/20 + integral/10000 + derivative*3/2;
 
-	else {
-		if (power_difference < 0 &&
-			(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
-			set_motors(max + power_difference, max);
-		} else if (power_difference > 0 &&
-				   (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
-			set_motors(max, max - power_difference);
+		const int max = 60;
+		if(power_difference > max)
+		power_difference = max;
+		if(power_difference < -max)
+		power_difference = -max;
+		
+		if(g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250  && g_w2_sensors[2] >= 500  &&  g_w2_sensors[3] >= 250  &&g_w2_sensors[4] >= 500){
+			break;
+		}
+		else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100){
+			break;
+		}
+		else if(g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[0] < 100){ //for the south and west borders of the grid
+			break;
+		}
+		else if(g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[2] <100 && g_w2_sensors[0] < 100){ //sharp right corners
+			break;
+		}
+
+		else{
+			if(power_difference < 0 && (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) ){
+			set_motors(max+power_difference, max);}
+			else if( power_difference > 0 && ( g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)){
+			set_motors(max, max-power_difference);}
 		}
 	}
 }
@@ -127,7 +122,7 @@ void w2_turn_north() {
 			break;
 
 		case W2_ORT_EAST:
-			w2_grid_rotation_left();
+			w2_grid_rotation_half_left();
 			break;
 
 		case W2_ORT_SOUTH:
@@ -135,7 +130,7 @@ void w2_turn_north() {
 			break;
 
 		case W2_ORT_WEST:
-			w2_grid_rotation_right();
+			w2_grid_rotation_half_right();
 			break;
 	}
 
@@ -148,7 +143,7 @@ void w2_turn_west() {
 			break;
 
 		case W2_ORT_NORTH:
-			w2_grid_rotation_left();
+			w2_grid_rotation_half_left();
 			break;
 
 		case W2_ORT_EAST:
@@ -156,7 +151,7 @@ void w2_turn_west() {
 			break;
 
 		case W2_ORT_SOUTH:
-			w2_grid_rotation_right();
+			w2_grid_rotation_half_right();
 			;
 			break;
 	}
@@ -170,7 +165,7 @@ void w2_turn_south() {
 			break;
 
 		case W2_ORT_WEST:
-			w2_grid_rotation_left();
+			w2_grid_rotation_half_left();
 			break;
 
 		case W2_ORT_NORTH:
@@ -178,7 +173,7 @@ void w2_turn_south() {
 			break;
 
 		case W2_ORT_EAST:
-			w2_grid_rotation_right();
+			w2_grid_rotation_half_right();
 			;
 			break;
 	}
@@ -192,7 +187,7 @@ void w2_turn_east() {
 			break;
 
 		case W2_ORT_SOUTH:
-			w2_grid_rotation_left();
+			w2_grid_rotation_half_left();
 			break;
 
 		case W2_ORT_WEST:
@@ -200,7 +195,7 @@ void w2_turn_east() {
 			break;
 
 		case W2_ORT_NORTH:
-			w2_grid_rotation_right();
+			w2_grid_rotation_half_right();
 			;
 			break;
 	}
@@ -273,12 +268,14 @@ void w2_mode_grid() {
 		g_w2_destination.x = g_w2_order[i].x;
 		g_w2_destination.y = g_w2_order[i].y;
 
+		w2_location_message();
 		delay(1000);
 		w2_go_to_x();
 		w2_go_to_y();
 	}
 	w2_end_destination();
 
+	w2_location_message();
 	delay(1000);
 	w2_go_to_y();
 	w2_go_to_x();
@@ -288,14 +285,9 @@ void w2_mode_grid() {
 
 /*
 void w2_mode_maze() {
-
 	unsigned int last_proportional = 0;
 	long integral				   = 0;
 
-	// set up the 3pi
-	if (g_w2_maze_status == 0) {
-		w2_initialize();
-	}
 	clear();
 	print("MAZE");
 
@@ -335,11 +327,6 @@ void w2_mode_maze() {
 
 		if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 && g_w2_sensors[3] < 100 &&
 			g_w2_sensors[4] < 100) {
-			w2_cross_walk();
-			if (g_w2_parcour_mode == gridMode) {
-				break;
-			}
-			// full_rotation();
 		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 &&
 				   g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {
 			w2_crossway_detection();
diff --git a/robot/mode_grid.h b/robot/mode_grid.h
index c8e649f..3ba3db0 100644
--- a/robot/mode_grid.h
+++ b/robot/mode_grid.h
@@ -9,7 +9,9 @@
  */
 void w2_mode_grid();
 
-void w2_crosswalk_detection();
+extern int g_w2_transition;
+
+void w2_crosswalk_stroll();
 
 typedef enum {
 	W2_ORT_NORTH,
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
index 04a3bc2..da3fa2a 100644
--- a/robot/mode_maze.c
+++ b/robot/mode_maze.c
@@ -1,7 +1,7 @@
 #include "mode_maze.h"
 #include "orangutan_shim.h"
 #include "movement.h"
-#include "transition.h"
+#include "mode_grid.h"
 
 unsigned int g_w2_last_proportional = 0;
 long g_w2_integral					= 0;
@@ -21,56 +21,13 @@ void w2_mode_maze() {
 
 	if (g_w2_sensors[0] < 100 && g_w2_sensors[1] < 100 && g_w2_sensors[2] < 100 &&
 		g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) {
-		// grid detectie
-		/*set_motors(0,0);
-		delay_ms(450);
-		set_motors(50,50);
-		delay_ms(180);
-		if ( g_w2_sensors[2] >= 100 || g_w2_sensors[3] >= 100 || g_w2_sensors[1] >= 100 ||
-		g_w2_sensors[0] >= 100 || g_w2_sensors[4] >= 100)
-		{
-			set_motors(0,0);
-			delay_ms(15000);
-			set_motors(50,50);
-			delay_ms(180);
-			if (g_w2_sensors[2] >= 100 || g_w2_sensors[3] >= 100 || g_w2_sensors[1] >= 100 ||
-		g_w2_sensors[0] >= 100
-		|| g_w2_sensors[4] >= 100 )
-			{
-				set_motors(0,0);
-				delay_ms(1500);
-				set_motors(50,50);
-				delay_ms(180);
-				if (g_w2_sensors[2] >= 100 || g_w2_sensors[3] >= 100 || g_w2_sensors[1] >= 100 ||
-		g_w2_sensors[0] >= 100 || g_w2_sensors[4] >= 100)
-				{
-					print("GRID!");
-					set_motors(0,0);
-					delay_ms(10000);
-
-				}
-			}
-
-		}
-		else if(g_w2_sensors[0] < 100 && g_w2_sensors[1] <100 && g_w2_sensors[2] < 100 &&
-		g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100){*/
-		w2_full_rotation();
-		//}
-
+		w2_crosswalk_stroll();
 	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 250 && g_w2_sensors[2] >= 500 &&
 			   g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) {
-		w2_crossway_detection();
-	}
-	// else if(g_w2_sensors[0] >= 500  && g_w2_sensors[2] < 50 &&g_w2_sensors[4] >= 500){
-	// intersection_detection();
-	//}
-	else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
-		w2_half_rotation_left();
-	}
-	// else if(g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 && g_w2_sensors[0] < 100){
-	// half_rotation_right();
-	//}
-	else {
+		w2_maze_rotation_half_left();
+	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+		w2_maze_rotation_half_left();
+	} else {
 		if (power_difference < 0 &&
 			(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100))
 			set_motors(max + power_difference, max);
diff --git a/robot/movement.c b/robot/movement.c
index b4ad3c1..82829c1 100644
--- a/robot/movement.c
+++ b/robot/movement.c
@@ -4,35 +4,57 @@
 unsigned int g_w2_sensors[5]		= {0};
 unsigned int g_w2_position			= 0;
 
-void w2_full_rotation() {
-	set_motors(0, 0);
+void w2_maze_rotation_full(){
+	set_motors(0,0);
 	delay_ms(500);
-	set_motors(60, -60);
+	set_motors(60,-60);
 	delay_ms(540);
-	set_motors(0, 0);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	set_motors(0,0);
+	g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
 	delay_ms(500);
 }
 
-void w2_half_rotation_left() {
-	set_motors(0, 0);
-	set_motors(50, 50);
+void w2_maze_rotation_half_left(){
+	set_motors(0,0);
+	set_motors(50,50);
 	delay_ms(150);
-	set_motors(-30, 30);
+	set_motors(-30,30);
 	delay_ms(600);
-	set_motors(0, 0);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	set_motors(0,0);
+	g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
 	delay_ms(500);
+
 }
-void w2_half_rotation_right() {
-	set_motors(0, 0);
-	set_motors(50, 50);
+void w2_maze_rotation_half_right(){
+	set_motors(0,0);
+	set_motors(50,50);
 	delay_ms(150);
-	set_motors(30, -30);
+	set_motors(30,-30);
 	delay_ms(600);
-	set_motors(0, 0);
-	set_motors(50, 50);
+	set_motors(0,0);
+	set_motors(50,50);
 	delay_ms(150);
-	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
+	g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
 	delay_ms(500);
 }
+
+void w2_grid_rotation_full(){
+	set_motors(60,-60);
+	delay_ms(540);
+	set_motors(10,10);
+	g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+}
+
+void w2_grid_rotation_half_left(){
+	set_motors(-30,30);
+	delay_ms(600);
+	set_motors(10,10);
+	g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+}
+
+void w2_grid_rotation_half_right(){
+	set_motors(30,-30);
+	delay_ms(600);
+	set_motors(10,10);
+	g_w2_position = read_line(g_w2_sensors,IR_EMITTERS_ON);
+}
diff --git a/robot/movement.h b/robot/movement.h
index 20a5d70..21e20c6 100644
--- a/robot/movement.h
+++ b/robot/movement.h
@@ -3,7 +3,9 @@
 extern unsigned int g_w2_sensors[5];
 extern unsigned int g_w2_position;
 
-void w2_full_rotation();
-void w2_half_rotation_left();
-void w2_half_rotation_right();
-
+void w2_maze_rotation_full();
+void w2_maze_rotation_half_left();
+void w2_maze_rotation_half_right();
+void w2_grid_rotation_full();
+void w2_grid_rotation_half_left();
+void w2_grid_rotation_half_right();
diff --git a/robot/transition.c b/robot/transition.c
deleted file mode 100644
index 5b3244f..0000000
--- a/robot/transition.c
+++ /dev/null
@@ -1,8 +0,0 @@
-#include "transition.h"
-#include "movement.h"
-
-int g_w2_transition; //TODO: document
-					 //
-void w2_crossway_detection() { w2_half_rotation_left(); }
-void w2_intersection_detection() { w2_half_rotation_left(); }
-
diff --git a/robot/transition.h b/robot/transition.h
deleted file mode 100644
index 854c517..0000000
--- a/robot/transition.h
+++ /dev/null
@@ -1,6 +0,0 @@
-#pragma once
-
-extern int g_w2_transition;
-
-void w2_crossway_detection();
-void w2_intersection_detection();
diff --git a/shared/errcatch.h b/shared/errcatch.h
index a56bc00..07835c4 100644
--- a/shared/errcatch.h
+++ b/shared/errcatch.h
@@ -97,3 +97,4 @@ void w2_errcatch_throw_msg(w2_e_errorcode code, uint16_t length, const char *mes
  * TODO: doesn't handle null pointers from malloc
  */
 w2_s_error *w2_alloc_error(w2_e_errorcode code, uint16_t length, const char *message);
+