diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index e1be096..482e947 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -3,12 +3,9 @@ import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; import java.util.List; import org.littletonrobotics.junction.Logger; @@ -17,147 +14,15 @@ public class Field { // Measured public static final Distance centerField_x_pos = Inches.of(325.06); public static final Distance centerField_y_pos = Inches.of(158.32); - private static final Distance hub_x_centerPos = Inches.of(181.56); - private static final Distance hub_x_len = Inches.of(47); - private static final Distance hub_y_len = Inches.of(47); - private static final Distance hub_z_len = Inches.of(72); - private static final Distance trench_y_len = Inches.of(49.86); - private static final Distance bump_y_len = Inches.of(73); - private static final Distance depot_y_centerPos = centerField_y_pos.plus(Inches.of(75.93)); - private static final Distance depot_x_len = Inches.of(27); - private static final Distance depot_y_len = Inches.of(42); - private static final Distance tower_y_centerPos = centerField_y_pos.minus(Inches.of(11.46)); - private static final Distance tower_x_len = Inches.of(43.8); - private static final Distance tower_y_len = Inches.of(47); - private static final Distance fuel_x_len = Inches.of(35.95 * 2); - private static final Distance fuel_y_len = Inches.of(90.95 * 2); // Constructed public static final Distance field_x_len = centerField_x_pos.times(2); public static final Distance field_y_len = centerField_y_pos.times(2); - private static final Distance allianceZone_x_len = hub_x_centerPos.minus(hub_x_len.div(2)); - private static final Distance neutralZone_x_len = - (centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len.div(2)))).times(2); - private static final Pose2d fieldCenter = - new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); - public static final Pose3d blueHubCenter = - new Pose3d(hub_x_centerPos, centerField_y_pos, hub_z_len, Rotation3d.kZero); - public static final Pose3d redHubCenter = - new Pose3d( - field_x_len.minus(hub_x_centerPos), centerField_y_pos, hub_z_len, Rotation3d.kZero); - public static final Pose3d redRightTarget = - new Pose3d( - new Translation3d(Inches.of(651.2), Inches.of(49.86), Inches.of(0)), Rotation3d.kZero); - - public static final Pose3d blueLeftTarget = - new Pose3d( - new Translation3d(Inches.of(0), Inches.of(200.86), Inches.of(0)), Rotation3d.kZero); - - public static final Pose3d redLeftTarget = - new Pose3d( - new Translation3d(Inches.of(651.2), Inches.of(200.86), Inches.of(0)), Rotation3d.kZero); - - public static final Pose3d blueRightTarget = - new Pose3d(new Translation3d(Inches.of(0), Inches.of(49.86), Inches.of(0)), Rotation3d.kZero); + // private static final Pose2d fieldCenter = + // new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); enum Region { - BlueZone( - new Rectangle2d( - new Translation2d(0, 0), new Translation2d(allianceZone_x_len, field_y_len))), - RightNeutralZone( - new Rectangle2d( - new Translation2d(centerField_x_pos.minus(neutralZone_x_len.div(2.0)), Inches.of(0)), - new Translation2d( - centerField_x_pos.plus(neutralZone_x_len.div(2.0)), centerField_y_pos))), - LeftNeutralZone( - new Rectangle2d( - new Translation2d( - centerField_x_pos.minus(neutralZone_x_len.div(2.0)), centerField_y_pos), - new Translation2d(centerField_x_pos.plus(neutralZone_x_len.div(2.0)), field_y_len))), - RedZone( - new Rectangle2d( - new Translation2d(field_x_len.minus(allianceZone_x_len), Inches.of(0)), - new Translation2d(field_x_len, field_y_len))), - BlueHub(new Rectangle2d(blueHubCenter.toPose2d(), hub_x_len, hub_y_len)), - RedHub(new Rectangle2d(redHubCenter.toPose2d(), hub_x_len, hub_y_len)), - RedLeftTrench( - new Rectangle2d( - new Translation2d( - field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), Inches.of(0)), - new Translation2d( - field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), trench_y_len))), - RedRightTrench( - new Rectangle2d( - new Translation2d( - field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), - field_y_len.minus(trench_y_len)), - new Translation2d( - field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), field_y_len))), - BlueLeftTrench( - new Rectangle2d( - new Translation2d( - hub_x_centerPos.minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), - new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), field_y_len))), - BlueRightTrench( - new Rectangle2d( - new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), Inches.of(0)), - new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), trench_y_len))), - RedLeftBump( - new Rectangle2d( - new Translation2d( - field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), - centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), - new Translation2d( - field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), - centerField_y_pos.minus(hub_y_len.div(2))))), - RedRightBump( - new Rectangle2d( - new Translation2d( - field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), - centerField_y_pos.plus(hub_y_len.div(2))), - new Translation2d( - field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), - centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), - BlueLeftBump( - new Rectangle2d( - new Translation2d( - hub_x_centerPos.minus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2))), - new Translation2d( - hub_x_centerPos.plus(hub_x_len.div(2)), - centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), - BlueRightBump( - new Rectangle2d( - new Translation2d( - hub_x_centerPos.minus(hub_x_len.div(2)), - centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), - new Translation2d( - hub_x_centerPos.plus(hub_x_len.div(2)), - centerField_y_pos.minus(hub_y_len.div(2))))), - BlueDepot( - new Rectangle2d( - new Translation2d(Inches.of(0), depot_y_centerPos.minus(depot_y_len.div(2))), - new Translation2d(depot_x_len, depot_y_centerPos.plus(depot_y_len.div(2))))), - RedDepot( - new Rectangle2d( - new Translation2d( - field_x_len.minus(depot_x_len), - field_y_len.minus(depot_y_centerPos).minus(depot_y_len.div(2))), - new Translation2d( - field_x_len, field_y_len.minus(depot_y_centerPos).plus(depot_y_len.div(2))))), - BlueTower( - new Rectangle2d( - new Translation2d(Inches.of(0), tower_y_centerPos.minus(tower_y_len.div(2))), - new Translation2d(tower_x_len, tower_y_centerPos.plus(tower_y_len.div(2))))), - RedTower( - new Rectangle2d( - new Translation2d( - field_x_len.minus(tower_x_len), - field_y_len.minus(tower_y_centerPos).minus(tower_y_len.div(2))), - new Translation2d( - field_x_len, field_y_len.minus(tower_y_centerPos).plus(tower_y_len.div(2))))), - - FuelArrangement(new Rectangle2d(fieldCenter, fuel_x_len, fuel_y_len)), Field(new Rectangle2d(new Translation2d(0, 0), new Translation2d(field_x_len, field_y_len))); private final Rectangle2d rect; @@ -175,19 +40,6 @@ public Pose2d getCenter() { } } - public static Region getZone(Pose2d pose) { - if (Region.BlueZone.contains(pose)) { - return Region.BlueZone; - } - if (Region.RedZone.contains(pose)) { - return Region.RedZone; - } - if (Region.LeftNeutralZone.contains(pose)) { - return Region.LeftNeutralZone; - } - return Region.RightNeutralZone; - } - public static void plotRegions() { for (Region region : Region.values()) { Logger.recordOutput( diff --git a/src/main/java/frc/game/GameState.java b/src/main/java/frc/game/GameState.java index 6562aa4..cb7a8b9 100644 --- a/src/main/java/frc/game/GameState.java +++ b/src/main/java/frc/game/GameState.java @@ -1,7 +1,5 @@ package frc.game; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Robot; @@ -14,15 +12,10 @@ public class GameState { public enum GamePhase { None("0:00 - 0:00"), Autonomous("0:20 - 0:00"), - Transition("2:20 - 2:10"), - Shift1("2:10 - 1:45"), - Shift2("1:45 - 1:20"), - Shift3("1:20 - 0:55"), - Shift4("0:55 - 0:30"), + MidGame("2:20 - 0:30"), EndGame("0:30 - 0:00"); - public static final List TELEOP = - List.of(Transition, Shift1, Shift2, Shift3, Shift4, EndGame); + public static final List TELEOP = List.of(MidGame, EndGame); final double countDownFrom; final double countDownUntil; @@ -47,7 +40,6 @@ private static int parseSeconds(String time) { } } - private static Alliance autoWinner; private static Alliance myAlliance; public static GamePhase getCurrentPhase() { @@ -75,39 +67,6 @@ public static Alliance getMyAlliance() { return myAlliance; } - public static Optional getAutoWinner() { - if (autoWinner == null) { - var gameData = DriverStation.getGameSpecificMessage(); - if (gameData.length() > 0) { - autoWinner = - switch (gameData.charAt(0)) { - case 'B' -> Alliance.Blue; - case 'R' -> Alliance.Red; - default -> null; - }; - } - } - return Optional.ofNullable(autoWinner); - } - - public static boolean isMyHubActive() { - switch (getCurrentPhase()) { - case None: - case Autonomous: - case Transition: - case EndGame: - return true; - case Shift1: - case Shift3: - return autoWinner != null && myAlliance != null && autoWinner != myAlliance; - case Shift2: - case Shift4: - return autoWinner != null && myAlliance != null && autoWinner == myAlliance; - default: - return false; - } - } - public static double getMatchTime() { return DriverStation.getMatchTime(); } @@ -118,35 +77,13 @@ public static Optional getAlliance() { public static void logValues() { getMyAlliance(); - getAutoWinner(); Logger.recordOutput("GameState/IsDSAttached", DriverStation.isDSAttached()); Logger.recordOutput("GameState/IsFMSAttached", DriverStation.isFMSAttached()); Logger.recordOutput("GameState/MatchType", DriverStation.getMatchType()); Logger.recordOutput("GameState/IsAutonomus", DriverStation.isAutonomous()); Logger.recordOutput("GameState/MatchTime", DriverStation.getMatchTime()); - Logger.recordOutput("GameState/AutoWinner", autoWinner); Logger.recordOutput("GameState/Alliance", myAlliance); Logger.recordOutput("GameState/GameData", DriverStation.getGameSpecificMessage()); Logger.recordOutput("GameState/CurrentPhase", getCurrentPhase()); - Logger.recordOutput("GameState/IsMyHubActive", isMyHubActive()); - } - - public static Pose3d getTarget(Pose2d robotPose) { - if (Robot.getAlliance() == Alliance.Red) { - if (robotPose.getMeasureX().gt(Field.redHubCenter.getMeasureX())) { - return Field.redHubCenter; - } - if (robotPose.getMeasureY().gt(Field.centerField_y_pos)) { - return Field.redLeftTarget; - } - return Field.redRightTarget; - } - if (robotPose.getMeasureX().lt(Field.blueHubCenter.getMeasureX())) { - return Field.blueHubCenter; - } - if (robotPose.getMeasureY().gt(Field.centerField_y_pos)) { - return Field.blueLeftTarget; - } - return Field.blueRightTarget; } } diff --git a/src/main/java/frc/robot/PneumaticsSimulator.java b/src/main/java/frc/lib/PneumaticsSimulator.java similarity index 99% rename from src/main/java/frc/robot/PneumaticsSimulator.java rename to src/main/java/frc/lib/PneumaticsSimulator.java index 69283ed..303353a 100644 --- a/src/main/java/frc/robot/PneumaticsSimulator.java +++ b/src/main/java/frc/lib/PneumaticsSimulator.java @@ -5,7 +5,7 @@ // license that can be found in the LICENSE file // at the root directory of this project. -package frc.robot; +package frc.lib; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ab0e59..c1a0f37 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -37,8 +37,8 @@ public static final class FeatureFlags { /** Enable to print loop timing when total exceeds 20ms. */ public static final boolean PROFILING_ENABLED = false; - /** Set to false to disable the hopper subsystem entirely. */ - public static final boolean HOPPER_ENABLED = false; + public static final boolean LEDS_ENABLED = false; + public static final boolean VISION_ENABLED = false; } public final class RobotConstants { @@ -72,8 +72,6 @@ public static final class DIOPorts { public static final int[] AUTONOMOUS_MODE_SELECTOR = {0, 1, 2}; public static final int ALLIANCE_COLOR_SELECTOR = 3; - - public static final int TURRET_ABS_ENCODER = 4; } public static final class CANBusPorts { @@ -86,20 +84,6 @@ public static final class CAN2 { // Drivetrain public static final int GYRO = 0; - - // Launcher - public static final int TURRET = 12; - public static final int HOOD = 13; - public static final int FLYWHEEL_LEADER = 14; - public static final int FLYWHEEL_FOLLOWER = 15; - - // Feeder - public static final int SPINDEXER = 16; - public static final int KICKER = 17; - - // Intake - public static final int INTAKE_ROLLER_LOWER = 22; - public static final int INTAKE_ROLLER_UPPER = 23; } public static final class CANHD { @@ -124,14 +108,4 @@ public static final class CANHD { public static final int BACK_LEFT_TURN_ABS_ENC = 45; } } - - public static final class PneumaticChannels { - // hopper - public static final int HOPPER_FWD = 14; - public static final int HOPPER_REV = 15; - - // intake arm - public static final int INTAKE_ARM_FWD = 0; - public static final int INTAKE_ARM_REV = 1; - } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 795e20d..e969e7e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,17 +17,14 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.REVPHSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -35,7 +32,6 @@ import frc.game.Field; import frc.game.GameState; import frc.lib.AllianceSelector; -import frc.lib.AutoOption; import frc.lib.AutoSelector; import frc.lib.CommandZorroController; import frc.lib.ControllerSelector; @@ -43,20 +39,10 @@ import frc.lib.ControllerSelector.DriverConfig; import frc.lib.ControllerSelector.DriverController; import frc.lib.ControllerSelector.OperatorConfig; -import frc.lib.LoggedCompressor; import frc.lib.LoggedPowerDistribution; -import frc.lib.ZorroController.Axis; import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.DIOPorts; import frc.robot.Constants.FeatureFlags; -import frc.robot.auto.B_LeftTrenchAuto; -import frc.robot.auto.B_RightTrenchAuto; -import frc.robot.auto.NewB_LeftTrenchMoveFirstAuto; -import frc.robot.auto.NewB_RightTrenchMoveFirstAuto; -import frc.robot.auto.NewR_LeftTrenchMoveFirstAuto; -import frc.robot.auto.NewR_RightTrenchMoveFirstAuto; -import frc.robot.auto.R_LeftTrenchAuto; -import frc.robot.auto.R_RightTrenchAuto; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; @@ -65,36 +51,6 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSimWPI; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.feeder.KickerIO; -import frc.robot.subsystems.feeder.KickerIOSimSpark; -import frc.robot.subsystems.feeder.KickerIOSpark; -import frc.robot.subsystems.feeder.SpindexerIO; -import frc.robot.subsystems.feeder.SpindexerIOSimSpark; -import frc.robot.subsystems.feeder.SpindexerIOSpark; -import frc.robot.subsystems.hopper.Hopper; -import frc.robot.subsystems.hopper.HopperIO; -import frc.robot.subsystems.hopper.HopperIOReal; -import frc.robot.subsystems.hopper.HopperIOSim; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.intake.IntakeArmIO; -import frc.robot.subsystems.intake.IntakeArmIOReal; -import frc.robot.subsystems.intake.IntakeArmIOSim; -import frc.robot.subsystems.intake.IntakeConstants; -import frc.robot.subsystems.intake.IntakeConstants.RollerConstants; -import frc.robot.subsystems.intake.RollerIO; -import frc.robot.subsystems.intake.RollerIOSimSpark; -import frc.robot.subsystems.intake.RollerIOSpark; -import frc.robot.subsystems.launcher.FlywheelIO; -import frc.robot.subsystems.launcher.FlywheelIOSimTalonFX; -import frc.robot.subsystems.launcher.FlywheelIOTalonFX; -import frc.robot.subsystems.launcher.HoodIO; -import frc.robot.subsystems.launcher.HoodIOSimSpark; -import frc.robot.subsystems.launcher.HoodIOSpark; -import frc.robot.subsystems.launcher.Launcher; -import frc.robot.subsystems.launcher.TurretIO; -import frc.robot.subsystems.launcher.TurretIOSimSpark; -import frc.robot.subsystems.launcher.TurretIOSpark; import frc.robot.subsystems.leds.LEDController; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; @@ -144,14 +100,7 @@ public class Robot extends LoggedRobot { // Subsystems private Drive drive; private Vision vision; - private Launcher launcher; - private Feeder feeder; - private Intake intake; - private Hopper hopper; - private LEDController leds = LEDController.getInstance(); - private LoggedCompressor compressor; - private PneumaticsSimulator pneumaticsSimulator; - + private final LEDController leds = FeatureFlags.LEDS_ENABLED ? LEDController.getInstance() : null; // Battery simulation constants private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc private final LinearFilter vBusFilter = LinearFilter.singlePoleIIR(0.04, Robot.defaultPeriodSecs); @@ -191,30 +140,16 @@ public Robot() { new ModuleIOTalonFX(DriveConstants.FRONT_RIGHT), new ModuleIOTalonFX(DriveConstants.BACK_LEFT), new ModuleIOTalonFX(DriveConstants.BACK_RIGHT)); - vision = - new Vision( - drive::addVisionMeasurement, - drive::getFieldRelativeHeading, - new VisionIOPhotonVision(FRONT_RIGHT_CAMERA), - new VisionIOPhotonVision(FRONT_LEFT_CAMERA), - new VisionIOPhotonVision(BACK_RIGHT_CAMERA), - new VisionIOPhotonVision(BACK_LEFT_CAMERA)); - launcher = - new Launcher( - drive::getPose, - drive::getRobotRelativeChassisSpeeds, - new TurretIOSpark(), - new FlywheelIOTalonFX(), - new HoodIOSpark()); - if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOReal()); - intake = - new Intake( - new RollerIOSpark(RollerConstants.UPPER_ROLLER_CONFIG), - new RollerIOSpark(RollerConstants.LOWER_ROLLER_CONFIG), - new IntakeArmIOReal()); - feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); - compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor"); - + if (FeatureFlags.VISION_ENABLED) { + vision = + new Vision( + drive::addVisionMeasurement, + drive::getFieldRelativeHeading, + new VisionIOPhotonVision(FRONT_RIGHT_CAMERA), + new VisionIOPhotonVision(FRONT_LEFT_CAMERA), + new VisionIOPhotonVision(BACK_RIGHT_CAMERA), + new VisionIOPhotonVision(BACK_LEFT_CAMERA)); + } // Start kernel log monitoring (singleton, starts automatically on first call) KernelLogMonitor.getInstance(); break; @@ -232,31 +167,16 @@ public Robot() { new ModuleIOSimWPI(DriveConstants.FRONT_RIGHT), new ModuleIOSimWPI(DriveConstants.BACK_LEFT), new ModuleIOSimWPI(DriveConstants.BACK_RIGHT)); - vision = - new Vision( - drive::addVisionMeasurement, - drive::getFieldRelativeHeading, - new VisionIOPhotonVisionSim(FRONT_RIGHT_CAMERA, drive::getPose), - new VisionIOPhotonVisionSim(FRONT_LEFT_CAMERA, drive::getPose), - new VisionIOPhotonVisionSim(BACK_RIGHT_CAMERA, drive::getPose), - new VisionIOPhotonVisionSim(BACK_LEFT_CAMERA, drive::getPose)); - launcher = - new Launcher( - drive::getPose, - drive::getRobotRelativeChassisSpeeds, - new TurretIOSimSpark(), - new FlywheelIOSimTalonFX(), - new HoodIOSimSpark()); - feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); - if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOSim()); - var intakeArmIOSim = new IntakeArmIOSim(); - intake = - new Intake( - new RollerIOSimSpark(RollerConstants.UPPER_ROLLER_CONFIG), - new RollerIOSimSpark(RollerConstants.LOWER_ROLLER_CONFIG), - intakeArmIOSim); - pneumaticsSimulator = - new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1)); + if (FeatureFlags.VISION_ENABLED) { + vision = + new Vision( + drive::addVisionMeasurement, + drive::getFieldRelativeHeading, + new VisionIOPhotonVisionSim(FRONT_RIGHT_CAMERA, drive::getPose), + new VisionIOPhotonVisionSim(FRONT_LEFT_CAMERA, drive::getPose), + new VisionIOPhotonVisionSim(BACK_RIGHT_CAMERA, drive::getPose), + new VisionIOPhotonVisionSim(BACK_LEFT_CAMERA, drive::getPose)); + } break; case REPLAY: // Replaying a log @@ -275,30 +195,22 @@ public Robot() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - vision = - new Vision( - drive::addVisionMeasurement, - drive::getFieldRelativeHeading, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}); - launcher = - new Launcher( - drive::getPose, - drive::getRobotRelativeChassisSpeeds, - new TurretIO() {}, - new FlywheelIO() {}, - new HoodIO() {}); - if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIO() {}); - intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}); - feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); + if (FeatureFlags.VISION_ENABLED) { + vision = + new Vision( + drive::addVisionMeasurement, + drive::getFieldRelativeHeading, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}, + new VisionIO() {}); + } break; } // Start background threads (for non-blocking CAN/network reads) SparkOdometryThread.getInstance().start(); - VisionThread.getInstance().start(); + if (FeatureFlags.VISION_ENABLED) VisionThread.getInstance().start(); CanandgyroThread.getInstance().start(); // Start AdvantageKit logger @@ -307,17 +219,6 @@ public Robot() { // Disable LiveWindow telemetry (subsystem motor sendables) — eliminates SmartDashboard overhead edu.wpi.first.wpilibj.livewindow.LiveWindow.disableAllTelemetry(); - // Wire the hopper/intake interlocks. Done here (after both subsystems exist) to avoid a - // circular dependency between the two subsystems. - if (FeatureFlags.HOPPER_ENABLED) { - intake.setDeployInterlock( - hopper::isDeployed, - () -> hopper.getDeployCommand().withTimeout(IntakeConstants.INTERLOCK_SETTLE_SECONDS)); - hopper.setRetractInterlock( - intake::isStowed, - () -> intake.getStopCommand().withTimeout(IntakeConstants.INTERLOCK_SETTLE_SECONDS)); - } - configureControlPanelBindings(); configureAutoOptions(); @@ -333,17 +234,6 @@ public Robot() { ::get) .onTrue(new InstantCommand(drive::zeroAbsoluteEncoders).ignoringDisable(true)); Field.plotRegions(); - - feeder.setDefaultCommand(Commands.startEnd(feeder::stop, () -> {}, feeder).withName("Stop")); - intake.setDefaultCommand(intake.getDefaultCommand()); - launcher.setDefaultCommand( - launcher - .initializeHoodCommand() - .andThen( - new RunCommand( - () -> launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()), - launcher) - .withName("Aim at hub"))); } /** This function is called periodically during all modes. */ @@ -362,12 +252,11 @@ public void robotPeriodic() { logCANBus("CAN2", Constants.CANBusPorts.CAN2.BUS); logCANBus("CANHD", Constants.CANBusPorts.CANHD.BUS); powerDistribution.log(); - if (compressor != null) compressor.log(); logHIDs(); logScheduler(); + GameState.logValues(); Logger.recordOutput("USB/FreeSpaceMB", getUSBStorageFreeSpace() / 1024 / 1024); - GameState.logValues(); long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Publish kernel log events to NetworkTables (only runs on real robot) @@ -403,7 +292,7 @@ public void robotPeriodic() { /** This function is called once when the robot is disabled. */ @Override public void disabledInit() { - leds.clear(); + if (leds != null) leds.clear(); } /** This function is called periodically when disabled. */ @@ -412,90 +301,68 @@ public void disabledPeriodic() { allianceSelector.disabledPeriodic(); autoSelector.disabledPeriodic(); ControllerSelector.getInstance().scan(false); - leds.displayAutoSelection(); + if (leds != null) leds.displayAutoSelection(); var autoOption = autoSelector.get(); autoOption.ifPresent( a -> a.getInitialPose() - .ifPresent(targetPose -> leds.displayPoseSeek(drive.getPose(), targetPose))); + .ifPresent( + targetPose -> { + if (leds != null) leds.displayPoseSeek(drive.getPose(), targetPose); + })); } /** This function is called once when autonomous mode is enabled. */ @Override public void autonomousInit() { - if (hopper != null) hopper.getRetractCommand().schedule(); drive.setDefaultCommand(Commands.runOnce(drive::stop, drive).withName("Stop")); autoSelector.scheduleAuto(); - leds.clear(); + if (leds != null) leds.clear(); } /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() { - leds.displayHubCountdown(); - leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning()); - } + public void autonomousPeriodic() {} /** This function is called once when teleop mode is enabled. */ @Override public void teleopInit() { - if (hopper != null && !hopper.isDeployed() && !hopper.isStowed()) - hopper.getRetractCommand().schedule(); autoSelector.cancelAuto(); ControllerSelector.getInstance().scan(true); - leds.clear(); + if (leds != null) leds.clear(); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - leds.displayHubCountdown(); - leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning()); - if (!DriverStation.isFMSAttached()) { - leds.displayCompressorState(compressor != null && compressor.isEnabled()); - } - } + public void teleopPeriodic() {} /** This function is called once when test mode is enabled. */ @Override public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); - leds.clear(); + if (leds != null) leds.clear(); } /** This function is called periodically during test mode. */ @Override - public void testPeriodic() { - leds.displayHubCountdown(); - leds.displayRobotState(() -> launcher.isOnTarget(), () -> feeder.isSpinning()); - } + public void testPeriodic() {} /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { - leds.clear(); + if (leds != null) leds.clear(); } /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { - // Skip battery simulation during replay (pneumaticsSimulator is only initialized in SIM mode) - if (pneumaticsSimulator == null) return; - - // Update battery voltage based on total current draw this cycle - pneumaticsSimulator.update(Robot.defaultPeriodSecs); RoboRioSim.setVInVoltage( vBusFilter.calculate( Math.max( 0.0, BatterySim.calculateDefaultBatteryLoadedVoltage( - drive.getSimCurrentDrawAmps(), - launcher.getSimCurrentDrawAmps(), - feeder.getSimCurrentDrawAmps(), - intake.getSimCurrentDrawAmps(), - pneumaticsSimulator.getCompressorCurrentAmps(), - ELECTRONICS_OVERHEAD_AMPS)))); + drive.getSimCurrentDrawAmps(), ELECTRONICS_OVERHEAD_AMPS)))); } private void configureControlPanelBindings() { @@ -546,44 +413,12 @@ public boolean getFieldRelativeInput() { () -> controller.getFieldRelativeInput(), allianceSelector::fieldRotated)); - // Reset gyro to 0° when button G is pressed + // Reset gyro to 0° when button G is pressed zorroDriver .GIn() .onTrue( Commands.runOnce(() -> DriveCommands.resetDriverForward(drive)).ignoringDisable(true)); - // Toggle hopper: deploy if stowed, stow if deployed (retracting intake first if needed). - // runOnce has no subsystem requirements so it always executes; the scheduled command - // requires hopper and will interrupt whatever is currently running on that subsystem. - if (FeatureFlags.HOPPER_ENABLED) - zorroDriver - .DIn() - .onTrue( - Commands.runOnce( - () -> - (hopper.isDeployed() ? hopper.getRetractCommand() : hopper.getDeployCommand()) - .schedule())); - - // Desaturate turret and advance feeder - zorroDriver.AIn().whileTrue(createDesaturateAndShootCommand(controller)); - - // Launcher - Trigger launcherEnabled = zorroDriver.axisGreaterThan(Axis.kLeftDial.value, 0.5).debounce(0.1); - launcherEnabled - .or(() -> DriverStation.isFMSAttached()) - .whileTrue( - launcher - .initializeHoodCommand() - .andThen( - new RunCommand( - () -> - launcher.aim(GameState.getTarget(drive.getPose()).getTranslation()), - launcher) - .withName("Aim at hub"))); - - // Intake - zorroDriver.HIn().whileTrue(intake.getDeployCommand()); - return controller; } @@ -620,83 +455,12 @@ public boolean getFieldRelativeInput() { () -> controller.getFieldRelativeInput(), allianceSelector::fieldRotated)); - // Reset gyro to 0° when B button is pressed + // Reset gyro to 0° when B button is pressed xboxDriver .b() .onTrue( Commands.runOnce(() -> DriveCommands.resetDriverForward(drive)).ignoringDisable(true)); - // xboxDriver - // .a() - // .whileTrue( - // Commands.run(() -> launcher.aim(GameState.getMyHubPose().getTranslation()), launcher) - // .withName("Aim at hub")); - - // xboxDriver - // .y() - // .whileTrue( - // Commands.run(() -> launcher.aim(GameState.getFieldTarget().getTranslation()), - // launcher) - // .withName("Aim at Target")); - - // // Point at Hub while A button is held - // xboxDriver - // .a() - // .whileTrue( - // DriveCommands.joystickDriveAtFixedOrientation( - // drive, - // () -> -xboxDriver.getLeftY(), - // () -> -xboxDriver.getLeftX(), - // () -> - // GameState.getMyHubPose() - // .toPose2d() - // .getTranslation() - // .minus(drive.getPose().getTranslation()) - // .getAngle(), - // allianceSelector::fieldRotated)); - - // // Point in the direction of the commanded translation while Y button is held - // xboxDriver - // .y() - // .whileTrue( - // DriveCommands.joystickDrivePointedForward( - // drive, - // () -> -xboxDriver.getLeftY(), - // () -> -xboxDriver.getLeftX(), - // allianceSelector::fieldRotated)); - - // Point at vision target while A button is held - // xboxDriver - // .a() - // .whileTrue( - // DriveCommands.pointAtTarget( - // drive, () -> vision.getTargetX(0), allianceSelector::fieldRotated)); - - // Drive 1m forward while A button is held - // xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1))); - - // Align with pose, approaching in correct orientation from 1 m away - // xboxDriver - // .a() - // .whileTrue( - // PathCommands.dockToTargetPose( - // drive, new Pose2d(8.2296, 4.1148, Rotation2d.kZero), Meters.of(1))); - - // Drive to point, approaching in correct orientation from 2 m away - // xboxDriver - // .a() - // .whileTrue( - // PathCommands.dockToTargetPoint(drive, new Translation2d(8.2296, 4.1148), Meters.of(2))); - - // Switch to X pattern when X button is pressed - // xboxDriver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - - // Desaturate turret and advance feeder - xboxDriver.a().whileTrue(createDesaturateAndShootCommand(controller)); - - // Intake - xboxDriver.rightBumper().whileTrue(intake.getDeployCommand()); - return controller; } @@ -746,47 +510,9 @@ public boolean getFieldRelativeInput() { return controller; } - public void bindXboxOperator(int port, DriverController driver) { - var xboxOperator = new CommandXboxController(port); - - // Intake - xboxOperator.b().whileTrue(intake.getDeployCommand()); - // xboxOperator.b().and(() -> hopper.isDeployed()).whileTrue(intake.getDeployCommand()); + public void bindXboxOperator(int port, DriverController driver) {} - xboxOperator.y().whileTrue(intake.getReverseCommand()); - // xboxOperator.y().and(() -> hopper.isDeployed()).whileTrue(intake.getReverseCommand()); - - // Feeder - xboxOperator.a().whileTrue(feeder.getSpinForwardCommand()); - - xboxOperator.x().whileTrue(feeder.getReverseCommand()); - - // Desaturate turret and advance feeder - xboxOperator.rightBumper().whileTrue(createDesaturateAndShootCommand(driver)); - } - - public void configureAutoOptions() { - autoSelector.addAuto( - new AutoOption(Alliance.Blue, 1, new B_LeftTrenchAuto(drive, feeder, intake, launcher))); - autoSelector.addAuto( - new AutoOption(Alliance.Red, 1, new R_LeftTrenchAuto(drive, feeder, intake, launcher))); - autoSelector.addAuto( - new AutoOption(Alliance.Blue, 2, new B_RightTrenchAuto(drive, feeder, intake, launcher))); - autoSelector.addAuto( - new AutoOption(Alliance.Red, 2, new R_RightTrenchAuto(drive, feeder, intake, launcher))); - autoSelector.addAuto( - new AutoOption( - Alliance.Blue, 3, new NewB_LeftTrenchMoveFirstAuto(drive, feeder, intake, launcher))); - autoSelector.addAuto( - new AutoOption( - Alliance.Red, 3, new NewR_LeftTrenchMoveFirstAuto(drive, feeder, intake, launcher))); - autoSelector.addAuto( - new AutoOption( - Alliance.Blue, 4, new NewB_RightTrenchMoveFirstAuto(drive, feeder, intake, launcher))); - autoSelector.addAuto( - new AutoOption( - Alliance.Red, 4, new NewR_RightTrenchMoveFirstAuto(drive, feeder, intake, launcher))); - } + public void configureAutoOptions() {} public static Alliance getAlliance() { return allianceSelector.getAllianceColor(); @@ -798,27 +524,6 @@ public static long getUSBStorageFreeSpace() { return new java.io.File("/U").getFreeSpace(); } - private Command createDesaturateAndShootCommand(DriverController driver) { - return Commands.parallel( - DriveCommands.joystickDrive( - drive, - driver::getXTranslationInput, - driver::getYTranslationInput, - // launcher::desaturateTurret, - () -> { - if (launcher.isTurretDesaturated()) { - return driver.getRotationInput(); - } else { - return launcher.getTurretDesaturationDelta(); - } - }, - driver::getFieldRelativeInput, - allianceSelector::fieldRotated) - .withName("Desaturate turret"), - Commands.sequence( - Commands.waitUntil(launcher::isTurretDesaturated), feeder.getSpinForwardCommand())); - } - private static void logHIDs() { for (int port = 0; port < DriverStation.kJoystickPorts; port++) { if (!DriverStation.isJoystickConnected(port)) continue; @@ -842,11 +547,7 @@ private static void logHIDs() { private void logScheduler() { Logger.recordOutput("Commands/ActiveCommands", activeCommands.toArray(new String[0])); logSubsystem("Drive", drive); - logSubsystem("Vision", vision); - logSubsystem("Launcher", launcher); - logSubsystem("Feeder", feeder); - if (hopper != null) logSubsystem("Hopper", hopper); - logSubsystem("Intake", intake); + if (vision != null) logSubsystem("Vision", vision); logAlerts(); } diff --git a/src/main/java/frc/robot/auto/AutoMode.java b/src/main/java/frc/robot/auto/AutoMode.java index 0bc4c2c..a786ff3 100644 --- a/src/main/java/frc/robot/auto/AutoMode.java +++ b/src/main/java/frc/robot/auto/AutoMode.java @@ -13,23 +13,16 @@ import choreo.trajectory.SwerveSample; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; import java.util.Optional; public abstract class AutoMode { private final AutoFactory autoFactory; protected final Drive drive; - protected final Feeder feeder; - protected final Intake intake; - public AutoMode(Drive drivetrain, Feeder feeder, Intake intake) { + public AutoMode(Drive drivetrain) { this.drive = drivetrain; - this.feeder = feeder; - this.intake = intake; autoFactory = new AutoFactory( drivetrain::getPose, @@ -61,9 +54,4 @@ public SwerveSample[] getLoggableTrajectory() { protected Command stopDrive() { return DriveCommands.getStopCommand(drive); } - - protected Command shakeAndFeed(double timeoutSeconds) { - return Commands.parallel(feeder.getSpinForwardCommand(), intake.getShakeIntakeCommand()) - .withTimeout(timeoutSeconds); - } } diff --git a/src/main/java/frc/robot/auto/B_LeftTrenchAuto.java b/src/main/java/frc/robot/auto/B_LeftTrenchAuto.java deleted file mode 100644 index d79af34..0000000 --- a/src/main/java/frc/robot/auto/B_LeftTrenchAuto.java +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.commands.DriveCommands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class B_LeftTrenchAuto extends AutoMode { - private final Launcher launcher; - private final AutoRoutine routine; - private final AutoTrajectory blueLeftNeutralZone; - private final AutoTrajectory blueLeftTransitionToNZ; - - public B_LeftTrenchAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - launcher = launcherSubsystem; - routine = getAutoFactory().newRoutine("B_LeftTrenchAuto"); - blueLeftNeutralZone = routine.trajectory("BlueLeftNeutralZone"); - blueLeftTransitionToNZ = routine.trajectory("BlueLeftTransitionToNZ"); - } - - @Override - public String getName() { - return "BlueLeftTrenchAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return blueLeftNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - blueLeftNeutralZone.resetOdometry(), - DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta) - .withTimeout(1.5), - feeder.getSpinForwardCommand().withTimeout(3.0), - Commands.parallel( - blueLeftNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0)))); - - blueLeftNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - blueLeftTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - blueLeftTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/B_LeftTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/B_LeftTrenchMoveFirstAuto.java deleted file mode 100644 index b909612..0000000 --- a/src/main/java/frc/robot/auto/B_LeftTrenchMoveFirstAuto.java +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class B_LeftTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory blueLeftNeutralZone; - private final AutoTrajectory blueLeftTransitionToNZ; - - public B_LeftTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("B_LeftTrenchMoveFirstAuto"); - blueLeftNeutralZone = routine.trajectory("BlueLeftNinetyNeutralZone"); - blueLeftTransitionToNZ = routine.trajectory("BlueLeftTransitionToNZ"); - } - - @Override - public String getName() { - return "BlueLeftTrenchMoveFirstAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return blueLeftNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - blueLeftNeutralZone.resetOdometry(), - Commands.parallel( - blueLeftNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - blueLeftNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - blueLeftTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - blueLeftTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/B_RightTrenchAuto.java b/src/main/java/frc/robot/auto/B_RightTrenchAuto.java deleted file mode 100644 index 28bde6d..0000000 --- a/src/main/java/frc/robot/auto/B_RightTrenchAuto.java +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.commands.DriveCommands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class B_RightTrenchAuto extends AutoMode { - private final Launcher launcher; - private final AutoRoutine routine; - private final AutoTrajectory blueRightNeutralZone; - private final AutoTrajectory blueRightTransitionToNZ; - - public B_RightTrenchAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - launcher = launcherSubsystem; - routine = getAutoFactory().newRoutine("B_RightTrenchAuto"); - blueRightNeutralZone = routine.trajectory("BlueRightNeutralZone"); - blueRightTransitionToNZ = routine.trajectory("BlueRightTransitionToNZ"); - } - - @Override - public String getName() { - return "BlueRightTrenchAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return blueRightNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - blueRightNeutralZone.resetOdometry(), - DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta) - .withTimeout(1.5), - feeder.getSpinForwardCommand().withTimeout(3.0), - Commands.parallel( - blueRightNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0)))); - - blueRightNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - blueRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - blueRightTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/B_RightTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/B_RightTrenchMoveFirstAuto.java deleted file mode 100644 index e5beb5c..0000000 --- a/src/main/java/frc/robot/auto/B_RightTrenchMoveFirstAuto.java +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class B_RightTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory blueRightNeutralZone; - private final AutoTrajectory blueRightTransitionToNZ; - - public B_RightTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("B_RightTrenchMoveFirstAuto"); - blueRightNeutralZone = routine.trajectory("BlueRightNinetyNeutralZone"); - blueRightTransitionToNZ = routine.trajectory("BlueRightTransitionToNZ"); - } - - @Override - public String getName() { - return "BlueRightTrenchMoveFirstAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return blueRightNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - blueRightNeutralZone.resetOdometry(), - Commands.parallel( - blueRightNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - blueRightNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - blueRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - blueRightTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/NewB_LeftTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/NewB_LeftTrenchMoveFirstAuto.java deleted file mode 100644 index 58345f4..0000000 --- a/src/main/java/frc/robot/auto/NewB_LeftTrenchMoveFirstAuto.java +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class NewB_LeftTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory BlueLeftNinetyNeutralZone; - private final AutoTrajectory NewBlueLeftTransition; - - public NewB_LeftTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("NewB_LeftTrenchMoveFirstAuto"); - BlueLeftNinetyNeutralZone = routine.trajectory("BlueLeftNinetyNeutralZone"); - NewBlueLeftTransition = routine.trajectory("NewBlueLeftTransition"); - } - - @Override - public String getName() { - return "NewB_LeftTrenchMoveFirstAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return BlueLeftNinetyNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - BlueLeftNinetyNeutralZone.resetOdometry(), - Commands.parallel( - BlueLeftNinetyNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - BlueLeftNinetyNeutralZone.done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - NewBlueLeftTransition.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - NewBlueLeftTransition.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/NewB_RightTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/NewB_RightTrenchMoveFirstAuto.java deleted file mode 100644 index 3069d0e..0000000 --- a/src/main/java/frc/robot/auto/NewB_RightTrenchMoveFirstAuto.java +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class NewB_RightTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory NewBlueRightNeutralZone; - private final AutoTrajectory NewBlueRightTransition; - - public NewB_RightTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("NewB_RightTrenchMoveFirstAuto"); - NewBlueRightNeutralZone = routine.trajectory("NewBlueRightNeutralZone"); - NewBlueRightTransition = routine.trajectory("NewBlueRightTransition"); - } - - @Override - public String getName() { - return "NewB_RightTrenchMoveFirstAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return NewBlueRightNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - NewBlueRightNeutralZone.resetOdometry(), - Commands.parallel( - NewBlueRightNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - NewBlueRightNeutralZone.done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - NewBlueRightTransition.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - NewBlueRightTransition.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/NewR_LeftTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/NewR_LeftTrenchMoveFirstAuto.java deleted file mode 100644 index af2ec1e..0000000 --- a/src/main/java/frc/robot/auto/NewR_LeftTrenchMoveFirstAuto.java +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class NewR_LeftTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory NewRedLeftNeutralZone; - private final AutoTrajectory NewRedLeftTransition; - - public NewR_LeftTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("NewR_LeftTrenchMoveFirstAuto"); - NewRedLeftNeutralZone = routine.trajectory("NewRedLeftNeutralZone"); - NewRedLeftTransition = routine.trajectory("NewRedLeftTransition"); - } - - @Override - public String getName() { - return "RedLeftTrenchMoveFirstAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return NewRedLeftNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - NewRedLeftNeutralZone.resetOdometry(), - Commands.parallel( - NewRedLeftNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - NewRedLeftNeutralZone.done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - NewRedLeftTransition.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - NewRedLeftTransition.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/NewR_RightTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/NewR_RightTrenchMoveFirstAuto.java deleted file mode 100644 index dd43bd2..0000000 --- a/src/main/java/frc/robot/auto/NewR_RightTrenchMoveFirstAuto.java +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class NewR_RightTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory NewRedRightNeutralZone; - private final AutoTrajectory NewRedRightTransition; - - public NewR_RightTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("NewR_RightTrenchMoveFirstAuto"); - NewRedRightNeutralZone = routine.trajectory("NewRedRightNeutralZone"); - NewRedRightTransition = routine.trajectory("NewRedRightTransition"); - } - - @Override - public String getName() { - return "NewRedRightNeutralZone"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return NewRedRightNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - NewRedRightNeutralZone.resetOdometry(), - Commands.parallel( - NewRedRightNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - NewRedRightNeutralZone.done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - NewRedRightTransition.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - NewRedRightTransition.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/R_LeftTrenchAuto.java b/src/main/java/frc/robot/auto/R_LeftTrenchAuto.java deleted file mode 100644 index f224101..0000000 --- a/src/main/java/frc/robot/auto/R_LeftTrenchAuto.java +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.commands.DriveCommands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class R_LeftTrenchAuto extends AutoMode { - private final Launcher launcher; - private final AutoRoutine routine; - private final AutoTrajectory redLeftNeutralZone; - private final AutoTrajectory redLeftTransitionToNZ; - - public R_LeftTrenchAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - launcher = launcherSubsystem; - routine = getAutoFactory().newRoutine("R_LeftTrenchAuto"); - redLeftNeutralZone = routine.trajectory("RedLeftNeutralZone"); - redLeftTransitionToNZ = routine.trajectory("RedLeftTransitionToNZ"); - } - - @Override - public String getName() { - return "RedLeftTrenchAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return redLeftNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - redLeftNeutralZone.resetOdometry(), - DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta) - .withTimeout(1.5), - feeder.getSpinForwardCommand().withTimeout(3.0), - Commands.parallel( - redLeftNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0)))); - - redLeftNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - redLeftTransitionToNZ.cmd(), - Commands.sequence( - Commands.waitSeconds(1.0), intake.getDeployCommand().withTimeout(4.0))))); - - redLeftTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/R_LeftTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/R_LeftTrenchMoveFirstAuto.java deleted file mode 100644 index 309dad9..0000000 --- a/src/main/java/frc/robot/auto/R_LeftTrenchMoveFirstAuto.java +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class R_LeftTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory redLeftNeutralZone; - private final AutoTrajectory redLeftTransitionToNZ; - - public R_LeftTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("R_LeftTrenchMoveFirstAuto"); - redLeftNeutralZone = routine.trajectory("RedLeftNinetyNeutralZone"); - redLeftTransitionToNZ = routine.trajectory("RedLeftTransitionToNZ"); - } - - @Override - public String getName() { - return "RedLeftTrenchMoveFirstAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return redLeftNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - redLeftNeutralZone.resetOdometry(), - Commands.parallel( - redLeftNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - redLeftNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - redLeftTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - redLeftTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/R_RightTrenchAuto.java b/src/main/java/frc/robot/auto/R_RightTrenchAuto.java deleted file mode 100644 index b45e0fd..0000000 --- a/src/main/java/frc/robot/auto/R_RightTrenchAuto.java +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.commands.DriveCommands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class R_RightTrenchAuto extends AutoMode { - private final Launcher launcher; - private final AutoRoutine routine; - private final AutoTrajectory redRightNeutralZone; - private final AutoTrajectory redRightTransitionToNZ; - - public R_RightTrenchAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - launcher = launcherSubsystem; - routine = getAutoFactory().newRoutine("R_RightTrenchAuto"); - redRightNeutralZone = routine.trajectory("RedRightNeutralZone"); - redRightTransitionToNZ = routine.trajectory("RedRightTransitionToNZ"); - } - - @Override - public String getName() { - return "RedRightTrenchAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return redRightNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - redRightNeutralZone.resetOdometry(), - DriveCommands.getChassisAimingCommand(drive, launcher::getTurretDesaturationDelta) - .withTimeout(1.5), - feeder.getSpinForwardCommand().withTimeout(3.0), - Commands.parallel( - redRightNeutralZone.cmd(), intake.getDeployCommand().withTimeout(10.0)))); - - redRightNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - redRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - redRightTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/auto/R_RightTrenchMoveFirstAuto.java b/src/main/java/frc/robot/auto/R_RightTrenchMoveFirstAuto.java deleted file mode 100644 index a624cfb..0000000 --- a/src/main/java/frc/robot/auto/R_RightTrenchMoveFirstAuto.java +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.auto; - -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.launcher.Launcher; - -public class R_RightTrenchMoveFirstAuto extends AutoMode { - private final AutoRoutine routine; - private final AutoTrajectory redRightNeutralZone; - private final AutoTrajectory redRightTransitionToNZ; - - public R_RightTrenchMoveFirstAuto( - Drive drivetrain, - Feeder feederSubsystem, - Intake intakeSubsystem, - Launcher launcherSubsystem) { - super(drivetrain, feederSubsystem, intakeSubsystem); - routine = getAutoFactory().newRoutine("R_RightTrenchMoveFirstAuto"); - redRightNeutralZone = routine.trajectory("RedRightNinetyNeutralZone"); - redRightTransitionToNZ = routine.trajectory("RedRightTransitionToNZ"); - } - - @Override - public String getName() { - return "RedRightTrenchMoveFirstAuto"; - } - - @Override - public AutoTrajectory getInitialTrajectory() { - return redRightNeutralZone; - } - - @Override - public AutoRoutine getAutoRoutine() { - - routine - .active() - .onTrue( - Commands.sequence( - redRightNeutralZone.resetOdometry(), - Commands.parallel( - redRightNeutralZone.cmd(), - Commands.sequence( - Commands.waitSeconds(0.8), intake.getDeployCommand().withTimeout(9.2))))); - - redRightNeutralZone - .done() - .onTrue( - Commands.sequence( - stopDrive(), - shakeAndFeed(5.0), - Commands.parallel( - redRightTransitionToNZ.cmd(), intake.getDeployCommand().withTimeout(5.0)))); - - redRightTransitionToNZ.done().onTrue(Commands.sequence(stopDrive(), shakeAndFeed(5.0))); - - return routine; - } -} diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java deleted file mode 100644 index c0f034b..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import org.littletonrobotics.junction.Logger; - -public class Feeder extends SubsystemBase { - private final SpindexerIO spindexerIO; - private final KickerIO kickerIO; - - private final SpindexerIOInputsAutoLogged spindexerInputs = new SpindexerIOInputsAutoLogged(); - private final KickerIOInputsAutoLogged kickerInputs = new KickerIOInputsAutoLogged(); - - private final Alert spindexerDisconnectedAlert; - private final Alert kickerDisconnectedAlert; - - public Feeder(SpindexerIO spindexerIO, KickerIO kickerIO) { - this.spindexerIO = spindexerIO; - this.kickerIO = kickerIO; - - spindexerDisconnectedAlert = new Alert("Disconnected spindexer motor", AlertType.kError); - kickerDisconnectedAlert = new Alert("Disconnected kicker motor", AlertType.kError); - } - - @Override - public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - spindexerIO.updateInputs(spindexerInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - kickerIO.updateInputs(kickerInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - Logger.processInputs("Spindexer", spindexerInputs); - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - Logger.processInputs("Kicker", kickerInputs); - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - spindexerDisconnectedAlert.set(!spindexerInputs.connected); - kickerDisconnectedAlert.set(!kickerInputs.connected); - Logger.recordOutput("Faults/Feeder/SpindexerDisconnected", !spindexerInputs.connected); - Logger.recordOutput("Faults/Feeder/KickerDisconnected", !kickerInputs.connected); - - // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { - long totalMs = (t4 - t0) / 1_000_000; - if (totalMs > 2) { - System.out.println( - "[Feeder] spindexer=" - + (t1 - t0) / 1_000_000 - + "ms kicker=" - + (t2 - t1) / 1_000_000 - + "ms spindexerLog=" - + (t3 - t2) / 1_000_000 - + "ms kickerLog=" - + (t4 - t3) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } - } - } - - public void stop() { - spindexerIO.setOpenLoop(Volts.of(0.0)); - kickerIO.setOpenLoop(Volts.of(0.0)); - } - - public void spinForward() { - spindexerIO.setVelocity(MetersPerSecond.of(4.5)); - kickerIO.setVelocity(MetersPerSecond.of(8.0)); - } - - public Command getSpinForwardCommand() { - return Commands.startEnd(this::spinForward, this::stop, this).withName("Spin Forward"); - } - - public Command getReverseCommand() { - return Commands.startEnd(this::reverse, this::stop, this).withName("Reverse"); - } - - public void reverse() { - spindexerIO.setVelocity(MetersPerSecond.of(-2.0)); - kickerIO.setVelocity(MetersPerSecond.of(-6.0)); - } - - public boolean isSpinning() { - return spindexerInputs.velocityMetersPerSec >= 0.5; - } - - /** Returns the total motor current draw for battery simulation. */ - public double getSimCurrentDrawAmps() { - return spindexerInputs.currentAmps + kickerInputs.currentAmps; - } -} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java deleted file mode 100644 index 52fce7f..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; - -public final class FeederConstants { - public static final class SpindexerConstants { - // Geometry - public static final Distance RADIUS = Inches.of(3.0); - - // Motor - public static final double MOTOR_REDUCTION = 1.0; - public static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); - public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = - MetersPerSecond.of(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / MOTOR_REDUCTION); - - // Encoder - public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters - public static final double ENCODER_VELOCITY_FACTOR = - ENCODER_POSITION_FACTOR / 60.0; // Meters/sec - - // Simulation - public static final double kP = 0.005; - public static final double kD = 0.005; - } - - public static final class KickerConstants { - // Geometry - public static final Distance RADIUS = Inches.of(1.5); - - // Motor - public static final double MOTOR_REDUCTION = 5.0; - public static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); - public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = - MetersPerSecond.of(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / MOTOR_REDUCTION); - - // Encoder - public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters - public static final double ENCODER_VELOCITY_FACTOR = - ENCODER_POSITION_FACTOR / 60.0; // Meters/sec - - // Simulation - public static final double kP = 0.0025; - public static final double kD = 0.0025; - } -} diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIO.java b/src/main/java/frc/robot/subsystems/feeder/KickerIO.java deleted file mode 100644 index 57d182e..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIO.java +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import org.littletonrobotics.junction.AutoLog; - -public interface KickerIO { - @AutoLog - public static class KickerIOInputs { - public boolean connected = false; - public double velocityMetersPerSec = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - } - - public default void updateInputs(KickerIOInputs inputs) {} - - public default void setOpenLoop(Voltage volts) {} - - public default void setVelocity(LinearVelocity tangentialVelocity) {} -} diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java deleted file mode 100644 index 8d9df7b..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.feeder.FeederConstants.KickerConstants.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.sim.SparkFlexSim; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class KickerIOSimSpark implements KickerIO { - private static final double KICKER_MOI_KG_M2 = 0.00052; - - private final DCMotorSim kickerSim; - - private final SparkFlex flex; - private final SparkClosedLoopController controller; - private final SparkFlexSim flexSim; - - public KickerIOSimSpark() { - flex = new SparkFlex(CAN2.KICKER, MotorType.kBrushless); - controller = flex.getClosedLoopController(); - - var config = new SparkFlexConfig(); - config - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - config - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); - - flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - flexSim = new SparkFlexSim(flex, GEARBOX); - - kickerSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, MOTOR_REDUCTION), - GEARBOX); - } - - @Override - public void updateInputs(KickerIOInputs inputs) { - // Update simulation state - double busVoltage = RoboRioSim.getVInVoltage(); - kickerSim.setInput(flexSim.getAppliedOutput() * busVoltage); - kickerSim.update(Robot.defaultPeriodSecs); - flexSim.iterate(kickerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); - - // Update inputs - inputs.connected = true; - inputs.velocityMetersPerSec = flexSim.getVelocity() * RADIUS.in(Meters); - inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); - inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); - } - - @Override - public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * tangentialVelocity.in(MetersPerSecond) - / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); - controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java deleted file mode 100644 index 8422a0f..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static frc.robot.subsystems.feeder.FeederConstants.KickerConstants.*; -import static frc.robot.util.SparkUtil.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; -import frc.robot.Constants.RobotConstants; -import frc.robot.util.SparkOdometryThread; -import frc.robot.util.SparkOdometryThread.SparkInputs; - -public class KickerIOSpark implements KickerIO { - - private final SparkFlex flex; - private final RelativeEncoder encoder; - private final SparkClosedLoopController controller; - private final SparkInputs sparkInputs; - - public KickerIOSpark() { - flex = new SparkFlex(CAN2.KICKER, MotorType.kBrushless); - encoder = flex.getEncoder(); - controller = flex.getClosedLoopController(); - - var config = new SparkFlexConfig(); - config - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - config - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) - .uvwAverageDepth(2) - .uvwMeasurementPeriod(8); - - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); - - tryUntilOk( - flex, - 5, - () -> - flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder); - } - - @Override - public void updateInputs(KickerIOInputs inputs) { - - inputs.connected = sparkInputs.isConnected(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity() * RADIUS.in(Meters); - inputs.appliedVolts = sparkInputs.getAppliedVolts(); - inputs.currentAmps = sparkInputs.getOutputCurrent(); - } - - @Override - public void setOpenLoop(Voltage volts) { - flex.setVoltage(volts); - ; - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * tangentialVelocity.in(MetersPerSecond) - / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); - controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIO.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIO.java deleted file mode 100644 index 6cbe96b..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIO.java +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import org.littletonrobotics.junction.AutoLog; - -public interface SpindexerIO { - @AutoLog - public static class SpindexerIOInputs { - public boolean connected = false; - public double velocityMetersPerSec = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - } - - public default void updateInputs(SpindexerIOInputs inputs) {} - - public default void setOpenLoop(Voltage volts) {} - - public default void setVelocity(LinearVelocity tangentialVelocity) {} -} diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java deleted file mode 100644 index 2df3665..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java +++ /dev/null @@ -1,103 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.feeder.FeederConstants.SpindexerConstants.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.sim.SparkFlexSim; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class SpindexerIOSimSpark implements SpindexerIO { - private static final double SPINDEXER_MOI_KG_M2 = 0.00207; - - private final DCMotorSim spindexerSim; - - private final SparkFlex flex; - private final SparkClosedLoopController controller; - private final SparkFlexSim flexSim; - - public SpindexerIOSimSpark() { - flex = new SparkFlex(CAN2.SPINDEXER, MotorType.kBrushless); - controller = flex.getClosedLoopController(); - - var config = new SparkFlexConfig(); - config - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - config - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); - - flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - flexSim = new SparkFlexSim(flex, GEARBOX); - - spindexerSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, SPINDEXER_MOI_KG_M2, MOTOR_REDUCTION), - GEARBOX); - } - - @Override - public void updateInputs(SpindexerIOInputs inputs) { - // Update simulation state - double busVoltage = RoboRioSim.getVInVoltage(); - spindexerSim.setInput(flexSim.getAppliedOutput() * busVoltage); - spindexerSim.update(Robot.defaultPeriodSecs); - flexSim.iterate( - spindexerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); - - // Update inputs - inputs.connected = true; - inputs.velocityMetersPerSec = flexSim.getVelocity() * RADIUS.in(Meters); - inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); - inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); - } - - @Override - public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * tangentialVelocity.in(MetersPerSecond) - / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); - controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java deleted file mode 100644 index 1d41ed7..0000000 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.feeder; - -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static frc.robot.subsystems.feeder.FeederConstants.SpindexerConstants.*; -import static frc.robot.util.SparkUtil.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; -import frc.robot.Constants.RobotConstants; -import frc.robot.util.SparkOdometryThread; -import frc.robot.util.SparkOdometryThread.SparkInputs; - -public class SpindexerIOSpark implements SpindexerIO { - - private final SparkFlex flex; - private final RelativeEncoder encoder; - private final SparkClosedLoopController controller; - private final SparkInputs sparkInputs; - - public SpindexerIOSpark() { - flex = new SparkFlex(CAN2.SPINDEXER, MotorType.kBrushless); - encoder = flex.getEncoder(); - controller = flex.getClosedLoopController(); - - var config = new SparkFlexConfig(); - config - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - config - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) - .uvwAverageDepth(2) - .uvwMeasurementPeriod(8); - - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); - - tryUntilOk( - flex, - 5, - () -> - flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder); - } - - @Override - public void updateInputs(SpindexerIOInputs inputs) { - - inputs.connected = sparkInputs.isConnected(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity() * RADIUS.in(Meters); - inputs.appliedVolts = sparkInputs.getAppliedVolts(); - inputs.currentAmps = sparkInputs.getOutputCurrent(); - } - - @Override - public void setOpenLoop(Voltage volts) { - flex.setVoltage(volts); - ; - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * tangentialVelocity.in(MetersPerSecond) - / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); - controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java deleted file mode 100644 index 64316bd..0000000 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.hopper; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -public class Hopper extends SubsystemBase { - private final HopperIO hopperIO; - private final HopperIOInputsAutoLogged hopperInputs = new HopperIOInputsAutoLogged(); - - // Injected after both subsystems are created to avoid a circular dependency. - // When set, getRetractCommand() will stop the intake first if needed. - private BooleanSupplier intakeIsStowed; - private Supplier intakeStopCommand; - - public Hopper(HopperIO hopperIO) { - this.hopperIO = hopperIO; - } - - @Override - public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - hopperIO.updateInputs(hopperInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - Logger.processInputs("Hopper", hopperInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { - long totalMs = (t2 - t0) / 1_000_000; - if (totalMs > 2) { - System.out.println( - "[Hopper] update=" - + (t1 - t0) / 1_000_000 - + "ms log=" - + (t2 - t1) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } - } - } - - public void retract() { - hopperIO.retract(); - } - - public void deploy() { - hopperIO.deploy(); - } - - public boolean isDeployed() { - return hopperInputs.isDeployed == DoubleSolenoid.Value.kForward; - } - - public boolean isStowed() { - return hopperInputs.isDeployed == DoubleSolenoid.Value.kReverse; - } - - public Command getDeployCommand() { - return Commands.startEnd(this::deploy, () -> {}, this).withName("Deploy Hopper"); - } - - /** - * Configures the intake stow interlock for getRetractCommand(). Must be called after both the - * Hopper and Intake subsystems are created. - * - * @param intakeIsStowed supplier returning true when the intake arm is stowed - * @param intakeStopCommand factory that returns a fresh command to stop and stow the intake - */ - public void setRetractInterlock( - BooleanSupplier intakeIsStowed, Supplier intakeStopCommand) { - this.intakeIsStowed = intakeIsStowed; - this.intakeStopCommand = intakeStopCommand; - } - - public Command getRetractCommand() { - if (intakeIsStowed == null) { - return Commands.startEnd(this::retract, () -> {}, this).withName("Retract Hopper"); - } - return Commands.sequence( - Commands.either(Commands.none(), intakeStopCommand.get(), intakeIsStowed), - Commands.startEnd(this::retract, () -> {}, this)) - .withName("Retract Hopper"); - } -} diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperIO.java b/src/main/java/frc/robot/subsystems/hopper/HopperIO.java deleted file mode 100644 index 34c0d10..0000000 --- a/src/main/java/frc/robot/subsystems/hopper/HopperIO.java +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.hopper; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import org.littletonrobotics.junction.AutoLog; - -public interface HopperIO { - @AutoLog - public static class HopperIOInputs { - public DoubleSolenoid.Value isDeployed = DoubleSolenoid.Value.kReverse; - } - - public default void updateInputs(HopperIOInputs inputs) {} - - public default void deploy() {} - - public default void retract() {} -} diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java b/src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java deleted file mode 100644 index ca00f00..0000000 --- a/src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.hopper; - -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; - -public class HopperIOReal implements HopperIO { - public final DoubleSolenoid hopperPneumatic; - - public HopperIOReal() { - hopperPneumatic = new DoubleSolenoid(PneumaticsModuleType.REVPH, HOPPER_FWD, HOPPER_REV); - } - - @Override - public void updateInputs(HopperIOInputs inputs) { - inputs.isDeployed = hopperPneumatic.get(); - } - - @Override - public void deploy() { - hopperPneumatic.set(kForward); - } - - @Override - public void retract() { - hopperPneumatic.set(kReverse); - } -} diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java b/src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java deleted file mode 100644 index 4e20127..0000000 --- a/src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.hopper; - -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; - -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.simulation.DoubleSolenoidSim; - -public class HopperIOSim implements HopperIO { - public final DoubleSolenoidSim hopperPneumatic; - - public HopperIOSim() { - hopperPneumatic = new DoubleSolenoidSim(PneumaticsModuleType.REVPH, HOPPER_FWD, HOPPER_REV); - } - - @Override - public void updateInputs(HopperIOInputs inputs) { - inputs.isDeployed = hopperPneumatic.get(); - } - - @Override - public void deploy() { - hopperPneumatic.set(kForward); - } - - @Override - public void retract() { - hopperPneumatic.set(kReverse); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java deleted file mode 100644 index 53294b8..0000000 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Volts; - -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -public class Intake extends SubsystemBase { - private final RollerIO upperRollerIO; - private final RollerIO lowerRollerIO; - private final IntakeArmIO intakeArmIO; - - private final RollerIOInputsAutoLogged upperRollerInputs = new RollerIOInputsAutoLogged(); - private final RollerIOInputsAutoLogged lowerRollerInputs = new RollerIOInputsAutoLogged(); - private final IntakeArmIOInputsAutoLogged intakeArmInputs = new IntakeArmIOInputsAutoLogged(); - - private final Alert upperRollerDisconnectedAlert; - private final Alert lowerRollerDisconnectedAlert; - - // Injected after both subsystems are created to avoid a circular dependency. - // When set, getDeployCommand() and getReverseCommand() will deploy the hopper first if needed. - private BooleanSupplier hopperIsDeployed; - private Supplier hopperDeployCommand; - - public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intakeArmIO) { - this.upperRollerIO = upperRollerIO; - this.lowerRollerIO = lowerRollerIO; - this.intakeArmIO = intakeArmIO; - - upperRollerDisconnectedAlert = new Alert("Disconnected upper intake roller", AlertType.kError); - lowerRollerDisconnectedAlert = new Alert("Disconnected lower intake roller", AlertType.kError); - } - - @Override - public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - upperRollerIO.updateInputs(upperRollerInputs); - lowerRollerIO.updateInputs(lowerRollerInputs); - intakeArmIO.updateInputs(intakeArmInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - Logger.processInputs("UpperRoller", upperRollerInputs); - Logger.processInputs("LowerRoller", lowerRollerInputs); - Logger.processInputs("IntakeArm", intakeArmInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - upperRollerDisconnectedAlert.set(!upperRollerInputs.connected); - lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected); - Logger.recordOutput("Faults/Intake/UpperRollerDisconnected", !upperRollerInputs.connected); - Logger.recordOutput("Faults/Intake/LowerRollerDisconnected", !lowerRollerInputs.connected); - - // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { - long totalMs = (t2 - t0) / 1_000_000; - if (totalMs > 2) { - System.out.println( - "[Intake] update=" - + (t1 - t0) / 1_000_000 - + "ms log=" - + (t2 - t1) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } - } - } - - public void stop() { - upperRollerIO.setOpenLoop(Volts.of(0.0)); - lowerRollerIO.setOpenLoop(Volts.of(0.0)); - intakeArmIO.retract(); - } - - public void deployArm() { - intakeArmIO.deploy(); - } - - public void retractArm() { - intakeArmIO.retract(); - } - - public Boolean isDeployed() { - return intakeArmInputs.isDeployed == DoubleSolenoid.Value.kForward; - } - - public boolean isStowed() { - return intakeArmInputs.isDeployed == DoubleSolenoid.Value.kReverse; - } - - /** - * Configures the hopper deploy interlock for getDeployCommand() and getReverseCommand(). Must be - * called after both the Intake and Hopper subsystems are created. - * - * @param hopperIsDeployed supplier returning true when the hopper is deployed - * @param hopperDeployCommand factory that returns a fresh command to deploy the hopper - */ - public void setDeployInterlock( - BooleanSupplier hopperIsDeployed, Supplier hopperDeployCommand) { - this.hopperIsDeployed = hopperIsDeployed; - this.hopperDeployCommand = hopperDeployCommand; - } - - public Command getStopCommand() { - return Commands.startEnd(this::stop, () -> {}, this).withName("Retract and stop"); - } - - @Override - public Command getDefaultCommand() { - return getStopCommand(); - } - - public Command getDeployCommand() { - return Commands.sequence( - hopperInterlock(), - Commands.runOnce(this::deployArm, this), - this.idle().withTimeout(0.5), - Commands.startEnd( - () -> { - upperRollerIO.setVelocity(MetersPerSecond.of(6.0)); - lowerRollerIO.setVelocity(MetersPerSecond.of(6.0)); - }, - () -> {}, - this)) - .withName("Intake"); - } - - /** Returns the total motor current draw for battery simulation. */ - public double getSimCurrentDrawAmps() { - return upperRollerInputs.currentAmps + lowerRollerInputs.currentAmps; - } - - public Command getReverseCommand() { - return Commands.sequence( - hopperInterlock(), - Commands.runOnce(this::deployArm, this), - this.idle().withTimeout(0.5), - Commands.startEnd( - () -> { - upperRollerIO.setVelocity(MetersPerSecond.of(-4.0)); - lowerRollerIO.setVelocity(MetersPerSecond.of(-4.0)); - }, - () -> {}, - this)) - .withName("Reverse"); - } - - /** Returns the hopper deploy interlock step, or a no-op if no interlock has been configured. */ - private Command hopperInterlock() { - if (hopperIsDeployed == null) { - return Commands.none(); - } - return Commands.either(Commands.none(), hopperDeployCommand.get(), hopperIsDeployed); - } - - public Command getShakeIntakeCommand() { - return Commands.sequence( - this.idle().withTimeout(1.0), - Commands.runOnce(this::stop, this), - this.idle().withTimeout(1.0), - Commands.runOnce(this::deployArm, this)) - .repeatedly(); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java deleted file mode 100644 index ec38272..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java +++ /dev/null @@ -1,24 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import org.littletonrobotics.junction.AutoLog; - -public interface IntakeArmIO { - @AutoLog - public static class IntakeArmIOInputs { - public DoubleSolenoid.Value isDeployed = DoubleSolenoid.Value.kReverse; - } - - public default void updateInputs(IntakeArmIOInputs inputs) {} - - public default void deploy() {} - - public default void retract() {} -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java deleted file mode 100644 index d4b73e8..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; - -public class IntakeArmIOReal implements IntakeArmIO { - public final DoubleSolenoid intakeArmPneumatic; - - public IntakeArmIOReal() { - intakeArmPneumatic = - new DoubleSolenoid(PneumaticsModuleType.REVPH, INTAKE_ARM_FWD, INTAKE_ARM_REV); - } - - @Override - public void updateInputs(IntakeArmIOInputs inputs) { - inputs.isDeployed = intakeArmPneumatic.get(); - } - - @Override - public void deploy() { - intakeArmPneumatic.set(kForward); - } - - @Override - public void retract() { - intakeArmPneumatic.set(kReverse); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java deleted file mode 100644 index 8e27678..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java +++ /dev/null @@ -1,39 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.*; -import static frc.robot.Constants.PneumaticChannels.*; - -import edu.wpi.first.wpilibj.PneumaticsModuleType; -import edu.wpi.first.wpilibj.simulation.DoubleSolenoidSim; - -public class IntakeArmIOSim implements IntakeArmIO { - public final DoubleSolenoidSim intakeArmPneumatic; - - public IntakeArmIOSim() { - intakeArmPneumatic = - new DoubleSolenoidSim(PneumaticsModuleType.REVPH, INTAKE_ARM_FWD, INTAKE_ARM_REV); - intakeArmPneumatic.set(kReverse); - } - - @Override - public void updateInputs(IntakeArmIOInputs inputs) { - inputs.isDeployed = intakeArmPneumatic.get(); - } - - @Override - public void deploy() { - intakeArmPneumatic.set(kForward); - } - - @Override - public void retract() { - intakeArmPneumatic.set(kReverse); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java deleted file mode 100644 index 90c2c1b..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.Constants.CANBusPorts.CAN2; - -public class IntakeConstants { - /** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */ - public static final double INTERLOCK_SETTLE_SECONDS = 1.0; - - public static class RollerConstants { - public static final Distance RADIUS = Inches.of(0.85); - - // Motor controller - public static final double MOTOR_REDUCTION = 1.0; - public static final double MAX_ACCELERATION = 4000.0; - public static final double MAX_JERK = 40000.0; - - // Encoder - public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters - public static final double ENCODER_VELOCITY_FACTOR = - ENCODER_POSITION_FACTOR / 60.0; // Meters/sec - - // Configs - public record RollerConfig(int port, CANBus bus, boolean inverted) {} - - public static final RollerConfig UPPER_ROLLER_CONFIG = - new RollerConfig(CAN2.INTAKE_ROLLER_UPPER, CAN2.BUS, true); - public static final RollerConfig LOWER_ROLLER_CONFIG = - new RollerConfig(CAN2.INTAKE_ROLLER_LOWER, CAN2.BUS, true); - - public static class SparkConfig { - public static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); - public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = - MetersPerSecond.of(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / MOTOR_REDUCTION); - public static final double kP = 0.001; - public static final double kD = 0.0; - } - - public static class TalonConfig { - public static final DCMotor GEARBOX = DCMotor.getKrakenX60(1); - public static final Slot0Configs VELOCITY_VOLTAGE_GAINS = - new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); - public static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = - new Slot1Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(2.5); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIO.java b/src/main/java/frc/robot/subsystems/intake/RollerIO.java deleted file mode 100644 index a2eb344..0000000 --- a/src/main/java/frc/robot/subsystems/intake/RollerIO.java +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import org.littletonrobotics.junction.AutoLog; - -public interface RollerIO { - @AutoLog - public static class RollerIOInputs { - public boolean connected = false; - public double velocityMetersPerSec = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - } - - public default void updateInputs(RollerIOInputs inputs) {} - - public default void setOpenLoop(Voltage volts) {} - - public default void setVelocity(LinearVelocity tangentialVelocity) {} -} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java deleted file mode 100644 index a2e12c9..0000000 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java +++ /dev/null @@ -1,102 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.SparkConfig.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.sim.SparkFlexSim; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class RollerIOSimSpark implements RollerIO { - private static final double KICKER_MOI_KG_M2 = 0.00052; - - private final DCMotorSim rollerSim; - - private final SparkFlex flex; - private final SparkClosedLoopController controller; - private final SparkFlexSim flexSim; - - public RollerIOSimSpark(RollerConfig rollerConfig) { - flex = new SparkFlex(rollerConfig.port(), MotorType.kBrushless); - controller = flex.getClosedLoopController(); - - var config = new SparkFlexConfig(); - config - .inverted(rollerConfig.inverted()) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - config - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); - - flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - flexSim = new SparkFlexSim(flex, GEARBOX); - - rollerSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, MOTOR_REDUCTION), - GEARBOX); - } - - @Override - public void updateInputs(RollerIOInputs inputs) { - // Update simulation state - double busVoltage = RoboRioSim.getVInVoltage(); - rollerSim.setInput(flexSim.getAppliedOutput() * busVoltage); - rollerSim.update(Robot.defaultPeriodSecs); - flexSim.iterate(rollerSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); - - // Update inputs - inputs.connected = true; - inputs.velocityMetersPerSec = flexSim.getVelocity() * RADIUS.in(Meters); - inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); - inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); - } - - @Override - public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * tangentialVelocity.in(MetersPerSecond) - / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); - controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java deleted file mode 100644 index 723a883..0000000 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.TalonConfig.*; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.MotorConstants.KrakenX60Constants; -import frc.robot.Robot; - -public class RollerIOSimTalonFX implements RollerIO { - private final DCMotorSim rollerSim; - - private final TalonFX motor; - private final TalonFXConfiguration config; - private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - private final VoltageOut voltageRequest = new VoltageOut(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0).withSlot(1); - private final NeutralOut brake = new NeutralOut(); - - // Inputs from intake motor - private final StatusSignal velocity; - private final StatusSignal acceleration; - private final StatusSignal appliedVolts; - private final StatusSignal supplyCurrent, torqueCurrent; - private final StatusSignal dutyCycle; - - public RollerIOSimTalonFX(RollerConfig rollerConfig) { - motor = new TalonFX(rollerConfig.port(), rollerConfig.bus()); - config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = - rollerConfig.inverted() - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = VELOCITY_VOLTAGE_GAINS; - config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.TorqueCurrent.PeakReverseTorqueCurrent = - -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25)); - - rollerSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, 0.0005, MOTOR_REDUCTION), GEARBOX); - - velocity = motor.getVelocity(); - acceleration = motor.getAcceleration(); - appliedVolts = motor.getMotorVoltage(); - supplyCurrent = motor.getSupplyCurrent(); - dutyCycle = motor.getDutyCycle(); - torqueCurrent = motor.getTorqueCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, velocity, acceleration, appliedVolts, supplyCurrent, dutyCycle, torqueCurrent); - } - - @Override - public void updateInputs(RollerIOInputs inputs) { - inputs.connected = - connectedDebounce.calculate( - BaseStatusSignal.refreshAll( - velocity, acceleration, appliedVolts, supplyCurrent, dutyCycle, torqueCurrent) - .isOK()); - - // Update simulation state - var motorSim = motor.getSimState(); - motorSim.setSupplyVoltage(RoboRioSim.getVInVoltage()); - rollerSim.setInput(motorSim.getMotorVoltage()); - rollerSim.update(Robot.defaultPeriodSecs); - motorSim.setRawRotorPosition(rollerSim.getAngularPositionRotations() * MOTOR_REDUCTION); - motorSim.setRotorVelocity(rollerSim.getAngularVelocity().times(MOTOR_REDUCTION)); - - inputs.appliedVolts = appliedVolts.getValueAsDouble(); - inputs.currentAmps = supplyCurrent.getValueAsDouble(); - inputs.velocityMetersPerSec = - velocity.getValue().in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION; - } - - @Override - public void setOpenLoop(Voltage volts) { - if (volts.in(Volts) < 1e-6) { - motor.setControl(brake); - } else { - motor.setControl(voltageRequest.withOutput(volts)); - } - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - AngularVelocity angularVelocity = - RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / RADIUS.in(Meters)); - motor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java deleted file mode 100644 index 35b1728..0000000 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.SparkConfig.*; -import static frc.robot.util.SparkUtil.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; -import frc.robot.Constants.RobotConstants; -import frc.robot.util.SparkOdometryThread; -import frc.robot.util.SparkOdometryThread.SparkInputs; - -public class RollerIOSpark implements RollerIO { - private final SparkFlex flex; - private final RelativeEncoder encoder; - private final SparkClosedLoopController controller; - private final SparkInputs sparkInputs; - - public RollerIOSpark(RollerConfig rollerConfig) { - flex = new SparkFlex(rollerConfig.port(), MotorType.kBrushless); - encoder = flex.getEncoder(); - controller = flex.getClosedLoopController(); - - var config = new SparkFlexConfig(); - config - .inverted(rollerConfig.inverted()) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - config - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) - .uvwAverageDepth(2) - .uvwMeasurementPeriod(8); - - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); - - tryUntilOk( - flex, - 5, - () -> - flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - sparkInputs = SparkOdometryThread.getInstance().registerSpark(flex, encoder); - } - - @Override - public void updateInputs(RollerIOInputs inputs) { - inputs.connected = sparkInputs.isConnected(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity() * RADIUS.in(Meters); - inputs.appliedVolts = sparkInputs.getAppliedVolts(); - inputs.currentAmps = sparkInputs.getOutputCurrent(); - } - - @Override - public void setOpenLoop(Voltage volts) { - flex.setVoltage(volts); - ; - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * tangentialVelocity.in(MetersPerSecond) - / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); - controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java deleted file mode 100644 index fb1d3e4..0000000 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; -import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.TalonConfig.*; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.ParentDevice; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.MotorConstants.KrakenX60Constants; - -public class RollerIOTalonFX implements RollerIO { - private final TalonFX motor; - private final TalonFXConfiguration config; - private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - private final VoltageOut voltageRequest = new VoltageOut(0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0).withSlot(1); - private final NeutralOut brake = new NeutralOut(); - - // private final TrapezoidProfile profile = - // new TrapezoidProfile(new TrapezoidProfile.Constraints(MAX_ACCELERATION, MAX_JERK)); - // Inputs from intake motor - private final StatusSignal velocity; - private final StatusSignal appliedVolts; - private final StatusSignal supplyCurrent; - - public RollerIOTalonFX(RollerConfig rollerConfig) { - motor = new TalonFX(rollerConfig.port(), rollerConfig.bus()); - config = new TalonFXConfiguration(); - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.TorqueCurrent.PeakReverseTorqueCurrent = - -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.MotorOutput.Inverted = - rollerConfig.inverted() - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = VELOCITY_VOLTAGE_GAINS; - config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; - tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25)); // -1 tryUntilOkay - - velocity = motor.getVelocity(); - appliedVolts = motor.getMotorVoltage(); - supplyCurrent = motor.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, appliedVolts, supplyCurrent); - - // Disable all signals not explicitly configured above to reduce CAN bus load - ParentDevice.optimizeBusUtilizationForAll(motor); - } - - @Override - public void updateInputs(RollerIOInputs inputs) { - inputs.connected = - connectedDebounce.calculate( - BaseStatusSignal.refreshAll(velocity, appliedVolts, supplyCurrent).isOK()); - - inputs.appliedVolts = appliedVolts.getValueAsDouble(); - inputs.currentAmps = supplyCurrent.getValueAsDouble(); - inputs.velocityMetersPerSec = - velocity.getValue().in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION; - } - - @Override - public void setOpenLoop(Voltage volts) { - if (volts.in(Volts) < 1e-6) { - motor.setControl(brake); - } else { - motor.setControl(voltageRequest.withOutput(volts)); - } - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - AngularVelocity angularVelocity = - RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / RADIUS.in(Meters)); - - // TrapezoidProfile.State goal = - // new TrapezoidProfile.State(angularVelocity.in(RotationsPerSecond), 0); - // TrapezoidProfile.State setpoint = - // new TrapezoidProfile.State( - // intakeVelocity.getValueAsDouble(), intakeAcceleration.getValueAsDouble()); - - // setpoint = profile.calculate(Robot.defaultPeriodSecs, setpoint, goal); - - // velocityTorqueCurrentRequest.Velocity = setpoint.position; - // velocityTorqueCurrentRequest.Acceleration = setpoint.velocity; - motor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java deleted file mode 100644 index b580597..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import org.littletonrobotics.junction.AutoLog; - -public interface FlywheelIO { - @AutoLog - public static class FlywheelIOInputs { - public boolean connected = false; - public double velocityMetersPerSec = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - // Follower motor telemetry (logged via @AutoLog, not directly in IO layer) - public double followerAppliedVolts = 0.0; - public double followerCurrentAmps = 0.0; - } - - public default void updateInputs(FlywheelIOInputs inputs) {} - - public default void setOpenLoop(Voltage volts) {} - - public default void setVelocity(LinearVelocity tangentialVelocity) {} -} diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java deleted file mode 100644 index d7659a7..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.sim.ChassisReference; -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.KrakenX60Constants; -import frc.robot.Robot; - -public class FlywheelIOSimTalonFX implements FlywheelIO { - private static final double FLYWHEEL_MOI_KG_M2 = 0.00026; - - private final DCMotorSim flywheelSim; - - private final TalonFX flywheelLeaderTalon; - private final TalonFX flywheelFollowerTalon; - private final TalonFXConfiguration config; - private final Debouncer flywheelConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - // Voltage control requests - private final VoltageOut voltageRequest = new VoltageOut(0); - // private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0).withSlot(1); - - // Inputs from flywheel motor - private final StatusSignal flywheelVelocity; - private final StatusSignal flywheelAppliedVolts; - private final StatusSignal flywheelCurrent; - - public FlywheelIOSimTalonFX() { - flywheelLeaderTalon = new TalonFX(CAN2.FLYWHEEL_LEADER, CAN2.BUS); - flywheelFollowerTalon = new TalonFX(CAN2.FLYWHEEL_FOLLOWER, CAN2.BUS); - // Configuration - config = new TalonFXConfiguration(); - config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = VELOCITY_VOLTAGE_GAINS; - config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.TorqueCurrent.PeakReverseTorqueCurrent = - -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); - tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); - - var flywheelMotorSim = flywheelLeaderTalon.getSimState(); - flywheelMotorSim.Orientation = ChassisReference.Clockwise_Positive; - flywheelMotorSim.setMotorType(MotorType.KrakenX60); - - flywheelSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, FLYWHEEL_MOI_KG_M2, MOTOR_REDUCTION), - GEARBOX); - - flywheelVelocity = flywheelLeaderTalon.getVelocity(); - flywheelAppliedVolts = flywheelLeaderTalon.getMotorVoltage(); - flywheelCurrent = flywheelLeaderTalon.getStatorCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, flywheelVelocity, flywheelAppliedVolts, flywheelCurrent); - - flywheelFollowerTalon.setControl( - new Follower(CAN2.FLYWHEEL_LEADER, MotorAlignmentValue.Opposed)); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - // Synchronous refresh for leader/follower motors to ensure consistent state - inputs.connected = - flywheelConnectedDebounce.calculate( - BaseStatusSignal.refreshAll(flywheelVelocity, flywheelAppliedVolts, flywheelCurrent) - .isOK()); - - // Update simulation state - var flywheelMotorSim = flywheelLeaderTalon.getSimState(); - flywheelMotorSim.setSupplyVoltage(RoboRioSim.getVInVoltage()); - flywheelSim.setInput(flywheelMotorSim.getMotorVoltage()); - flywheelSim.update(Robot.defaultPeriodSecs); - flywheelMotorSim.setRawRotorPosition( - flywheelSim.getAngularPositionRotations() * MOTOR_REDUCTION); - flywheelMotorSim.setRotorVelocity(flywheelSim.getAngularVelocity().times(MOTOR_REDUCTION)); - - inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); - inputs.currentAmps = flywheelCurrent.getValueAsDouble(); - inputs.velocityMetersPerSec = - (flywheelVelocity.getValue().in(RadiansPerSecond) * WHEEL_RADIUS.in(Meters)) - / MOTOR_REDUCTION; - } - - @Override - public void setOpenLoop(Voltage volts) { - flywheelLeaderTalon.setControl(voltageRequest.withOutput(volts)); - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - var angularVelocity = - RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / WHEEL_RADIUS.in(Meters)); - flywheelLeaderTalon.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java deleted file mode 100644 index ac3db5a..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java +++ /dev/null @@ -1,78 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class FlywheelIOSimWPI implements FlywheelIO { - private static final double KP_SIM = 0.1; - - private final DCMotorSim flywheelSim; - - private boolean closedLoop = false; - private PIDController velocityController = new PIDController(KP_SIM, 0.0, 0.0); - private double appliedVolts = 0.0; - private double feedforwardVolts = 0.0; - - public FlywheelIOSimWPI() { - flywheelSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, 0.004, MOTOR_REDUCTION), GEARBOX); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - // Run closed-loop control - if (closedLoop) { - appliedVolts = - velocityController.calculate(flywheelSim.getAngularVelocityRadPerSec()) - + feedforwardVolts; - } else { - velocityController.reset(); - } - - // Update simulation state - flywheelSim.setInputVoltage( - MathUtil.clamp( - appliedVolts, -RobotConstants.NOMINAL_VOLTAGE, RobotConstants.NOMINAL_VOLTAGE)); - flywheelSim.update(Robot.defaultPeriodSecs); - - // Update turn inputs - inputs.connected = true; - inputs.velocityMetersPerSec = - flywheelSim.getAngularVelocityRadPerSec() * WHEEL_RADIUS.in(Meters); - inputs.appliedVolts = appliedVolts; - inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps()); - } - - @Override - public void setOpenLoop(Voltage volts) { - closedLoop = false; - appliedVolts = volts.in(Volts); - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - closedLoop = true; - this.feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * tangentialVelocity.in(MetersPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - velocityController.setSetpoint(tangentialVelocity.in(MetersPerSecond)); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java deleted file mode 100644 index f8d0e0d..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.*; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.units.measure.AngularAcceleration; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.KrakenX60Constants; -import frc.robot.Robot; - -public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX flywheelLeaderTalon; - private final TalonFX flywheelFollowerTalon; - private final TalonFXConfiguration config; - private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - // Voltage control requests - private final VoltageOut voltageRequest = new VoltageOut(0); - // private final VelocityVoltage velocityVoltageRequest = new VelocityVoltage(0.0); - private final VelocityTorqueCurrentFOC velocityTorqueCurrentRequest = - new VelocityTorqueCurrentFOC(0.0).withSlot(1); - private final NeutralOut brake = new NeutralOut(); - - private final TrapezoidProfile profile = - new TrapezoidProfile(new TrapezoidProfile.Constraints(MAX_ACCELERATION, MAX_JERK)); - - // Inputs from flywheel motor - private final StatusSignal flywheelVelocity; - private final StatusSignal flywheelAcceleration; - private final StatusSignal flywheelAppliedVolts, followerAppliedVolts; - private final StatusSignal flywheelCurrent, followerCurrent; - - public FlywheelIOTalonFX() { - flywheelLeaderTalon = new TalonFX(CAN2.FLYWHEEL_LEADER, CAN2.BUS); - flywheelFollowerTalon = new TalonFX(CAN2.FLYWHEEL_FOLLOWER, CAN2.BUS); - // Configuration - config = new TalonFXConfiguration(); - config.MotorOutput.withNeutralMode(NeutralModeValue.Brake) - .withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = VELOCITY_VOLTAGE_GAINS; - config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.TorqueCurrent.PeakReverseTorqueCurrent = - -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); - tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); - - flywheelVelocity = flywheelLeaderTalon.getVelocity(); - flywheelAcceleration = flywheelLeaderTalon.getAcceleration(); - flywheelAppliedVolts = flywheelLeaderTalon.getMotorVoltage(); - flywheelCurrent = flywheelLeaderTalon.getSupplyCurrent(); - - followerAppliedVolts = flywheelFollowerTalon.getMotorVoltage(); - followerCurrent = flywheelFollowerTalon.getSupplyCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - flywheelVelocity, - flywheelAcceleration, - flywheelAppliedVolts, - flywheelCurrent, - followerAppliedVolts, - followerCurrent); - - // Note: Do not use optimizeBusUtilizationForAll() here - leader/follower - // configurations require certain status signals for proper follower behavior - - flywheelFollowerTalon.setControl( - new Follower(CAN2.FLYWHEEL_LEADER, MotorAlignmentValue.Opposed)); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - // Synchronous refresh for leader/follower motors to ensure consistent state - inputs.connected = - connectedDebounce.calculate( - BaseStatusSignal.refreshAll( - flywheelVelocity, - flywheelAcceleration, - flywheelAppliedVolts, - flywheelCurrent, - followerAppliedVolts, - followerCurrent) - .isOK()); - - inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); - inputs.currentAmps = flywheelCurrent.getValueAsDouble(); - inputs.velocityMetersPerSec = - (flywheelVelocity.getValue().in(RadiansPerSecond) * WHEEL_RADIUS.in(Meters)) - / MOTOR_REDUCTION; - - // Populate follower telemetry via inputs struct (AdvantageKit best practice: - // IO layers should be pure - only populate inputs, logging happens via @AutoLog) - inputs.followerAppliedVolts = followerAppliedVolts.getValueAsDouble(); - inputs.followerCurrentAmps = followerCurrent.getValueAsDouble(); - } - - @Override - public void setOpenLoop(Voltage volts) { - if (volts.in(Volts) < 1e-6) { - flywheelLeaderTalon.setControl(brake); - } else { - flywheelLeaderTalon.setControl(voltageRequest.withOutput(volts)); - } - } - - @Override - public void setVelocity(LinearVelocity tangentialVelocity) { - AngularVelocity angularVelocity = - RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / WHEEL_RADIUS.in(Meters)); - - TrapezoidProfile.State goal = - new TrapezoidProfile.State(angularVelocity.in(RotationsPerSecond), 0); - TrapezoidProfile.State setpoint = - new TrapezoidProfile.State( - flywheelVelocity.getValueAsDouble(), flywheelAcceleration.getValueAsDouble()); - - setpoint = profile.calculate(Robot.defaultPeriodSecs, setpoint, goal); - - velocityTorqueCurrentRequest.Velocity = setpoint.position; - velocityTorqueCurrentRequest.Acceleration = setpoint.velocity; - flywheelLeaderTalon.setControl(velocityTorqueCurrentRequest); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIO.java b/src/main/java/frc/robot/subsystems/launcher/HoodIO.java deleted file mode 100644 index 9dd4be4..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIO.java +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import org.littletonrobotics.junction.AutoLog; - -public interface HoodIO { - @AutoLog - public static class HoodIOInputs { - public boolean connected = false; - public Rotation2d position = Rotation2d.kZero; - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - } - - public default void updateInputs(HoodIOInputs inputs) {} - - public default void setOpenLoop(Voltage volts) {} - - public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {} - - public default void setVelocity(AngularVelocity angularVelocity) {} - - public default void configureSoftLimits(boolean enable) {} - - public default void resetEncoder() {} -} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java deleted file mode 100644 index dc6d2eb..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java +++ /dev/null @@ -1,156 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.sim.SparkMaxSim; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEO550Constants; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class HoodIOSimSpark implements HoodIO { - - private final DCMotorSim hoodSim; - - private final SparkMax max; - private final SparkClosedLoopController controller; - private final SparkMaxSim maxSim; - - private final SparkMaxConfig hoodConfig; - - public HoodIOSimSpark() { - max = new SparkMax(CAN2.HOOD, MotorType.kBrushless); - controller = max.getClosedLoopController(); - - hoodConfig = new SparkMaxConfig(); - - hoodConfig - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - hoodConfig - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - - hoodConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kPSimPos, 0.0, kDSimPos) - .allowedClosedLoopError(kAllowableError.in(Radians), ClosedLoopSlot.kSlot0); - - hoodConfig - .softLimit - .forwardSoftLimit(MAX_POS_RAD) - .forwardSoftLimitEnabled(true) - .reverseSoftLimit(MIN_POS_RAD) - .reverseSoftLimitEnabled(true); - - hoodConfig - .signals - .absoluteEncoderPositionAlwaysOn(true) - .absoluteEncoderPositionPeriodMs(20) - .absoluteEncoderVelocityAlwaysOn(true) - .absoluteEncoderVelocityPeriodMs(20) - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); - - max.configure(hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - maxSim = new SparkMaxSim(max, GEARBOX); - - hoodSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, 0.004, MOTOR_REDUCTION), GEARBOX); - - hoodSim.setState(MIN_POS_RAD, 0); - maxSim.setPosition(MIN_POS_RAD); - } - - @Override - public void updateInputs(HoodIOInputs inputs) { - // Update simulation state - double busVoltage = RoboRioSim.getVInVoltage(); - hoodSim.setInput(maxSim.getAppliedOutput() * busVoltage); - hoodSim.update(Robot.defaultPeriodSecs); - - if (maxSim.getPosition() > MAX_POS_RAD) { - hoodSim.setState(MAX_POS_RAD, 0); - maxSim.setPosition(MAX_POS_RAD); - } - - maxSim.iterate(hoodSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); - - // Update inputs - inputs.connected = true; - inputs.position = new Rotation2d(maxSim.getPosition()); - inputs.velocityRadPerSec = maxSim.getVelocity(); - inputs.appliedVolts = maxSim.getAppliedOutput() * maxSim.getBusVoltage(); - inputs.currentAmps = Math.abs(maxSim.getMotorCurrent()); - } - - @Override - public void setOpenLoop(Voltage volts) { - maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); - } - - @Override - public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - double setpoint = MathUtil.clamp(rotation.getRadians(), MIN_POS_RAD, MAX_POS_RAD); - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * angularVelocity.in(RadiansPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - controller.setSetpoint( - setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); - } - - @Override - public void setVelocity(AngularVelocity angularVelocity) { - double setpoint = angularVelocity.in(RadiansPerSecond); - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * angularVelocity.in(RadiansPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - controller.setSetpoint( - setpoint, ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); - } - - @Override - public void configureSoftLimits(boolean enable) { - hoodConfig.softLimit.forwardSoftLimitEnabled(enable); - hoodConfig.softLimit.reverseSoftLimitEnabled(enable); - } - - @Override - public void resetEncoder() { - maxSim.setPosition(MAX_POS_RAD); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java deleted file mode 100644 index 0741a65..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class HoodIOSimWPI implements HoodIO { - - private final DCMotorSim hoodSim; - - private boolean closedLoop = false; - private PIDController positionController = new PIDController(kPSimPos, 0.0, kDSimPos); - private double appliedVolts = 0.0; - private double feedforwardVolts = 0.0; - - public HoodIOSimWPI() { - hoodSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, 0.004, MOTOR_REDUCTION), GEARBOX); - - hoodSim.setState(0, 0.0); - - positionController.setTolerance(kAllowableError.in(Radians)); - } - - @Override - public void updateInputs(HoodIOInputs inputs) { - // Run closed-loop control - if (closedLoop) { - appliedVolts = - positionController.calculate(hoodSim.getAngularPositionRad()) + feedforwardVolts; - } else { - positionController.reset(); - } - - // Update simulation state - hoodSim.setInputVoltage( - MathUtil.clamp( - appliedVolts, -RobotConstants.NOMINAL_VOLTAGE, RobotConstants.NOMINAL_VOLTAGE)); - hoodSim.update(Robot.defaultPeriodSecs); - - // Update turn inputs - inputs.connected = true; - inputs.position = new Rotation2d(hoodSim.getAngularPositionRad()); - inputs.velocityRadPerSec = hoodSim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; - inputs.currentAmps = Math.abs(hoodSim.getCurrentDrawAmps()); - - if (hoodSim.getAngularPositionRad() > MAX_POS_RAD) { - hoodSim.setState(MAX_POS_RAD, 0); - } - } - - @Override - public void setOpenLoop(Voltage volts) { - closedLoop = false; - appliedVolts = volts.in(Volts); - } - - @Override - public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - closedLoop = true; - double setpoint = MathUtil.clamp(rotation.getRadians(), MIN_POS_RAD, MAX_POS_RAD); - this.feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * angularVelocity.in(RadiansPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - positionController.setSetpoint(setpoint); - } - - @Override - public void setVelocity(AngularVelocity angularVelocity) { - closedLoop = true; - double setpoint = angularVelocity.in(RadiansPerSecond); - hoodSim.setAngularVelocity(setpoint); - } - - @Override - public void configureSoftLimits(boolean enable) {} - - @Override - public void resetEncoder() { - hoodSim.setAngle(MAX_POS_RAD); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java deleted file mode 100644 index 6d94eed..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ /dev/null @@ -1,145 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.*; -import static frc.robot.util.SparkUtil.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEO550Constants; -import frc.robot.Constants.RobotConstants; -import frc.robot.util.SparkOdometryThread; -import frc.robot.util.SparkOdometryThread.SparkInputs; - -public class HoodIOSpark implements HoodIO { - - private final SparkMax hoodSpark; - private final RelativeEncoder encoderSpark; - private final SparkClosedLoopController hoodController; - private final SparkInputs sparkInputs; - - private final SparkMaxConfig hoodConfig; - - private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - - public HoodIOSpark() { - hoodSpark = new SparkMax(CAN2.HOOD, MotorType.kBrushless); - encoderSpark = hoodSpark.getEncoder(); - hoodController = hoodSpark.getClosedLoopController(); - - hoodConfig = new SparkMaxConfig(); - - hoodConfig - .inverted(true) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - hoodConfig - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - - hoodConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kPRealPos, 0.0, 0.0, ClosedLoopSlot.kSlot0) - .pid(kPRealVel, 0.0, 0.0, ClosedLoopSlot.kSlot1) - .allowedClosedLoopError(kAllowableError.in(Radians), ClosedLoopSlot.kSlot0); - - hoodConfig - .softLimit - .forwardSoftLimit(MAX_POS_RAD) - .forwardSoftLimitEnabled(true) - .reverseSoftLimit(MIN_POS_RAD) - .reverseSoftLimitEnabled(true); - - hoodConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); - - tryUntilOk( - hoodSpark, - 5, - () -> - hoodSpark.configure( - hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - // Register with background thread for non-blocking CAN reads - sparkInputs = SparkOdometryThread.getInstance().registerSpark(hoodSpark, encoderSpark); - } - - @Override - public void updateInputs(HoodIOInputs inputs) { - // Read from cached values (non-blocking) - updated by SparkOdometryThread - inputs.position = new Rotation2d(sparkInputs.getPosition()); - inputs.velocityRadPerSec = sparkInputs.getVelocity(); - inputs.appliedVolts = sparkInputs.getAppliedVolts(); - inputs.currentAmps = sparkInputs.getOutputCurrent(); - inputs.connected = connectedDebounce.calculate(sparkInputs.isConnected()); - } - - @Override - public void setOpenLoop(Voltage volts) { - hoodSpark.setVoltage(volts); - } - - @Override - public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - double setpoint = MathUtil.clamp(rotation.getRadians(), MIN_POS_RAD, MAX_POS_RAD); - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * angularVelocity.in(RadiansPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - hoodController.setSetpoint( - setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); - } - - @Override - public void setVelocity(AngularVelocity angularVelocity) { - double setpoint = angularVelocity.in(RadiansPerSecond); - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * angularVelocity.in(RadiansPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - hoodController.setSetpoint( - setpoint, ControlType.kVelocity, ClosedLoopSlot.kSlot1, feedforwardVolts); - } - - @Override - public void configureSoftLimits(boolean enable) { - hoodConfig.softLimit.forwardSoftLimitEnabled(enable); - hoodConfig.softLimit.reverseSoftLimitEnabled(enable); - tryUntilOk( - hoodSpark, - 5, - () -> - hoodSpark.configure( - hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters)); - } - - @Override - public void resetEncoder() { - encoderSpark.setPosition(MAX_POS_RAD); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java deleted file mode 100644 index b98ff95..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ /dev/null @@ -1,580 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.*; -import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.BALL_TO_HOOD_OFFSET; -import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.Robot; -import frc.robot.subsystems.launcher.LauncherConstants.FlywheelConstants.FlywheelScaling; -import java.util.ArrayList; -import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Launcher extends SubsystemBase { - private final TurretIO turretIO; - private final FlywheelIO flywheelIO; - private final HoodIO hoodIO; - - private final TurretIOInputsAutoLogged turretInputs = new TurretIOInputsAutoLogged(); - private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - private final HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); - - private final Supplier chassisPoseSupplier; - private final Supplier chassisSpeedsSupplier; - - private final Alert turretDisconnectedAlert; - private final Alert flywheelDisconnectedAlert; - private final Alert hoodDisconnectedAlert; - - private Translation3d vectorTurretBaseToTarget = new Translation3d(); - private Pose3d turretBasePose = new Pose3d(); - private Translation3d v0nominalLast = new Translation3d(); - private Translation3d v0replannedLast = new Translation3d(); - private Rotation2d horizontalAimAngle = Rotation2d.kZero; - - // Setpoint tracking for isOnTarget() tolerance check - private Rotation2d turretSetpoint = Rotation2d.kZero; - private Rotation2d hoodSetpoint = Rotation2d.kZero; - - // Cached values for deferred logging (populated in aim(), logged in periodic()) - private boolean hasCachedAimData = false; - private Translation3d cachedBaseSpeeds = new Translation3d(); - private boolean cachedFlywheelVelocityTooLow = false; - private Translation3d cachedActualD = new Translation3d(); - private Translation3d cachedActualV = new Translation3d(); - - // Fuel ballistics simulation - private final ArrayList fuelNominal = new ArrayList<>(); - private final ArrayList fuelReplanned = new ArrayList<>(); - private final ArrayList fuelActual = new ArrayList<>(); - private double fuelSpawnTimer = 0.0; - private double ballisticSimTimer = 0.0; - private double ballisticLogTimer = 0.0; - - // Turret desaturation - private final PIDController headingController = new PIDController(1.5, 0.0, 0.1); - - public Launcher( - Supplier chassisPoseSupplier, - Supplier chassisSpeedsSupplier, - TurretIO turretIO, - FlywheelIO flywheelIO, - HoodIO hoodIO) { - this.turretIO = turretIO; - this.flywheelIO = flywheelIO; - this.hoodIO = hoodIO; - - this.chassisPoseSupplier = chassisPoseSupplier; - this.chassisSpeedsSupplier = chassisSpeedsSupplier; - - turretDisconnectedAlert = new Alert("Disconnected turret motor", AlertType.kError); - flywheelDisconnectedAlert = new Alert("Disconnected flywheel motor", AlertType.kError); - hoodDisconnectedAlert = new Alert("Disconnected hood motor", AlertType.kError); - - headingController.setTolerance(MARGIN_RAD); - } - - @Override - public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - turretIO.updateInputs(turretInputs); - flywheelIO.updateInputs(flywheelInputs); - hoodIO.updateInputs(hoodInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - Logger.processInputs("Turret", turretInputs); - Logger.processInputs("Flywheel", flywheelInputs); - Logger.processInputs("Hood", hoodInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - turretDisconnectedAlert.set(!turretInputs.motorControllerConnected); - flywheelDisconnectedAlert.set(!flywheelInputs.connected); - hoodDisconnectedAlert.set(!hoodInputs.connected); - Logger.recordOutput( - "Faults/Launcher/TurretDisconnected", !turretInputs.motorControllerConnected); - Logger.recordOutput("Faults/Launcher/FlywheelDisconnected", !flywheelInputs.connected); - Logger.recordOutput("Faults/Launcher/HoodDisconnected", !hoodInputs.connected); - - // Log cached aim data (deferred from aim() to avoid Logger overhead in hot path) - if (hasCachedAimData) { - logCachedAimData(); - hasCachedAimData = false; - } - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Update and plot ball trajectories - if (LOG_FUEL_TRAJECTORIES) { - double dt = Robot.defaultPeriodSecs; - - ballisticSimTimer += dt; - ballisticLogTimer += dt; - - boolean doSim = ballisticSimTimer >= BALLISTIC_SIM_PERIOD; - boolean doLog = ballisticLogTimer >= BALLISTIC_LOG_PERIOD; - - if (doSim) { - ballisticSimTimer = 0.0; - - updateBallisticsSim(fuelNominal, NOMINAL_KEY, BALLISTIC_SIM_PERIOD, doLog); - updateBallisticsSim(fuelReplanned, REPLANNED_KEY, BALLISTIC_SIM_PERIOD, doLog); - updateBallisticsSim(fuelActual, ACTUAL_KEY, BALLISTIC_SIM_PERIOD, doLog); - } - - if (doLog) { - ballisticLogTimer = 0.0; - } - } - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { - long totalMs = (t4 - t0) / 1_000_000; - if (totalMs > 3) { - System.out.println( - "[Launcher] update=" - + (t1 - t0) / 1_000_000 - + "ms log=" - + (t2 - t1) / 1_000_000 - + "ms aimLog=" - + (t3 - t2) / 1_000_000 - + "ms ballistics=" - + (t4 - t3) / 1_000_000 - + "ms total=" - + totalMs - + "ms"); - } - } - } - - public void stop() { - turretIO.setOpenLoop(Volts.of(0.0)); - flywheelIO.setOpenLoop(Volts.of(0.0)); - hoodIO.setOpenLoop(Volts.of(0.0)); - } - - /** Returns the total motor current draw for battery simulation. Flywheel counts 2 motors. */ - public double getSimCurrentDrawAmps() { - return flywheelInputs.currentAmps * 2 + turretInputs.currentAmps + hoodInputs.currentAmps; - } - - public void aim(Translation3d target) { - long aimStart = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Get vector from static target to turret - turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(CHASSIS_TO_TURRET_BASE); - vectorTurretBaseToTarget = target.minus(turretBasePose.getTranslation()); - - // Compute distance-based impact angle - double horizontalDistance = - Math.hypot(vectorTurretBaseToTarget.getX(), vectorTurretBaseToTarget.getY()); - Rotation2d dynamicImpactAngle = getImpactAngle(horizontalDistance); - - // Set flywheel speed assuming a motionless robot - var v0_nominal = getV0Nominal(vectorTurretBaseToTarget, dynamicImpactAngle, NOMINAL_KEY); - var flywheelSetpoint = MetersPerSecond.of(flywheelSetpointfromBallistics(v0_nominal.getNorm())); - flywheelIO.setVelocity(flywheelSetpoint); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Get translation velocities (m/s) of the turret caused by motion of the chassis - var robotRelative = chassisSpeedsSupplier.get(); - var fieldRelative = - ChassisSpeeds.fromRobotRelativeSpeeds( - robotRelative, turretBasePose.toPose2d().getRotation()); - var v_base = getTurretBaseSpeeds(turretBasePose.toPose2d().getRotation(), fieldRelative); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Get actual initial shot speed - double initialSpeedMetersPerSec = - ballisticsFromFlywheelSetpoint(flywheelInputs.velocityMetersPerSec); - - // Replan shot using actual initial shot speed speed - var v0_total = - getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, REPLANNED_KEY); - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Point turret to align velocity vectors - var v0_flywheel = v0_total.minus(v_base); - - // Check if v0_flywheel has non-zero horizontal component - double v0_horizontal = Math.hypot(v0_flywheel.getX(), v0_flywheel.getY()); - if (!Double.isFinite(v0_horizontal) || v0_horizontal < 1e-6) { - // Flywheel velocity is too low or target unreachable, cache for deferred logging - cachedFlywheelVelocityTooLow = true; - hasCachedAimData = true; - return; - } - cachedFlywheelVelocityTooLow = false; - - horizontalAimAngle = new Rotation2d(v0_flywheel.getX(), v0_flywheel.getY()); - turretSetpoint = horizontalAimAngle.minus(turretBasePose.toPose2d().getRotation()); - turretIO.setPosition( - turretSetpoint, - RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus().times(2.0)); - hoodSetpoint = new Rotation2d(v0_horizontal, v0_flywheel.getZ()).minus(BALL_TO_HOOD_OFFSET); - hoodIO.setPosition(hoodSetpoint, RadiansPerSecond.of(0)); - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Get actual hood & turret position - Rotation2d hoodPosition = hoodInputs.position.plus(BALL_TO_HOOD_OFFSET); - Rotation2d turretPosition = - turretInputs.relativePosition.plus(turretBasePose.toPose2d().getRotation()); - - // Get mechanism angles - double hoodCos = hoodPosition.getCos(); - double hoodSin = hoodPosition.getSin(); - double turretCos = turretPosition.getCos(); - double turretSin = turretPosition.getSin(); - - // Build actual velocities - double vx = hoodCos * turretCos * initialSpeedMetersPerSec + v_base.getX(); - double vy = hoodCos * turretSin * initialSpeedMetersPerSec + v_base.getY(); - double vz = hoodSin * initialSpeedMetersPerSec; - Translation3d v0_actual = new Translation3d(vx, vy, vz); - - // Cache values for deferred logging (will be logged in periodic()) - cachedActualD = vectorTurretBaseToTarget; - cachedActualV = v0_actual; - hasCachedAimData = true; - long t5 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - - // Spawn simulated fuel - fuelSpawnTimer += Robot.defaultPeriodSecs; - if (fuelSpawnTimer >= FUEL_SPAWN_PERIOD && LOG_FUEL_TRAJECTORIES) { - fuelSpawnTimer = 0.0; - - fuelNominal.add( - new BallisticObject(turretBasePose.getTranslation(), v0_nominal, target.getMeasureZ())); - fuelReplanned.add( - new BallisticObject(turretBasePose.getTranslation(), v0_total, target.getMeasureZ())); - fuelActual.add( - new BallisticObject(turretBasePose.getTranslation(), v0_actual, target.getMeasureZ())); - } - - // Profiling output for aim() - if (Constants.FeatureFlags.PROFILING_ENABLED) { - long totalUs = (t5 - aimStart) / 1_000; - if (totalUs > 500) { - System.out.println( - "[Launcher.aim] v0nom=" - + (t1 - aimStart) / 1_000 - + "us baseSpeeds=" - + (t2 - t1) / 1_000 - + "us v0replan=" - + (t3 - t2) / 1_000 - + "us setPos=" - + (t4 - t3) / 1_000 - + "us rest=" - + (t5 - t4) / 1_000 - + "us total=" - + totalUs - + "us"); - } - } - } - - @AutoLogOutput(key = "Launcher/TurretPose") - public Pose2d getTurretPose() { - return new Pose2d( - turretBasePose.getX(), - turretBasePose.getY(), - turretBasePose.getRotation().toRotation2d().plus(turretInputs.relativePosition)); - } - - @AutoLogOutput(key = "Launcher/HorizontalAimAngle") - public Rotation2d getHorizontalAimAngle() { - return horizontalAimAngle; - } - - public double getTurretDesaturationDelta() { - return headingController.calculate(-turretInputs.oversaturationLessMargin, 0); - } - - public boolean isTurretDesaturated() { - // return headingController.atSetpoint(); - return Math.abs(turretInputs.oversaturation) < Units.degreesToRadians(8); - } - - @AutoLogOutput(key = "Launcher/IsOnTarget") - public boolean isOnTarget() { - double tolerance = IS_ON_TARGET_TOLERANCE.in(Radians); - double hoodError = Math.abs(hoodInputs.position.minus(hoodSetpoint).getRadians()); - double turretError = Math.abs(turretInputs.relativePosition.minus(turretSetpoint).getRadians()); - return hoodError < tolerance && turretError < tolerance; - } - - private Translation3d getTurretBaseSpeeds(Rotation2d rotation, ChassisSpeeds chassisSpeeds) { - double vx = chassisSpeeds.vxMetersPerSecond; - double vy = chassisSpeeds.vyMetersPerSecond; - double omega = chassisSpeeds.omegaRadiansPerSecond; - - double rx = CHASSIS_TO_TURRET_BASE.getX(); - double ry = CHASSIS_TO_TURRET_BASE.getY(); - - double offX = -omega * ry; - double offY = omega * rx; - - double cos = rotation.getCos(); - double sin = rotation.getSin(); - - double baseVx = vx + offX * cos - offY * sin; - double baseVy = vy + offX * sin + offY * cos; - - var baseSpeeds = new Translation3d(baseVx, baseVy, 0); - - // Cache for deferred logging (will be logged in periodic()) - cachedBaseSpeeds = baseSpeeds; - - return baseSpeeds; - } - - private Rotation2d getImpactAngle(double horizontalDistanceMeters) { - double t = - (horizontalDistanceMeters - IMPACT_ANGLE_CLOSE_DISTANCE.in(Meters)) - / (IMPACT_ANGLE_FAR_DISTANCE.in(Meters) - IMPACT_ANGLE_CLOSE_DISTANCE.in(Meters)); - t = MathUtil.clamp(t, 0.0, 1.0); - - double angleDeg = - MathUtil.interpolate(IMPACT_ANGLE_CLOSE.getDegrees(), IMPACT_ANGLE_FAR.getDegrees(), t); - return Rotation2d.fromDegrees(angleDeg); - } - - private Translation3d getV0Nominal(Translation3d d, Rotation2d impactAngle, String key) { - double dr = Math.hypot(d.getX(), d.getY()); - // Use !(x >= threshold) instead of (x < threshold) to catch NaN - if (!(dr >= 1e-6)) { - Logger.recordOutput("Launcher/" + key + "/Reachable", false); - // log(d, v0, key); - return v0nominalLast; - } - - double dz = d.getZ(); - - double denominator = 2 * (dz + dr * impactAngle.getTan()); - // Guard: denominator <= 0 means target is unreachable with this impact angle (would require - // negative velocity or infinite speed). Using < 1e-6 threshold also catches near-zero values - // that would cause numerical instability in sqrt(G / denominator). - // Use !(x >= threshold) instead of (x < threshold) to catch NaN - if (!(denominator >= 1e-6)) { - Logger.recordOutput("Launcher/" + key + "/Reachable", false); - return v0nominalLast; - } - double v_0r = dr * Math.sqrt(GRAVITY / denominator); - if (!(v_0r >= 1e-6)) { - Logger.recordOutput("Launcher/" + key + "/Reachable", false); - return v0nominalLast; - } - double v_0z = (GRAVITY * dr) / v_0r - v_0r * impactAngle.getTan(); - - double v_0x = v_0r * d.toTranslation2d().getAngle().getCos(); - double v_0y = v_0r * d.toTranslation2d().getAngle().getSin(); - - v0nominalLast = new Translation3d(v_0x, v_0y, v_0z); - Logger.recordOutput("Launcher/" + key + "/Reachable", true); - log(d, v0nominalLast, key); - return v0nominalLast; - } - - private Translation3d getV0Replanned(Translation3d d, double v_flywheel, String key) { - // Geometry - double dr = Math.hypot(d.getX(), d.getY()); - // Use !(x >= threshold) instead of (x < threshold) to catch NaN - if (!(dr >= 1e-6)) { - Logger.recordOutput("Launcher/" + key + "/Reachable", false); - // log(d, v0, key); - return v0replannedLast; - } - - double dz = d.getZ(); - - // Unit vectors toward target in XY plane - double rHatX = d.getX() / dr; - double rHatY = d.getY() / dr; - - double v_sq = v_flywheel * v_flywheel; - double discriminant = v_sq * v_sq - GRAVITY * (GRAVITY * dr * dr + 2 * dz * v_sq); - - // Guard: discriminant < 0 means target is beyond maximum range for current flywheel speed. - // Using < 1e-6 threshold adds safety margin against sqrt of tiny negative values from - // floating-point errors at the edge of reachable range. - // Use !(x >= threshold) instead of (x < threshold) to catch NaN - if (!(discriminant >= 1e-6)) { - Logger.recordOutput("Launcher/" + key + "/Reachable", false); - return v0replannedLast; - } - - // High-arc solution (lower trajectory would use v_sq - sqrt(discriminant)) - double tanTheta = (v_sq + Math.sqrt(discriminant)) / (GRAVITY * dr); - - // sin(2*atan(x)) = 2x/(1+x²) - avoids two trig function calls - Logger.recordOutput( - "Launcher/" + key + "/PredictedRange", - (v_sq * 2.0 * tanTheta) / (GRAVITY * (1.0 + tanTheta * tanTheta))); - - // Effective velocity available for ballistics - double v_r = v_flywheel / Math.sqrt(1 + tanTheta * tanTheta); - double v_z = v_r * tanTheta; - - double v_required = Math.hypot(v_r, v_z); - - // Use !(x >= threshold) instead of (x < threshold) to catch NaN - if (!(v_required >= 1e-6)) { - Logger.recordOutput("Launcher/" + key + "/Reachable", false); - // log(d, v0, key); - return v0replannedLast; - } - - // Scale to match actual flywheel speed - double scale = v_flywheel / v_required; - - v_r *= scale; - v_z *= scale; - - // Back to field frame - v0replannedLast = new Translation3d(rHatX * v_r, rHatY * v_r, v_z); - Logger.recordOutput("Launcher/" + key + "/Reachable", true); - log(d, v0replannedLast, key); - return v0replannedLast; - } - - /** Logs cached aim data that was deferred from aim() to avoid Logger overhead in hot path. */ - private void logCachedAimData() { - Logger.recordOutput("Launcher/BaseSpeeds", cachedBaseSpeeds); - Logger.recordOutput("Launcher/Flywheel velocity too low", cachedFlywheelVelocityTooLow); - - if (!cachedFlywheelVelocityTooLow) { - log(cachedActualD, cachedActualV, ACTUAL_KEY); - } - } - - private void log(Translation3d d, Translation3d v, String key) { - Logger.recordOutput("Launcher/" + key + "/InitialVelocities", v); - Logger.recordOutput("Launcher/" + key + "/InitialSpeedMetersPerSecond", v.getNorm()); - - var v_2d = v.toTranslation2d(); - var v_r = v_2d.getNorm(); - var v_z = v.getZ(); - - // Zero-velocity guard: avoid getAngle() on zero-length vectors - // Use !(v_r >= 1e-6) instead of (v_r < 1e-6) to also catch NaN - if (!(v_r >= 1e-6)) { - Logger.recordOutput("Launcher/" + key + "/VerticalLaunchAngleDegrees", 0.0); - Logger.recordOutput("Launcher/" + key + "/HorizontalLaunchAngleDegrees", 0.0); - Logger.recordOutput("Launcher/" + key + "/TravelTime", 0.0); - } else { - // Logger.recordOutput( - // "Launcher/" + key + "/VerticalLaunchAngleDegrees", - // new Translation2d(v_r, v_z).getAngle().getDegrees()); - Logger.recordOutput( - "Launcher/" + key + "/HorizontalLaunchAngleDegrees", v_2d.getAngle().getDegrees()); - - double dr = Math.hypot(d.getX(), d.getY()); - Logger.recordOutput("Launcher/" + key + "/TravelTime", dr / v_r); - } - - var max_height = turretBasePose.getZ() + v_z * v_z / (2 * GRAVITY); - Logger.recordOutput("Launcher/" + key + "/MaxHeight", max_height); - - boolean clearsCeiling = Meters.of(max_height).plus(FUEL_RADIUS).lt(CEILING_HEIGHT); - Logger.recordOutput("Launcher/" + key + "/ClearsCeiling", clearsCeiling); - } - - private static class BallisticObject { - double px, py, pz; - double vx, vy, vz; - double targetHeight; - - BallisticObject(Translation3d position, Translation3d velocity, Distance targetHeight) { - this.px = position.getX(); - this.py = position.getY(); - this.pz = position.getZ(); - this.vx = velocity.getX(); - this.vy = velocity.getY(); - this.vz = velocity.getZ(); - this.targetHeight = targetHeight.in(Meters); - } - - private Translation3d getPosition() { - return new Translation3d(px, py, pz); - } - } - - private void updateBallisticsSim( - ArrayList traj, String key, double dt, boolean log) { - - for (int i = traj.size() - 1; i >= 0; i--) { - BallisticObject o = traj.get(i); - - o.vz -= GRAVITY * dt; - o.px += o.vx * dt; - o.py += o.vy * dt; - o.pz += o.vz * dt; - - if (o.pz < o.targetHeight && o.vz < 0) { - traj.remove(i); - } - } - - if (log) { - Logger.recordOutput("Launcher/" + key + "/FuelTrajectory", getBallTrajectory(traj)); - } - } - - public Translation3d[] getBallTrajectory(ArrayList traj) { - Translation3d[] t = new Translation3d[traj.size()]; - for (int i = 0; i < traj.size(); i++) { - t[i] = traj.get(i).getPosition(); - } - return t; - } - - public Command initializeHoodCommand() { - return new StartEndCommand( - // initialize - () -> { - hoodIO.configureSoftLimits(false); - hoodIO.setOpenLoop(Volts.of(1.0)); - }, - // end - () -> { - hoodIO.configureSoftLimits(true); - hoodIO.resetEncoder(); - }, - // requirements - this) - .withTimeout(1.0) - .withName("Initialize hood"); - } - - private double flywheelSetpointfromBallistics(double ballistics) { - return FlywheelScaling.COEFFICIENT * Math.pow(ballistics, FlywheelScaling.EXPONENT); - } - - private double ballisticsFromFlywheelSetpoint(double setpoint) { - return Math.pow(setpoint / FlywheelScaling.COEFFICIENT, 1.0 / FlywheelScaling.EXPONENT); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java deleted file mode 100644 index 182270d..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ /dev/null @@ -1,156 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import frc.robot.Constants; - -public final class LauncherConstants { - - // Geometry - public static final Distance FUEL_RADIUS = Inches.of(3); - public static final Distance CEILING_HEIGHT = Feet.of(11).plus(Inches.of(2)); - public static final double GRAVITY = 9.81; - - // Distance-based impact angle: steeper at close range, shallower at far range - public static final Distance IMPACT_ANGLE_CLOSE_DISTANCE = Meters.of(2.0); - public static final Distance IMPACT_ANGLE_FAR_DISTANCE = Meters.of(6.0); - public static final Rotation2d IMPACT_ANGLE_CLOSE = Rotation2d.fromDegrees(55); - public static final Rotation2d IMPACT_ANGLE_FAR = Rotation2d.fromDegrees(40); - - // Logging / simulation periods - public static final boolean LOG_FUEL_TRAJECTORIES; - public static final double FUEL_SPAWN_PERIOD; - public static final double BALLISTIC_SIM_PERIOD; - public static final double BALLISTIC_LOG_PERIOD; - - static { - switch (Constants.currentMode) { - case REAL -> { - LOG_FUEL_TRAJECTORIES = true; - FUEL_SPAWN_PERIOD = 0.2; - BALLISTIC_SIM_PERIOD = 0.1; - BALLISTIC_LOG_PERIOD = 0.25; - } - - case SIM -> { - LOG_FUEL_TRAJECTORIES = true; - FUEL_SPAWN_PERIOD = 0.1; - BALLISTIC_SIM_PERIOD = 0.05; - BALLISTIC_LOG_PERIOD = 0.1; - } - - case REPLAY -> { - LOG_FUEL_TRAJECTORIES = false; - FUEL_SPAWN_PERIOD = 0.0; - BALLISTIC_SIM_PERIOD = 0.0; - BALLISTIC_LOG_PERIOD = 0.0; - } - - default -> { - LOG_FUEL_TRAJECTORIES = true; - FUEL_SPAWN_PERIOD = 0.1; - BALLISTIC_SIM_PERIOD = 0.05; - BALLISTIC_LOG_PERIOD = 0.1; - } - } - } - - public static final String NOMINAL_KEY = "Nominal"; - public static final String REPLANNED_KEY = "Replanned"; - public static final String ACTUAL_KEY = "Actual"; - - // Tolerance for isOnTarget() check (independent of motor controller allowable error) - public static final Angle IS_ON_TARGET_TOLERANCE = Degrees.of(2.0); - - public static final class TurretConstants { - // Geometry - public static final Transform3d CHASSIS_TO_TURRET_BASE = - new Transform3d(Inches.of(-4.000), Inches.of(6.500), Inches.of(16.331), Rotation3d.kZero); - public static final Rotation2d ABS_ENCODER_OFFSET = new Rotation2d(5.157); - public static final Rotation2d MECHANISM_OFFSET = Rotation2d.kZero; - public static final double UPPER_LIMIT_RAD = Units.degreesToRadians(270); - public static final double LOWER_LIMIT_RAD = Units.degreesToRadians(45); - public static final double MARGIN_RAD = Units.degreesToRadians(5); - - // Position controller - public static final double kP = 0.5; - public static final double kD = 0.05; - public static final Angle kAllowableError = Degrees.of(0.25); - - // Motor controller - public static final double MOTOR_REDUCTION = 9.0 * 72.0 / 12.0; - public static final DCMotor GEARBOX = DCMotor.getNeo550(1); - public static final AngularVelocity MAX_ANGULAR_VELOCITY = - RadiansPerSecond.of(GEARBOX.freeSpeedRadPerSec / MOTOR_REDUCTION); - public static final double ENCODER_POSITION_FACTOR = (2 * Math.PI) / MOTOR_REDUCTION; // Radians - public static final double ENCODER_VELOCITY_FACTOR = - (2 * Math.PI) / (60.0 * MOTOR_REDUCTION); // Rad/sec - } - - public static final class FlywheelConstants { - - public static final class FlywheelScaling { - public static final double EXPONENT = 1.8; - public static final double COEFFICIENT = 0.335; - } - - public static final Distance WHEEL_RADIUS = Inches.of(1.5); - - // Velocity Controller - public static final double MAX_ACCELERATION = 4000.0; - public static final double MAX_JERK = 40000.0; - - // Motor controller - public static final double MOTOR_REDUCTION = 1.0; - public static final DCMotor GEARBOX = DCMotor.getKrakenX60(2); - public static final AngularVelocity MAX_ANGULAR_VELOCITY = - RadiansPerSecond.of(GEARBOX.freeSpeedRadPerSec / MOTOR_REDUCTION); - public static final Slot0Configs VELOCITY_VOLTAGE_GAINS = - new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); - public static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = - new Slot1Configs().withKP(12).withKI(0.0).withKD(0.0).withKS(2.5); - } - - public static final class HoodConstants { - public static final Rotation2d BALL_TO_HOOD_OFFSET = new Rotation2d(Degrees.of(0)); - public static final Angle kAllowableError = Degrees.of(0.25); - - // Position controller - public static final double kPRealPos = 0.35; - public static final double kPSimPos = 1.5; - public static final double kDSimPos = 0.05; - public static final Angle MIN_POSITION = Degrees.of(60); - public static final Angle MAX_POSITION = Degrees.of(80); - public static final double MIN_POS_RAD = MIN_POSITION.in(Radians); - public static final double MAX_POS_RAD = MAX_POSITION.in(Radians); - - // Velocity controller - public static final double kPRealVel = 0.2; - - // Motor controller - public static final double MOTOR_REDUCTION = 5.0 * 256.0 / 16.0; - public static final DCMotor GEARBOX = DCMotor.getNeo550(1); - public static final AngularVelocity MAX_ANGULAR_VELOCITY = - RadiansPerSecond.of(GEARBOX.freeSpeedRadPerSec / MOTOR_REDUCTION); - public static final double ENCODER_POSITION_FACTOR = 2 * Math.PI / MOTOR_REDUCTION; // Radians - public static final double ENCODER_VELOCITY_FACTOR = - (2 * Math.PI) / (60.0 * MOTOR_REDUCTION); // Rad/sec - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java b/src/main/java/frc/robot/subsystems/launcher/TurretIO.java deleted file mode 100644 index b4bfeaf..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIO.java +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import org.littletonrobotics.junction.AutoLog; - -public interface TurretIO { - @AutoLog - public static class TurretIOInputs { - public boolean motorControllerConnected = false; - public Rotation2d relativePosition = Rotation2d.kZero; - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - - public boolean absoluteEncoderConnected = false; - public Rotation2d absolutePosition = Rotation2d.kZero; - - public double oversaturation; - public double oversaturationLessMargin; - } - - public default void updateInputs(TurretIOInputs inputs) {} - - public default void setOpenLoop(Voltage volts) {} - - public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {} -} diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java deleted file mode 100644 index c2f853f..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.sim.SparkMaxSim; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.MotorConstants.NEO550Constants; -import frc.robot.Constants.RobotConstants; -import frc.robot.Robot; - -public class TurretIOSimSpark implements TurretIO { - private static final double TURRET_MOI_KG_M2 = 0.237; - - private final DCMotorSim turnSim; - - private final SparkMax turnSpark; - private final SparkClosedLoopController controller; - private final SparkMaxSim turnSparkSim; - - private double oversaturation = 0.0; - private double oversaturationLessMargin = 0.0; - - public TurretIOSimSpark() { - turnSpark = new SparkMax(CAN2.TURRET, MotorType.kBrushless); - controller = turnSpark.getClosedLoopController(); - - var turnConfig = new SparkMaxConfig(); - - turnConfig - .inverted(false) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - turnConfig - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - - turnConfig - .softLimit - .forwardSoftLimit(UPPER_LIMIT_RAD) - .forwardSoftLimitEnabled(true) - .reverseSoftLimit(LOWER_LIMIT_RAD) - .reverseSoftLimitEnabled(true); - - turnConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kP, 0.0, kD) - .allowedClosedLoopError(kAllowableError.in(Radians), ClosedLoopSlot.kSlot0); - - turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); - - turnSpark.configure(turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - turnSparkSim = new SparkMaxSim(turnSpark, GEARBOX); - - turnSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, TURRET_MOI_KG_M2, MOTOR_REDUCTION), - GEARBOX); - - turnSim.setState(2.0 * Math.PI - MECHANISM_OFFSET.getRadians(), 0); - turnSparkSim.setPosition(turnSim.getAngularPositionRad()); - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - // Update simulation state - double busVoltage = RoboRioSim.getVInVoltage(); - turnSim.setInput(turnSparkSim.getAppliedOutput() * busVoltage); - turnSim.update(Robot.defaultPeriodSecs); - turnSparkSim.iterate( - turnSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); - - // Update inputs - inputs.motorControllerConnected = true; - inputs.relativePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET); - inputs.velocityRadPerSec = turnSparkSim.getVelocity(); - inputs.appliedVolts = turnSparkSim.getAppliedOutput() * turnSparkSim.getBusVoltage(); - inputs.currentAmps = Math.abs(turnSparkSim.getMotorCurrent()); - - inputs.absoluteEncoderConnected = true; - inputs.absolutePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET); - - inputs.oversaturation = oversaturation; - inputs.oversaturationLessMargin = oversaturationLessMargin; - } - - @Override - public void setOpenLoop(Voltage volts) { - oversaturation = 0.0; - oversaturationLessMargin = 0.0; - controller.setSetpoint(volts.in(Volts), ControlType.kVoltage); - } - - @Override - public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - double setpoint = - MathUtil.inputModulus( - rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2.0 * Math.PI); - double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD); - double clampedSetpointWithMargin = - MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD); - oversaturation = setpoint - clampedSetpoint; - oversaturationLessMargin = setpoint - clampedSetpointWithMargin; - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * angularVelocity.in(RadiansPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - controller.setSetpoint( - clampedSetpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java deleted file mode 100644 index 05c83d3..0000000 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright (c) 2025-2026 Triple Helix Robotics, FRC Team 2363 -// https://github.com/TripleHelixProgramming -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - -package frc.robot.subsystems.launcher; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; -import static frc.robot.util.SparkUtil.*; - -import com.revrobotics.PersistMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import frc.robot.Constants.CANBusPorts.CAN2; -import frc.robot.Constants.DIOPorts; -import frc.robot.Constants.MotorConstants.NEO550Constants; -import frc.robot.Constants.RobotConstants; -import frc.robot.util.SparkOdometryThread; -import frc.robot.util.SparkOdometryThread.SparkInputs; - -public class TurretIOSpark implements TurretIO { - - private final SparkBase turnSpark; - private final RelativeEncoder turnSparkEncoder; - private final SparkClosedLoopController controller; - private final DutyCycleEncoder absoluteEncoder; - private final SparkInputs sparkInputs; - - private final Debouncer motorControllerConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private final Debouncer absEncoderConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private boolean relativeEncoderSeeded = false; - private double oversaturation = 0.0; - private double oversaturationLessMargin = 0.0; - - public TurretIOSpark() { - turnSpark = new SparkMax(CAN2.TURRET, MotorType.kBrushless); - controller = turnSpark.getClosedLoopController(); - turnSparkEncoder = turnSpark.getEncoder(); - absoluteEncoder = - new DutyCycleEncoder( - new DigitalInput(DIOPorts.TURRET_ABS_ENCODER), - 2 * Math.PI, - ABS_ENCODER_OFFSET.getRadians() + MECHANISM_OFFSET.getRadians()); - - var turnConfig = new SparkMaxConfig(); - - turnConfig - .inverted(true) - .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) - .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); - - turnConfig - .encoder - .positionConversionFactor(ENCODER_POSITION_FACTOR) - .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - - turnConfig - .softLimit - .forwardSoftLimit(UPPER_LIMIT_RAD) - .forwardSoftLimitEnabled(true) - .reverseSoftLimit(LOWER_LIMIT_RAD) - .reverseSoftLimitEnabled(true); - - turnConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kP, 0.0, kD) - .allowedClosedLoopError(kAllowableError.in(Radians), ClosedLoopSlot.kSlot0); - - turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); - - tryUntilOk( - turnSpark, - 5, - () -> - turnSpark.configure( - turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - // Register with background thread for non-blocking CAN reads - sparkInputs = SparkOdometryThread.getInstance().registerSpark(turnSpark, turnSparkEncoder); - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - if (!relativeEncoderSeeded && inputs.absoluteEncoderConnected) { - turnSparkEncoder.setPosition(absoluteEncoder.get()); - relativeEncoderSeeded = true; - } - - // Read from cached values (non-blocking) - updated by SparkOdometryThread - inputs.relativePosition = new Rotation2d(sparkInputs.getPosition()).plus(MECHANISM_OFFSET); - inputs.velocityRadPerSec = sparkInputs.getVelocity(); - inputs.appliedVolts = sparkInputs.getAppliedVolts(); - inputs.currentAmps = sparkInputs.getOutputCurrent(); - inputs.motorControllerConnected = - motorControllerConnectedDebounce.calculate(sparkInputs.isConnected()); - - // Absolute encoder is read directly (DIO, not CAN - already fast) - inputs.absoluteEncoderConnected = - absEncoderConnectedDebounce.calculate(absoluteEncoder.isConnected()); - inputs.absolutePosition = new Rotation2d(absoluteEncoder.get()); - - inputs.oversaturation = oversaturation; - inputs.oversaturationLessMargin = oversaturationLessMargin; - } - - @Override - public void setOpenLoop(Voltage volts) { - oversaturation = 0.0; - oversaturationLessMargin = 0.0; - controller.setSetpoint(volts.in(Volts), ControlType.kVoltage); - } - - @Override - public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - double setpoint = - MathUtil.inputModulus( - rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2 * Math.PI); - double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD); - double clampedSetpointWithMargin = - MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD); - oversaturation = setpoint - clampedSetpoint; - oversaturationLessMargin = setpoint - clampedSetpointWithMargin; - double feedforwardVolts = - RobotConstants.NOMINAL_VOLTAGE - * angularVelocity.in(RadiansPerSecond) - / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); - controller.setSetpoint( - clampedSetpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); - } -} diff --git a/src/main/java/frc/robot/subsystems/leds/LEDController.java b/src/main/java/frc/robot/subsystems/leds/LEDController.java index 3c09ec3..4627cd7 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDController.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDController.java @@ -170,10 +170,7 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) { 10.0, // Fill color () -> { - if (GameState.isMyHubActive()) { - return GameState.getMyAlliance() == Alliance.Blue ? Color.kBlue : Color.kRed; - } - return GameState.getMyAlliance() == Alliance.Blue ? Color.kRed : Color.kBlue; + return GameState.getMyAlliance() == Alliance.Blue ? Color.kBlue : Color.kRed; }, // Background color Color.kBlack,