diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 433e649..0d976ee 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -505,7 +505,7 @@ public Command getDepotClimbAuto() { lockHoodUnderTrench(routine, TrenchPoses.getClosestTrenchPose(swerve.getPose()), 1); Path[] paths = {Path.PLtoD, Path.DtoCL}; Command autoCommand = - paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoFalse()); + paths[0].getTrajectory(routine).resetOdometry().alongWith(setleftClimbAutoTrue()); for (Path p : paths) { autoCommand = autoCommand.andThen(runPath(p, routine)); @@ -548,4 +548,8 @@ public Command getTestAuto() { public Command waitUntilEmpty() { return Commands.waitSeconds(3.0); } + + public Command getJustScoreAuto() { + return setAutoScoreReqTrue().andThen(Commands.waitSeconds(5)).andThen(setAutoScoreReqFalse()); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a0964d1..f5c3cfd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,7 +36,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Superstructure.SuperState; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOSim; import frc.robot.components.candle.CANdleIOReal; @@ -69,6 +68,7 @@ import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils.ClimbTargets; +import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.FieldUtils.TrenchPoses; import frc.robot.utils.autoaim.AutoAim; import java.util.Arrays; @@ -508,7 +508,11 @@ public Robot() { driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); shooter.setDefaultCommand( - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, superstructure::canScore)); + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + superstructure::inScoringArea, + () -> FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose())); swerve.setDefaultCommand( swerve .driveOpenLoopFieldRelative( @@ -600,36 +604,36 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta : Rotation2d.k180deg))); // autoaim (alpha) - autoAimReq - .and(() -> ROBOT_EDITION == RobotEdition.ALPHA) - .whileTrue( - swerve.faceHub( - () -> - -1 - * modifyJoystick(driver.getLeftY()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - () -> - -1 - * modifyJoystick(driver.getLeftX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - ROBOT_EDITION == RobotEdition.ALPHA - ? AutoAim.ALPHA_HUB_SHOT_TREE - : AutoAim.COMP_HUB_SHOT_TREE)); - - new Trigger(swerve::isNearTrench) - .and(() -> Superstructure.getState() != SuperState.INTAKE) - .and(() -> isTeleopEnabled()) - .and(() -> !Superstructure.getPoseOverride()) - .whileTrue( - swerve - .trenchAlign( - () -> - modifyJoystick(driver.getLeftY()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - () -> - modifyJoystick(driver.getLeftX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()) - .alongWith(Commands.print("afkljsdflkjs"))); + // autoAimReq + // .and(() -> ROBOT_EDITION == RobotEdition.ALPHA) + // .whileTrue( + // swerve.faceHub( + // () -> + // -1 + // * modifyJoystick(driver.getLeftY()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // () -> + // -1 + // * modifyJoystick(driver.getLeftX()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // ROBOT_EDITION == RobotEdition.ALPHA + // ? AutoAim.ALPHA_HUB_SHOT_TREE + // : AutoAim.COMP_HUB_SHOT_TREE)); + + // new Trigger(swerve::isNearTrench) + // .and(() -> Superstructure.getState() != SuperState.INTAKE) + // .and(() -> isTeleopEnabled()) + // .and(() -> !Superstructure.getPoseOverride()) + // .whileTrue( + // swerve + // .trenchAlign( + // () -> + // modifyJoystick(driver.getLeftY()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + // () -> + // modifyJoystick(driver.getLeftX()) + // * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()) + // .alongWith(Commands.print("afkljsdflkjs"))); // current zero shooter hood driver @@ -722,6 +726,7 @@ private void addAutos() { autoChooser.addOption("Depot Climb", autos.getDepotClimbAuto()); autoChooser.addOption("Depot Outpost Climb", autos.getDepotOutpostClimbAuto()); autoChooser.addOption("Test Auto", autos.getTestAuto()); + autoChooser.addOption("Just Score", autos.getJustScoreAuto()); haveAutosGenerated = true; System.out.println("Done generating autos"); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 7c60780..fedf75e 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -197,8 +197,8 @@ private void addTriggers() { operator.leftBumper().onTrue(Commands.runOnce(() -> feedTarget = FeedTarget.LEFT)); operator.rightBumper().onTrue(Commands.runOnce(() -> feedTarget = FeedTarget.RIGHT)); - operator.leftTrigger().onTrue(Commands.runOnce(() -> poseOverride = true)); - operator.rightTrigger().onTrue(Commands.runOnce(() -> poseOverride = false)); + // operator.leftTrigger().onTrue(Commands.runOnce(() -> poseOverride = true)); + // operator.rightTrigger().onTrue(Commands.runOnce(() -> poseOverride = false)); operator .povLeft() @@ -384,14 +384,22 @@ private void addCommands() { intake.restExtended(), // intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( SuperState.INTAKE, intake.intake(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( @@ -565,19 +573,31 @@ private void addCommands() { SuperState.PRE_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); bindCommands( SuperState.CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.retract()); bindCommands( SuperState.POST_CLIMB, intake.restRetracted(), indexer.rest(), - shooter.rest(swerve::getPose, swerve::getVelocityFieldRelative, this::canScore), + shooter.rest( + swerve::getPose, + swerve::getVelocityFieldRelative, + this::inScoringArea, + () -> FeedTargets.getFeedTarget(feedTarget).getPose()), climber.extend()); bindCommands( diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index ffeeed3..f9442e6 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -71,10 +71,10 @@ public Command kick(DoubleSupplier flywheelSpeedSupplier) { double spinnerSpeed = surfaceSpeedInPerSec / (Math.PI * SPINNER_DIAMETER_INCHES); Logger.recordOutput("Indexer/Spinner/Adjusted speed", spinnerSpeed); Logger.recordOutput("Indexer/Kicker/Adjusted speed", kickerSpeed); - spinnerIO.setRollerVelocity(spinnerSpeed - 1); + // spinnerIO.setRollerVelocity(spinnerSpeed - 1); // kickerIO.setRollerVelocity(kickerSpeed - 5); - // spinnerIO.setRollerVelocity(20); - kickerIO.setRollerVelocity(15); + spinnerIO.setRollerVelocity(60); + kickerIO.setRollerVelocity(25); }) // .withTimeout(3), // this.run( diff --git a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java index 05d72a2..eaf9622 100644 --- a/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/SlapdownSubsystem.java @@ -25,7 +25,7 @@ import org.littletonrobotics.junction.Logger; public class SlapdownSubsystem extends SubsystemBase implements Intake { - public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromRotations(-0.051025); + public static final Rotation2d PIVOT_MIN_POSITION = Rotation2d.fromRotations(-0.052002); public static final Rotation2d PIVOT_MAX_POSITION = Rotation2d.fromDegrees(122); // Not so sure abt this one... public static final Rotation2d PIVOT_EXTENDED_POSITION = PIVOT_MIN_POSITION; @@ -132,7 +132,13 @@ public Command restExtended() { pivotIO.setMotorPositionSetpoint(PIVOT_EXTENDED_POSITION); rollerIO.setRollerVoltage(0.0); }) - .unless(atExtensionTrigger); + .until(atExtensionTrigger) + .andThen( + this.run( + () -> { + pivotIO.setMotorVoltage(0); + rollerIO.setRollerVoltage(0); + })); } @Override @@ -204,8 +210,8 @@ public static TalonFXConfiguration getPivotConfig() { config.CurrentLimits.SupplyCurrentLimitEnable = true; // TODO: TUNE - config.MotionMagic.MotionMagicCruiseVelocity = 1; - config.MotionMagic.MotionMagicAcceleration = 1; + config.MotionMagic.MotionMagicCruiseVelocity = 5; + config.MotionMagic.MotionMagicAcceleration = 5; return config; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d57152f..6b9f01a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,7 +40,8 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore); + BooleanSupplier canScore, + Supplier feedTarget); /** Run balls out from the shooter. This is for antijamming the robot */ public Command spit(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 5f208e4..054753b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -119,7 +119,8 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore) { + BooleanSupplier canScore, + Supplier feedTarget) { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ROTATION); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index e70fd51..5d190b8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -27,7 +27,6 @@ import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; -import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.BooleanSupplier; @@ -204,24 +203,33 @@ public Command feed( public Command rest( Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier, - BooleanSupplier canScore) { + BooleanSupplier inScoringArea, + Supplier feedTarget) { return this.run( () -> { hoodIO.setHoodPosition(HOOD_MIN_ANGLE); flywheelIO.setFlywheelVoltage(0.0); - if (canScore.getAsBoolean()) { + // if (canScore.getAsBoolean()) { + // turretIO.setTurretPosition( + // AutoAim.getTurretFeedTargetRotation( + // FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getTranslation(), + // robotPoseSupplier.get(), + // chassisSpeedsSupplier.get())); + // } else { + if (inScoringArea.getAsBoolean()) { turretIO.setTurretPosition( - AutoAim.getTurretFeedTargetRotation( - FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getTranslation(), + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), robotPoseSupplier.get(), chassisSpeedsSupplier.get())); } else { turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), + AutoAim.getTurretFeedTargetRotation( + feedTarget.get().getTranslation(), robotPoseSupplier.get(), chassisSpeedsSupplier.get())); } + // } }); } @@ -253,38 +261,40 @@ public Command score( Supplier robotPoseSupplier, Supplier shotDataSupplier, Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - switch (Superstructure.getFixedShotTarget()) { - // in front of left trench with intake facing trench - case LEFT: - hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); - // in front of tower with intake facing left (to avoid deadzone) - case MID: - hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); - // in front of right trench with intake facing alliance wall - case RIGHT: - hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); - case NONE: - hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); - flywheelIO.setMotionProfiledFlywheelVelocity( - shotDataSupplier.get().flywheelVelocityRotPerSec()); - turretIO.setTurretPosition( - AutoAim.getTurretHubTargetRotation( - FieldUtils.getCurrentHubTranslation(), - robotPoseSupplier.get(), - chassisSpeedsSupplier.get())); - } - }); + return resetTurretToCalculatedPosition() + .andThen( + this.run( + () -> { + switch (Superstructure.getFixedShotTarget()) { + // in front of left trench with intake facing trench + case LEFT: + hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); + // in front of tower with intake facing left (to avoid deadzone) + case MID: + hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); + // in front of right trench with intake facing alliance wall + case RIGHT: + hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); + case NONE: + hoodIO.setHoodPosition(shotDataSupplier.get().hoodAngle()); + flywheelIO.setMotionProfiledFlywheelVelocity( + shotDataSupplier.get().flywheelVelocityRotPerSec()); + turretIO.setTurretPosition( + AutoAim.getTurretHubTargetRotation( + FieldUtils.getCurrentHubTranslation(), + robotPoseSupplier.get(), + chassisSpeedsSupplier.get())); + } + })); } @Override diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 5d61059..6a12c50 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -61,63 +61,84 @@ public class AutoAim { static { // For hub shot tree // TODO min shot COMP_HUB_SHOT_TREE.put( - Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 2, 1.04)); + Units.inchesToMeters(24 + 17), new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6, 1.04)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), - new ShotData(Rotation2d.fromDegrees(25), 35 - 2, 1.14)); + new ShotData(Rotation2d.fromDegrees(25), 35 - 6, 1.14)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), - new ShotData(Rotation2d.fromDegrees(26), 37 - 2, 1.10)); + new ShotData(Rotation2d.fromDegrees(26), 37 - 6, 1.10)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), - new ShotData(Rotation2d.fromDegrees(30), 37 - 2, 1.09)); + new ShotData(Rotation2d.fromDegrees(30), 37 - 6, 1.09)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), - new ShotData(Rotation2d.fromDegrees(33), 37 - 2, 1.15)); + new ShotData(Rotation2d.fromDegrees(33), 37 - 6, 1.15)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), - new ShotData(Rotation2d.fromDegrees(36), 38 - 2, 1.23)); + new ShotData(Rotation2d.fromDegrees(36), 38 - 6, 1.23)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), - new ShotData(Rotation2d.fromDegrees(38), 39 - 2, 1.33)); + new ShotData(Rotation2d.fromDegrees(38), 39 - 6, 1.33)); COMP_HUB_SHOT_TREE.put( Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), - new ShotData(Rotation2d.fromDegrees(39), 40.5 - 1, 1.35)); + new ShotData(Rotation2d.fromDegrees(39), 40.5 - 6, 1.35)); } // Ig we'll see if we need more than 1 feed shot tree 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, 0)); - FEED_SHOT_TREE.put(Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38, 0.0)); - FEED_SHOT_TREE.put(Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40, 0.0)); - - FEED_SHOT_TREE.put(Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38, 1.42)); - FEED_SHOT_TREE.put(Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40, 1.36)); - FEED_SHOT_TREE.put(Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41, 1.34)); - FEED_SHOT_TREE.put(Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42, 1.25)); - FEED_SHOT_TREE.put(Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43, 1.28)); - FEED_SHOT_TREE.put(Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45, 1.27)); - FEED_SHOT_TREE.put(Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46, 1.32)); - FEED_SHOT_TREE.put(Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48, 1.4)); - - FEED_SHOT_TREE.put(Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49, 1.3)); - FEED_SHOT_TREE.put(Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53, 1.33)); - FEED_SHOT_TREE.put(Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57, 1.3)); - FEED_SHOT_TREE.put(Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57, 1.2)); - FEED_SHOT_TREE.put(Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59, 1.2)); + FEED_SHOT_TREE.put( + Units.feetToMeters(2), new ShotData(Rotation2d.fromDegrees(23.16), 20 - 2, 0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(4), new ShotData(Rotation2d.fromDegrees(30), 40 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(6), new ShotData(Rotation2d.fromDegrees(40), 30 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(8), new ShotData(Rotation2d.fromDegrees(40), 32 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(10), new ShotData(Rotation2d.fromDegrees(40), 35 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(12), new ShotData(Rotation2d.fromDegrees(40), 40 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(14), new ShotData(Rotation2d.fromDegrees(45), 38 - 2, 0.0)); + FEED_SHOT_TREE.put( + Units.feetToMeters(16), new ShotData(Rotation2d.fromDegrees(45), 40 - 2, 0.0)); + + FEED_SHOT_TREE.put( + Units.feetToMeters(18), new ShotData(Rotation2d.fromDegrees(40), 38 - 2, 1.42)); + FEED_SHOT_TREE.put( + Units.feetToMeters(20), new ShotData(Rotation2d.fromDegrees(43), 40 - 2, 1.36)); + FEED_SHOT_TREE.put( + Units.feetToMeters(22), new ShotData(Rotation2d.fromDegrees(45), 41 - 2, 1.34)); + FEED_SHOT_TREE.put( + Units.feetToMeters(24), new ShotData(Rotation2d.fromDegrees(47), 42 - 2, 1.25)); + FEED_SHOT_TREE.put( + Units.feetToMeters(26), new ShotData(Rotation2d.fromDegrees(48), 43 - 2, 1.28)); + FEED_SHOT_TREE.put( + Units.feetToMeters(28), new ShotData(Rotation2d.fromDegrees(49), 45 - 2, 1.27)); + FEED_SHOT_TREE.put( + Units.feetToMeters(30), new ShotData(Rotation2d.fromDegrees(49), 46 - 2, 1.32)); + FEED_SHOT_TREE.put( + Units.feetToMeters(32), new ShotData(Rotation2d.fromDegrees(49), 48 - 2, 1.4)); + + FEED_SHOT_TREE.put( + Units.feetToMeters(34), new ShotData(Rotation2d.fromDegrees(52), 49 - 2, 1.3)); + FEED_SHOT_TREE.put( + Units.feetToMeters(36), new ShotData(Rotation2d.fromDegrees(53), 53 - 2, 1.33)); + FEED_SHOT_TREE.put( + Units.feetToMeters(38), new ShotData(Rotation2d.fromDegrees(53), 57 - 2, 1.3)); + FEED_SHOT_TREE.put( + Units.feetToMeters(40), new ShotData(Rotation2d.fromDegrees(55), 57 - 2, 1.2)); + FEED_SHOT_TREE.put( + Units.feetToMeters(42), new ShotData(Rotation2d.fromDegrees(56), 59 - 2, 1.2)); // TODO: POPULATE beyond 24 feet and time of flight }