diff options
| author | Peter de Kok <p.j.s.d.kok@gmail.com> | 2015-06-10 17:28:01 +0200 |
|---|---|---|
| committer | Peter de Kok <p.j.s.d.kok@gmail.com> | 2015-06-10 17:28:01 +0200 |
| commit | ed1ee8406fa5fd41236b60b37125d90f87256702 (patch) | |
| tree | 387da6b748073820e224f68b59278c01fa2eb58e | |
| parent | baa758df26075e61eaeb71d7e7c65cecef10133d (diff) | |
| download | code-ed1ee8406fa5fd41236b60b37125d90f87256702.tar.gz | |
committable I hope
| -rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 14 | ||||
| -rw-r--r-- | Venus_Skeleton/calibration_wall-e.h | 2 |
2 files changed, 10 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); diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h index 7b404dc..16e0ebe 100644 --- a/Venus_Skeleton/calibration_wall-e.h +++ b/Venus_Skeleton/calibration_wall-e.h @@ -26,3 +26,5 @@ #define CAL_MOVE_STRAIGHT 0.0154 #define CAL_TURRET_TO_CENTER_DIST 6 + +#define CAL_SAMPLE_TURRET_MAX_DIST 27
\ No newline at end of file |