322 lines
16 KiB
Java
322 lines
16 KiB
Java
package frc.robot;
|
|
|
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
import frc.robot.Constants;
|
|
import frc.robot.Constants.ShootingRegion;
|
|
import frc.robot.Constants.ShootingRegionDimensions;
|
|
|
|
import org.littletonrobotics.junction.Logger;
|
|
|
|
import edu.wpi.first.math.MathUtil;
|
|
import edu.wpi.first.math.geometry.Pose3d;
|
|
import edu.wpi.first.math.util.Units;
|
|
import edu.wpi.first.units.measure.Angle;
|
|
|
|
import java.util.Map;
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Determines the region for the robot based on its location and alliance.
|
|
* Assumes the robot is a 27" square. Thresholds based on observations from simulator.
|
|
* @param robotPose the robot's pose
|
|
* @param alliance the red/blue alliance of the robot, as a sanity check
|
|
* @return the robot's shooting region
|
|
*/
|
|
public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
|
|
ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION; // the default return value if robot isn't in a valid region
|
|
double robotX = robotPose.getX();
|
|
double robotY = robotPose.getY();
|
|
|
|
// Recommended: Add a sanity check for invalid robot poses.
|
|
|
|
if (alliance == Alliance.Blue) {
|
|
if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
|
|
region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
|
|
}
|
|
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
|
|
region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
|
|
}
|
|
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
|
|
region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;
|
|
}
|
|
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
|
|
robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
|
|
region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
|
|
}
|
|
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
|
|
robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
|
|
region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
|
|
}
|
|
}
|
|
else if (alliance == Alliance.Red) {
|
|
if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
|
|
region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
|
|
}
|
|
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
|
|
region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
|
|
}
|
|
else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
|
|
robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
|
|
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
|
|
region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
|
|
}
|
|
else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
|
|
robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
|
|
region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
|
|
}
|
|
else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
|
|
robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
|
|
region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
|
|
}
|
|
}
|
|
|
|
Logger.recordOutput("Utils-ShootingRegion", region);
|
|
|
|
return region;
|
|
}
|
|
|
|
|
|
/**
|
|
* Calculates the ideal hood angle based on the provided distance
|
|
* between the robot's turret and the target (hub).
|
|
* Implements the equation to find hood angle for scoring:
|
|
* Angle = MaxAngle - ((Distance - ShortDistance) / Div), where
|
|
* MaxAngle = maximum hood angle for scoring
|
|
* Distance = distance to target
|
|
* ShortDistance = shortest distance used when evaluating hood angles
|
|
* LongDistance = longest distance used when evaluating hood angles
|
|
* Div = the distance from the target that requires 1° of hood angle change
|
|
* = (LongDistance - ShortDistance) / (MaxAngle - MinAngle)
|
|
* These were all defined as constants in Constants.java.
|
|
* @param distanceInMeters distance between turret and target, in meters
|
|
* @return the ideal nearest-integer hood angle in degrees relative to the floor
|
|
*/
|
|
public static int getScoringHoodAngleDegForDistance(double distanceInMeters){
|
|
double calculatedAngleDeg = Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG; // init to the passing value
|
|
double distanceInInches = Units.metersToInches(distanceInMeters); // Convert to inches to match theory equations
|
|
|
|
// Limit the distance according to the min and max that were used in the theory calculations
|
|
MathUtil.clamp(distanceInInches, Constants.ShooterConstants.SCORING_DISTANCE_SHORT_IN, Constants.ShooterConstants.SCORING_DISTANCE_LONG_IN);
|
|
|
|
// Perfom the main calculation (Equation 2-5 in the AutoShooter document), then round the result to the nearest degree
|
|
calculatedAngleDeg = Constants.ShooterConstants.HOOD_ANGLE_SCORING_MAX_DEG - (((distanceInInches) - Constants.ShooterConstants.SCORING_DISTANCE_SHORT_IN) /
|
|
Constants.ShooterConstants.HOOD_ANGLE_CALC_DIVISION);
|
|
int hoodAngleDeg = (int) Math.round(calculatedAngleDeg);
|
|
|
|
return (hoodAngleDeg);
|
|
}
|
|
|
|
|
|
|
|
public static double getHoodAngleForDistance(double distanceInMeters){
|
|
if(distanceInMeters >= 4){
|
|
return 200;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
/**
|
|
* Calculates the appropriate launch speed in ft/s for scoring based on
|
|
* the provided hood angle and target distance in meters.
|
|
* This is done based on a series of quadratic regressions
|
|
* See the AutoShooter theory document.
|
|
* @param hoodAngleDeg the hood angle that the calculation should assume
|
|
* @param distanceInMeters distance between turret and target, in meters
|
|
* @return the appropriate launch speed for scoring in feet per second
|
|
* Returns 0 if a launch speed cannot be determined.
|
|
*/
|
|
public static double findLaunchSpeedScoring(int hoodAngleDeg, double distanceInMeters){
|
|
// Check that the requested hood angle is in the expected shooting range
|
|
if ((hoodAngleDeg < Constants.ShooterConstants.HOOD_ANGLE_SCORING_MIN_DEG) ||
|
|
(hoodAngleDeg > Constants.ShooterConstants.HOOD_ANGLE_SCORING_MAX_DEG)) {
|
|
return 0;
|
|
}
|
|
|
|
// Check that the requested distance is reasonable. If the scoring distance is outside of this range, there was a problem.
|
|
if ((distanceInMeters < Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MIN_METERS) ||
|
|
(distanceInMeters > Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MAX_METERS)) {
|
|
return 0;
|
|
}
|
|
|
|
// Get the quadratic coefficients for the hood angle. Return with the error value if none are found.
|
|
Constants.ShooterConstants.QuadraticCoef coefficients = Constants.ShooterConstants.SCORING_SPEED_COEFS_METERS.get(hoodAngleDeg);
|
|
if (coefficients == null) {
|
|
return 0;
|
|
}
|
|
|
|
// Perform the quadratic calculation using the coefficients we retrieved if successful
|
|
double x = distanceInMeters; // Rename to "x" just to make the next expression simpler
|
|
double launchSpeedFPS = (coefficients.a() * x * x) + (coefficients.b() * x) + coefficients.c();
|
|
|
|
return launchSpeedFPS;
|
|
}
|
|
|
|
/**
|
|
* Calculates the appropriate launch speed in ft/s for passing based on
|
|
* the provided hood angle and target distance in meters.
|
|
* This is done based on a series of quadratic regressions
|
|
* See the AutoShooter theory document.
|
|
* @param hoodAngleDeg the hood angle that the calculation should assume
|
|
* @param distanceInMeters distance between turret and target, in meters
|
|
* @return the appropriate launch speed for passing in feet per second
|
|
* Returns 0 if a launch speed cannot be determined.
|
|
*/
|
|
public static double findLaunchSpeedPassing(int hoodAngleDeg, double distanceInMeters){
|
|
// Check that the requested hood angle is in the expected angle range for passing.
|
|
if (hoodAngleDeg != Constants.ShooterConstants.HOOD_ANGLE_PASSING_DEG) {
|
|
return 0;
|
|
}
|
|
|
|
// Check that the distance is reasonable. If the scoring distance is outside of this range, there was a problem.
|
|
if ((distanceInMeters < Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MIN_METERS) ||
|
|
(distanceInMeters > Constants.ShooterConstants.SCORING_DISTANCE_FIELD_MAX_METERS)) {
|
|
return 0;
|
|
}
|
|
|
|
// Get the quadratic coefficients for the hood angle. Return with the error value if none are found.
|
|
Constants.ShooterConstants.QuadraticCoef coefficients = Constants.ShooterConstants.PASSING_SPEED_COEFS_METERS.get(hoodAngleDeg);
|
|
if (coefficients == null) {
|
|
return 0;
|
|
}
|
|
|
|
// Perform the quadratic calculation using the coefficients we retrieved if successful
|
|
double x = distanceInMeters; // Rename to "x" just to make the next expression simpler
|
|
double launchSpeedFPS = (coefficients.a() * x * x) + (coefficients.b() * x) + coefficients.c();
|
|
|
|
return launchSpeedFPS;
|
|
}
|
|
|
|
/**
|
|
* Estimates the fuel's time of flight until it his the target,
|
|
* where the target is the top of the hub when scoring and the floor when passing
|
|
* launch speed in feet/second.
|
|
* This performs Equations 3-7 and 3-8 in the AutoShooter document.
|
|
* @param scoring true if the bot is scoring into the hub (false means passing)
|
|
* @param launchAngleDeg the target launch angle in degrees
|
|
* @param launchSpeedFPS the target launch speed in ft/s
|
|
* @return the approxmate fuel flight time in seconds
|
|
*/
|
|
public static double findTimeOfFlightScoring(boolean scoring, int launchAngleDeg, double launchSpeedFPS){
|
|
double velocityVertFPS = launchSpeedFPS * Math.sin(Math.toRadians((double)launchAngleDeg)); // The vertical component of fuel's velocity
|
|
double kGravityInFeet = 32.2; // Gravity constant in ft/s^2
|
|
double deltaHFeet = 0; // Heigh difference between launcher and target
|
|
if (scoring) {
|
|
deltaHFeet = (Constants.ShooterConstants.SHOOTER_HEIGHT_INCHES - Constants.ShooterConstants.HUB_OPENING_HEIGHT_INCHES) / 12;
|
|
} else {
|
|
deltaHFeet = Constants.ShooterConstants.SHOOTER_HEIGHT_INCHES / 12;
|
|
}
|
|
|
|
return ((velocityVertFPS + Math.sqrt(Math.pow(velocityVertFPS, 2) - (2 * kGravityInFeet * deltaHFeet))) / kGravityInFeet);
|
|
|
|
}
|
|
|
|
/**
|
|
* Calculates the flywheel RPM that's required to achieve the provided
|
|
* launch speed in feet/second.
|
|
* This performs Equation 2-3 in the AutoShooter document.
|
|
* @param launchSpeedFPS the goal launch speed in ft/s
|
|
* @return the appropriate flywheel speed in RPM
|
|
*/
|
|
public static double GetShooterRPMForLaunchSpeed(double launchSpeedFPS){
|
|
double flywheelDiameterFeet = Constants.ShooterConstants.SHOOTER_FLYWHEEL_DIAMETER_IN / 12; // Change to feet to match launch speed units
|
|
double efficiency = Constants.ShooterConstants.SHOOTER_EFFICIENCY_GENERAL; // Use the appropriate efficiency
|
|
|
|
double shooterRPM = (launchSpeedFPS / (efficiency * flywheelDiameterFeet * Math.PI)) * 60; // Perform the calculation
|
|
return shooterRPM;
|
|
}
|
|
|
|
public static double getLauncherRPMForDistance(double distanceInMeters){
|
|
return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
|
|
}
|
|
|
|
/**
|
|
* Determines the appropriate shooting target for the robot based on its alliance
|
|
* and its current region on the field. There are 10 possible targets:
|
|
* Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
|
|
* and these exist for both red and blue alliances.
|
|
* Right/left are from driver's perspective.
|
|
* @param region the identifier of the robot's current field region
|
|
* @param alliance the red/blue alliance of the robot, as a sanity check
|
|
* @return the shooting target for a robot with the provided properties. Returns all-zero
|
|
* Pose3d object if the region/alliance combination is invalid.
|
|
*/
|
|
public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
|
|
|
|
Pose3d shootingTarget = new Pose3d();
|
|
if (alliance == Alliance.Blue) {
|
|
switch (region) {
|
|
case BLUE_OWN_ALLIANCE_ZONE:
|
|
shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
|
|
break;
|
|
case BLUE_LEFT_NEUTRAL_ZONE:
|
|
shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
case BLUE_RIGHT_NEUTRAL_ZONE:
|
|
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
|
|
shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
|
|
shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
else if (alliance == Alliance.Red) {
|
|
switch (region) {
|
|
case RED_OWN_ALLIANCE_ZONE:
|
|
shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
|
|
break;
|
|
case RED_LEFT_NEUTRAL_ZONE:
|
|
shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
case RED_RIGHT_NEUTRAL_ZONE:
|
|
shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
|
|
shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
|
|
shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
return shootingTarget;
|
|
}
|
|
|
|
}
|