diff options
Diffstat (limited to 'robot/mode_maze.c')
| -rw-r--r-- | robot/mode_maze.c | 15 |
1 files changed, 10 insertions, 5 deletions
diff --git a/robot/mode_maze.c b/robot/mode_maze.c index eb75e3c..0350205 100644 --- a/robot/mode_maze.c +++ b/robot/mode_maze.c @@ -6,6 +6,8 @@ unsigned int g_w2_last_proportional = 0; long g_w2_integral = 0; + +//main function for maze mode void w2_mode_maze() { // PID controller g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); @@ -20,14 +22,17 @@ void w2_mode_maze() { if (power_difference < -max) power_difference = -max; 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) { + g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { //dead ends 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) { + } + 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) { //crossways or intersection w2_maze_rotation_half_left(); - } else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { + } + else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { //left corners w2_maze_rotation_half_left(); - } else { + } + else { //normal line following 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); |