diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 25cba78f..7782091f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -74,7 +74,6 @@ import frc.robot.utils.FieldUtils.TrenchPoses; import frc.robot.utils.FuelSim; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.NewAutoAim; import java.io.File; import java.util.Arrays; import java.util.Optional; @@ -186,7 +185,7 @@ public enum RobotEdition { private final SlewRateLimiter xAccelLimiter = new SlewRateLimiter(1); private final SlewRateLimiter yAccelLimiter = new SlewRateLimiter(1); - private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(3.0); + private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(0.5); private static int lowBatteryCycleCount = 0; private static final double lowBatteryVoltage = @@ -651,6 +650,13 @@ public Robot() { fuelSim.setSubticks(5); // fuelSim.start(); + + // Log climb poses + Logger.recordOutput( + "AutoAlign/Climb Targets", + Arrays.stream(ClimbTargets.values()) + .map(target -> target.getPose()) + .toArray(Pose2d[]::new)); } /** Scales a joystick value for teleop driving */ @@ -713,7 +719,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta Commands.parallel( shooter.runHoodCurrentZeroing(), intake.runCurrentZeroing()))); - new Trigger(() -> NewAutoAim.targetInTurretDeadzone()) + new Trigger(() -> AutoAim.targetInTurretDeadzone()) .onTrue(driver.rumbleCmd(1, 1).withTimeout(0.25)); // .alongWith(operator.rumbleCmd(1, 1).withTimeout(0.25))); // ---zeroing stuff--- @@ -765,7 +771,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta // driver // .leftBumper() // .and( - new Trigger(NewAutoAim::targetInTurretDeadzone) + new Trigger(AutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAScoreState()) .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) @@ -782,7 +788,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), shooter::getTurretPosition)); - new Trigger(NewAutoAim::targetInTurretDeadzone) + new Trigger(AutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAFeedState()) .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) @@ -922,29 +928,24 @@ public void robotPeriodic() { }); updateAlerts(); - Logger.recordOutput("Flywheel Fudge Factor", AutoAim.getFudgeFactor()); - - // Log climb poses - Logger.recordOutput( - "AutoAlign/Climb Targets", - Arrays.stream(ClimbTargets.values()) - .map(target -> target.getPose()) - .toArray(Pose2d[]::new)); + Logger.recordOutput("AutoAim/Flywheel Fudge Factor", AutoAim.getFudgeFactor()); Logger.recordOutput( "trench poses", Arrays.stream(TrenchPoses.values()).map(target -> target.getPose()).toArray(Pose2d[]::new)); - Logger.recordOutput("Turret/out of range", NewAutoAim.targetInTurretDeadzone()); + Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone()); noLogStickAlert.set(!directory.exists()); Logger.recordOutput( - "Distance to hub", + "AutoAim/Distance to hub", shooter .getTurretPose(swerve.getPose()) .getTranslation() .getDistance(FieldUtils.getCurrentHubTranslation())); + Logger.recordOutput( + "AutoAim/Feed Target", FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose()); } public void updateAlerts() { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index e4bbf472..7d44f086 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -24,7 +24,7 @@ import frc.robot.utils.FieldUtils; import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.NewAutoAim; +import frc.robot.utils.autoaim.ShotTrees; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -310,7 +310,7 @@ private void addTriggers() { // .debounce(0.05) .and(new Trigger(shooter::atHoodSetpoint).debounce(0.05)) .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)) - .and(new Trigger(NewAutoAim::targetInTurretDeadzone).negate()); + .and(new Trigger(AutoAim::targetInTurretDeadzone).negate()); } private void addTransitions() { @@ -345,7 +345,7 @@ private void addTransitions() { bindTransition( SuperState.SCORE_FLOW, SuperState.SPIN_UP_SCORE_FLOW, - new Trigger(NewAutoAim::targetInTurretDeadzone)); + new Trigger(AutoAim::targetInTurretDeadzone)); bindTransition( SuperState.SPIN_UP_SCORE_FLOW, @@ -385,7 +385,7 @@ private void addTransitions() { bindTransition( SuperState.FEED_FLOW, SuperState.SPIN_UP_FEED_FLOW, - new Trigger(NewAutoAim::targetInTurretDeadzone)); + new Trigger(AutoAim::targetInTurretDeadzone)); bindTransition( SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate())); @@ -488,13 +488,11 @@ private void addCommands() { indexer.rest(), shooter.feed( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - AutoAim.FEED_SHOT_TREE), - () -> FeedTargets.getFeedTarget(feedTarget).getPose(), - swerve::getPose), + ShotTrees.FEED_SHOT_TREE)), climber.retract()); bindCommands( @@ -502,22 +500,20 @@ private void addCommands() { intake.agitate(), indexer.kick( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - AutoAim.FEED_SHOT_TREE) + ShotTrees.FEED_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), shooter.feed( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - AutoAim.FEED_SHOT_TREE), - () -> FeedTargets.getFeedTarget(feedTarget).getPose(), - swerve::getPose), + ShotTrees.FEED_SHOT_TREE)), climber.retract()); bindCommands( @@ -526,13 +522,11 @@ private void addCommands() { indexer.index(), shooter.feed( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - AutoAim.FEED_SHOT_TREE), - () -> FeedTargets.getFeedTarget(feedTarget).getPose(), - swerve::getPose), + ShotTrees.FEED_SHOT_TREE)), climber.retract()); bindCommands( @@ -540,22 +534,20 @@ private void addCommands() { intake.intake(), indexer.kick( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - AutoAim.FEED_SHOT_TREE) + ShotTrees.FEED_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), shooter.feed( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FeedTargets.getFeedTarget(feedTarget).getTranslation(), - AutoAim.FEED_SHOT_TREE), - () -> FeedTargets.getFeedTarget(feedTarget).getPose(), - swerve::getPose), + ShotTrees.FEED_SHOT_TREE)), climber.retract()); bindCommands( @@ -564,13 +556,13 @@ private void addCommands() { indexer.rest(), shooter.score( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)), + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -579,24 +571,24 @@ private void addCommands() { // intake.restExtended(), indexer.kick( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE) + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), shooter.score( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)), + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -605,13 +597,13 @@ private void addCommands() { indexer.rest(), shooter.score( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)), + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -619,24 +611,24 @@ private void addCommands() { intake.intake(), indexer.kick( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE) + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), shooter.score( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)), + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -678,13 +670,13 @@ private void addCommands() { indexer.rest(), shooter.score( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)), + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.extend()); bindCommands( @@ -692,24 +684,24 @@ private void addCommands() { intake.restRetracted(), indexer.kick( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE) + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), shooter.score( () -> - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( swerve.getPose(), swerve.getVelocityRobotRelative(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)), + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.extend()); bindCommands( diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 718f4705..447f7e98 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.utils.autoaim.NewAutoAim.ShotParams; +import frc.robot.utils.autoaim.AutoAim.ShotParams; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -27,10 +27,7 @@ public interface Shooter extends Subsystem { * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current * pose + feed target */ - public Command feed( - Supplier shotParamsSupplier, - Supplier feedTarget, - Supplier robotPoseSupplier); + public Command feed(Supplier shotParamsSupplier); /** Not running (set to 0) */ public Command rest( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index c53070be..a3dbb3e6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -22,9 +22,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; -import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.AutoAim.ShotParams; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; -import frc.robot.utils.autoaim.NewAutoAim.ShotParams; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -91,18 +90,10 @@ public Command score(Supplier shotParamsSupplier) { } @Override - public Command feed( - Supplier shotParamsSupplier, - Supplier feedTarget, - Supplier robotPoseSupplier) { + public Command feed(Supplier shotParamsSupplier) { return this.run( () -> { - ShotData shotData = - AutoAim.FEED_SHOT_TREE.get( - robotPoseSupplier - .get() - .getTranslation() - .getDistance(feedTarget.get().getTranslation())); + ShotData shotData = shotParamsSupplier.get().shotData(); hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); }); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 8ec4c0a8..72bcd35c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -45,8 +45,8 @@ import frc.robot.utils.FieldUtils; import frc.robot.utils.FuelSim; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.NewAutoAim; -import frc.robot.utils.autoaim.NewAutoAim.ShotParams; +import frc.robot.utils.autoaim.AutoAim.ShotParams; +import frc.robot.utils.autoaim.ShotTrees; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -241,15 +241,11 @@ public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distan public void simulationPeriodic() {} @Override - public Command feed( - Supplier shotParamsSupplier, - Supplier feedTarget, - Supplier robotPoseSupplier) { + public Command feed(Supplier shotParamsSupplier) { return resetTurretToCalculatedPosition() .andThen( this.run( () -> { - Logger.recordOutput("Robot/Feed Target", feedTarget.get()); hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); // flywheelIO.setMotionProfiledFlywheelVelocity( @@ -279,13 +275,13 @@ public Command rest( // } else { if (inScoringArea.getAsBoolean()) { turretIO.setTurretPosition( - NewAutoAim.getParametersMechA( + AutoAim.getShotParameters( robotPoseSupplier.get(), chassisSpeedsSupplierRobotRel.get(), FieldUtils.getCurrentHubTranslation(), Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE) + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE) .turretAngle()); // turretIO.setTurretPosition( // AutoAim.getTurretHubTargetRotation( @@ -294,10 +290,12 @@ public Command rest( // chassisSpeedsSupplier.get())); } else { turretIO.setTurretPosition( - AutoAim.getTurretFeedTargetRotation( - feedTarget.get().getTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplierRobotRel.get())); + AutoAim.getShotParameters( + robotPoseSupplier.get(), + chassisSpeedsSupplierRobotRel.get(), + feedTarget.get().getTranslation(), + ShotTrees.FEED_SHOT_TREE) + .turretAngle()); } // } }); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 141c3d42..00b4ad4c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -38,7 +37,6 @@ import frc.robot.Robot; import frc.robot.Robot.RobotEdition; import frc.robot.Robot.RobotMode; -import frc.robot.Superstructure; import frc.robot.Superstructure.FeedTarget; import frc.robot.components.camera.Camera; import frc.robot.components.camera.CameraIOReal; @@ -67,7 +65,7 @@ import frc.robot.utils.Tracer; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.AutoAlign; -import frc.robot.utils.autoaim.InterpolatingShotTree; +import frc.robot.utils.autoaim.ShotTrees; import java.util.Arrays; import java.util.List; import java.util.Map; @@ -571,8 +569,8 @@ public Command translateToPose( .alongWith( Commands.run( () -> { - Logger.recordOutput("AutoAim/Target Pose", target.get()); - Logger.recordOutput("AutoAim/Speeds Modifier", speedsModifier.get()); + Logger.recordOutput("AutoAlign/Target Pose", target.get()); + Logger.recordOutput("AutoAlign/Speeds Modifier", speedsModifier.get()); }))); } @@ -598,8 +596,8 @@ private Command translateToPose(Supplier target, Supplier .alongWith( Commands.run( () -> { - Logger.recordOutput("AutoAim/Target Pose", target.get()); - Logger.recordOutput("AutoAim/Speeds Modifier", speedsModifier.get()); + Logger.recordOutput("AutoAlign/Target Pose", target.get()); + Logger.recordOutput("AutoAlign/Speeds Modifier", speedsModifier.get()); }))); } @@ -701,14 +699,6 @@ private Command driveWithHeadingSnap( // return driveWithHeadingSnap(() -> AutoAim.getSOTMYaw(getPose(), getVelocityFieldRelative()), // xVel, yVel); // } - public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel, InterpolatingShotTree tree) { - return driveWithHeadingSnap( - () -> - AutoAim.getVirtualTargetYaw( - getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), getPose(), tree), - xVel, - yVel); - } public Command faceHubComp( DoubleSupplier xVel, DoubleSupplier yVel, Supplier turretRotation) { @@ -722,11 +712,12 @@ public Command faceHubComp( // get desired rotation to point at target Rotation2d turretTargetRotation = - AutoAim.getVirtualTargetYaw( - getVelocityFieldRelative(), - FieldUtils.getCurrentHubTranslation(), - turretPose, - AutoAim.COMP_HUB_SHOT_TREE); + AutoAim.getShotParameters( + getPose(), + getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + ShotTrees.COMP_HUB_SHOT_TREE) + .turretAngle(); // subtract that from rotation to point at target turretTargetRotation = turretTargetRotation.minus(getRotation()); Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); @@ -753,14 +744,14 @@ public Command faceFeedComp( .transformBy( new Transform2d( TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); - // get desired rotation to point at target Rotation2d turretTargetRotation = - AutoAim.getVirtualTargetYaw( - getVelocityFieldRelative(), - FeedTargets.getFeedTarget(feedTargetSupplier.get()).getTranslation(), - turretPose, - AutoAim.FEED_SHOT_TREE); + AutoAim.getShotParameters( + getPose(), + getVelocityRobotRelative(), + FeedTargets.getFeedTarget(feedTargetSupplier.get()).getTranslation(), + ShotTrees.FEED_SHOT_TREE) + .turretAngle(); // subtract that from rotation to point at target turretTargetRotation = turretTargetRotation.minus(getRotation()); Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); @@ -775,33 +766,6 @@ public Command faceFeedComp( yVel); } - public boolean isFacingTarget(InterpolatingShotTree tree) { - switch (Superstructure.getShotTarget()) { // ugh maybe this should be in robot.java - case SCORE: - return isFacingHub(tree); - case FEED: - return isFacingFeedTarget(); - default: - return false; - } - } - - public boolean isFacingHub(InterpolatingShotTree tree) { - Rotation2d target = - AutoAim.getVirtualTargetYaw( - getVelocityFieldRelative(), FieldUtils.getCurrentHubTranslation(), getPose(), tree); - return MathUtil.isNear( - target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees - } - - public boolean isFacingFeedTarget() { - Translation2d feedTarget = - FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose().getTranslation(); - Rotation2d target = AutoAim.getTargetRotation(feedTarget, getPose()); - return MathUtil.isNear( - target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees - } - // public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { // return driveWithHeadingSnap( // () -> { @@ -920,12 +884,6 @@ public Pose2d getPose() { return estimator.getEstimatedPosition(); } - @AutoLogOutput(key = "Autoaim/Distance To Hub") - public static double distanceToHub(Pose2d pose) { - double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); - return distance; - } - public Pose3d getPose3d() { return new Pose3d(getPose()); } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 15d4c4a6..cf57d54b 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -15,323 +15,144 @@ public class AutoAim { - private static boolean outOfRange = false; // TODO not sure if this should be true by default + public record ShotParams(ShotData shotData, Rotation2d turretAngle) {} - public static double LATENCY_COMPENSATION_SECS = - // new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency - // comp - 0.0; - // public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time - - public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); - - public static final Rotation2d LEFT_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-73.916016); - public static final Rotation2d MID_FIXED_SHOT_TURRET_ANGLE = Rotation2d.fromDegrees(-82); - public static final Rotation2d RIGHT_FIXED_SHOT_TURRET_ANGLE = - Rotation2d.fromDegrees(-109.775391); + private static boolean outOfRange = false; private static int fudgeFactor = 0; - static { // For hub shot tree - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 + 17), new ShotData(Rotation2d.fromDegrees(8), 27.5, 1.46)); - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), - new ShotData(Rotation2d.fromDegrees(6), 30, 1.55)); - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), - new ShotData(Rotation2d.fromDegrees(10.5), 30, 1.54)); - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), - new ShotData(Rotation2d.fromDegrees(14.5), 30, 1.54)); - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), - new ShotData(Rotation2d.fromDegrees(18.25), 30, 1.52)); - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), - new ShotData(Rotation2d.fromDegrees(21.5), 30, 1.46)); - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), - new ShotData(Rotation2d.fromDegrees(24.5), 30, 1.35)); - ALPHA_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), - new ShotData(Rotation2d.fromDegrees(28), 30, 1.36)); - } - - public static final InterpolatingShotTree COMP_HUB_SHOT_TREE = new InterpolatingShotTree(); + private static double lastVxMetersPerSec = 0.0; + private static double lastVyMetersPerSec = 0.0; + private static double lastOmegaRadPerSec = 0.0; + private static double lastRunTimeSec = 0.0; - static { - COMP_HUB_SHOT_TREE.put( - 1.716849, - new ShotData( - Rotation2d.fromDegrees(23 - 13.16), - 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 0.8)); - COMP_HUB_SHOT_TREE.put( - 2.017596, - new ShotData( - Rotation2d.fromDegrees(23 - 13.16), - 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 0.9)); - COMP_HUB_SHOT_TREE.put( - 2.423868, - new ShotData( - Rotation2d.fromDegrees(25 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.1)); - COMP_HUB_SHOT_TREE.put( - 2.664198, - new ShotData( - Rotation2d.fromDegrees(26 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); - COMP_HUB_SHOT_TREE.put( - 2.903207, - new ShotData( - Rotation2d.fromDegrees(30 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); - COMP_HUB_SHOT_TREE.put( - 3.156802, - new ShotData( - Rotation2d.fromDegrees(32 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.23)); - COMP_HUB_SHOT_TREE.put( - 3.437033, - new ShotData( - Rotation2d.fromDegrees(34 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.25)); - COMP_HUB_SHOT_TREE.put( - 3.611052, - new ShotData( - Rotation2d.fromDegrees(38 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.24)); - COMP_HUB_SHOT_TREE.put( - 3.773999, - new ShotData( - Rotation2d.fromDegrees(39 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.21)); - COMP_HUB_SHOT_TREE.put( - 3.899275, - new ShotData( - Rotation2d.fromDegrees(40 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); - COMP_HUB_SHOT_TREE.put( - 4.138058, - new ShotData( - Rotation2d.fromDegrees(41 - 13.16), - 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.13)); - COMP_HUB_SHOT_TREE.put( - 4.602258, - new ShotData( - Rotation2d.fromDegrees(43 - 13.16), - 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.15)); - COMP_HUB_SHOT_TREE.put( - 4.893493, - new ShotData( - Rotation2d.fromDegrees(45 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); - COMP_HUB_SHOT_TREE.put( - 5.225402, - new ShotData( - Rotation2d.fromDegrees(47 - 13.16), - 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); - COMP_HUB_SHOT_TREE.put( - 5.584793, - new ShotData( - Rotation2d.fromDegrees(49 - 13.16), - 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.17)); - } + public static double LATENCY_COMPENSATION_SECS = + // new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); + // comp + 0.0; - // Ig we'll see if we need more than 1 feed shot tree - public static final InterpolatingShotTree FEED_SHOT_TREE = new InterpolatingShotTree(); + // TODO: FIX ROTATION AND REDUCE DEFENDABLILTY + /** + * Gets the required ShotParams (ShotData and turret rotation) required to fire a ball into the + * hub, calculating a shot based on the passed in position, velocity, and shot tree. + * + * @param estimatedPose the robot pose estimated by the odometry and vision + * @param robotRelativeVelocity the robot relative ChassisSpeeds the robot is driving at + * @param target the target to shoot towards + * @param tree the InterpolatingShotTree used for this target (i.e. for feeding or scoring), + * populated with working measurements from known distances + * @return the ShotParams to hit the target + */ + public static ShotParams getShotParameters( + Pose2d estimatedPose, + ChassisSpeeds robotRelativeVelocity, + Translation2d target, + InterpolatingShotTree tree) { - static { // For feed shot tree - FEED_SHOT_TREE.put( - Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 - 2, 0)); // - 2, 0)); - FEED_SHOT_TREE.put( - Units.feetToMeters(4), - new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0)); // - 2, 0.0)); - FEED_SHOT_TREE.put( - Units.feetToMeters(6), - new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0)); // - 2, 0.0)); - FEED_SHOT_TREE.put( - Units.feetToMeters(8), - new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0)); // - 2, 0.0)); - FEED_SHOT_TREE.put( - Units.feetToMeters(10), - new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0)); // - 2, 0.0)); - FEED_SHOT_TREE.put( - Units.feetToMeters(12), - new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0)); // - 2, 0.0)); - FEED_SHOT_TREE.put( - Units.feetToMeters(14), - new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0)); // - 2, 0.0)); - FEED_SHOT_TREE.put( - Units.feetToMeters(16), - new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0)); // - 2, 0.0)); + double currentTimeSec = Timer.getFPGATimestamp(); + double deltaTime = currentTimeSec - lastRunTimeSec; - FEED_SHOT_TREE.put( - Units.feetToMeters(18), - new ShotData( - Rotation2d.fromDegrees(40 - 13.16), - 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.42)); // - 2, 1.42)); - FEED_SHOT_TREE.put( - Units.feetToMeters(20), - new ShotData( - Rotation2d.fromDegrees(43 - 13.16), - 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.36)); // - 2, 1.36)); - FEED_SHOT_TREE.put( - Units.feetToMeters(22), - new ShotData( - Rotation2d.fromDegrees(45 - 13.16), - 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.34)); // - 2, 1.34)); - FEED_SHOT_TREE.put( - Units.feetToMeters(24), - new ShotData( - Rotation2d.fromDegrees(47 - 13.16), - 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.25)); // - 2, 1.25)); - FEED_SHOT_TREE.put( - Units.feetToMeters(26), - new ShotData( - Rotation2d.fromDegrees(48 - 13.16), - 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.28)); // - 2, 1.28)); - FEED_SHOT_TREE.put( - Units.feetToMeters(28), - new ShotData( - Rotation2d.fromDegrees(49 - 13.16), - 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.27)); // - 2, 1.27)); - FEED_SHOT_TREE.put( - Units.feetToMeters(30), - new ShotData( - Rotation2d.fromDegrees(49 - 13.16), - 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.32)); // - 2, 1.32)); - FEED_SHOT_TREE.put( - Units.feetToMeters(32), - new ShotData( - Rotation2d.fromDegrees(49 - 13.16), - 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.4)); // - 2, 1.4)); + double axMetersPerSecSq = + (robotRelativeVelocity.vxMetersPerSecond - lastVxMetersPerSec) / deltaTime; + double ayMetersPerSecSq = + (robotRelativeVelocity.vyMetersPerSecond - lastVyMetersPerSec) / deltaTime; + double alphaRadPerSecSq = + (robotRelativeVelocity.omegaRadiansPerSecond - lastOmegaRadPerSec) / deltaTime; - FEED_SHOT_TREE.put( - Units.feetToMeters(34), - new ShotData( - Rotation2d.fromDegrees(52 - 13.16), - 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.3)); // - 2, 1.3)); - FEED_SHOT_TREE.put( - Units.feetToMeters(36), - new ShotData( - Rotation2d.fromDegrees(53 - 13.16), - 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.33)); // - 2, 1.33)); - FEED_SHOT_TREE.put( - Units.feetToMeters(38), - new ShotData( - Rotation2d.fromDegrees(53 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.3)); // - 2, 1.3)); - FEED_SHOT_TREE.put( - Units.feetToMeters(40), - new ShotData( - Rotation2d.fromDegrees(55 - 13.16), - 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); // - 2, 1.2)); - FEED_SHOT_TREE.put( - Units.feetToMeters(42), - new ShotData( - Rotation2d.fromDegrees(56 - 13.16), - 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, - 1.2)); // - 2, 1.2)); + lastVxMetersPerSec = robotRelativeVelocity.vxMetersPerSecond; + lastVyMetersPerSec = robotRelativeVelocity.vyMetersPerSecond; + lastOmegaRadPerSec = robotRelativeVelocity.omegaRadiansPerSecond; - // TODO: POPULATE beyond 24 feet and time of flight - } + lastRunTimeSec = currentTimeSec; - public static Pose2d getTurretPose(Pose2d robotPose) { - Pose2d turretPose = - robotPose.transformBy( + // Calculate estimated pose while accounting movement and acceleration during phase delay + estimatedPose = + estimatedPose.exp( + new Twist2d( + (robotRelativeVelocity.vxMetersPerSecond * LATENCY_COMPENSATION_SECS) + + (0.5 * axMetersPerSecSq * Math.pow(LATENCY_COMPENSATION_SECS, 2)), + (robotRelativeVelocity.vyMetersPerSecond * LATENCY_COMPENSATION_SECS) + + (0.5 * ayMetersPerSecSq * Math.pow(LATENCY_COMPENSATION_SECS, 2)), + (robotRelativeVelocity.omegaRadiansPerSecond * LATENCY_COMPENSATION_SECS) + + (0.5 * alphaRadPerSecSq * Math.pow(LATENCY_COMPENSATION_SECS, 2)))); + + // Calculate turret position + Pose2d turretPosition = + estimatedPose.transformBy( new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); - return turretPose; - } - - public static double distanceToHub(Pose2d pose) { - double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); - Logger.recordOutput("Autoaim/Distance To Hub", distance); - return distance; - } + // Calculate distance from turret to target + double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); + + // Calculate angle of linear velocity from angular velocity + double turretRadiusMeters = + Math.hypot( + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX(), + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY()); + Rotation2d turretToRobotAngleRads = + Rotation2d.fromRadians( + Math.atan2( + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY(), + TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX())); + Rotation2d turretLinearVelAngle = turretToRobotAngleRads.minus(Rotation2d.kCCW_90deg); + + // Calculate turret velocity, accounting for angular velocity + double turretVelocityX = + robotRelativeVelocity.vxMetersPerSecond + + (robotRelativeVelocity.omegaRadiansPerSecond + * turretRadiusMeters + * turretLinearVelAngle.getSin()); + double turretVelocityY = + robotRelativeVelocity.vyMetersPerSecond + + (robotRelativeVelocity.omegaRadiansPerSecond + * turretRadiusMeters + * turretLinearVelAngle.getCos()); + Logger.recordOutput( + "LaunchCalculator/Turret Velocity", new Translation2d(turretVelocityX, turretVelocityY)); + // Account for imparted velocity by robot (turret) to offset + double timeOfFlight; + Pose2d lookaheadPose = turretPosition; + double lookaheadTurretToTargetDistance = turretToTargetDistance; + for (int i = 0; i < 20; i++) { + // Find time of flight for a shot from the current lookahead pose + timeOfFlight = tree.get(lookaheadTurretToTargetDistance).timeOfFlightSecs(); + // Extrapolate velocity over time of flight of the shot + double offsetX = turretVelocityX * timeOfFlight; + double offsetY = turretVelocityY * timeOfFlight; + + Logger.recordOutput("LaunchCalculator/Offset", new Translation2d(offsetX, offsetY)); + // Update lookahead pose + lookaheadPose = + turretPosition.transformBy(new Transform2d(offsetX, offsetY, Rotation2d.kZero)); + // Update distance + lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); + } - public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPose) { - Translation2d robotToTarget = target.minus(robotPose.getTranslation()); - Rotation2d rot = Rotation2d.fromRadians(Math.atan2(robotToTarget.getY(), robotToTarget.getX())); - Logger.recordOutput("Autoaim/Target Rotation", rot); - return rot; - } + // Calculate parameters accounted for imparted velocity + // Rotation2d turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();\ + Rotation2d turretAngle = getTargetRotation(target, lookaheadPose); + turretAngle = getTurretTargetRotation(turretAngle, lookaheadPose); - public static Rotation2d getVirtualTargetYaw( - Translation2d target, ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose, double tof) { - Translation2d vtarget = getVirtualSOTMTarget(target, fieldRelativeSpeeds, tof); - return getTargetRotation(vtarget, robotPose); - } + // Log calculated values + Logger.recordOutput("LaunchCalculator/LookaheadPose", lookaheadPose); + Logger.recordOutput("LaunchCalculator/TurretToTargetDistance", lookaheadTurretToTargetDistance); - // lock in - public static Translation2d getVirtualSOTMTarget( - Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) { - // velocity times shot time is how translated it is - Translation2d vtarget = - target.minus( - new Translation2d( - fieldRelativeSpeeds.vxMetersPerSecond * timeOfFlightSecs, - fieldRelativeSpeeds.vyMetersPerSecond * timeOfFlightSecs)); - Logger.recordOutput("Autoaim/Virtual Target", vtarget); - return vtarget; + return new ShotParams(tree.get(lookaheadTurretToTargetDistance), turretAngle); } - public static Rotation2d getVirtualTargetYaw( - ChassisSpeeds fieldRelativeSpeeds, - Translation2d targetTranslation, - Pose2d robotPose, - InterpolatingShotTree tree) { - double tof = tree.calculateShot(robotPose, targetTranslation).timeOfFlightSecs(); - return getVirtualTargetYaw(targetTranslation, fieldRelativeSpeeds, robotPose, tof); - } - - // if we have a turret im going to assume we're on comp - public static Rotation2d getTurretTargetRotation( - Translation2d target, - Pose2d robotPose, - ChassisSpeeds chassisSpeeds, - InterpolatingShotTree shotTree) { - Pose2d turretPose = getTurretPose(robotPose); + /** + * Calculates the required turret position to point the turret along the passed in yaw. Clamps the + * rotation to within the deadzone + * + * @param targetRotation the target yaw + * @param robotPose the robot's position and rotation + * @return the position the turret should go to to down the passed in rotation + */ + public static Rotation2d getTurretTargetRotation(Rotation2d targetRotation, Pose2d robotPose) { - // get desired rotation to point at target - Rotation2d turretTargetRotation = - AutoAim.getVirtualTargetYaw(chassisSpeeds, target, turretPose, shotTree); // subtract that from rotation to point at target - turretTargetRotation = turretTargetRotation.minus(robotPose.getRotation()); + Rotation2d turretTargetRotation = targetRotation.minus(robotPose.getRotation()); Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); - // clamp between min and max turret angle - // turretTargetRotations = - // MathUtil.clamp( - // turretTargetRotations, - // TurretSubsystem.TURRET_MIN_ANGLE.getRotations(), - // TurretSubsystem.TURRET_MAX_ANGLE.getRotations()); + // -5 is some insane fudge factor i forgot where it's from double turretTargetDegrees = turretTargetRotation.getDegrees() - 5; // If its in the deadzone, clamp to nearest hardstop outOfRange = @@ -340,7 +161,7 @@ public static Rotation2d getTurretTargetRotation( if (outOfRange) { turretTargetDegrees = // If the requested angle is greater than the halfway point in the deadzone, go to the - // read hardstop, otherwise go to forward hardstop + // left hardstop, otherwise go to forward hardstop turretTargetDegrees > (TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() + TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees()) @@ -348,7 +169,6 @@ public static Rotation2d getTurretTargetRotation( ? TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees() + 2 : TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - 2; } - Logger.recordOutput("Turret/Clamped target", Rotation2d.fromDegrees(turretTargetDegrees)); // Now we need to rewrap this angle to always be negative, with 0 as the forward hardstop turretTargetDegrees = MathUtil.inputModulus(turretTargetDegrees, -360, 0); @@ -357,156 +177,51 @@ public static Rotation2d getTurretTargetRotation( return Rotation2d.fromDegrees(turretTargetDegrees); } - public static Rotation2d getTurretHubTargetRotation( - Translation2d target, Pose2d robotPose, ChassisSpeeds chassisSpeeds) { - return getTurretTargetRotation(target, robotPose, chassisSpeeds, COMP_HUB_SHOT_TREE); - } - - public static Rotation2d getTurretFeedTargetRotation( - Translation2d target, Pose2d robotPose, ChassisSpeeds chassisSpeeds) { - return getTurretTargetRotation(target, robotPose, chassisSpeeds, FEED_SHOT_TREE); - } - - public static ShotData getSOTMShotData( - Pose2d robotPose, - Translation2d targetTranslation, - ChassisSpeeds fieldRelativeSpeeds, - InterpolatingShotTree tree) { - - ShotData unadjustedShot = tree.calculateShot(robotPose, targetTranslation); - Translation2d virtualTarget = - getVirtualSOTMTarget( - targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs()); - Pose2d turretPose = getTurretPose(robotPose); - - return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); - } - - public static ShotData getSOTMShotDataNewtonsMethod( - Pose2d robotPose, - Translation2d targetTranslation, - ChassisSpeeds fieldRelativeSpeeds, - InterpolatingShotTree tree) { - - ShotData baseline = tree.calculateShot(robotPose, targetTranslation); - - Pose2d turretPose = getTurretPose(robotPose); - Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); - - double distance = turretToTarget.getNorm(); - - // get just direction of vector because its vector divded by length - // dont want to account for magnitude bc the speed we are going and shot tree do - // and we just want direction to find dot product - Translation2d shotDirection = turretToTarget.div(distance); - - // dot product! <3 - // get how fast we are going towards where we are shooting - // vectors of robot times direction - // positive if going towrds - // zero is moving perpedicular - // negative is going away - double robotVelocityAlongShot = - fieldRelativeSpeeds.vxMetersPerSecond * shotDirection.getX() - + fieldRelativeSpeeds.vyMetersPerSecond * shotDirection.getY(); - - // required velocity is like velocity the ball must have so it hits the target while the robot - // moving - // because the ball velocity is robot velocity + ball velocity - // so velocity ball needs to go is our distance / tof - the dot product or velocity along that - // shot to account for the robots velocity along the shot - double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - robotVelocityAlongShot; - - return calculateShotAdjustments(distance, baseline, requiredVelocity, tree); - } - /** - * @param distance distance to target - * @param baseline daseline parameters from tree - * @param requiredVelocity required horizontal velocity magnitude - * @return adjusted shooter command + * Gets the rotation required to point at the target, given the current position + * + * @param target the target to point at + * @param robotPose the current position + * @return the rotation required to point at the target */ - private static ShotData calculateShotAdjustments( - double distance, ShotData baseline, double requiredVelocity, InterpolatingShotTree tree) { - - ShotData currentParams = baseline; - double currentDistance = distance; - double currentTime = currentParams.timeOfFlightSecs(); - double currentVelocity = currentDistance / currentTime; - - // iterate - for (int i = 0; i < 20; i++) { - final double EPSILON = 0.001; - // get deriv of velocity (dis/time) - double lowVel = - (currentDistance - EPSILON) / tree.get(currentDistance - EPSILON).timeOfFlightSecs(); - double highVel = - (currentDistance + EPSILON) / tree.get(currentDistance + EPSILON).timeOfFlightSecs(); - double velDeriv = (highVel - lowVel) / (EPSILON * 2); - // newtons method: xn+1 = xn - f(xn)/deriv(xn) - // so estimate for new dist is difference between current velocity required velocity over the - // deriv - // this makes sense because if current vel is larger it will lower current distance to account - // for that and if requird is larger it will increase to account for that - currentDistance -= (currentVelocity - requiredVelocity) / velDeriv; - // update - currentParams = tree.get(currentDistance); - currentTime = currentParams.timeOfFlightSecs(); - currentVelocity = currentDistance / currentTime; - } - return new ShotData( - currentParams.hoodAngle(), - currentParams.flywheelVelocityRotPerSec(), - currentParams.timeOfFlightSecs()); - } - - public static ShotData getCompensatedSOTMShotData( - Pose2d robotPose, - Translation2d targetTranslation, - ChassisSpeeds fieldRelativeSpeeds, - InterpolatingShotTree tree) { - ChassisSpeeds robotRelativeSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, robotPose.getRotation()); - // calculate latency compensated pose - Pose2d compensatedPose = - robotPose.exp( - new Twist2d( - robotRelativeSpeeds.vxMetersPerSecond - * (LATENCY_COMPENSATION_SECS - // + SPIN_UP_SECS - ), - robotRelativeSpeeds.vyMetersPerSecond - * (LATENCY_COMPENSATION_SECS - // + SPIN_UP_SECS - ), - robotRelativeSpeeds.omegaRadiansPerSecond - * (LATENCY_COMPENSATION_SECS - // + SPIN_UP_SECS - ))); - return getSOTMShotData(compensatedPose, targetTranslation, fieldRelativeSpeeds, tree); - } - - public static ShotData getLeftFixedShotData() { - return new ShotData(Rotation2d.fromDegrees(36), 36, 0); - } - - public static ShotData getRightFixedShotData() { - return new ShotData(Rotation2d.fromDegrees(23.16), 35.7, 0); + public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPose) { + Translation2d robotToTarget = target.minus(robotPose.getTranslation()); + Rotation2d rot = Rotation2d.fromRadians(Math.atan2(robotToTarget.getY(), robotToTarget.getX())); + Logger.recordOutput("AutoAlign/Target Rotation", rot); + return rot; } - public static ShotData getMidFixedShotData() { - return new ShotData(Rotation2d.fromDegrees(32.84), 35, 0); + /** + * Returns the absolute distance between the passed in pose and the current alliance hub + * + * @param pose + * @return the pose's distance to the hub + */ + public static double distanceToHub(Pose2d pose) { + return pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); } + /** Increase the flywheel fudge factor by 1 */ public static void incrementFudgeFactor() { fudgeFactor++; } + /** Decrease the flywheel fudge factor by 1 */ public static void decrementFudgeFactor() { fudgeFactor--; } + /** Get the current flywheel fudge factor */ public static int getFudgeFactor() { return fudgeFactor; } + + /** + * Returns whether or not the current target is in the turret deadzone + * + * @return true if the target is in the deadzone, false if not + */ + public static boolean targetInTurretDeadzone() { + return outOfRange; + } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAlign.java b/src/main/java/frc/robot/utils/autoaim/AutoAlign.java index c555b24a..6d987a46 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAlign.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAlign.java @@ -72,7 +72,7 @@ public static double calculateRotationVelocity( HEADING_CONTROLLER.calculate(robotHeading.getRadians(), targetHeading.getRadians()) + HEADING_CONTROLLER.getSetpoint().velocity; Logger.recordOutput( - "AutoAim/Target Speeds Robot Relative", new ChassisSpeeds(0.0, 0.0, omegaRadsPerSec)); + "AutoAlign/Target Speeds Robot Relative", new ChassisSpeeds(0.0, 0.0, omegaRadsPerSec)); return omegaRadsPerSec; } @@ -112,7 +112,7 @@ public static ChassisSpeeds calculateSpeeds( + HEADING_CONTROLLER.getSetpoint().velocity); } Logger.recordOutput( - "AutoAim/Target Speeds Robot Relative", + "AutoAlign/Target Speeds Robot Relative", ChassisSpeeds.fromFieldRelativeSpeeds(speeds, robotPose.getRotation())); return speeds; diff --git a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java b/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java deleted file mode 100644 index 579f6702..00000000 --- a/src/main/java/frc/robot/utils/autoaim/NewAutoAim.java +++ /dev/null @@ -1,266 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.utils.autoaim; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.subsystems.shooter.TurretSubsystem; -import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; -import org.littletonrobotics.junction.Logger; - -/** Add your docs here. */ -public class NewAutoAim { - private static boolean outOfRange = false; // TODO not sure if this should be true by default - - private static double lastVxMetersPerSec = 0.0; - private static double lastVyMetersPerSec = 0.0; - private static double lastOmegaRadPerSec = 0.0; - private static double lastRunTimeSec = 0.0; - - public record ShotParams(ShotData shotData, Rotation2d turretAngle) {} - - public static ShotParams calculate( - Pose2d robotPose, - ChassisSpeeds fieldRelativeSpeeds, - Translation2d target, - InterpolatingShotTree tree) { - ChassisSpeeds robotRelativeSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, robotPose.getRotation()); - // this is robot relative speeds because "The twist is a change in pose in the robot's - // coordinate frame" - Pose2d compensatedPose = - robotPose.exp( - new Twist2d( - robotRelativeSpeeds.vxMetersPerSecond * (AutoAim.LATENCY_COMPENSATION_SECS), - robotRelativeSpeeds.vyMetersPerSecond * (AutoAim.LATENCY_COMPENSATION_SECS), - robotRelativeSpeeds.omegaRadiansPerSecond * (AutoAim.LATENCY_COMPENSATION_SECS))); - Pose2d turretPose = AutoAim.getTurretPose(compensatedPose); - double distanceToTarget = robotPose.getTranslation().getDistance(target); - - // the turret isn't in the center of the robot, so if the robot is rotating the turret's x and y - // are changing slightly - double turretFieldRelativeVelocityX = - fieldRelativeSpeeds.vxMetersPerSecond - + fieldRelativeSpeeds.omegaRadiansPerSecond - * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() - * Math.cos(compensatedPose.getRotation().getRadians()) - - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() - * Math.sin(compensatedPose.getRotation().getRadians())); - double turretFieldRelativeVelocityY = - fieldRelativeSpeeds.vyMetersPerSecond - + fieldRelativeSpeeds.omegaRadiansPerSecond - * (TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX() - * Math.cos(compensatedPose.getRotation().getRadians()) - - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY() - * Math.sin(compensatedPose.getRotation().getRadians())); - - Pose2d lookaheadPose = turretPose; - - // for (int i = 0; i < 20; i++) { - // double tof = tree.get(distanceToTarget).timeOfFlightSecs(); - // lookaheadPose = - // turretPose.transformBy( - // new Transform2d( - // new Translation2d(turretFieldRelativeVelocityX * tof, - // turretFieldRelativeVelocityY * tof), - // Rotation2d.kZero)); - // distanceToTarget = lookaheadPose.getTranslation().getDistance(target); - // } - double tof = tree.get(distanceToTarget).timeOfFlightSecs(); - - ShotData baseline = tree.calculateShot(robotPose, target); - - Translation2d turretToTarget = target.minus(turretPose.getTranslation()); - - double distance = turretToTarget.getNorm(); - - // get just direction of vector because its vector divded by length - // dont want to account for magnitude bc the speed we are going and shot tree do - // and we just want direction to find dot product - Translation2d shotDirection = turretToTarget.div(distance); - - // dot product! <3 - // get how fast we are going towards where we are shooting - // vectors of robot times direction - // positive if going towrds - // zero is moving perpedicular - // negative is going away - double turretVelocityAlongShot = - turretFieldRelativeVelocityX * shotDirection.getX() - + turretFieldRelativeVelocityY * shotDirection.getY(); - - // required velocity is like velocity the ball must have so it hits the target while the robot - // moving - // because the ball velocity is robot velocity + ball velocity - // so velocity ball needs to go is our distance / tof - the dot product or velocity along that - // shot to account for the robots velocity along the shot - double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - turretVelocityAlongShot; - - // we be newtoning - for (int i = 0; i < 20; i++) { - final double EPSILON = 0.001; - // get deriv of velocity (dis/time) - double lowVel = - (distanceToTarget - EPSILON) / tree.get(distanceToTarget - EPSILON).timeOfFlightSecs(); - double highVel = - (distanceToTarget + EPSILON) / tree.get(distanceToTarget + EPSILON).timeOfFlightSecs(); - double velDeriv = (highVel - lowVel) / (EPSILON * 2); - // newtons method: xn+1 = xn - f(xn)/deriv(xn) - // so estimate for new dist is difference between current velocity required velocity over the - // deriv - // this makes sense because if current vel is larger it will lower current distance to account - // for that and if requird is larger it will increase to account for that - distanceToTarget -= (distanceToTarget / tof - requiredVelocity) / velDeriv; - // update - tof = tree.get(distanceToTarget).timeOfFlightSecs(); - } - - // TODO if we're behind the hub just don't shoot - - Rotation2d turretTarget = target.minus(lookaheadPose.getTranslation()).getAngle(); - turretTarget = getTurretTargetRotation(turretTarget, robotPose); - - return new ShotParams( - tree.get(distanceToTarget), getTurretTargetRotation(turretTarget, robotPose)); - } - - public static Rotation2d getTurretTargetRotation( - Rotation2d turretTargetRotation, Pose2d robotPose) { - - // subtract that from rotation to point at target - turretTargetRotation = turretTargetRotation.minus(robotPose.getRotation()); - Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); - // -5 is some insane fudge factor i forgot where it's from - double turretTargetDegrees = turretTargetRotation.getDegrees() - 5; - // If its in the deadzone, clamp to nearest hardstop - outOfRange = - turretTargetDegrees > TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - && (turretTargetDegrees < TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees()); - if (outOfRange) { - turretTargetDegrees = - // If the requested angle is greater than the halfway point in the deadzone, go to the - // left hardstop, otherwise go to forward hardstop - turretTargetDegrees - > (TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - + TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees()) - / 2 - ? TurretSubsystem.TURRET_LEFT_HARDSTOP_ANGLE.getDegrees() + 2 - : TurretSubsystem.TURRET_FORWARD_HARDSTOP_ANGLE.getDegrees() - 2; - } - - Logger.recordOutput("Turret/Clamped target", Rotation2d.fromDegrees(turretTargetDegrees)); - // Now we need to rewrap this angle to always be negative, with 0 as the forward hardstop - turretTargetDegrees = MathUtil.inputModulus(turretTargetDegrees, -360, 0); - Logger.recordOutput("Turret/Wrapped target", Rotation2d.fromDegrees(turretTargetDegrees)); - // ship it - return Rotation2d.fromDegrees(turretTargetDegrees); - } - - public static ShotParams getParametersMechA( - Pose2d estimatedPose, - ChassisSpeeds robotRelativeVelocity, - Translation2d target, - InterpolatingShotTree tree) { - - double currentTimeSec = Timer.getFPGATimestamp(); - double deltaTime = currentTimeSec - lastRunTimeSec; - - double axMetersPerSecSq = - (robotRelativeVelocity.vxMetersPerSecond - lastVxMetersPerSec) / deltaTime; - double ayMetersPerSecSq = - (robotRelativeVelocity.vyMetersPerSecond - lastVyMetersPerSec) / deltaTime; - double alphaRadPerSecSq = - (robotRelativeVelocity.omegaRadiansPerSecond - lastOmegaRadPerSec) / deltaTime; - - lastVxMetersPerSec = robotRelativeVelocity.vxMetersPerSecond; - lastVyMetersPerSec = robotRelativeVelocity.vyMetersPerSecond; - lastOmegaRadPerSec = robotRelativeVelocity.omegaRadiansPerSecond; - - lastRunTimeSec = currentTimeSec; - - // Calculate estimated pose while accounting movement and acceleration during phase delay - estimatedPose = - estimatedPose.exp( - new Twist2d( - (robotRelativeVelocity.vxMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) - + (0.5 * axMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), - (robotRelativeVelocity.vyMetersPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) - + (0.5 * ayMetersPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)), - (robotRelativeVelocity.omegaRadiansPerSecond * AutoAim.LATENCY_COMPENSATION_SECS) - + (0.5 * alphaRadPerSecSq * Math.pow(AutoAim.LATENCY_COMPENSATION_SECS, 2)))); - - // Calculate turret position - Pose2d turretPosition = - estimatedPose.transformBy( - new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); - // Calculate distance from turret to target - double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); - - // Calculate angle of linear velocity from angular velocity - double turretRadiusMeters = - Math.hypot( - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX(), - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY()); - Rotation2d turretToRobotAngleRads = - Rotation2d.fromRadians( - Math.atan2( - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getY(), - TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION.getX())); - Rotation2d turretLinearVelAngle = turretToRobotAngleRads.minus(Rotation2d.kCCW_90deg); - - // Calculate turret velocity, accounting for angular velocity - double turretVelocityX = - robotRelativeVelocity.vxMetersPerSecond - + (robotRelativeVelocity.omegaRadiansPerSecond - * turretRadiusMeters - * turretLinearVelAngle.getSin()); - double turretVelocityY = - robotRelativeVelocity.vyMetersPerSecond - + (robotRelativeVelocity.omegaRadiansPerSecond - * turretRadiusMeters - * turretLinearVelAngle.getCos()); - Logger.recordOutput( - "LaunchCalculator/Turret Velocity", new Translation2d(turretVelocityX, turretVelocityY)); - // Account for imparted velocity by robot (turret) to offset - double timeOfFlight; - Pose2d lookaheadPose = turretPosition; - double lookaheadTurretToTargetDistance = turretToTargetDistance; - for (int i = 0; i < 20; i++) { - // Find time of flight for a shot from the current lookahead pose - timeOfFlight = tree.get(lookaheadTurretToTargetDistance).timeOfFlightSecs(); - // Extrapolate velocity over time of flight of the shot - double offsetX = turretVelocityX * timeOfFlight; - double offsetY = turretVelocityY * timeOfFlight; - - Logger.recordOutput("LaunchCalculator/Offset", new Translation2d(offsetX, offsetY)); - // Update lookahead pose - lookaheadPose = - turretPosition.transformBy(new Transform2d(offsetX, offsetY, Rotation2d.kZero)); - // Update distance - lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); - } - - // Calculate parameters accounted for imparted velocity - // Rotation2d turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();\ - Rotation2d turretAngle = AutoAim.getTargetRotation(target, lookaheadPose); - turretAngle = getTurretTargetRotation(turretAngle, estimatedPose); - - // Log calculated values - Logger.recordOutput("LaunchCalculator/LookaheadPose", lookaheadPose); - Logger.recordOutput("LaunchCalculator/TurretToTargetDistance", lookaheadTurretToTargetDistance); - - return new ShotParams(tree.get(lookaheadTurretToTargetDistance), turretAngle); - } - - public static boolean targetInTurretDeadzone() { - return outOfRange; - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/ShotTrees.java b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java new file mode 100644 index 00000000..5b9342ac --- /dev/null +++ b/src/main/java/frc/robot/utils/autoaim/ShotTrees.java @@ -0,0 +1,256 @@ +package frc.robot.utils.autoaim; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.subsystems.shooter.TurretSubsystem; +import frc.robot.utils.autoaim.AutoAim.ShotParams; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; + +public class ShotTrees { + public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); + + static { // For hub shot tree + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 + 17), new ShotData(Rotation2d.fromDegrees(8), 27.5, 1.46)); + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), + new ShotData(Rotation2d.fromDegrees(6), 30, 1.55)); + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), + new ShotData(Rotation2d.fromDegrees(10.5), 30, 1.54)); + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), + new ShotData(Rotation2d.fromDegrees(14.5), 30, 1.54)); + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), + new ShotData(Rotation2d.fromDegrees(18.25), 30, 1.52)); + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), + new ShotData(Rotation2d.fromDegrees(21.5), 30, 1.46)); + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), + new ShotData(Rotation2d.fromDegrees(24.5), 30, 1.35)); + ALPHA_HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), + new ShotData(Rotation2d.fromDegrees(28), 30, 1.36)); + } + + public static final InterpolatingShotTree COMP_HUB_SHOT_TREE = new InterpolatingShotTree(); + + static { + COMP_HUB_SHOT_TREE.put( + 1.716849, + new ShotData( + Rotation2d.fromDegrees(23 - 13.16), + 30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.8)); + COMP_HUB_SHOT_TREE.put( + 2.017596, + new ShotData( + Rotation2d.fromDegrees(23 - 13.16), + 33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 0.9)); + COMP_HUB_SHOT_TREE.put( + 2.423868, + new ShotData( + Rotation2d.fromDegrees(25 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.1)); + COMP_HUB_SHOT_TREE.put( + 2.664198, + new ShotData( + Rotation2d.fromDegrees(26 - 13.16), + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 2.903207, + new ShotData( + Rotation2d.fromDegrees(30 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 3.156802, + new ShotData( + Rotation2d.fromDegrees(32 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.23)); + COMP_HUB_SHOT_TREE.put( + 3.437033, + new ShotData( + Rotation2d.fromDegrees(34 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.25)); + COMP_HUB_SHOT_TREE.put( + 3.611052, + new ShotData( + Rotation2d.fromDegrees(38 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.24)); + COMP_HUB_SHOT_TREE.put( + 3.773999, + new ShotData( + Rotation2d.fromDegrees(39 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.21)); + COMP_HUB_SHOT_TREE.put( + 3.899275, + new ShotData( + Rotation2d.fromDegrees(40 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 4.138058, + new ShotData( + Rotation2d.fromDegrees(41 - 13.16), + 34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.13)); + COMP_HUB_SHOT_TREE.put( + 4.602258, + new ShotData( + Rotation2d.fromDegrees(43 - 13.16), + 34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.15)); + COMP_HUB_SHOT_TREE.put( + 4.893493, + new ShotData( + Rotation2d.fromDegrees(45 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 5.225402, + new ShotData( + Rotation2d.fromDegrees(47 - 13.16), + 35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); + COMP_HUB_SHOT_TREE.put( + 5.584793, + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.17)); + } + + public static final InterpolatingShotTree FEED_SHOT_TREE = new InterpolatingShotTree(); + + static { // For feed shot tree + FEED_SHOT_TREE.put( + Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 - 2, 0)); // - 2, 0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(4), + new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0)); // - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(6), + new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0)); // - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(8), + new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0)); // - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(10), + new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0)); // - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(12), + new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0)); // - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(14), + new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0)); // - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(16), + new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0)); // - 2, 0.0)); + + FEED_SHOT_TREE.put( + Units.feetToMeters(18), + new ShotData( + Rotation2d.fromDegrees(40 - 13.16), + 36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.42)); // - 2, 1.42)); + FEED_SHOT_TREE.put( + Units.feetToMeters(20), + new ShotData( + Rotation2d.fromDegrees(43 - 13.16), + 38 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.36)); // - 2, 1.36)); + FEED_SHOT_TREE.put( + Units.feetToMeters(22), + new ShotData( + Rotation2d.fromDegrees(45 - 13.16), + 39 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.34)); // - 2, 1.34)); + FEED_SHOT_TREE.put( + Units.feetToMeters(24), + new ShotData( + Rotation2d.fromDegrees(47 - 13.16), + 40 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.25)); // - 2, 1.25)); + FEED_SHOT_TREE.put( + Units.feetToMeters(26), + new ShotData( + Rotation2d.fromDegrees(48 - 13.16), + 41 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.28)); // - 2, 1.28)); + FEED_SHOT_TREE.put( + Units.feetToMeters(28), + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 43 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.27)); // - 2, 1.27)); + FEED_SHOT_TREE.put( + Units.feetToMeters(30), + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 44 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.32)); // - 2, 1.32)); + FEED_SHOT_TREE.put( + Units.feetToMeters(32), + new ShotData( + Rotation2d.fromDegrees(49 - 13.16), + 46 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.4)); // - 2, 1.4)); + + FEED_SHOT_TREE.put( + Units.feetToMeters(34), + new ShotData( + Rotation2d.fromDegrees(52 - 13.16), + 47 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.3)); // - 2, 1.3)); + FEED_SHOT_TREE.put( + Units.feetToMeters(36), + new ShotData( + Rotation2d.fromDegrees(53 - 13.16), + 51 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.33)); // - 2, 1.33)); + FEED_SHOT_TREE.put( + Units.feetToMeters(38), + new ShotData( + Rotation2d.fromDegrees(53 - 13.16), + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.3)); // - 2, 1.3)); + FEED_SHOT_TREE.put( + Units.feetToMeters(40), + new ShotData( + Rotation2d.fromDegrees(55 - 13.16), + 55 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); // - 2, 1.2)); + FEED_SHOT_TREE.put( + Units.feetToMeters(42), + new ShotData( + Rotation2d.fromDegrees(56 - 13.16), + 57 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1, + 1.2)); // - 2, 1.2)); + } + + // Not really using these but ig i'll keep them + public static ShotParams getLeftFixedShotParams() { + return new ShotParams( + new ShotData(Rotation2d.fromDegrees(36), 36, 0), Rotation2d.fromDegrees(-73.916016)); + } + + public static ShotParams getRightFixedShotParams() { + return new ShotParams( + new ShotData(Rotation2d.fromDegrees(23.16), 35.7, 0), Rotation2d.fromDegrees(-82)); + } + + public static ShotParams getMidFixedShotParams() { + return new ShotParams( + new ShotData(Rotation2d.fromDegrees(32.84), 35, 0), Rotation2d.fromDegrees(-109.775391)); + } +}