diff options
Diffstat (limited to 'robot/modes.c')
| -rw-r--r-- | robot/modes.c | 20 |
1 files changed, 19 insertions, 1 deletions
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); +} |