last few touches for assessment

4 files changed, 20 insertions, 36 deletions

diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index 5b733af..0f02f37 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -15,16 +15,12 @@ void w2_short_drive() {
 void w2_home() {
 	set_motors(0, 0);
 	delay_ms(150);
-	clear();
-	print("CHARGING");
 	set_motors(30, 30);
 	delay_ms(600);
 	set_motors(0, 0);
-	play_frequency(300, 500, 7);
 	delay_ms(600);
 	g_w2_position		= read_line(g_w2_sensors, IR_EMITTERS_ON);
 	g_w2_charged_status = 1;
-	clear();
 	delay_ms(2000);
 }
 
@@ -40,8 +36,6 @@ void w2_charge_cross_walk() {
 		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);
-			// clear();
-			print("WALK");
 			g_w2_transition++;
 			if (g_w2_transition == 3) { // TODO: document g_w2_transition
 				set_motors(40, 40);
@@ -62,9 +56,6 @@ void w2_charge_cross_walk() {
 void w2_mode_chrg() {
 	unsigned int last_proportional = 0;
 	long integral				   = 0;
-	// initialize();
-	clear();
-	print("CHARGE");
 
 	while (1) {
 		// Get the position of the line.  Note that we *must* provide
@@ -113,13 +104,11 @@ void w2_mode_chrg() {
 			w2_maze_rotation_full();
 			w2_short_drive();
 		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {
-			clear();
 			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) {
-			clear();
 			w2_maze_rotation_half_left();
 		} else {
 			if (power_difference < 0 &&
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 16dbef3..235ac66 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -7,24 +7,12 @@ int g_w2_order_number;
 
 int g_w2_maze_status = 0;
 
-w2_s_grid_coordinate g_w2_order[4] = {
-	{0, 0},
-	{1, 1},
-	{2, 2},
-	{3, 3},
-};
+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;
 
-void w2_location_message() {
-	clear();
-	print_long(g_w2_location.x);
-	print(",");
-	print_long(g_w2_location.y);
-	delay(200);
-}
-
 int g_w2_detection = 0;
 int g_w2_transition;
 char g_w2_x_location = 0;
@@ -39,8 +27,6 @@ void w2_crosswalk_stroll() {
 		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);
-			clear();
-			print("WALK");
 			g_w2_transition++;
 			if (g_w2_transition == 3) {
 				set_motors(40, 40);
@@ -206,8 +192,6 @@ void w2_turn_east() {
 
 void w2_arrived_message() {
 	if (g_w2_location.x == g_w2_destination.x && g_w2_location.y == g_w2_destination.y) {
-		clear();
-		print("ORDER ");
 		print_long(g_w2_order_number);
 		play_frequency(400, 500, 7);
 		delay(500);
@@ -256,27 +240,23 @@ void w2_go_to_y() {
 
 void w2_mode_grid() {
 	set_motors(0, 0);
-	clear();
-	print("GRID");
 	delay(500);
 
 	w2_begin_location();
 
 	// TODO: orders read here
-	for (int i = 0; i < 4; i++) {
+	for (int i = 0; i < g_w2_order_index; i++) {
 		g_w2_order_number = i + 1;
 
 		g_w2_destination.x = g_w2_order[i].x;
 		g_w2_destination.y = g_w2_order[i].y;
 
-		w2_location_message();
 		delay(1000);
 		w2_go_to_x();
 		w2_go_to_y();
 	}
 	w2_end_destination();
 
-	w2_location_message();
 	delay(1000);
 	w2_go_to_y();
 	w2_go_to_x();
diff --git a/robot/mode_grid.h b/robot/mode_grid.h
index 2df0d33..408a8ff 100644
--- a/robot/mode_grid.h
+++ b/robot/mode_grid.h
@@ -22,4 +22,6 @@ typedef struct {
 	int y;
 } w2_s_grid_coordinate;
 
-extern w2_s_grid_coordinate g_w2_order[4];
+extern w2_s_grid_coordinate g_w2_order[16];
+extern unsigned int g_w2_order_index;
+
diff --git a/robot/sercomm.c b/robot/sercomm.c
index efad449..d12fcca 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -4,6 +4,7 @@
 #include "../shared/bin.h"
 #include "../shared/errcatch.h"
 #include "../shared/serial_parse.h"
+#include "mode_grid.h"
 #include "hypervisor.h"
 #include "io.h"
 #include "mode_dirc.h"
@@ -117,7 +118,18 @@ void w2_cmd_dirc_rx(w2_s_bin *data) {
 
 void w2_cmd_cord_rx(w2_s_bin *data) { return; }
 
-void w2_cmd_bomd_rx(w2_s_bin *data) { return; }
+// #include <stdio.h>
+void w2_cmd_bomd_rx(w2_s_bin *data) {
+	/* W2_CAST_BIN(w2_s_cmd_bomd_rx, data, req);
+
+	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++; */
+}
 
 void w2_cmd_sres_rx(w2_s_bin *data) {
 	W2_CAST_BIN(w2_s_cmd_sres_rx, data, req);
@@ -149,6 +161,7 @@ void w2_cmd_info_rx(w2_s_bin *data) {
 	res_msg->mode_ms	 = (uint8_t)g_w2_hypervisor_ema_mode_ms;
 	res_msg->uptime_s	 = w2_bin_hton32((uint32_t)(g_w2_hypervisor_uptime_ms / 1e3));
 	res_msg->mode		 = g_w2_mode_history[g_w2_mode_history_index];
+	res_msg->battery_mv  = w2_bin_hton16(read_battery_millivolts());
 
 	w2_sercomm_append_msg(res_bin);
 	free(res_bin);