26 lines
909 B
Java
26 lines
909 B
Java
package frc.robot;
|
|
|
|
public class Utils {
|
|
|
|
/**
|
|
* Calculates the needed angle of the turret for a distance from the HUB.
|
|
* @param distanceToTag
|
|
* @return number of rotations of the turret, robot relative
|
|
*/
|
|
public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
|
|
|
|
// Calculate turret Angle
|
|
double cameraDistanceToTarget = distanceToTag + .6;
|
|
double cameraToShootDistance = 0.26035;
|
|
// a^2 + b^2 = c^2
|
|
double shooterDistanceToTarget = Math
|
|
.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
|
|
double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
|
|
double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
|
|
double robotTurretRotations = (robotTurretAngle / Math.PI);
|
|
|
|
return robotTurretRotations;
|
|
}
|
|
|
|
}
|