From 8e9bfbc9c44f1c09f5c93572d2ed3bf35cafbb9b Mon Sep 17 00:00:00 2001
From: Peter Wu <peter@lekensteyn.nl>
Date: Fri, 12 Jun 2015 13:21:07 +0200
Subject: Fix degree stop condition, reduce copy/paste errors

---
 Venus_Skeleton/Venus_Skeleton.ino | 103 +++++++++++++++-----------------------
 Venus_Skeleton/definitions.h      |   3 ++
 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
-- 
cgit v1.2.1