From 53a2051741582ebe738b4f8277bb5e841c10a8de Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 20:29:25 -0400 Subject: [PATCH 1/6] delete feeder, intake, launcher subsystems --- src/main/java/frc/robot/Robot.java | 75 --- src/main/java/frc/robot/auto/AutoMode.java | 14 +- .../frc/robot/subsystems/feeder/Feeder.java | 108 ---- .../subsystems/feeder/FeederConstants.java | 58 -- .../frc/robot/subsystems/feeder/KickerIO.java | 28 - .../subsystems/feeder/KickerIOSimSpark.java | 102 --- .../subsystems/feeder/KickerIOSpark.java | 98 --- .../robot/subsystems/feeder/SpindexerIO.java | 28 - .../feeder/SpindexerIOSimSpark.java | 103 ---- .../subsystems/feeder/SpindexerIOSpark.java | 98 --- .../frc/robot/subsystems/hopper/Hopper.java | 98 --- .../frc/robot/subsystems/hopper/HopperIO.java | 24 - .../robot/subsystems/hopper/HopperIOReal.java | 37 -- .../robot/subsystems/hopper/HopperIOSim.java | 37 -- .../frc/robot/subsystems/intake/Intake.java | 179 ------ .../robot/subsystems/intake/IntakeArmIO.java | 24 - .../subsystems/intake/IntakeArmIOReal.java | 38 -- .../subsystems/intake/IntakeArmIOSim.java | 39 -- .../subsystems/intake/IntakeConstants.java | 61 -- .../frc/robot/subsystems/intake/RollerIO.java | 28 - .../subsystems/intake/RollerIOSimSpark.java | 102 --- .../subsystems/intake/RollerIOSimTalonFX.java | 127 ---- .../subsystems/intake/RollerIOSpark.java | 95 --- .../subsystems/intake/RollerIOTalonFX.java | 117 ---- .../robot/subsystems/launcher/FlywheelIO.java | 31 - .../launcher/FlywheelIOSimTalonFX.java | 135 ---- .../subsystems/launcher/FlywheelIOSimWPI.java | 78 --- .../launcher/FlywheelIOTalonFX.java | 153 ----- .../frc/robot/subsystems/launcher/HoodIO.java | 36 -- .../subsystems/launcher/HoodIOSimSpark.java | 156 ----- .../subsystems/launcher/HoodIOSimWPI.java | 101 --- .../subsystems/launcher/HoodIOSpark.java | 145 ----- .../robot/subsystems/launcher/Launcher.java | 580 ------------------ .../launcher/LauncherConstants.java | 156 ----- .../robot/subsystems/launcher/TurretIO.java | 36 -- .../subsystems/launcher/TurretIOSimSpark.java | 139 ----- .../subsystems/launcher/TurretIOSpark.java | 153 ----- 37 files changed, 1 insertion(+), 3616 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/feeder/Feeder.java delete mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/feeder/KickerIO.java delete mode 100644 src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/feeder/SpindexerIO.java delete mode 100644 src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/hopper/Hopper.java delete mode 100644 src/main/java/frc/robot/subsystems/hopper/HopperIO.java delete mode 100644 src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeArmIO.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/RollerIO.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/HoodIO.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/Launcher.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/TurretIO.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java delete mode 100644 src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 795e20d..6a1085c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -65,36 +65,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,10 +114,6 @@ 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; @@ -199,20 +165,6 @@ public Robot() { 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"); // Start kernel log monitoring (singleton, starts automatically on first call) @@ -240,23 +192,6 @@ public Robot() { 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)); break; case REPLAY: // Replaying a log @@ -283,16 +218,6 @@ public Robot() { 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() {}); break; } 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/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); - } -} From 79f0d1874d0685f29a9e84c2b2ac2280c936870d Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 5 Jun 2026 13:15:56 -0400 Subject: [PATCH 2/6] delete auto routines and remove subsystem references from Robot All auto modes depended on feeder, intake, and launcher subsystems which were removed in the previous commit. Delete all auto subclasses and clean up Robot.java and Constants.java to remove every reference to those subsystems. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Constants.java | 29 --- src/main/java/frc/robot/Robot.java | 238 +----------------- .../java/frc/robot/auto/B_LeftTrenchAuto.java | 74 ------ .../robot/auto/B_LeftTrenchMoveFirstAuto.java | 70 ------ .../frc/robot/auto/B_RightTrenchAuto.java | 74 ------ .../auto/B_RightTrenchMoveFirstAuto.java | 70 ------ .../auto/NewB_LeftTrenchMoveFirstAuto.java | 69 ----- .../auto/NewB_RightTrenchMoveFirstAuto.java | 69 ----- .../auto/NewR_LeftTrenchMoveFirstAuto.java | 69 ----- .../auto/NewR_RightTrenchMoveFirstAuto.java | 69 ----- .../java/frc/robot/auto/R_LeftTrenchAuto.java | 76 ------ .../robot/auto/R_LeftTrenchMoveFirstAuto.java | 70 ------ .../frc/robot/auto/R_RightTrenchAuto.java | 74 ------ .../auto/R_RightTrenchMoveFirstAuto.java | 70 ------ 14 files changed, 7 insertions(+), 1114 deletions(-) delete mode 100644 src/main/java/frc/robot/auto/B_LeftTrenchAuto.java delete mode 100644 src/main/java/frc/robot/auto/B_LeftTrenchMoveFirstAuto.java delete mode 100644 src/main/java/frc/robot/auto/B_RightTrenchAuto.java delete mode 100644 src/main/java/frc/robot/auto/B_RightTrenchMoveFirstAuto.java delete mode 100644 src/main/java/frc/robot/auto/NewB_LeftTrenchMoveFirstAuto.java delete mode 100644 src/main/java/frc/robot/auto/NewB_RightTrenchMoveFirstAuto.java delete mode 100644 src/main/java/frc/robot/auto/NewR_LeftTrenchMoveFirstAuto.java delete mode 100644 src/main/java/frc/robot/auto/NewR_RightTrenchMoveFirstAuto.java delete mode 100644 src/main/java/frc/robot/auto/R_LeftTrenchAuto.java delete mode 100644 src/main/java/frc/robot/auto/R_LeftTrenchMoveFirstAuto.java delete mode 100644 src/main/java/frc/robot/auto/R_RightTrenchAuto.java delete mode 100644 src/main/java/frc/robot/auto/R_RightTrenchMoveFirstAuto.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ab0e59..9aba3ed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,9 +36,6 @@ public static enum Mode { 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 final class RobotConstants { @@ -72,8 +69,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 +81,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 +105,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 6a1085c..974b582 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,21 +21,17 @@ 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; import edu.wpi.first.wpilibj2.command.button.Trigger; 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; @@ -45,18 +41,9 @@ 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; @@ -116,7 +103,6 @@ public class Robot extends LoggedRobot { private Vision vision; private LEDController leds = LEDController.getInstance(); private LoggedCompressor compressor; - private PneumaticsSimulator pneumaticsSimulator; // Battery simulation constants private static final double ELECTRONICS_OVERHEAD_AMPS = 4.5; // RoboRIO + radio + PDH + misc @@ -232,17 +218,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(); @@ -258,17 +233,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. */ @@ -292,7 +256,6 @@ public void robotPeriodic() { logScheduler(); 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) @@ -348,7 +311,6 @@ public void disabledPeriodic() { /** 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(); @@ -356,16 +318,11 @@ public void autonomousInit() { /** 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(); @@ -374,8 +331,6 @@ public void teleopInit() { /** 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()); } @@ -391,10 +346,7 @@ public void testInit() { /** 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 @@ -405,22 +357,12 @@ public void simulationInit() { /** 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() { @@ -471,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; } @@ -545,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; } @@ -671,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(); @@ -723,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; @@ -768,10 +548,6 @@ 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); logAlerts(); } 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; - } -} From 8effc8dece1f42add33bc8668c1182a7391b9cbd Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 5 Jun 2026 13:31:26 -0400 Subject: [PATCH 3/6] gate LED, Vision, and compressor behind feature flags LEDController and Vision are disabled via LEDS_ENABLED / VISION_ENABLED flags in FeatureFlags (both false). Compressor is removed entirely. VisionThread start and all leds/vision call sites are guarded accordingly. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Constants.java | 3 + src/main/java/frc/robot/Robot.java | 90 +++++++++++++------------- 2 files changed, 47 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9aba3ed..c1a0f37 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,6 +36,9 @@ public static enum Mode { public static final class FeatureFlags { /** Enable to print loop timing when total exceeds 20ms. */ public static final boolean PROFILING_ENABLED = false; + + public static final boolean LEDS_ENABLED = false; + public static final boolean VISION_ENABLED = false; } public final class RobotConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 974b582..baf36c2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,6 @@ 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; @@ -39,7 +38,6 @@ 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.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.DIOPorts; @@ -101,9 +99,7 @@ public class Robot extends LoggedRobot { // Subsystems private Drive drive; private Vision vision; - private LEDController leds = LEDController.getInstance(); - private LoggedCompressor compressor; - + 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); @@ -143,16 +139,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)); - 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; @@ -170,14 +166,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)); + 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 @@ -196,20 +194,22 @@ public Robot() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - vision = - new Vision( - drive::addVisionMeasurement, - drive::getFieldRelativeHeading, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}); + 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 @@ -251,7 +251,6 @@ public void robotPeriodic() { logCANBus("CAN2", Constants.CANBusPorts.CAN2.BUS); logCANBus("CANHD", Constants.CANBusPorts.CANHD.BUS); powerDistribution.log(); - if (compressor != null) compressor.log(); logHIDs(); logScheduler(); @@ -291,7 +290,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. */ @@ -300,12 +299,15 @@ 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. */ @@ -313,7 +315,7 @@ public void disabledPeriodic() { public void autonomousInit() { 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. */ @@ -325,23 +327,19 @@ public void autonomousPeriodic() {} public void teleopInit() { 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() { - 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. */ @@ -351,7 +349,7 @@ 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. */ @@ -547,7 +545,7 @@ private static void logHIDs() { private void logScheduler() { Logger.recordOutput("Commands/ActiveCommands", activeCommands.toArray(new String[0])); logSubsystem("Drive", drive); - logSubsystem("Vision", vision); + if (vision != null) logSubsystem("Vision", vision); logAlerts(); } From a06ff2b7555097518a1ef6854a36b594c8dd08ff Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 5 Jun 2026 13:32:11 -0400 Subject: [PATCH 4/6] move PneumaticsSimulator to frc.lib Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/{robot => lib}/PneumaticsSimulator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/java/frc/{robot => lib}/PneumaticsSimulator.java (99%) 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; From ce4ecfbd550adeb87fdbb66167d8456b466d1210 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 5 Jun 2026 13:38:34 -0400 Subject: [PATCH 5/6] remove Rebuilt-specific field regions, hub logic, and shift phases Strips 2026 Rebuilt hub/trench/zone geometry from Field.java, removes autoWinner, isMyHubActive, getTarget, and per-shift GamePhases from GameState.java, and simplifies LEDController's alliance color fill. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/game/Field.java | 152 +----------------- src/main/java/frc/game/GameState.java | 67 +------- .../robot/subsystems/leds/LEDController.java | 5 +- 3 files changed, 5 insertions(+), 219 deletions(-) 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/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, From fc7239f460d9dd09d53638e3d29fb2fa038ed5ff Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Fri, 5 Jun 2026 13:54:51 -0400 Subject: [PATCH 6/6] call GameState.logValues() in robotPeriodic Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Robot.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index baf36c2..e969e7e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.game.Field; +import frc.game.GameState; import frc.lib.AllianceSelector; import frc.lib.AutoSelector; import frc.lib.CommandZorroController; @@ -253,6 +254,7 @@ public void robotPeriodic() { powerDistribution.log(); logHIDs(); logScheduler(); + GameState.logValues(); Logger.recordOutput("USB/FreeSpaceMB", getUSBStorageFreeSpace() / 1024 / 1024); long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0;