sort of working merge and code seperation

1 files changed, 6 insertions, 49 deletions

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);