Merge remote-tracking branch 'origin/peter'

1 files changed, 24 insertions, 15 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index bab00e2..efc1322 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -433,14 +433,14 @@ void maneuver(int speedLeft, int speedRight){
 }
 
 void stopMovement() {
-	DEBUG_PRINT(" Stop Movement\n");
+	//DEBUG_PRINT(" Stop Movement\n");
 	maneuver(0, 0);
 	timerMovementStart = 0;
 	timerMovementStop = 0;
 }
 
 void stopAllServos() {
-	DEBUG_PRINT(" Stop All Servos\n");
+	//DEBUG_PRINT(" Stop All Servos\n");
 	maneuver(0, 0); 
 	timerTurret = 0;
 	timerMovementStart = 0;
@@ -529,9 +529,9 @@ void moveStraight(int direction, float distance) {
 	float drivetime = distance / CAL_MOVE_STRAIGHT;
 	
 	if (!timerMovementStop) {
-		DEBUG_PRINT("  Start straight move: ");
-		DEBUG_PRINT(distance);
-		DEBUG_PRINT(" - ");
+		//DEBUG_PRINT("  Start straight move: ");
+		//DEBUG_PRINT(distance);
+		//DEBUG_PRINT(" - ");
 		counterMovement++;
 		// timer = now() + direction * formula
 		timerMovementStart = millis();
@@ -539,13 +539,13 @@ void moveStraight(int direction, float distance) {
 		
 		// If input is positive drive forward
 		if(distance > 0) {
-			DEBUG_PRINT(" fw\n");
-			maneuver(100, 100);
+			//DEBUG_PRINT(" fw\n");
+			//maneuver(100, 100);
 		}
 		// If input is negative drive backwards
 		if(distance < 0){
-			DEBUG_PRINT(" bw\n");
-			maneuver(-100, -100);
+			//DEBUG_PRINT(" bw\n");
+			//maneuver(-100, -100);
 		}
 	} else {
 		float dist = (millis() - timerMovementStart) * CAL_MOVE_STRAIGHT;
@@ -709,8 +709,8 @@ int eliminateDirections(int directionArray[]) {
 			}
 		}
 		
-		DEBUG_PRINT(" ");
-		DEBUG_PRINT(directionArray[i]);
+		//DEBUG_PRINT(" ");
+		//DEBUG_PRINT(directionArray[i]);
 		if (directionArray[i]) {
 			numPossibleDirections++;
 			// Implement PI's choices (if still possible)
@@ -730,6 +730,7 @@ int eliminateDirections(int directionArray[]) {
 
 void checkSample() {
 	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected[counterTurret] || data.sample_gripper_detected) && (data.obstacle_turret_distances[counterTurret] > 37)) {
+		DEBUG_PRINT("\n  -- FOUND SAMPLE?\n");
 		// One of the turret sees a sample (or the lab)
 		opMode(OPMODE_CHECKSAMPLE);
 		
@@ -759,26 +760,34 @@ void confirmSample(int turretVal) {
 		currValDirectionDegree += (x == 0 ? 0 : 10);
 		currValDirectionDegree *= sign;
 		
+		DEBUG_PRINT("\n currvaldeg = ");
+		DEBUG_PRINT(currValDirectionDegree);
+		
 		counterSampleConfirm++;
 	} else {
+		DEBUG_PRINT("\n currvaldeg = ");
+		DEBUG_PRINT(currValDirectionDegree);
 		// 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
+			DEBUG_PRINT("\n---GRAB SAMPLE");
 			opMode(OPMODE_GRABSAMPLE);
 		}
 		// Check if a sample is seen by the gripperSampleDetector
 		if (data.sample_gripper_detected && (counterSampleConfirm == 1 || counterSampleConfirm == 3)) {
 			stopMovement();
-			DEBUG_PRINT("sample detected");
+			DEBUG_PRINT("\nsample detected");
 			currValDirectionDegree = 0;
 			counterSampleConfirm = 2;
 		}
 		// Check if movement has stopped (turn and forward), without seeing a sample (second turn). -> False positive
 		if (!counterMovement && counterSampleConfirm == 3) {
+			DEBUG_PRINT("\n---FALSE POSITIVE")
 			opMode(OPMODE_WAIT);
 		}
 		// Check if movement has stopped (turn and forward), without seeing a sample (first turn). -> Turn other way
 		if (!counterMovement && counterSampleConfirm == 1) {
+			DEBUG_PRINT("\ncheck other side");
 			int sign = currValDirectionDegree < 0 ? 1 : -1;
 			currValDirectionDegree = 60 * sign;
 			counterSampleConfirm = 3;
@@ -863,8 +872,8 @@ void loop() {
 	sensObstacleTurret();
 	sensIRLine(L);
 	sensIRLine(R);
-	//sensSampleTurret();
-	//sensSampleGripper();
+	sensSampleTurret();
+	sensSampleGripper();
 	
 	//sensLab();
 
@@ -906,7 +915,7 @@ void loop() {
 		if (moveTurnTo(directionToDegree(currValDirection))) {
 			currValDirection = 0;
 		}
-		moveStraight(currValDirection, 30.0);
+		moveStraight(currValDirection, 100.0); // find distance
 		break;
 		
 		case OPMODE_CHECKSAMPLE: