aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--robot/errcatch.c37
-rw-r--r--robot/io.c14
-rw-r--r--robot/modes.c3
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;
}
diff --git a/robot/io.c b/robot/io.c
index 42c3a4b..91a62b2 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -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);