[WIP] last touches for second assessment

1 files changed, 75 insertions, 76 deletions

diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 47391a1..c7349ab 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -1,4 +1,5 @@
 #include "mode_chrg.h"
+#include "io.h"
 #include "../shared/bool.h"
 #include "hypervisor.h"
 #include "mode_grid.h"
@@ -6,32 +7,34 @@
 #include "movement.h"
 #include "orangutan_shim.h"
 
+#include <stdio.h>
+
 bool g_w2_chrg_aligned;
 
 void w2_short_drive() {
-	set_motors(50, 50);
+	w2_set_motors(50, 50);
 	delay(150);
-	set_motors(0, 0);
+	w2_set_motors(0, 0);
 }
 
 // crosswalk from charging station back to maze
 void w2_charge_cross_walk() {
 	if (g_w2_transition == 0) {
-		set_motors(-30, 30);
+		w2_set_motors(-30, 30);
 		delay(50);
 	}
 	while (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) {
-		set_motors(15, 15);
+		w2_set_motors(15, 15);
 		delay(550);
 		g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 		if (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100) {
-			set_motors(0, 0);
+			w2_set_motors(0, 0);
 			g_w2_transition++;
 			if (g_w2_transition == 3) { // TODO: document g_w2_transition
-				set_motors(40, 40);
+				w2_set_motors(40, 40);
 				delay(600);
-				set_motors(0, 0);
+				w2_set_motors(0, 0);
 
 				g_w2_transition = 0;
 				w2_modes_swap(W2_M_MAZE);
@@ -48,80 +51,76 @@ void w2_mode_chrg_onswitch() { g_w2_chrg_aligned = false; }
 
 // main function for charge mode
 void w2_mode_chrg() {
-	unsigned int last_proportional = 0;
-	long integral				   = 0;
-
-	while (1) {
-		// 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.
-		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 - last_proportional;
-		integral += proportional;
-
-		// Remember the last position.
-		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 + 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;
-
-		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_charge_cross_walk();
-			if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) {
-				break;
-			}
+	static unsigned int last_proportional = 0;
+	static long integral				   = 0;
+
+	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 - last_proportional;
+	integral += proportional;
+
+	// Remember the last position.
+	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 + 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;
+
+	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_charge_cross_walk();
+		if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_MAZE) {
+			return;
 		}
+	}
 
-		else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 &&
-				 g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) {
-			if (g_w2_target_area == W2_AREA_CHRG) {
-				if (!g_w2_chrg_aligned) {
-					set_motors(0, 0);
-					delay_ms(150);
-					set_motors(30, 30);
-					delay_ms(600);
-					set_motors(0, 0);
-					delay_ms(600);
-					g_w2_chrg_aligned = true;
-				}
-			} else {
-				delay(200);
-				w2_maze_rotation_full();
-				w2_short_drive();
+	else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 500 && g_w2_sensors[2] >= 500 &&
+			 g_w2_sensors[3] >= 500 && g_w2_sensors[4] >= 500) {
+		if (g_w2_target_area == W2_AREA_CHRG) {
+			if (!g_w2_chrg_aligned) {
+				w2_set_motors(0, 0);
+				delay_ms(150);
+				w2_set_motors(30, 30);
+				delay_ms(600);
+				w2_set_motors(0, 0);
+				delay_ms(600);
+				g_w2_chrg_aligned = true;
 			}
-		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
-			w2_maze_rotation_half_left();
+		} else {
+			delay(200);
+			w2_maze_rotation_full();
+			w2_short_drive();
 		}
+	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
+		w2_maze_rotation_half_left();
+	}
 
-		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_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);
-			} else if (power_difference > 0 &&
-					   (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
-				set_motors(max, max - power_difference);
-			}
+	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_maze_rotation_half_left();
+	} else {
+		if (power_difference < 0 &&
+			(g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
+			w2_set_motors(max + power_difference, max);
+		} else if (power_difference > 0 &&
+				   (g_w2_sensors[2] > 100 || g_w2_sensors[3] > 100 || g_w2_sensors[1] > 100)) {
+			w2_set_motors(max, max - power_difference);
 		}
 	}
 }
+