From a8b4777f6f395a4d0576bdccaee4704e22d23d01 Mon Sep 17 00:00:00 2001
From: Peter de Kok <p.j.s.d.kok@gmail.com>
Date: Wed, 10 Jun 2015 18:35:42 +0200
Subject: check Sample & confirm Sample created

---
 Venus_Skeleton/Venus_Skeleton.ino | 70 ++++++++++++++++++++++++++++-----------
 1 file changed, 50 insertions(+), 20 deletions(-)

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index c9481e2..5873726 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -520,6 +520,7 @@ void moveTurnToDeg(int degree) {
 	
 	if (timerMovementStop < millis()) {
 		stopMovement();
+		currValDirectionDegree = 0;
 		for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) {
 			data.obstacle_turret_distances[k] = 300;
 		}
@@ -598,6 +599,9 @@ void opMode(opmode_t opmode) {
 	DEBUG_PRINT("\nOPMODE");
 	DEBUG_PRINT(opmode);
 	DEBUG_PRINT("\n\n");
+	if(operationMode != opmode) {
+		counterSampleConfirm = 0;
+	}
 	operationMode = opmode;
 	counterTurretWait = 0;
 }
@@ -732,10 +736,14 @@ int eliminateDirections(int directionArray[]) {
 }
 
 void checkSample() {
-	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected || data.sample_gripper_detected)) {
+	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected[counterTurret] || data.sample_gripper_detected) && (data.obstacle_turret_distances[counterTurret] > 37)) {
 		// One of the turret sees a sample (or the lab)
 		opMode(OPMODE_CHECKSAMPLE);
-		stopAllServos();
+		
+		// If sample straight ahead, set counterTurret to 6
+		if (data.sample_gripper_detected) {
+			counterTurret = 3;
+		}
 		sensSampleTurret();
 		sensSampleGripper();
 		sensObstacleTurret();
@@ -744,21 +752,41 @@ void checkSample() {
 
 bool confirmSample(int turretVal) {
 	if (!counterSampleConfirm) {
-		
+		stopAllServos();
 		// Turret direction in degrees calculated from forward. (negative is left, positive is right)
-		int checkTurretDeg = (int)((counterTurret - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1);
+		int checkTurretDeg = (int)((turretSequence[counterTurret] - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1);
+		int sign = checkTurretDeg < 0 ? -1 : 1;
 		
+		// Calculate angle the robot needs to turn to point in direction of possible sample.
 		int checkTurretDegAbs = 90 - abs(checkTurretDeg);
-		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;
+		int x = CAL_SAMPLE_TURRET_MAX_DIST * cos((checkTurretDegAbs/180.0)*PI);
+		int y = CAL_SAMPLE_TURRET_MAX_DIST * sin((checkTurretDegAbs/180.0)*PI) + CAL_TURRET_TO_CENTER_DIST;
+		int currValDirectionDegree = (int)((atan2(x, y)/PI) * 180.0);
+		currValDirectionDegree += (x == 0 ? 0 : 10);
+		currValDirectionDegree *= sign;
 		
 		counterSampleConfirm++;
 	} else {
-		if (data.sample_gripper_detected) {
+		// Check if a sample was seen by the gripperSampleDetector and the movement to drive the grippers over it are done
+		if (!counterMovement && counterSampleConfirm == 2) {
+			// Pick it up
+			opMode(OPMODE_GRABSAMPLE);
+		}
+		// Check if a sample is seen by the gripperSampleDetector
+		if (data.sample_gripper_detected && (counterSampleConfirm == 1 || counterSampleConfirm == 3)) {
 			stopMovement();
 			currValDirectionDegree = 0;
+			counterSampleConfirm = 2;
+		}
+		// Check if movement has stopped (turn and forward), without seeing a sample (second turn). -> False positive
+		if (!counterMovement && counterSampleConfirm == 3) {
+			opMode(OPMODE_WAIT);
+		}
+		// Check if movement has stopped (turn and forward), without seeing a sample (first turn). -> Turn other way
+		if (!counterMovement && counterSampleConfirm == 1) {
+			int sign = currValDirectionDegree < 0 ? 1 : -1;
+			currValDirectionDegree = 60 * sign;
+			counterSampleConfirm = 3
 		}
 	}
 }
@@ -837,7 +865,8 @@ void loop() {
 	//sensSampleGripper();
 	
 	//sensLab();
-	
+
+	checkSample();
 	checkFreePath();
 
 	// calculateOrientation();
@@ -872,7 +901,6 @@ void loop() {
 		break;
 		
 		case OPMODE_MAPPING:
-		checkSample();
 		turnTurretToNext();
 		checkBestRoute(0);
 		moveTurnTo(currValDirection);
@@ -880,24 +908,26 @@ void loop() {
 		break;
 		
 		case OPMODE_CHECKSAMPLE:
-		
-		
-		
-		
+		// Sequence:
+		// - Turn a little further then turret direction or until sample detected
+		// - Drive forward 15 cm (either to pick up, or try a little further)
+		// - Turn other way for 60deg or until sample detected
+		// - Drive forward 15 cm (either to pick up, or to finish sequence)
+		// - Either a false positive or a success
 		confirmSample(counterTurret);
+		turnTurretToNext();
 		moveTurnToDeg(currValDirectionDegree);
-		moveStraight(currValDirectionDegree, 10.0);
-		
-		
-		
-		
+		moveStraight(currValDirectionDegree, 15.0);
 		break;
 		
 		case OPMODE_GRABSAMPLE:	
+		// Close grippers to calibration value (sample sensor should not see grippers)
 		break;
 		
 		case OPMODE_GOTOLABLOCATION:
 		sensBeaconTurret();
+		// check if sample still there.
+		// calculate prefered direction to (0, 0)
 		break;
 		
 		case OPMODE_WAITFORLAB:
-- 
cgit v1.2.1