implement mode switching tab

2 files changed, 5 insertions, 2 deletions

diff --git a/robot/mode_halt.c b/robot/mode_halt.c
index 88d6183..bc501a2 100644
--- a/robot/mode_halt.c
+++ b/robot/mode_halt.c
@@ -1,3 +1,6 @@
 #include "mode_halt.h"
+#include "orangutan_shim.h"
 
-void w2_mode_halt() { return; }
+void w2_mode_halt() {
+	set_motors(0, 0);
+}
diff --git a/robot/modes.c b/robot/modes.c
index 600c9a1..4995d6f 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -46,7 +46,7 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
 	// forward mode change to sercomm
 	W2_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin);
 	msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX;
-	msg->mode	= new_mode;
+	msg->mode	= g_w2_mode_history[g_w2_mode_history_index];
 
 	w2_sercomm_append_msg(msg_bin);
 	free(msg_bin);