[WIP] last touches for second assessment

1 files changed, 4 insertions, 2 deletions

diff --git a/robot/io.c b/robot/io.c
index 8add1e9..9977e2f 100644
--- a/robot/io.c
+++ b/robot/io.c
@@ -27,11 +27,13 @@ void w2_io_object_detection() {
 		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);
-
-		set_motors(0, 0);
 	}
 }
 
+void w2_set_motors(int left, int right) {
+	set_motors(left * (1 - g_w2_io_object_detected), right * (1 - g_w2_io_object_detected));
+}
+
 void w2_io_battery_logic() {
 	if (w2_hypervisor_time_end(W2_TIMER_BATTERY_MEASUREMENT) <= W2_BATTERY_MEAS_FREQ) return;
 	w2_hypervisor_time_start(W2_TIMER_BATTERY_MEASUREMENT);