diff options
| -rw-r--r-- | robot/errcatch.c | 37 | ||||
| -rw-r--r-- | robot/io.c | 14 | ||||
| -rw-r--r-- | robot/modes.c | 3 |
3 files changed, 29 insertions, 25 deletions
diff --git a/robot/errcatch.c b/robot/errcatch.c index ed4de51..fc6a1e8 100644 --- a/robot/errcatch.c +++ b/robot/errcatch.c @@ -1,26 +1,36 @@ #include <string.h> +#include <stdio.h> #include "../shared/errcatch.h" #include "modes.h" #include "orangutan_shim.h" #include "sercomm.h" -// #include <stdio.h> -// unsigned int g_w2_err_index = 0; -// unsigned int g_w2_err_disp[4] = {0}; +void w2_errcatch_display_error(uint8_t code) { + if (code == W2_E_WARN_UNCAUGHT_ERROR) { + lcd_goto_xy(5, 0); + print("[u]"); + } else { + char code_str[5]; + sprintf(code_str, "0x%02x", code); + lcd_goto_xy(0, 0); + print(code_str); + } +} + +void w2_errcatch_error_beep(uint8_t code) { + if (code == W2_E_WARN_UNCAUGHT_ERROR) return; + uint8_t severity = code & W2_E_TYPE_MASK; + if ((severity ^ W2_E_TYPE_CRIT) == 0) { + play("L70 O6 fRfRfRfRf"); + } else if ((severity ^ W2_E_TYPE_WARN) == 0) { + play("L40 O5 eRe"); + } +} void w2_errcatch_handle_error(w2_s_error *error) { uint8_t severity = error->code & W2_E_TYPE_MASK; -// clear(); -// g_w2_err_disp[g_w2_err_index++] = error->code; -// char disp[32]; -// sprintf(disp, "%02x %02x", g_w2_err_disp[0], g_w2_err_disp[1]); -// print(disp); -// lcd_goto_xy(0, 1); -// sprintf(disp, "%02x %02x", g_w2_err_disp[2], g_w2_err_disp[3]); -// print(disp); - // trigger emergency mode for critical errors if ((severity ^ W2_E_TYPE_CRIT) == 0) w2_modes_call(W2_M_HALT); @@ -57,5 +67,8 @@ void w2_errcatch_handle_error(w2_s_error *error) { w2_sercomm_append_msg(msg_bin); free(msg_bin); + w2_errcatch_display_error(error->code); + w2_errcatch_error_beep(error->code); + return; } @@ -17,21 +17,13 @@ void w2_io_object_detection() { if (!g_w2_io_object_detected && front_distance >= W2_IO_DISTANCE_CLOSE_THRESHOLD) { g_w2_io_object_detected = true; w2_hypervisor_time_start(W2_TIMER_OBJECT_DETECTION); - lcd_goto_xy(0, 0); - print("detected"); + w2_errcatch_throw(W2_E_WARN_OBSTACLE_DETECTED); } if (g_w2_io_object_detected) { - if (front_distance <= W2_IO_DISTANCE_FAR_THRESHOLD) { - g_w2_io_object_detected = false; - lcd_goto_xy(0, 0); - print(" "); - } - if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT) { + if (front_distance <= W2_IO_DISTANCE_FAR_THRESHOLD) g_w2_io_object_detected = false; + if (w2_hypervisor_time_end(W2_TIMER_OBJECT_DETECTION) >= W2_IO_DISTANCE_TOO_CLOSE_TIMEOUT) w2_errcatch_throw(W2_E_CRIT_OBSTACLE_STUCK); - lcd_goto_xy(0, 0); - print("unavoid "); - } set_motors(0, 0); } diff --git a/robot/modes.c b/robot/modes.c index a54bc43..d706c2f 100644 --- a/robot/modes.c +++ b/robot/modes.c @@ -30,8 +30,7 @@ void w2_modes_init() { 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 = - g_w2_mode_history_index + (new_mode == W2_M_PREV ? -1 : 1) * (replace - 1); + int16_t next_history_index = g_w2_mode_history_index + (new_mode == W2_M_PREV ? -1 : 1) * !replace; 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); |