diff options
Diffstat (limited to 'Venus_Skeleton/Venus_Skeleton.ino')
| -rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 14 |
1 files changed, 8 insertions, 6 deletions
diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index 2ea94d7..7ed20a0 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -520,7 +520,6 @@ void moveTurnToDeg(int degree) { if (timerMovementStop < millis()) { stopMovement(); - currValDirection = 0; for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) { data.obstacle_turret_distances[k] = 300; } @@ -750,14 +749,17 @@ bool confirmSample(int turretVal) { int checkTurretDeg = (int)((counterTurret - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1); int checkTurretDegAbs = 90 - abs(checkTurretDeg); - int x = dist * sin((checkTurretDegAbs/180.0)*PI); - int y = dist * cos((checkTurretDegAbs/180.0)*PI) + CAL_TURRET_TO_CENTER_DIST; + 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; counterSampleConfirm++; } else { - if data.sample_gripper_detected + if (data.sample_gripper_detected) { + stopMovement(); + currValDirectionDegree = 0; + } } } @@ -882,9 +884,9 @@ void loop() { - confirmSample(); + confirmSample(counterTurret); moveTurnToDeg(currValDirectionDegree); - + moveStraight(currValDirectionDegree); |