From 29f1a90f8cf07bffa9b53c9994cb9f2698fce920 Mon Sep 17 00:00:00 2001 From: lonkaars Date: Wed, 8 Jun 2022 11:41:26 +0200 Subject: WIP merge --- robot/mode_maze.c | 58 +++---------------------------------------------------- 1 file changed, 3 insertions(+), 55 deletions(-) (limited to 'robot/mode_maze.c') diff --git a/robot/mode_maze.c b/robot/mode_maze.c index a178d3a..04a3bc2 100644 --- a/robot/mode_maze.c +++ b/robot/mode_maze.c @@ -1,72 +1,20 @@ #include "mode_maze.h" #include "orangutan_shim.h" +#include "movement.h" +#include "transition.h" -unsigned int g_w2_sensors[5] = {0}; -unsigned int g_w2_position = 0; unsigned int g_w2_last_proportional = 0; long g_w2_integral = 0; -void w2_full_rotation() { - set_motors(0, 0); - delay_ms(500); - set_motors(60, -60); - delay_ms(540); - 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); - delay_ms(150); - set_motors(-30, 30); - delay_ms(600); - 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); - delay_ms(150); - set_motors(30, -30); - delay_ms(600); - set_motors(0, 0); - set_motors(50, 50); - delay_ms(150); - g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); - delay_ms(500); -} -void w2_crossway_detection() { w2_half_rotation_left(); } -void w2_intersection_detection() { w2_half_rotation_left(); } void w2_mode_maze() { - // Get the position of the line. Note that we *must* provide - // the "sensors" argument to read_line() here, even though we - // are not interested in the individual sensor readings. + // PID controller g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON); - - // The "proportional" term should be 0 when we are on the line. int proportional = ((int)g_w2_position) - 2000; - - // Compute the derivative (change) and integral (sum) of the - // position. int derivative = proportional - g_w2_last_proportional; g_w2_integral += proportional; - - // Remember the last position. g_w2_last_proportional = proportional; - - // Compute the difference between the two motor power settings, - // m1 - m2. If this is a positive number the robot will turn - // to the right. If it is a negative number, the robot will - // turn to the left, and the magnitude of the number determines - // the sharpness of the turn. int power_difference = proportional / 20 + g_w2_integral / 10000 + derivative * 3 / 2; - // Compute the actual motor settings. We never set either motor - // to a negative value. - const int max = 60; if (power_difference > max) power_difference = max; if (power_difference < -max) power_difference = -max; -- cgit v1.2.3