From 119b1e2bf782e42376019a7c7ebbbb05b9fcd2d7 Mon Sep 17 00:00:00 2001 From: Peter de Kok Date: Wed, 10 Jun 2015 17:07:27 +0200 Subject: latest --- Venus_Skeleton/Venus_Skeleton.ino | 24 +++++++++--------------- Venus_Skeleton/calibration_wall-e.h | 1 + 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index 531235c..16e9f53 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -760,20 +760,15 @@ bool confirmSample(int turretVal) { // Turret direction in degrees calculated from forward. (negative is left, positive is right) int checkTurretDeg = (int)((counterTurret - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1); - int checkTurretDegAbs = 90 - abs(checkTurretDeg); - int currValDirectionDegree = - moveTurnToDeg - } - - - if (data.sample_turret_detected[turretVal] || data.sample_gripper_detected[turretVal]) { - if (!data.sample_gripper_detected[turretVal]) { - - // Turn robot to sample - // If still no sample_gripper_detected return false - } - // What is distance + int x = dist * sin((checkTurretDegAbs/180.0)*PI); + int y = 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 } } @@ -899,8 +894,7 @@ void loop() { confirmSample(); - moveTurnToDeg(); - currValTurret + moveTurnToDeg(currValDirectionDegree); diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h index 2c88c38..524656c 100644 --- a/Venus_Skeleton/calibration_wall-e.h +++ b/Venus_Skeleton/calibration_wall-e.h @@ -25,3 +25,4 @@ // Straight calibration ((1/ms) per cm) #define CAL_MOVE_TURN 0.0154 +#define CAL_TURRET_TO_CENTER_DIST 6 \ No newline at end of file -- cgit v1.2.1