added error beeps and fix mode history IOB error

1 files changed, 3 insertions, 11 deletions

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