diff options
author | Peter Wu <peter@lekensteyn.nl> | 2015-06-17 16:28:16 +0200 |
---|---|---|
committer | Peter Wu <peter@lekensteyn.nl> | 2015-06-17 16:28:16 +0200 |
commit | 940be0b262f372b2eeb883e3ccebf27863d4f1c2 (patch) | |
tree | beb66a2f3b54cf302cb17c42abeffb19697a1146 /Venus_Skeleton | |
parent | 8c61ee80774a7c07705241ec725833379c2d8f4f (diff) | |
download | code-940be0b262f372b2eeb883e3ccebf27863d4f1c2.tar.gz |
Always stop in OPMODE_WAIT
Diffstat (limited to 'Venus_Skeleton')
-rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 14 |
1 files 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 |