diff options
Diffstat (limited to 'robot/modes.c')
| -rw-r--r-- | robot/modes.c | 30 |
1 files changed, 25 insertions, 5 deletions
diff --git a/robot/modes.c b/robot/modes.c index 1f36703..8d3a099 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -1,15 +1,31 @@ +#include <stdbool.h> + +#include "../shared/util.h" +#include "errcatch.h" #include "modes.h" #include "sercomm.h" -void (*g_w2_current_mode)() = &w2_mode_halt; +/** 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_main() { (*g_w2_current_mode)(); } +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); + } -void w2_modes_switch(w2_e_mode new_mode) { if (new_mode == W2_M_PREV) { - // TODO implement previous mode buffer + g_w2_mode_history_index = next_history_index; } else { - g_w2_current_mode = W2_MODES[new_mode]; + 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 @@ -22,3 +38,7 @@ void w2_modes_switch(w2_e_mode new_mode) { free(msg); free(msg_bin); } + +void w2_modes_call(w2_e_mode mode) { w2_modes_switch(mode, false); } + +void w2_modes_swap(w2_e_mode mode) { w2_modes_switch(mode, true); } |