[WIP] last touches for second assessment

11 files changed, 133 insertions, 124 deletions

diff --git a/robot/io.c b/robot/io.c
index 8add1e9..9977e2f 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -27,11 +27,13 @@ void w2_io_object_detection() {
 		if (front_distance <= W2_IO_DISTANCE_FAR_THRESHOLD) g_w2_io_object_detected = false;
 		if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT)
 			w2_errcatch_throw(W2_E_CRIT_OBSTACLE_STUCK);
-
-		set_motors(0, 0);
 	}
 }
 
+void w2_set_motors(int left, int right) {
+	set_motors(left * (1 - g_w2_io_object_detected), right * (1 - g_w2_io_object_detected));
+}
+
 void w2_io_battery_logic() {
 	if (w2_hypervisor_time_end(W2_TIMER_BATTERY_MEASUREMENT) <= W2_BATTERY_MEAS_FREQ) return;
 	w2_hypervisor_time_start(W2_TIMER_BATTERY_MEASUREMENT);
diff --git a/robot/io.h b/robot/io.h
index 5e469bb..5b79c52 100644
--- a/robot/io.h
+++ b/robot/io.h
@@ -10,3 +10,6 @@ extern bool g_w2_io_object_detected;
 
 /** @brief i/o module main */
 void w2_io_main();
+
+/** @brief pololu library `set_motors` override */
+void w2_set_motors(int left, int right);
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);
 		}
 	}
 }
+
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index d9c0d17..86756f6 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -11,4 +11,4 @@
 int16_t g_w2_mode_dirc_motor_l = 0;
 int16_t g_w2_mode_dirc_motor_r = 0;
 
-void w2_mode_dirc() { set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); }
+void w2_mode_dirc() { w2_set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r); }
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index cfecae5..ff80dd2 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -1,5 +1,6 @@
 #include "mode_grid.h"
 #include "hypervisor.h"
+#include "io.h"
 #include "modes.h"
 #include "movement.h"
 #include "orangutan_shim.h"
@@ -12,13 +13,8 @@ int g_w2_order_number;
 
 int g_w2_maze_status = 0;
 
-w2_s_grid_coordinate g_w2_order[16] = {
-	{0, 0},
-	{3, 4},
-	{2, 1},
-	{4, 2},
-};
-unsigned int g_w2_order_index = 4;
+w2_s_grid_coordinate g_w2_order[16];
+unsigned int g_w2_order_index = 0;
 w2_s_grid_coordinate g_w2_location;
 w2_s_grid_coordinate g_w2_destination;
 w2_e_orientation g_w2_direction;
@@ -32,15 +28,15 @@ char g_w2_y_location = 0;
 void w2_crosswalk_stroll() {
 	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(290);
 		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) &&
 			(g_w2_target_area != W2_AREA_MAZE)) {
-			set_motors(0, 0);
+			w2_set_motors(0, 0);
 			g_w2_transition++;
 			if (g_w2_transition == 3) {
-				set_motors(40, 40);
+				w2_set_motors(40, 40);
 				delay(600);
 				g_w2_transition = 0;
 				w2_modes_swap(W2_M_GRID);
@@ -56,9 +52,9 @@ void w2_crosswalk_stroll() {
 }
 
 void w2_grid_crossway_detection() {
-	set_motors(50, 50);
+	w2_set_motors(50, 50);
 	delay_ms(150);
-	set_motors(10, 10);
+	w2_set_motors(10, 10);
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
@@ -96,10 +92,10 @@ void w2_grid_follow_line() {
 		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);
+				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)) {
-				set_motors(max, max - power_difference);
+				w2_set_motors(max, max - power_difference);
 			}
 		}
 	}
@@ -257,7 +253,7 @@ void w2_go_to_y() {
 
 // main function for grid mode
 void w2_mode_grid() {
-	set_motors(0, 0);
+	w2_set_motors(0, 0);
 	delay(500);
 
 	w2_begin_location();
diff --git a/robot/mode_halt.c b/robot/mode_halt.c
index acd55ee..268a922 100644
--- a/robot/mode_halt.c
+++ b/robot/mode_halt.c
@@ -1,5 +1,6 @@
 #include "mode_halt.h"
 #include "orangutan_shim.h"
+#include "io.h"
 
 // emergency stop
-void w2_mode_halt() { set_motors(0, 0); }
+void w2_mode_halt() { w2_set_motors(0, 0); }
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
index 72478e8..a7f76ec 100644
--- a/robot/mode_maze.c
+++ b/robot/mode_maze.c
@@ -2,6 +2,7 @@
 #include "mode_grid.h"
 #include "movement.h"
 #include "orangutan_shim.h"
+#include "io.h"
 
 unsigned int g_w2_last_proportional = 0;
 long g_w2_integral					= 0;
@@ -32,9 +33,9 @@ void w2_mode_maze() {
 	} 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);
+			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))
-			set_motors(max, max - power_difference);
+			w2_set_motors(max, max - power_difference);
 	}
 }
diff --git a/robot/mode_scal.c b/robot/mode_scal.c
index e4e282c..245fec4 100644
--- a/robot/mode_scal.c
+++ b/robot/mode_scal.c
@@ -1,14 +1,15 @@
 #include "mode_scal.h"
 #include "modes.h"
 #include "orangutan_shim.h"
