diff options
-rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 39 |
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: |