diff options
Diffstat (limited to 'Venus_Skeleton/Venus_Skeleton.ino')
| -rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 24 |
1 files changed, 9 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); |