From ed1ee8406fa5fd41236b60b37125d90f87256702 Mon Sep 17 00:00:00 2001 From: Peter de Kok Date: Wed, 10 Jun 2015 17:28:01 +0200 Subject: committable I hope --- Venus_Skeleton/Venus_Skeleton.ino | 14 ++++++++------ 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 -- cgit v1.2.1