1 files changed, 30 insertions, 14 deletions

diff --git a/robot/modes.c b/robot/modes.c
index 9877f6e..7decf47 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -1,16 +1,34 @@
 #include "modes.h"
+#include "../shared/errcatch.h"
 #include "../shared/protocol.h"
 #include "../shared/util.h"
-#include "errcatch.h"
 #include "sercomm.h"
 
-/** function pointer to current mode */
-// static void (*g_w2_current_mode)() = &w2_mode_halt;
-
-static void (*g_w2_mode_history[W2_MODE_HISTORY_BUFFER_SIZE])();
-static uint8_t g_w2_mode_history_index = 0;
+#include "mode_chrg.h"
+#include "mode_dirc.h"
+#include "mode_grid.h"
+#include "mode_halt.h"
+#include "mode_lcal.h"
+#include "mode_maze.h"
+#include "mode_scal.h"
+#include "mode_spin.h"
+
+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])();
+
+void w2_modes_init() {
+	g_w2_modes[W2_M_CHRG] = &w2_mode_chrg;
+	g_w2_modes[W2_M_DIRC] = &w2_mode_dirc;
+	g_w2_modes[W2_M_GRID] = &w2_mode_grid;
+	g_w2_modes[W2_M_HALT] = &w2_mode_halt;
+	g_w2_modes[W2_M_LCAL] = &w2_mode_lcal;
+	g_w2_modes[W2_M_MAZE] = &w2_mode_maze;
+	g_w2_modes[W2_M_SCAL] = &w2_mode_scal;
+	g_w2_modes[W2_M_SPIN] = &w2_mode_spin;
+}
 
-void w2_modes_main() { (*g_w2_mode_history[g_w2_mode_history_index])(); }
+void w2_modes_main() { (*g_w2_modes[g_w2_mode_history[g_w2_mode_history_index]])(); }
 
 void w2_modes_switch(w2_e_mode new_mode, bool replace) {
 	int16_t next_history_index =
@@ -24,17 +42,15 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
 		g_w2_mode_history_index = next_history_index;
 	} else {
 		g_w2_mode_history_index					   = next_history_index;
-		g_w2_mode_history[g_w2_mode_history_index] = W2_MODES[new_mode];
+		g_w2_mode_history[g_w2_mode_history_index] = 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_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin);
+	msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX;
+	msg->mode	= new_mode;
+
 	w2_sercomm_append_msg(msg_bin);
-	free(msg);
 	free(msg_bin);
 }