From ed1ee8406fa5fd41236b60b37125d90f87256702 Mon Sep 17 00:00:00 2001
From: Peter de Kok <p.j.s.d.kok@gmail.com>
Date: Wed, 10 Jun 2015 17:28:01 +0200
Subject: committable I hope

---
 Venus_Skeleton/Venus_Skeleton.ino | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

(limited to 'Venus_Skeleton/Venus_Skeleton.ino')

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 2ea94d7..7ed20a0 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -520,7 +520,6 @@ void moveTurnToDeg(int degree) {
 	
 	if (timerMovementStop < millis()) {
 		stopMovement();
-		currValDirection = 0;
 		for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) {
 			data.obstacle_turret_distances[k] = 300;
 		}
@@ -750,14 +749,17 @@ bool confirmSample(int turretVal) {
 		int checkTurretDeg = (int)((counterTurret - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1);
 		
 		int checkTurretDegAbs = 90 - abs(checkTurretDeg);
-		int x = dist * sin((checkTurretDegAbs/180.0)*PI);
-		int y = dist * cos((checkTurretDegAbs/180.0)*PI) + CAL_TURRET_TO_CENTER_DIST;
+		int x = CAL_SAMPLE_TURRET_MAX_DIST * sin((checkTurretDegAbs/180.0)*PI);
+		int y = CAL_SAMPLE_TURRET_MAX_DIST * cos((checkTurretDegAbs/180.0)*PI) + CAL_TURRET_TO_CENTER_DIST;
 		
 		int currValDirectionDegree = (int)((atan2(x / y)/PI) * 180.0 ) + 10;
 		
 		counterSampleConfirm++;
 	} else {
-		if data.sample_gripper_detected
+		if (data.sample_gripper_detected) {
+			stopMovement();
+			currValDirectionDegree = 0;
+		}
 	}
 }
 
@@ -882,9 +884,9 @@ void loop() {
 		
 		
 		
-		confirmSample();
+		confirmSample(counterTurret);
 		moveTurnToDeg(currValDirectionDegree);
-		
+		moveStraight(currValDirectionDegree);
 		
 		
 		
-- 
cgit v1.2.1