WIP merge

1 files changed, 3 insertions, 55 deletions

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;