19 files changed, 70 insertions, 15 deletions

diff --git a/robot/calibration.c b/robot/calibration.c
deleted file mode 100644
index 7788532..0000000
--- a/robot/calibration.c
+++ /dev/null
@@ -1 +0,0 @@
-#include "calibration.h"
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 99f1063..cf06f5d 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -2,7 +2,6 @@
 #include <string.h>
 
 #include "errcatch.h"
-#include "halt.h"
 #include "modes.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
@@ -52,7 +51,7 @@ void w2_errcatch_handle_error(w2_s_error *error) {
 	uint8_t severity = error->code & W2_E_TYPE_MASK;
 
 	// trigger emergency mode for critical errors
-	if ((severity ^ W2_E_TYPE_CRIT) == 0) g_w2_current_mode = &w2_mode_halt;
+	if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_switch(W2_M_HALT);
 
 	// TODO: handle more error types
 	switch (error->code) {
diff --git a/robot/grid.c b/robot/grid.c
deleted file mode 100644
index 0c83272..0000000
--- a/robot/grid.c
+++ /dev/null
@@ -1 +0,0 @@
-#include "grid.h"
diff --git a/robot/halt.c b/robot/halt.c
deleted file mode 100644
index 2f159f0..0000000
--- a/robot/halt.c
+++ /dev/null
@@ -1,5 +0,0 @@
-#include <stdbool.h>
-
-#include "halt.h"
-
-void w2_mode_halt() { return; }
diff --git a/robot/maze.c b/robot/maze.c
deleted file mode 100644
index a27414f..0000000
--- a/robot/maze.c
+++ /dev/null
@@ -1 +0,0 @@
-#include "maze.h"
diff --git a/robot/mode_grid.c b/robot/mode_grid.c
new file mode 100644
index 0000000..8526499
--- /dev/null
+++ b/robot/mode_grid.c
@@ -0,0 +1,3 @@
+#include "mode_grid.h"
+
+void w2_mode_grid() {}
diff --git a/robot/grid.h b/robot/mode_grid.h
index fcf9100..fcf9100 100644
--- a/robot/grid.h
+++ b/robot/mode_grid.h
diff --git a/robot/mode_halt.c b/robot/mode_halt.c
new file mode 100644
index 0000000..88d6183
--- /dev/null
+++ b/robot/mode_halt.c
@@ -0,0 +1,3 @@
+#include "mode_halt.h"
+
+void w2_mode_halt() { return; }
diff --git a/robot/halt.h b/robot/mode_halt.h
index d92905e..d92905e 100644
--- a/robot/halt.h
+++ b/robot/mode_halt.h
diff --git a/robot/mode_lcal.c b/robot/mode_lcal.c
new file mode 100644
index 0000000..6b4e736
--- /dev/null
+++ b/robot/mode_lcal.c
@@ -0,0 +1 @@
+#include "mode_lcal.h"
diff --git a/robot/calibration.h b/robot/mode_lcal.h
index 5c1af38..dd373f0 100644
--- a/robot/calibration.h
+++ b/robot/mode_lcal.h
@@ -6,4 +6,4 @@
  * turns robot on its own axis 360 degress, and aligns the front sensors with
  * the line if found, else triggers halt mode (emergency)
  */
-void w2_mode_calb();
+void w2_mode_lcal();
diff --git a/robot/mode_maze.c b/robot/mode_maze.c
new file mode 100644
index 0000000..6ae62be
--- /dev/null
+++ b/robot/mode_maze.c
@@ -0,0 +1,3 @@
+#include "mode_maze.h"
+
+void w2_mode_maze() {}
diff --git a/robot/maze.h b/robot/mode_maze.h
index 9fbeb8c..9fbeb8c 100644
--- a/robot/maze.h
+++ b/robot/mode_maze.h
diff --git a/robot/modes.c b/robot/modes.c
index c22875c..1f36703 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -1,6 +1,24 @@
 #include "modes.h"
-#include "halt.h"
+#include "sercomm.h"
 
 void (*g_w2_current_mode)() = &w2_mode_halt;
 
 void w2_modes_main() { (*g_w2_current_mode)(); }
+
+void w2_modes_switch(w2_e_mode new_mode) {
+	if (new_mode == W2_M_PREV) {
+		// TODO implement previous mode buffer
+	} else {
+		g_w2_current_mode = W2_MODES[new_mode];
+	}
+
+	// forward mode change to sercomm
+	size_t msg_size		  = sizeof(w2_s_cmd_mode_tx);
+	w2_s_cmd_mode_tx *msg = malloc(msg_size);
+	msg->opcode			  = W2_CMD_MODE | W2_CMDDIR_TX;
+	msg->mode			  = (uint8_t)new_mode;
+	w2_s_bin *msg_bin	  = w2_bin_s_alloc(msg_size, (uint8_t *)msg);
+	w2_sercomm_append_msg(msg_bin);
+	free(msg);
+	free(msg_bin);
+}
diff --git a/robot/modes.h b/robot/modes.h
index dd34690..fe81043 100644
--- a/robot/modes.h
+++ b/robot/modes.h
@@ -1,5 +1,11 @@
 #pragma once
 
+#include "mode_grid.h"
+#include "mode_halt.h"
+#include "mode_lcal.h"
+#include "mode_maze.h"
+
+/** function pointer to current mode */
 extern void (*g_w2_current_mode)();
 
 /**
@@ -8,3 +14,27 @@ extern void (*g_w2_current_mode)();
  * executes mode in g_w2_current_mode
  */
 void w2_modes_main();
+
+/** mode constants */
+typedef enum {
+	W2_M_PREV = -1,
+	W2_M_MAZE = 0,
+	W2_M_GRID = 1,
+	W2_M_HALT = 2,
+	W2_M_LCAL = 3,
+	W2_M_CHRG = 4,
+	W2_M_DIRC = 5,
+	W2_M_SPIN = 6,
+	W2_M_SCAL = 7,
+} w2_e_mode;
+
+/** array that maps w2_e_mode to mode function pointers */
+static const void(*const W2_MODES[]) = {
+	&w2_mode_maze,
+	&w2_mode_grid,
+	&w2_mode_grid,
+	&w2_mode_halt,
+};
+
+/** switch the current mode */
+void w2_modes_switch(w2_e_mode new_mode);
diff --git a/robot/readme.md b/robot/readme.md
index 2de14db..31acafc 100644
--- a/robot/readme.md
+++ b/robot/readme.md
@@ -62,7 +62,7 @@ what they're supposed to do:
 |maze            |`mode_maze  `|Jorn & Abdullaahi| controls robot during maze portion of map; hands off control to warehouse module|
 |warehouse       |`mode_grid  `|Loek| controls robot during warehouse portion of map; hands off control to maze module|
 |emergency stop  |`mode_halt  `|Fiona| stops all execution until emergency mode is reset by software or user|
-|calibration     |`mode_calb  `|Fiona| find line by turning on own axis if lost|
+|calibration     |`mode_lcal  `|Fiona| find line by turning on own axis if lost|
 
 ## some standards
 
diff --git a/robot/sercomm.c b/robot/sercomm.c
index a3ccdb0..e972f28 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -4,6 +4,7 @@
 #include "../shared/bin.h"
 #include "../shared/serial_parse.h"
 #include "hypervisor.h"
+#include "modes.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
 
@@ -66,7 +67,12 @@ void w2_cmd_ping_rx(w2_s_bin *data) {
 	free(return_message_bin);
 }
 
-void w2_cmd_mode_rx(w2_s_bin *data) { return; }
+void w2_cmd_mode_rx(w2_s_bin *data) {
+	w2_s_cmd_mode_rx *message = malloc(w2_cmd_sizeof(data->data, data->bytes));
+	memcpy(message, data->data, data->bytes);
+
+	w2_modes_switch(message->mode);
+}
 
 void w2_cmd_sped_rx(w2_s_bin *data) { return; }
 
diff --git a/robot/setup.c b/robot/setup.c
index d075c41..1ecda43 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -2,7 +2,6 @@
 
 #include "../shared/bin.h"
 #include "../shared/consts.h"
-#include "halt.h"
 #include "modes.h"
 #include "orangutan_shim.h"
 #include "sercomm.h"
diff --git a/robot/tests/mode.bin b/robot/tests/mode.bin
new file mode 100644
index 0000000..35ebd35
--- /dev/null
+++ b/robot/tests/mode.bin
@@ -0,0 +1 @@
+ÿ
\ No newline at end of file