diff options
| author | Peter de Kok <p.j.s.d.kok@gmail.com> | 2015-06-10 17:07:27 +0200 |
|---|---|---|
| committer | Peter de Kok <p.j.s.d.kok@gmail.com> | 2015-06-10 17:07:27 +0200 |
| commit | 119b1e2bf782e42376019a7c7ebbbb05b9fcd2d7 (patch) | |
| tree | 89a41f4134eeb6184dc45585e77b0789fd8b0006 | |
| parent | 144ef44e4a3ce3b27a9135a1366ecf4155b13886 (diff) | |
| download | code-119b1e2bf782e42376019a7c7ebbbb05b9fcd2d7.tar.gz | |
latest
| -rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 24 | ||||
| -rw-r--r-- | 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 |