added some debug printspeter

1 files changed, 9 insertions, 1 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 044becb..f28c942 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -794,26 +794,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;