added TARQ to robot and client

12 files changed, 52 insertions, 53 deletions

diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index be3fb77..50dc1ac 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -23,6 +23,8 @@ bool g_w2_ping_received	  = true;
 bool g_w2_ping_timeout	  = false;
 bool g_w2_connected		  = false;
 
+w2_e_target_area g_w2_target_area = W2_AREA_CHRG;
+
 void w2_hypervisor_main() {
 #ifdef W2_SIM
 	w2_sim_cycle_begin();
diff --git a/robot/hypervisor.h b/robot/hypervisor.h
index e9699cf..154ae58 100644
--- a/robot/hypervisor.h
+++ b/robot/hypervisor.h
@@ -5,6 +5,7 @@
 #include <stdint.h>
 
 #include "../shared/bool.h"
+#include "../shared/protocol.h"
 
 /** amount of parallel timers */
 #define W2_HYPERVISOR_TIMER_COUNT 2
@@ -26,6 +27,8 @@ extern bool g_w2_ping_received;
 extern bool g_w2_ping_timeout;
 extern bool g_w2_connected;
 
+extern w2_e_target_area g_w2_target_area;
+
 /**
  * backbone of all other modules
  *
diff --git a/robot/mode_chrg.c b/robot/mode_chrg.c
index a5910f2..c41c0f7 100644
--- a/robot/mode_chrg.c
+++ b/robot/mode_chrg.c
@@ -4,7 +4,7 @@
 #include "movement.h"
 #include "orangutan_shim.h"
 
-int g_w2_charged_status;	//used to detect the charging station (once)
+int g_w2_charged_status; // used to detect the charging station (once)
 
 void w2_short_drive() {
 	set_motors(50, 50);
@@ -12,7 +12,7 @@ void w2_short_drive() {
 	set_motors(0, 0);
 }
 
-//charging station
+// charging station
 void w2_home() {
 	set_motors(0, 0);
 	delay_ms(150);
@@ -25,7 +25,7 @@ void w2_home() {
 	delay_ms(2000);
 }
 
-//crosswalk from charging station back to maze
+// crosswalk from charging station back to maze
 void w2_charge_cross_walk() {
 	if (g_w2_transition == 0) {
 		set_motors(-30, 30);
@@ -55,7 +55,7 @@ void w2_charge_cross_walk() {
 	}
 }
 
-//main function for charge mode
+// main function for charge mode
 void w2_mode_chrg() {
 	unsigned int last_proportional = 0;
 	long integral				   = 0;
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
index 4364b67..7e6db49 100644
--- a/robot/mode_grid.c
+++ b/robot/mode_grid.c
@@ -19,12 +19,11 @@ w2_s_grid_coordinate g_w2_destination;
 w2_e_orientation g_w2_direction;
 
 int g_w2_detection = 0;
-int g_w2_transition;	//used for the crosswalk, used to count black lines
-char g_w2_x_location = 0; //current location in grid
+int g_w2_transition;	  // used for the crosswalk, used to count black lines
+char g_w2_x_location = 0; // current location in grid
 char g_w2_y_location = 0;
 
-
-//used for the crosswalk from maze to grid
+// used for the crosswalk from maze to grid
 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) {
@@ -57,7 +56,7 @@ void w2_grid_crossway_detection() {
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
-//main function for grid mode
+// main function for grid mode
 void w2_grid_follow_line() {
 	unsigned int last_proportional = 0;
 	long integral				   = 0;
@@ -75,9 +74,10 @@ void w2_grid_follow_line() {
 		if (power_difference < -max) power_difference = -max;
 
 		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) { //crossways/intersections
+			g_w2_sensors[3] >= 250 && g_w2_sensors[4] >= 500) { // crossways/intersections
 			break;
-		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) { //left corners
+		} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 &&
+				   g_w2_sensors[4] < 100) { // left corners
 			break;
 		} else if (g_w2_sensors[4] >= 500 && g_w2_sensors[3] >= 200 &&
 				   g_w2_sensors[0] < 100) { // for the south and west borders of the grid
@@ -99,22 +99,20 @@ void w2_grid_follow_line() {
 	}
 }
 
-//begin location when entering the grid
+// begin location when entering the grid
 void w2_begin_location() {
 	g_w2_location.x = 4;
 	g_w2_location.y = 0;
 	g_w2_direction	= W2_ORT_WEST;
 }
 
-
-//location of grid exit
+// location of grid exit
 void w2_end_destination() {
 	g_w2_destination.x = 4;
 	g_w2_destination.y = 4;
 }
 
-
-//turns are used to get the correct orientation when picking up orders
+// turns are used to get the correct orientation when picking up orders
 void w2_turn_north() {
 	switch (g_w2_direction) {
 		case W2_ORT_NORTH:
@@ -201,8 +199,7 @@ void w2_turn_east() {
 	g_w2_direction = W2_ORT_EAST;
 }
 
-
-//signals when the product is picked
+// signals when the product is picked
 void w2_arrived_message() {
 	if (g_w2_location.x == g_w2_destination.x && g_w2_location.y == g_w2_destination.y) {
 		play_frequency(400, 500, 7);
@@ -210,8 +207,7 @@ void w2_arrived_message() {
 	}
 }
 
-
-//go to correct x coordinate
+// go to correct x coordinate
 void w2_go_to_x() {
 	if (g_w2_location.x != g_w2_destination.x) {
 		while (g_w2_location.x != g_w2_destination.x) {
@@ -232,7 +228,7 @@ void w2_go_to_x() {
 	}
 }
 
-//go to correct y coordinate
+// go to correct y coordinate
 void w2_go_to_y() {
 	if (g_w2_location.y != g_w2_destination.y) {
 		while (g_w2_location.y != g_w2_destination.y) {
@@ -253,7 +249,7 @@ void w2_go_to_y() {
 	}
 }
 
-//main function for grid mode
+// main function for grid mode
 void w2_mode_grid() {
 	set_motors(0, 0);
 	delay(500);
diff --git a/robot/mode_grid.h b/robot/mode_grid.h
index 408a8ff..79a6475 100644
--- a/robot/mode_grid.h
+++ b/robot/mode_grid.h
@@ -24,4 +24,3 @@ typedef struct {
 
 extern w2_s_grid_coordinate g_w2_order[16];
 extern unsigned int g_w2_order_index;
-
diff --git a/robot/mode_halt.c b/robot/mode_halt.c
index 8bd051e..acd55ee 100644
--- a/robot/mode_halt.c
+++ b/robot/mode_halt.c
@@ -1,5 +1,5 @@
 #include "mode_halt.h"
 #include "orangutan_shim.h"
 
-//emergency stop
+// emergency stop
 void w2_mode_halt() { set_motors(0, 0); }
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
index 0350205..72478e8 100644
--- a/robot/mode_maze.c
+++ b/robot/mode_maze.c
@@ -6,8 +6,7 @@
 unsigned int g_w2_last_proportional = 0;
 long g_w2_integral					= 0;
 
-
-//main function for maze mode
+// main function for maze mode
 void w2_mode_maze() {
 	// PID controller
 	g_w2_position	 = read_line(g_w2_sensors, IR_EMITTERS_ON);
@@ -22,17 +21,15 @@ void w2_mode_maze() {
 	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) { //dead ends
+		g_w2_sensors[3] < 100 && g_w2_sensors[4] < 100) { // dead ends
 		w2_crosswalk_stroll();
-	} 
-	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) {	//crossways or intersection
+	} 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) { // crossways or intersection
 		w2_maze_rotation_half_left();
-	} 
-	else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 && g_w2_sensors[4] < 100) {	//left corners
+	} else if (g_w2_sensors[0] >= 500 && g_w2_sensors[1] >= 200 &&
+			   g_w2_sensors[4] < 100) { // left corners
 		w2_maze_rotation_half_left();
-	} 
-	else {		//normal line following
+	} 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);
diff --git a/robot/mode_scal.c b/robot/mode_scal.c
index 8835183..e4e282c 100644
--- a/robot/mode_scal.c
+++ b/robot/mode_scal.c
@@ -2,7 +2,7 @@
 #include "modes.h"
 #include "orangutan_shim.h"
 
-//callibrates the robot
+// callibrates the robot
 void w2_mode_scal() {
 	for (int counter = 0; counter < 80; counter++) {
 		if (counter < 20 || counter >= 60) {
diff --git a/robot/mode_spin.c b/robot/mode_spin.c
index 79f9dea..5065e13 100644
--- a/robot/mode_spin.c
+++ b/robot/mode_spin.c
@@ -1,6 +1,5 @@
 #include "mode_spin.h"
 #include "orangutan_shim.h"
 
-
-//wet floor simulation
+// wet floor simulation
 void w2_mode_spin() { set_motors(255, -255); }
diff --git a/robot/modes.c b/robot/modes.c
index aedc02a..559d81c 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -16,7 +16,7 @@ w2_e_mode g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE];
 uint8_t g_w2_mode_history_index = 0;
 void (*g_w2_modes[W2_MODE_COUNT])();
 
-//all of the different modi
+// all of the different modi
 void w2_modes_init() {
 	g_w2_modes[W2_M_CHRG] = &w2_mode_chrg;
 	g_w2_modes[W2_M_DIRC] = &w2_mode_dirc;
diff --git a/robot/movement.c b/robot/movement.c
index 37d66d5..e348410 100644
--- a/robot/movement.c
+++ b/robot/movement.c
@@ -1,11 +1,10 @@
 #include "movement.h"
 #include "orangutan_shim.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
+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
+// full rotation maze/charge
 void w2_maze_rotation_full() {
 	set_motors(0, 0);
 	delay_ms(500);
@@ -16,7 +15,7 @@ void w2_maze_rotation_full() {
 	delay_ms(500);
 }
 
-//left turn maze/charge
+// left turn maze/charge
 void w2_maze_rotation_half_left() {
 	set_motors(0, 0);
 	set_motors(50, 50);
@@ -28,7 +27,7 @@ void w2_maze_rotation_half_left() {
 	delay_ms(500);
 }
 
-//right turn maze/charge
+// right turn maze/charge
 void w2_maze_rotation_half_right() {
 	set_motors(0, 0);
 	set_motors(50, 50);
@@ -42,7 +41,7 @@ void w2_maze_rotation_half_right() {
 	delay_ms(500);
 }
 
-//180 turn in grid
+// 180 turn in grid
 void w2_grid_rotation_full() {
 	set_motors(60, -60);
 	delay_ms(540);
@@ -50,7 +49,7 @@ void w2_grid_rotation_full() {
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
-//left turn in grid
+// left turn in grid
 void w2_grid_rotation_half_left() {
 	set_motors(-30, 30);
 	delay_ms(600);
@@ -58,7 +57,7 @@ void w2_grid_rotation_half_left() {
 	g_w2_position = read_line(g_w2_sensors, IR_EMITTERS_ON);
 }
 
-//right turn in grid
+// right turn in grid
 void w2_grid_rotation_half_right() {
 	set_motors(30, -30);
 	delay_ms(600);
diff --git a/robot/sercomm.c b/robot/sercomm.c
index d12fcca..195c477 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -4,10 +4,10 @@
 #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"
+#include "mode_grid.h"
 #include "modes.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
@@ -124,10 +124,9 @@ void w2_cmd_bomd_rx(w2_s_bin *data) {
 
 	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;
+	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++; */
 }
 
@@ -161,7 +160,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());
+	res_msg->battery_mv	 = w2_bin_hton16(read_battery_millivolts());
 
 	w2_sercomm_append_msg(res_bin);
 	free(res_bin);
@@ -173,6 +172,11 @@ void w2_cmd_play_rx(w2_s_bin *data) { return; }
 
 void w2_cmd_cled_rx(w2_s_bin *data) { return; }
 
+void w2_cmd_tarq_rx(w2_s_bin *data) {
+	W2_CAST_BIN(w2_s_cmd_tarq_rx, data, req);
+	g_w2_target_area = req->target_area;
+}
+
 #pragma GCC diagnostic pop
 
 void w2_cmd_expt_tx(w2_s_bin *data) {}