From 1becbf9f7292309b123d9f5839e89bd22fd84e81 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 8 Jun 2022 14:16:28 +0200 Subject: sort of working merge and code seperation --- robot/mode_maze.c | 55 ++++++------------------------------------------------- 1 file changed, 6 insertions(+), 49 deletions(-) (limited to 'robot/mode_maze.c') 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); -- cgit v1.2.3