fix object detection

1 files changed, 15 insertions, 6 deletions

diff --git a/robot/io.c b/robot/io.c
index 717778a..42c3a4b 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -5,25 +5,34 @@
 #include "modes.h"
 #include "orangutan_shim.h"
 
-#include <stdio.h>
-
 bool g_w2_io_object_detected = false;
 
+#include <stdio.h>
+
 void w2_io_object_detection() {
-	unsigned int front_distance = analog_read(W2_FRONT_SENSOR_PIN);
+	unsigned int front_distance = analog_read(W2_SIDE_SENSOR_PIN);
 
 	if (g_w2_mode_history[g_w2_mode_history_index] == W2_M_HALT) return;
 
 	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);
-		w2_errcatch_throw(W2_E_WARN_OBSTACLE_DETECTED);
+		lcd_goto_xy(0, 0);
+		print("detected");
 	}
 
 	if (g_w2_io_object_detected) {
-		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)
+		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) {
 			w2_errcatch_throw(W2_E_CRIT_OBSTACLE_STUCK);
+			lcd_goto_xy(0, 0);
+			print("unavoid ");
+		}
+
 		set_motors(0, 0);
 	}
 }