summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorPeter Wu <peter@lekensteyn.nl>2015-06-17 16:28:16 +0200
committerPeter Wu <peter@lekensteyn.nl>2015-06-17 16:28:16 +0200
commit940be0b262f372b2eeb883e3ccebf27863d4f1c2 (patch)
treebeb66a2f3b54cf302cb17c42abeffb19697a1146
parent8c61ee80774a7c07705241ec725833379c2d8f4f (diff)
downloadcode-940be0b262f372b2eeb883e3ccebf27863d4f1c2.tar.gz
Always stop in OPMODE_WAIT
-rw-r--r--Venus_Skeleton/Venus_Skeleton.ino14
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