+#include "io.h"
 
 // callibrates the robot
 void w2_mode_scal() {
 	for (int counter = 0; counter < 80; counter++) {
 		if (counter < 20 || counter >= 60) {
-			set_motors(40, -40);
+			w2_set_motors(40, -40);
 		} else {
-			set_motors(-40, 40);
+			w2_set_motors(-40, 40);
 		}
 
 		calibrate_line_sensors(IR_EMITTERS_ON);
diff --git a/robot/mode_spin.c b/robot/mode_spin.c
index 5065e13..1c7cc11 100644
--- a/robot/mode_spin.c
+++ b/robot/mode_spin.c
@@ -1,5 +1,6 @@
 #include "mode_spin.h"
 #include "orangutan_shim.h"
+#include "io.h"
 
 // wet floor simulation
-void w2_mode_spin() { set_motors(255, -255); }
+void w2_mode_spin() { w2_set_motors(255, -255); }
diff --git a/robot/movement.c b/robot/movement.c
index e348410..b800464 100644
--- a/robot/movement.c
+++ b/robot/movement.c
@@ -1,41 +1,42 @@
 #include "movement.h"
 #include "orangutan_shim.h"
+#include "io.h"
 
 unsigned int g_w2_sensors[5] = {0}; // IR sensors on the bottom of the robot
 unsigned int g_w2_position	 = 0;	// position on the black line
 
 // full rotation maze/charge
 void w2_maze_rotation_full() {
-	set_motors(0, 0);
+	w2_set_motors(0, 0);
 	delay_ms(500);
-	set_motors(60, -60);
+	w2_set_motors(60, -60);
 	delay_ms(540);
-	set_motors(0, 0);
+	w2_set_motors(0, 0);
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 	delay_ms(500);
 }
 
 // left turn maze/charge
 void w2_maze_rotation_half_left() {
-	set_motors(0, 0);
-	set_motors(50, 50);
+	w2_set_motors(0, 0);
+	w2_set_motors(50, 50);
 	delay_ms(150);
-	set_motors(-30, 30);
+	w2_set_motors(-30, 30);
 	delay_ms(600);
-	set_motors(0, 0);
+	w2_set_motors(0, 0);
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 	delay_ms(500);
 }
 
 // right turn maze/charge
 void w2_maze_rotation_half_right() {
-	set_motors(0, 0);
-	set_motors(50, 50);
+	w2_set_motors(0, 0);
+	w2_set_motors(50, 50);
 	delay_ms(150);
-	set_motors(30, -30);
+	w2_set_motors(30, -30);
 	delay_ms(600);
-	set_motors(0, 0);
-	set_motors(50, 50);
+	w2_set_motors(0, 0);
+	w2_set_motors(50, 50);
 	delay_ms(150);
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 	delay_ms(500);
@@ -43,24 +44,24 @@ void w2_maze_rotation_half_right() {
 
 // 180 turn in grid
 void w2_grid_rotation_full() {
-	set_motors(60, -60);
+	w2_set_motors(60, -60);
 	delay_ms(540);
-	set_motors(10, 10);
+	w2_set_motors(10, 10);
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
 // left turn in grid
 void w2_grid_rotation_half_left() {
-	set_motors(-30, 30);
+	w2_set_motors(-30, 30);
 	delay_ms(600);
-	set_motors(10, 10);
+	w2_set_motors(10, 10);
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
 // right turn in grid
 void w2_grid_rotation_half_right() {
-	set_motors(30, -30);
+	w2_set_motors(30, -30);
 	delay_ms(600);
-	set_motors(10, 10);
+	w2_set_motors(10, 10);
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
diff --git a/robot/sercomm.c b/robot/sercomm.c
index 0f251d2..16134d1 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -118,16 +118,20 @@ void w2_cmd_dirc_rx(w2_s_bin *data) {
 
 void w2_cmd_cord_rx(w2_s_bin *data) { return; }
 
-// #include <stdio.h>
+#include <stdio.h>
 void w2_cmd_bomd_rx(w2_s_bin *data) {
-	/* W2_CAST_BIN(w2_s_cmd_bomd_rx, data, req);
+	W2_CAST_BIN(w2_s_cmd_bomd_rx, data, req);
+
+	req->position = w2_bin_hton32(req->position);
 
 	char buf[32];
 	clear();
-	sprintf(buf, "%lu, %lu", req->position % W2_MAP_DEFAULT_WIDTH, req->position /
-	W2_MAP_DEFAULT_WIDTH); print(buf); g_w2_order[g_w2_order_index].x = req->position %
-	W2_MAP_DEFAULT_WIDTH; g_w2_order[g_w2_order_index].y = req->position / W2_MAP_DEFAULT_WIDTH;
-	g_w2_order_index++; */
+	// sprintf(buf, "%lu, %lu", req->position % W2_MAP_DEFAULT_WIDTH, req->position / W2_MAP_DEFAULT_WIDTH);
+	sprintf(buf, "%lu", req->position);
+	print(buf);
+	g_w2_order[g_w2_order_index].x = req->position % W2_MAP_DEFAULT_WIDTH;
+	g_w2_order[g_w2_order_index].y = req->position / W2_MAP_DEFAULT_WIDTH;
+	g_w2_order_index++;
 }
 
 void w2_cmd_sres_rx(w2_s_bin *data) {