From 940be0b262f372b2eeb883e3ccebf27863d4f1c2 Mon Sep 17 00:00:00 2001 From: Peter Wu Date: Wed, 17 Jun 2015 16:28:16 +0200 Subject: Always stop in OPMODE_WAIT --- Venus_Skeleton/Venus_Skeleton.ino | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index d312d6b..2bfc549 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -631,16 +631,21 @@ void opMode(opmode_t opmode) { counterTurretWait = 0; } +/** + * Given the current known state (including sensor data), find out whether the + * robot can still move forward. + */ void checkFreePath() { - // Line detection + // Stop moving if the IR line sensor found a black spot if ((dataToPiChangedBits & (CHANGED_SENS_IRLEFT | CHANGED_SENS_IRRIGHT)) && (data.IR_left_detected || data.IR_right_detected)) { // Left or Right IR sensor sees inaccessible terrain opMode(OPMODE_WAIT); - stopMovement(); DEBUG_PRINT(" NO FREE PATH: left-"); DEBUG_PRINT(data.IR_left_detected); DEBUG_PRINT(" - right-"); DEBUG_PRINT(data.IR_right_detected); + // no need to check for other data, we cannot move anyway... + return; } // Ultrasound @@ -652,7 +657,6 @@ void checkFreePath() { (data.obstacle_turret_distances[counterTurret] < OBSTACLE_TOO_CLOSE)) { // Ultrasound sensor sees inaccessible terrain opMode(OPMODE_WAIT); - stopMovement(); DEBUG_PRINT(" NO FREE PATH: Obstacle turret-"); DEBUG_PRINT(data.obstacle_turret_distances[counterTurret]); @@ -927,9 +931,7 @@ void loop() { // finally execute the next action switch(operationMode) { case OPMODE_WAIT: - counterMovement = 0; - timerMovementStart = 0; - timerMovementStop = 0; + stopMovement(); if (!initialized) { // whenever the initial sequence to locate the robot has not happened, // keep performing that sequence -- cgit v1.2.1