aboutsummaryrefslogtreecommitdiff
path: root/robot/modes.c
diff options
context:
space:
mode:
Diffstat (limited to 'robot/modes.c')
-rw-r--r--robot/modes.c44
1 files changed, 41 insertions, 3 deletions
diff --git a/robot/modes.c b/robot/modes.c
index c22875c..8d3a099 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -1,6 +1,44 @@
+#include <stdbool.h>
+
+#include "../shared/util.h"
+#include "errcatch.h"
#include "modes.h"
-#include "halt.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;
+
+void w2_modes_main() { (*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 =
+ g_w2_mode_history_index + (new_mode == W2_M_PREV ? -1 : 1) * (replace - 1);
+ if (next_history_index == -1 || next_history_index == W2_MODE_HISTORY_BUFFER_SIZE - 1) {
+ next_history_index = W2_RANGE(0, next_history_index, W2_MODE_HISTORY_BUFFER_SIZE);
+ w2_errcatch_throw(W2_E_WARN_MODE_HISTORY_BUFFER_IOB);
+ }
+
+ if (new_mode == W2_M_PREV) {
+ 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];
+ }
+
+ // 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);
+}
-void (*g_w2_current_mode)() = &w2_mode_halt;
+void w2_modes_call(w2_e_mode mode) { w2_modes_switch(mode, false); }
-void w2_modes_main() { (*g_w2_current_mode)(); }
+void w2_modes_swap(w2_e_mode mode) { w2_modes_switch(mode, true); }