added error beeps and fix mode history IOB error

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);