next commit

1 files changed, 23 insertions, 22 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 7718d8a..044becb 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -442,14 +442,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;
@@ -469,9 +469,9 @@ void moveTurnTo(int direction) {
 	// if movementtimer is expired, stop movement.
 	
 	if (!timerMovementStop) {
-		DEBUG_PRINT("  Start turn move: ");
-		DEBUG_PRINT(direction);
-		DEBUG_PRINT(" - ");
+		//DEBUG_PRINT("  Start turn move: ");
+		//DEBUG_PRINT(direction);
+		//DEBUG_PRINT(" - ");
 		counterMovement++;
 		// timer = now() + direction * formula
 		timerMovementStart = millis();
@@ -481,14 +481,14 @@ void moveTurnTo(int direction) {
 		
 		maneuver(0,0);
 		if(direction < NUM_DIRECTIONS){
-			DEBUG_PRINT(" left\n");
+			//DEBUG_PRINT(" left\n");
 			// turn left
-			maneuver(-100,100);
+			//maneuver(-100,100);
 		}
 		if(direction >= NUM_DIRECTIONS){
-			DEBUG_PRINT(" right\n");
+			//DEBUG_PRINT(" right\n");
 			// Turn right
-			maneuver(100,-100);
+			//maneuver(100,-100);
 		}
 	} else {
 		int sign = direction < NUM_DIRECTIONS ? -1 : 1;
@@ -524,7 +524,7 @@ void moveTurnToDeg(int degree) {
 		counterMovement++;
 		// timer = now() + direction * formula
 		timerMovementStart = millis();
-		timerMovementStop = millis() + (degree % NUM_DIRECTIONS) * CAL_MOVE_TURN;
+		timerMovementStop = millis() + abs(degree) * CAL_MOVE_TURN;
 		
 		// DO A COMPASS CHECK
 		
@@ -563,9 +563,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();
@@ -573,13 +573,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;
@@ -743,8 +743,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)
@@ -764,6 +764,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);
 		
@@ -897,8 +898,8 @@ void loop() {
 	sensObstacleTurret();
 	sensIRLine(L);
 	sensIRLine(R);
-	//sensSampleTurret();
-	//sensSampleGripper();
+	sensSampleTurret();
+	sensSampleGripper();
 	
 	//sensLab();
 
@@ -940,7 +941,7 @@ void loop() {
 		turnTurretToNext();
 		checkBestRoute(0);
 		moveTurnTo(currValDirection);
-		moveStraight(currValDirection, 30.0);
+		moveStraight(currValDirection, 100.0); // find distance
 		break;
 		
 		case OPMODE_CHECKSAMPLE: