summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorPeter Wu <peter@lekensteyn.nl>2015-06-12 13:21:07 +0200
committerPeter Wu <peter@lekensteyn.nl>2015-06-12 13:21:49 +0200
commit8e9bfbc9c44f1c09f5c93572d2ed3bf35cafbb9b (patch)
treed891807ac07144aacc943a8b3a846a0a6a7b7fa8
parentf6655dbb1377bc84602c0e7b823a10acc3b8c2b0 (diff)
downloadcode-8e9bfbc9c44f1c09f5c93572d2ed3bf35cafbb9b.tar.gz
Fix degree stop condition, reduce copy/paste errors
-rw-r--r--Venus_Skeleton/Venus_Skeleton.ino103
-rw-r--r--Venus_Skeleton/definitions.h3
2 files changed, 44 insertions, 62 deletions
diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index c3832c1..bab00e2 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -447,58 +447,25 @@ void stopAllServos() {
timerMovementStop = 0;
}
-void moveTurnTo(int direction) {
- if (direction == 0 || direction == NUM_DIRECTIONS) {
- return;
- }
-
- // Make sure turret doesn't move
- timerTurret = 1;
-
- // if movementtimer is not started, start timer or something and initiate movement
- // else continue movement and return false
- // if movementtimer is expired, stop movement.
-
- if (!timerMovementStop) {
- DEBUG_PRINT(" Start turn move: ");
- DEBUG_PRINT(direction);
- DEBUG_PRINT(" - ");
- counterMovement++;
- // timer = now() + direction * formula
- timerMovementStart = millis();
- timerMovementStop = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS);
-
- // DO A COMPASS CHECK
-
- maneuver(0,0);
- if(direction < NUM_DIRECTIONS){
- DEBUG_PRINT(" left\n");
- // turn left
- maneuver(-100,100);
- }
- if(direction >= NUM_DIRECTIONS){
- DEBUG_PRINT(" right\n");
- // Turn right
- maneuver(100,-100);
- }
- } else {
- int sign = direction < NUM_DIRECTIONS ? -1 : 1;
- int deg = ((int)((millis() - timerMovementStart) * CAL_MOVE_TURN * sign)) % 360;
- data.robot_curr_deg = (data.robot_curr_deg + deg + 360*3) % 360;
- }
-
- if (timerMovementStop < millis()) {
- stopMovement();
- currValDirection = 0;
- for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) {
- data.obstacle_turret_distances[k] = 300;
- }
- }
-}
-
-void moveTurnToDeg(int degree) {
- if (degree == 0 || degree == NUM_DIRECTIONS) {
- return;
+/**
+ * Maps directions in the range [0..12] to [0..360] and [12..24] to [0..-360].
+ */
+int directionToDegree(int direction) {
+ int deg = (direction % NUM_DIRECTIONS) * 360 / NUM_DIRECTIONS;
+ // lower half is to the left (so negative, counter-clockwise)
+ return direction < NUM_DIRECTIONS ? -deg : deg;
+}
+
+/**
+ * Rotates the robot and calculates its current orientation. If the desired
+ * rotation has already been completed, nothing happens.
+ *
+ * @returns true when the rotation is completed.
+ */
+bool moveTurnTo(int degree) {
+ if (degree % 360 == 0) {
+ // already in the right orientation, no need to rotate.
+ return true;
}
// Make sure turret doesn't move
@@ -515,34 +482,42 @@ void moveTurnToDeg(int degree) {
counterMovement++;
// timer = now() + direction * formula
timerMovementStart = millis();
- timerMovementStop = millis() + (degree % NUM_DIRECTIONS) * CAL_MOVE_TURN;
+ timerMovementStop = timerMovementStart + CAL_MOVE_TURN * abs(degree);
// DO A COMPASS CHECK
maneuver(0,0);
- if(degree < 0){
+ if (degree < 0) {
DEBUG_PRINT(" left\n");
// turn left
maneuver(-100,100);
}
- if(degree > 0){
+ if (degree > 0) {
DEBUG_PRINT(" right\n");
// Turn right
maneuver(100,-100);
}
} else {
- int sign = degree < 0 ? -1 : 1;
- int deg = ((int)((millis() - timerMovementStart) * CAL_MOVE_TURN * sign)) % 360;
- data.robot_curr_deg = (data.robot_curr_deg + deg + 360*3) % 360;
+ int deg = (int)(CAL_MOVE_TURN * (millis() - timerMovementStart));
+ deg %= 360;
+ if (degree < 0) {
+ deg *= -1;
+ }
+ // +360 to ensure that the angle stays positive
+ data.robot_curr_deg = (data.robot_curr_deg + deg + 360) % 360;
}
if (timerMovementStop < millis()) {
stopMovement();
- currValDirectionDegree = 0;
for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) {
- data.obstacle_turret_distances[k] = 300;
+ data.obstacle_turret_distances[k] = OBSTACLE_FAR_AWAY;
}
+ // rotation is completed.
+ return true;
}
+
+ // in other cases the rotation is not completed yet.
+ return false;
}
void moveStraight(int direction, float distance) {
@@ -928,7 +903,9 @@ void loop() {
case OPMODE_MAPPING:
turnTurretToNext();
checkBestRoute(0);
- moveTurnTo(currValDirection);
+ if (moveTurnTo(directionToDegree(currValDirection))) {
+ currValDirection = 0;
+ }
moveStraight(currValDirection, 30.0);
break;
@@ -941,7 +918,9 @@ void loop() {
// - Either a false positive or a success
confirmSample(counterTurret);
turnTurretToNext();
- moveTurnToDeg(currValDirectionDegree);
+ if (moveTurnTo(currValDirectionDegree)) {
+ currValDirectionDegree = 0;
+ }
moveStraight(currValDirectionDegree, 15.0);
break;
diff --git a/Venus_Skeleton/definitions.h b/Venus_Skeleton/definitions.h
index dc7d540..ff50d82 100644
--- a/Venus_Skeleton/definitions.h
+++ b/Venus_Skeleton/definitions.h
@@ -26,6 +26,9 @@
// Threshold value for obstacle turret. No free path!
#define OBSTACLE_TOO_CLOSE 30
+// A very large ("infinity") distance value to an obstacle. It basically means
+// that no obstacle is detected.
+#define OBSTACLE_FAR_AWAY 300
// Maximum time between communications to Raspberry Pi (ms)
#define MAX_COMMUNICATION_DELAY 3000