From bbd671ee58c8ec3c8559a9e591b2c096a499fef2 Mon Sep 17 00:00:00 2001 From: fishnos Date: Sat, 7 Feb 2026 12:48:30 -0500 Subject: [PATCH 1/3] initial commit --- src/main/java/frc/robot/Dashboard.java | 130 +++++ src/main/java/frc/robot/Robot.java | 9 +- .../frc/robot/subsystems/shooter/Shooter.java | 111 +++-- .../robot/subsystems/shooter/ShooterIO.java | 67 ++- .../subsystems/shooter/ShooterIOSim.java | 75 ++- .../subsystems/shooter/ShooterIOTalonFX.java | 59 ++- .../robot/subsystems/swerve/SwerveDrive.java | 452 +++++++++--------- ...enix6-26.1.0.json => Phoenix6-26.1.1.json} | 62 +-- vendordeps/photonlib.json | 12 +- 9 files changed, 589 insertions(+), 388 deletions(-) create mode 100644 src/main/java/frc/robot/Dashboard.java rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.1.json} (92%) diff --git a/src/main/java/frc/robot/Dashboard.java b/src/main/java/frc/robot/Dashboard.java new file mode 100644 index 0000000..6f48ca2 --- /dev/null +++ b/src/main/java/frc/robot/Dashboard.java @@ -0,0 +1,130 @@ +package frc.robot; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.swerve.SwerveDrive; + +public class Dashboard { + private static Field2d robotField; + private static SwerveDrive swerveDrive; + + private static StringPublisher superstructureCurrentState; + private static StringPublisher superStructureDesiredState; + private static DoublePublisher matchTimePublisher; + + private static SendableChooser autoChooser; + private static SendableBuilderImpl autoBuilder; + + private static boolean initialized = false; + + private static void initialize() { + if (initialized) return; + + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable table = inst.getTable("Dashboard"); + + // Create publishers once + superstructureCurrentState = table.getStringTopic("Superstructure/CurrentState").publish(); + superStructureDesiredState = table.getStringTopic("Superstructure/DesiredState").publish(); + matchTimePublisher = table.getDoubleTopic("MatchTime").publish(); + + // Initialize Field2d + NetworkTable fieldTable = table.getSubTable("Field"); + robotField = new Field2d(); + + SendableBuilderImpl fieldBuilder = new SendableBuilderImpl(); + fieldBuilder.setTable(fieldTable); + robotField.initSendable(fieldBuilder); + fieldBuilder.startListeners(); + + // Initialize swerve drive sendable + swerveDrive = SwerveDrive.getInstance(); + NetworkTable swerveTable = table.getSubTable("Swerve Drive"); + + Sendable swerveSendable = new Sendable() { + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("SwerveDrive"); + + builder.addDoubleProperty("Front Left Angle", + () -> swerveDrive.getSwerveModulePositions()[0].angle.getRadians(), null); + builder.addDoubleProperty("Front Left Velocity", + () -> swerveDrive.getSwerveModuleStates()[0].speedMetersPerSecond, null); + + builder.addDoubleProperty("Front Right Angle", + () -> swerveDrive.getSwerveModulePositions()[1].angle.getRadians(), null); + builder.addDoubleProperty("Front Right Velocity", + () -> swerveDrive.getSwerveModuleStates()[1].speedMetersPerSecond, null); + + builder.addDoubleProperty("Back Left Angle", + () -> swerveDrive.getSwerveModulePositions()[2].angle.getRadians(), null); + builder.addDoubleProperty("Back Left Velocity", + () -> swerveDrive.getSwerveModuleStates()[2].speedMetersPerSecond, null); + + builder.addDoubleProperty("Back Right Angle", + () -> swerveDrive.getSwerveModulePositions()[3].angle.getRadians(), null); + builder.addDoubleProperty("Back Right Velocity", + () -> swerveDrive.getSwerveModuleStates()[3].speedMetersPerSecond, null); + + builder.addDoubleProperty("Robot Angle", + () -> RobotState.getInstance().getAngleToFloor().getRadians(), null); + } + }; + + SendableBuilderImpl swerveBuilder = new SendableBuilderImpl(); + swerveBuilder.setTable(swerveTable); + swerveSendable.initSendable(swerveBuilder); + swerveBuilder.startListeners(); + + // Auto chooser + autoChooser = new SendableChooser(); + autoChooser.setDefaultOption("None", new InstantCommand()); + + NetworkTable autoTable = table.getSubTable("AutoChooser"); + autoBuilder = new SendableBuilderImpl(); + autoBuilder.setTable(autoTable); + autoChooser.initSendable(autoBuilder); + autoBuilder.startListeners(); + autoBuilder.update(); + + initialized = true; + } + + public static void addAutoChooserOption(String key, Command command) { + autoChooser.addOption(key, command); + } + + public static Command getCurrentAutoCommand() { + return autoChooser.getSelected(); + } + + public static void updateData() { + initialize(); + + // Superstructure state logging + Superstructure superstructure = Superstructure.getInstance(); + superstructureCurrentState.set(superstructure.getCurrentState().name()); + superStructureDesiredState.set(superstructure.getDesiredState().name()); + + // Update match time + matchTimePublisher.set(DriverStation.getMatchTime()); + + // Update the field with current robot pose + robotField.setRobotPose(RobotState.getInstance().getEstimatedPose()); + + if (autoBuilder != null) { + autoBuilder.update(); + } + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d607706..4dc33f7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -87,8 +87,8 @@ public void robotInit() { DriverStationSim.notifyNewData(); } - break; - + break; + case REPLAY: // Replaying a log, set up replay source setUseTiming(false); // Run as fast as possible @@ -128,6 +128,7 @@ public void robotPeriodic() { // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + Dashboard.updateData(); } /** This function is called once each time the robot enters Disabled mode. */ @@ -161,7 +162,7 @@ private void linkFollowPathLogging() { Logger.recordOutput(pair.getFirst(), pair.getSecond()); }); } - + /** * This autonomous runs the autonomous command selected by your * {@link RobotContainer} class. @@ -169,7 +170,7 @@ private void linkFollowPathLogging() { @Override public void autonomousInit() { m_robotContainer.autonomousInit(); - + if (Constants.agentMode) { m_autonomousCommand = null; } else { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 858909a..2bba0ca 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -33,6 +33,7 @@ public enum HoodSetpoint { DYNAMIC(null); private final Rotation2d angle; + HoodSetpoint(Rotation2d angle) { this.angle = angle; } @@ -47,6 +48,7 @@ public enum TurretSetpoint { DYNAMIC(null); private final Rotation2d angle; + TurretSetpoint(Rotation2d angle) { this.angle = angle; } @@ -61,6 +63,7 @@ public enum FlywheelSetpoint { DYNAMIC(Double.NaN); private final double rps; + FlywheelSetpoint(double rps) { this.rps = rps; } @@ -79,7 +82,7 @@ public double getRps() { private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); private final ShooterConfig config; - + private final DashboardMotorControlLoopConfigurator hoodControlLoopConfigurator; private final DashboardMotorControlLoopConfigurator turretControlLoopConfigurator; private final DashboardMotorControlLoopConfigurator flywheelControlLoopConfigurator; @@ -91,43 +94,39 @@ public double getRps() { private Shooter() { boolean useSimulation = Constants.shouldUseSimulation(Constants.SimOnlySubsystems.SHOOTER); config = ConfigLoader.load( - "shooter", - ConfigLoader.getModeFolder(Constants.SimOnlySubsystems.SHOOTER), - ShooterConfig.class - ); + "shooter", + ConfigLoader.getModeFolder(Constants.SimOnlySubsystems.SHOOTER), + ShooterConfig.class); shooterIO = useSimulation ? new ShooterIOSim(config) : new ShooterIOTalonFX(config); - hoodControlLoopConfigurator = new DashboardMotorControlLoopConfigurator("Shooter/hoodControlLoop", - new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( - config.hoodKP, - config.hoodKI, - config.hoodKD, - config.hoodKS, - config.hoodKV, - config.hoodKA - ) - ); - turretControlLoopConfigurator = new DashboardMotorControlLoopConfigurator("Shooter/turretControlLoop", - new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( - config.turretKP, - config.turretKI, - config.turretKD, - config.turretKS, - config.turretKV, - config.turretKA - ) - ); - flywheelControlLoopConfigurator = new DashboardMotorControlLoopConfigurator("Shooter/flywheelControlLoop", - new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( - config.flywheelKP, - config.flywheelKI, - config.flywheelKD, - config.flywheelKS, - config.flywheelKV, - config.flywheelKA - ) - ); - } + hoodControlLoopConfigurator = new DashboardMotorControlLoopConfigurator("Shooter/hoodControlLoop", + new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( + config.hoodKP, + config.hoodKI, + config.hoodKD, + config.hoodKS, + config.hoodKV, + config.hoodKA)); + turretControlLoopConfigurator = new DashboardMotorControlLoopConfigurator("Shooter/turretControlLoop", + new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( + config.turretKP, + config.turretKI, + config.turretKD, + config.turretKS, + config.turretKV, + config.turretKA)); + flywheelControlLoopConfigurator = new DashboardMotorControlLoopConfigurator("Shooter/flywheelControlLoop", + new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( + config.flywheelKP, + config.flywheelKI, + config.flywheelKD, + config.flywheelKS, + config.flywheelKV, + config.flywheelKA)); + } + + // Coast override + private boolean coastOverride = false; @Override public void periodic() { @@ -145,6 +144,14 @@ public void periodic() { shooterIO.configureFlywheelControlLoop(flywheelControlLoopConfigurator.getConfig()); } + if (coastOverride) { + shooterIO.stop(); + } + } + + public void setCoastOverride(boolean override) { + coastOverride = override; + Logger.recordOutput("Shooter/CoastOverride", override); } // Supplier setters (called by Superstructure) @@ -175,15 +182,19 @@ public void setFlywheelSetpoint(FlywheelSetpoint setpoint) { setShotVelocity(target); } - // Direct setters for mechanism control (now private, used internally by state handlers) + // Direct setters for mechanism control (now private, used internally by state + // handlers) private void setHoodAngle(Rotation2d angle) { - double clampedAngle = MathUtil.clamp(angle.getRotations(), config.hoodMinAngleRotations, config.hoodMaxAngleRotations); + double clampedAngle = MathUtil.clamp(angle.getRotations(), config.hoodMinAngleRotations, + config.hoodMaxAngleRotations); hoodSetpointRotations = clampedAngle; Logger.recordOutput("Shooter/angleSetpointRotations", clampedAngle); Logger.recordOutput("Shooter/rawSetpointRotations", clampedAngle); - shooterIO.setAngle(clampedAngle); + if (!coastOverride) { + shooterIO.setAngle(clampedAngle); + } } private void setTurretAngle(Rotation2d angle) { @@ -192,9 +203,9 @@ private void setTurretAngle(Rotation2d angle) { double initAngle = angle.getDegrees(); double minDeg = config.turretMinAngleDeg; double maxDeg = config.turretMaxAngleDeg; - + double targetAngleDeg; - + // Check if the angle (or wrapped equivalents) are within the valid range if (initAngle >= minDeg && initAngle <= maxDeg) { targetAngleDeg = initAngle; @@ -211,13 +222,17 @@ private void setTurretAngle(Rotation2d angle) { turretSetpointRotations = clampedAngleRotations; Logger.recordOutput("Shooter/turretAngleSetpointRotations", clampedAngleRotations); - shooterIO.setTurretAngle(clampedAngleRotations); + if (!coastOverride) { + shooterIO.setTurretAngle(clampedAngleRotations); + } } private void setShotVelocity(double velocityRotationsPerSec) { flywheelSetpointRPS = velocityRotationsPerSec; Logger.recordOutput("Shooter/shotVelocitySetpointRotationsPerSec", velocityRotationsPerSec); - shooterIO.setShotVelocity(velocityRotationsPerSec); + if (!coastOverride) { + shooterIO.setShotVelocity(velocityRotationsPerSec); + } } // Mechanism getters @@ -264,7 +279,7 @@ public void disableFlywheelEStop() { public double calculateBackSpinRPM(double flywheelVelocityRotationsPerSec) { double flywheelSurfaceVel = flywheelVelocityRotationsPerSec * 2 * Math.PI * config.flywheelRadiusMeters; double backRollerSurfaceVel = flywheelVelocityRotationsPerSec * config.backRollerGearRatio - * 2 * Math.PI * config.backRollerRadiusMeters; + * 2 * Math.PI * config.backRollerRadiusMeters; double deltaV = flywheelSurfaceVel - backRollerSurfaceVel; double ballRadiusMeters = FieldConstants.fuelDiameter / 2.0; @@ -276,7 +291,7 @@ public double calculateBackSpinRPM(double flywheelVelocityRotationsPerSec) { public double calculateShotExitVelocityMetersPerSec(double flywheelVelocityRotationsPerSec) { double flywheelSurfaceVel = flywheelVelocityRotationsPerSec * 2 * Math.PI * config.flywheelRadiusMeters; double backRollerSurfaceVel = flywheelVelocityRotationsPerSec * config.backRollerGearRatio - * 2 * Math.PI * config.backRollerRadiusMeters; + * 2 * Math.PI * config.backRollerRadiusMeters; return (flywheelSurfaceVel + backRollerSurfaceVel) / 2.0; } @@ -292,12 +307,14 @@ public boolean isHoodAtSetpoint() { @AutoLogOutput(key = "Shooter/isTurretAtSetpoint") public boolean isTurretAtSetpoint() { - return Math.abs(shooterInputs.turretAngleRotations - turretSetpointRotations) < config.turretAngleToleranceRotations; + return Math.abs( + shooterInputs.turretAngleRotations - turretSetpointRotations) < config.turretAngleToleranceRotations; } @AutoLogOutput(key = "Shooter/isFlywheelAtSetpoint") public boolean isFlywheelAtSetpoint() { - return Math.abs(shooterInputs.flywheelVelocityRotationsPerSec - flywheelSetpointRPS) < config.flywheelVelocityToleranceRPS; + return Math.abs(shooterInputs.flywheelVelocityRotationsPerSec + - flywheelSetpointRPS) < config.flywheelVelocityToleranceRPS; } // Config getters diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index a9ae4a6..a9b7b63 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -35,21 +35,54 @@ class ShooterIOInputs { public double flywheelFollowerTemperatureFahrenheit = 0; } - public default void updateInputs(ShooterIOInputs inputs) {} - public default void setAngle(double angleRotations) {} - public default void setTurretAngle(double angleRotations) {} - public default void setShotVelocity(double velocityRotationsPerSec) {} - public default void setHoodTorqueCurrentFOC(double torqueCurrentFOC) {} - public default void setTurretTorqueCurrentFOC(double torqueCurrentFOC) {} - public default void setFlywheelVoltage(double voltage) {} - - public default void configureHoodControlLoop(MotorControlLoopConfig config) {} - public default void configureTurretControlLoop(MotorControlLoopConfig config) {} - public default void configureFlywheelControlLoop(MotorControlLoopConfig config) {} - public default void enableHoodEStop() {} - public default void disableHoodEStop() {} - public default void enableTurretEStop() {} - public default void disableTurretEStop() {} - public default void enableFlywheelEStop() {} - public default void disableFlywheelEStop() {} + public default void updateInputs(ShooterIOInputs inputs) { + } + + public default void setAngle(double angleRotations) { + } + + public default void setTurretAngle(double angleRotations) { + } + + public default void setShotVelocity(double velocityRotationsPerSec) { + } + + public default void setHoodTorqueCurrentFOC(double torqueCurrentFOC) { + } + + public default void setTurretTorqueCurrentFOC(double torqueCurrentFOC) { + } + + public default void setFlywheelVoltage(double voltage) { + } + + public default void configureHoodControlLoop(MotorControlLoopConfig config) { + } + + public default void configureTurretControlLoop(MotorControlLoopConfig config) { + } + + public default void configureFlywheelControlLoop(MotorControlLoopConfig config) { + } + + public default void enableHoodEStop() { + } + + public default void disableHoodEStop() { + } + + public default void enableTurretEStop() { + } + + public default void disableTurretEStop() { + } + + public default void enableFlywheelEStop() { + } + + public default void disableFlywheelEStop() { + } + + public default void stop() { + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java index 262a22f..5162a39 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -18,21 +18,16 @@ public class ShooterIOSim implements ShooterIO { // 2 motors for flywheel with 1:1 gearing private final DCMotor flywheelMotorModel = DCMotor.getKrakenX60Foc(2); - private final DCMotorSim hoodSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(hoodMotorModel, 0.00015, 21.428), // magic number because hood is not important - hoodMotorModel - ); - private final DCMotorSim turretSim = - new DCMotorSim( + private final DCMotorSim hoodSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(hoodMotorModel, 0.00015, 21.428), // magic number because hood is not + // important + hoodMotorModel); + private final DCMotorSim turretSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(turretMotorModel, 0.00015, 21.428), - turretMotorModel - ); - private final DCMotorSim flywheelSim = - new DCMotorSim( + turretMotorModel); + private final DCMotorSim flywheelSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(flywheelMotorModel, 0.00015, 1.53), - flywheelMotorModel - ); + flywheelMotorModel); private PIDController hoodFeedback; private ProfiledPIDController turretFeedback; @@ -62,13 +57,11 @@ public ShooterIOSim(ShooterConfig config) { // Turret uses a profiled PID controller in radians with trapezoidal constraints double turretMaxVelRadPerSec = Math.toRadians(config.turretMaxVelocityDegPerSec); double turretMaxAccelRadPerSec2 = Math.toRadians(config.turretMaxAccelerationDegPerSec2); - turretFeedback = - new ProfiledPIDController( + turretFeedback = new ProfiledPIDController( config.turretKP, config.turretKI, config.turretKD, - new TrapezoidProfile.Constraints(turretMaxVelRadPerSec, turretMaxAccelRadPerSec2) - ); + new TrapezoidProfile.Constraints(turretMaxVelRadPerSec, turretMaxAccelRadPerSec2)); flywheelFeedback = new PIDController(config.flywheelKP, config.flywheelKI, config.flywheelKD); @@ -92,37 +85,31 @@ public void updateInputs(ShooterIOInputs inputs) { hoodSim.setInputVoltage(0); } else if (isHoodClosedLoop) { hoodSim.setInputVoltage( - MathUtil.clamp( - hoodFeedback.calculate(hoodSim.getAngularPositionRad()), - -12, - 12 - ) - ); + MathUtil.clamp( + hoodFeedback.calculate(hoodSim.getAngularPositionRad()), + -12, + 12)); } if (isTurretEStopped) { turretSim.setInputVoltage(0); } else if (isTurretClosedLoop) { turretSim.setInputVoltage( - MathUtil.clamp( - turretFeedback.calculate(turretSim.getAngularPositionRad()), - -12, - 12 - ) - ); + MathUtil.clamp( + turretFeedback.calculate(turretSim.getAngularPositionRad()), + -12, + 12)); } if (isFlywheelEStopped) { flywheelSim.setInputVoltage(0); } else if (isFlywheelClosedLoop) { flywheelSim.setInputVoltage( - MathUtil.clamp( - flywheelFeedforward.calculate(desiredFlywheelVelocityRotationsPerSec) + - flywheelFeedback.calculate(flywheelSim.getAngularVelocityRadPerSec()), - -12, - 12 - ) - ); + MathUtil.clamp( + flywheelFeedforward.calculate(desiredFlywheelVelocityRotationsPerSec) + + flywheelFeedback.calculate(flywheelSim.getAngularVelocityRadPerSec()), + -12, + 12)); } hoodSim.update(dt); @@ -163,8 +150,8 @@ public void setAngle(double angleRotations) { } // Clamp angle within software limits double clampedAngle = MathUtil.clamp(angleRotations, - config.hoodMinAngleRotations, - config.hoodMaxAngleRotations); + config.hoodMinAngleRotations, + config.hoodMaxAngleRotations); hoodFeedback.setSetpoint(clampedAngle * (2 * Math.PI)); isHoodClosedLoop = true; } @@ -275,4 +262,16 @@ public void enableFlywheelEStop() { public void disableFlywheelEStop() { isFlywheelEStopped = false; } + + @Override + public void stop() { + hoodSim.setInputVoltage(0); + isHoodClosedLoop = false; + + turretSim.setInputVoltage(0); + isTurretClosedLoop = false; + + flywheelSim.setInputVoltage(0); + isFlywheelClosedLoop = false; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 2c5fcbb..2b68965 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -19,6 +19,7 @@ import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -91,8 +92,8 @@ public ShooterIOTalonFX(ShooterConfig config) { hoodConfig.Feedback.SensorToMechanismRatio = config.hoodMotorToOutputShaftRatio; hoodConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - hoodConfig.MotorOutput.Inverted = config.isHoodInverted ? - InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + hoodConfig.MotorOutput.Inverted = config.isHoodInverted ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; // Current and torque limiting hoodConfig.CurrentLimits.SupplyCurrentLimitEnable = false; @@ -128,8 +129,8 @@ public ShooterIOTalonFX(ShooterConfig config) { turretConfig.Feedback.SensorToMechanismRatio = config.turretMotorToOutputShaftRatio; turretConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - turretConfig.MotorOutput.Inverted = config.isTurretInverted ? - InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + turretConfig.MotorOutput.Inverted = config.isTurretInverted ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; // Current and torque limiting turretConfig.CurrentLimits.SupplyCurrentLimitEnable = false; @@ -172,8 +173,8 @@ public ShooterIOTalonFX(ShooterConfig config) { flywheelConfig.Feedback.SensorToMechanismRatio = config.flywheelMotorToOutputShaftRatio; flywheelConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; - flywheelConfig.MotorOutput.Inverted = config.isFlywheelInverted ? - InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + flywheelConfig.MotorOutput.Inverted = config.isFlywheelInverted ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; // Current and torque limiting flywheelConfig.CurrentLimits.SupplyCurrentLimitEnable = false; @@ -188,7 +189,8 @@ public ShooterIOTalonFX(ShooterConfig config) { flywheelMotor = new TalonFX(config.flywheelCanId, new CANBus(config.canBusName)); PhoenixUtil.tryUntilOk(5, () -> flywheelMotor.getConfigurator().apply(flywheelConfig, 0.25)); - // Flywheel follower motor configuration - uses same config as leader but set as follower + // Flywheel follower motor configuration - uses same config as leader but set as + // follower flywheelFollowerConfig = new TalonFXConfiguration(); flywheelFollowerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; @@ -208,8 +210,9 @@ public ShooterIOTalonFX(ShooterConfig config) { PhoenixUtil.tryUntilOk(5, () -> flywheelFollowerMotor.getConfigurator().apply(flywheelFollowerConfig, 0.25)); // Set follower to follow the leader motor - flywheelFollowerMotor.setControl(new Follower(config.flywheelCanId, - config.isFlywheelFollowerOppositeDirection ? MotorAlignmentValue.Opposed : MotorAlignmentValue.Aligned)); + flywheelFollowerMotor.setControl(new Follower(config.flywheelCanId, + config.isFlywheelFollowerOppositeDirection ? MotorAlignmentValue.Opposed + : MotorAlignmentValue.Aligned)); // Status signals hoodTorqueCurrent = hoodMotor.getTorqueCurrent().clone(); @@ -237,13 +240,13 @@ public ShooterIOTalonFX(ShooterConfig config) { flywheelVelocityStatusSignal = flywheelMotor.getVelocity().clone(); BaseStatusSignal.setUpdateFrequencyForAll(100, - hoodTorqueCurrent, hoodTemperature, hoodMotorVoltage, - turretTorqueCurrent, turretTemperature, turretMotorVoltage, - flywheelTorqueCurrent, flywheelTemperature, flywheelMotorVoltage, - flywheelFollowerTorqueCurrent, flywheelFollowerTemperature, flywheelFollowerMotorVoltage, - hoodPositionStatusSignal, hoodVelocityStatusSignal, - turretPositionStatusSignal, turretVelocityStatusSignal, - flywheelVelocityStatusSignal); + hoodTorqueCurrent, hoodTemperature, hoodMotorVoltage, + turretTorqueCurrent, turretTemperature, turretMotorVoltage, + flywheelTorqueCurrent, flywheelTemperature, flywheelMotorVoltage, + flywheelFollowerTorqueCurrent, flywheelFollowerTemperature, flywheelFollowerMotorVoltage, + hoodPositionStatusSignal, hoodVelocityStatusSignal, + turretPositionStatusSignal, turretVelocityStatusSignal, + flywheelVelocityStatusSignal); hoodMotor.optimizeBusUtilization(); turretMotor.optimizeBusUtilization(); @@ -254,13 +257,13 @@ public ShooterIOTalonFX(ShooterConfig config) { @Override public void updateInputs(ShooterIOInputs inputs) { BaseStatusSignal.refreshAll( - hoodTorqueCurrent, hoodTemperature, hoodMotorVoltage, - turretTorqueCurrent, turretTemperature, turretMotorVoltage, - flywheelTorqueCurrent, flywheelTemperature, flywheelMotorVoltage, - flywheelFollowerTorqueCurrent, flywheelFollowerTemperature, flywheelFollowerMotorVoltage, - hoodPositionStatusSignal, hoodVelocityStatusSignal, - turretPositionStatusSignal, turretVelocityStatusSignal, - flywheelVelocityStatusSignal); + hoodTorqueCurrent, hoodTemperature, hoodMotorVoltage, + turretTorqueCurrent, turretTemperature, turretMotorVoltage, + flywheelTorqueCurrent, flywheelTemperature, flywheelMotorVoltage, + flywheelFollowerTorqueCurrent, flywheelFollowerTemperature, flywheelFollowerMotorVoltage, + hoodPositionStatusSignal, hoodVelocityStatusSignal, + turretPositionStatusSignal, turretVelocityStatusSignal, + flywheelVelocityStatusSignal); inputs.hoodAngleRotations = hoodPositionStatusSignal.getValue().in(Rotations); inputs.hoodVelocityRotationsPerSec = hoodVelocityStatusSignal.getValue().in(RotationsPerSecond); @@ -293,7 +296,7 @@ public void updateInputs(ShooterIOInputs inputs) { public void setAngle(double angleRotations) { // Clamp angle within software limits double clampedAngle = Math.max(config.hoodMinAngleRotations, - Math.min(config.hoodMaxAngleRotations, angleRotations)); + Math.min(config.hoodMaxAngleRotations, angleRotations)); hoodMotor.setControl(hoodMotorRequest.withPosition(clampedAngle)); } @@ -403,4 +406,12 @@ public void disableFlywheelEStop() { PhoenixUtil.tryUntilOk(5, () -> flywheelMotor.getConfigurator().apply(flywheelConfig, 0.25)); PhoenixUtil.tryUntilOk(5, () -> flywheelFollowerMotor.getConfigurator().apply(flywheelFollowerConfig, 0.25)); } + + @Override + public void stop() { + hoodMotor.setControl(new CoastOut()); + turretMotor.setControl(new CoastOut()); + flywheelMotor.setControl(new CoastOut()); + flywheelFollowerMotor.setControl(new CoastOut()); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 5978d4c..3ef72ba 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -48,6 +48,7 @@ public class SwerveDrive extends SubsystemBase { private static SwerveDrive instance = null; + public static SwerveDrive getInstance() { if (instance == null) { instance = new SwerveDrive(); @@ -102,7 +103,6 @@ public enum CurrentTranslationOverrideState { CAPPED } - // FSM State Variables private DesiredSystemState desiredSystemState = DesiredSystemState.DISABLED; private CurrentSystemState currentSystemState = CurrentSystemState.DISABLED; @@ -134,13 +134,15 @@ public enum CurrentTranslationOverrideState { private PIDController omegaOverridePIDController; private PIDController snappedOmegaOverridePIDController; private static final double RANGED_ROTATION_MAX_VELOCITY_FACTOR = 0.6; - private static final double RANGED_ROTATION_BUFFER_RAD = Math.toRadians(15.0); // Buffer to prevent oscillation at boundaries + private static final double RANGED_ROTATION_BUFFER_RAD = Math.toRadians(15.0); // Buffer to prevent oscillation at + // boundaries private boolean shouldOverrideOmega = false; private double omegaOverride = 0.0; private double lastUnoverriddenOmega = 0.0; - // Translational speed freezing (used during shooting) - only vx/vy are frozen, omega remains controlled + // Translational speed freezing (used during shooting) - only vx/vy are frozen, + // omega remains controlled private boolean shouldOverrideTranslationalSpeedsFrozen = false; private double frozenVxMetersPerSec = 0.0; private double frozenVyMetersPerSec = 0.0; @@ -176,16 +178,16 @@ public enum CurrentTranslationOverrideState { private static final int BACK_RIGHT_INDEX = 3; private SwerveModulePosition[] modulePositions = new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; private SwerveModuleState[] moduleStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() }; private ChassisSpeeds desiredRobotRelativeSpeeds = new ChassisSpeeds(); @@ -208,140 +210,129 @@ public enum CurrentTranslationOverrideState { private SwerveDrive() { boolean useSimulation = Constants.shouldUseSimulation(Constants.SimOnlySubsystems.SWERVE); SwerveConfig swerveConfig = ConfigLoader.load( - "swerve", - ConfigLoader.getModeFolder(Constants.SimOnlySubsystems.SWERVE), - SwerveConfig.class - ); + "swerve", + ConfigLoader.getModeFolder(Constants.SimOnlySubsystems.SWERVE), + SwerveConfig.class); drivetrainConfig = swerveConfig.drivetrain; moduleGeneralConfig = swerveConfig.moduleGeneral; if (useSimulation) { modules = new ModuleIO[] { - new ModuleIOSim(moduleGeneralConfig, 0), - new ModuleIOSim(moduleGeneralConfig, 1), - new ModuleIOSim(moduleGeneralConfig, 2), - new ModuleIOSim(moduleGeneralConfig, 3) + new ModuleIOSim(moduleGeneralConfig, 0), + new ModuleIOSim(moduleGeneralConfig, 1), + new ModuleIOSim(moduleGeneralConfig, 2), + new ModuleIOSim(moduleGeneralConfig, 3) + }; + gyroIO = new GyroIO() { }; - gyroIO = new GyroIO() {}; } else if (Constants.currentMode == Constants.Mode.REPLAY) { modules = new ModuleIO[] { - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {} + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + } }; gyroIO = new GyroIOPigeon2(); } else { modules = new ModuleIO[] { - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontLeft), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontRight), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backLeft), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backRight) + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontLeft), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontRight), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backLeft), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backRight) }; gyroIO = new GyroIOPigeon2(); PhoenixOdometryThread.getInstance().start(); } driveControlLoopConfigurator = new DashboardMotorControlLoopConfigurator( - "Swerve/driveControlLoop", - new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( - moduleGeneralConfig.driveKP, - moduleGeneralConfig.driveKI, - moduleGeneralConfig.driveKD, - moduleGeneralConfig.driveKS, - moduleGeneralConfig.driveKV, - moduleGeneralConfig.driveKA - ) - ); + "Swerve/driveControlLoop", + new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( + moduleGeneralConfig.driveKP, + moduleGeneralConfig.driveKI, + moduleGeneralConfig.driveKD, + moduleGeneralConfig.driveKS, + moduleGeneralConfig.driveKV, + moduleGeneralConfig.driveKA)); steerControlLoopConfigurator = new DashboardMotorControlLoopConfigurator( - "Swerve/steerControlLoop", - new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( - moduleGeneralConfig.steerKP, - moduleGeneralConfig.steerKI, - moduleGeneralConfig.steerKD, - moduleGeneralConfig.steerKS, - moduleGeneralConfig.steerKV, - moduleGeneralConfig.steerKA - ) - ); + "Swerve/steerControlLoop", + new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( + moduleGeneralConfig.steerKP, + moduleGeneralConfig.steerKI, + moduleGeneralConfig.steerKD, + moduleGeneralConfig.steerKS, + moduleGeneralConfig.steerKV, + moduleGeneralConfig.steerKA)); kinematics = new SwerveDriveKinematics( - drivetrainConfig.getFrontLeftPositionMeters(), - drivetrainConfig.getFrontRightPositionMeters(), - drivetrainConfig.getBackLeftPositionMeters(), - drivetrainConfig.getBackRightPositionMeters() - ); + drivetrainConfig.getFrontLeftPositionMeters(), + drivetrainConfig.getFrontRightPositionMeters(), + drivetrainConfig.getBackLeftPositionMeters(), + drivetrainConfig.getBackRightPositionMeters()); - // Create the SysId routine - this is going to be in torque current foc units not voltage + // Create the SysId routine - this is going to be in torque current foc units + // not voltage driveCharacterizationSysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(1.5).per(Second), Volts.of(12), Seconds.of(15), // Use default config - (state) -> Logger.recordOutput("DriveCharacterizationSysIdRoutineState", state.toString()) - ), - new SysIdRoutine.Mechanism( - (torqueCurrentFOC) -> { - for (ModuleIO module : modules) { - module.setDriveTorqueCurrentFOC(torqueCurrentFOC.in(Volts), new Rotation2d(0)); - } - }, - null, // No log consumer, since data is recorded by AdvantageKit - this - ) - ); + new SysIdRoutine.Config( + Volts.of(1.5).per(Second), Volts.of(12), Seconds.of(15), // Use default config + (state) -> Logger.recordOutput("DriveCharacterizationSysIdRoutineState", state.toString())), + new SysIdRoutine.Mechanism( + (torqueCurrentFOC) -> { + for (ModuleIO module : modules) { + module.setDriveTorqueCurrentFOC(torqueCurrentFOC.in(Volts), new Rotation2d(0)); + } + }, + null, // No log consumer, since data is recorded by AdvantageKit + this)); steerCharacterizationSysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(1).per(Second), Volts.of(7), Seconds.of(10), // Use default config - (state) -> Logger.recordOutput("SteerCharacterizationSysIdRoutineState", state.toString()) - ), - new SysIdRoutine.Mechanism( - (torqueCurrentFOC) -> { - for (ModuleIO module : modules) { - module.setSteerTorqueCurrentFOC(torqueCurrentFOC.in(Volts), 0); - } - }, - null, // No log consumer, since data is recorded by AdvantageKit - this - ) - ); + new SysIdRoutine.Config( + Volts.of(1).per(Second), Volts.of(7), Seconds.of(10), // Use default config + (state) -> Logger.recordOutput("SteerCharacterizationSysIdRoutineState", state.toString())), + new SysIdRoutine.Mechanism( + (torqueCurrentFOC) -> { + for (ModuleIO module : modules) { + module.setSteerTorqueCurrentFOC(torqueCurrentFOC.in(Volts), 0); + } + }, + null, // No log consumer, since data is recorded by AdvantageKit + this)); // Configure FollowPath builder using drivetrain config followPathBuilder = new FollowPath.Builder( - this, - RobotState.getInstance()::getEstimatedPose, - RobotState.getInstance()::getRobotRelativeSpeeds, - this::driveRobotRelative, - new PIDController( - drivetrainConfig.followPathTranslationKP, - drivetrainConfig.followPathTranslationKI, - drivetrainConfig.followPathTranslationKD - ), - new PIDController( - drivetrainConfig.followPathRotationKP, - drivetrainConfig.followPathRotationKI, - drivetrainConfig.followPathRotationKD - ), - new PIDController( - drivetrainConfig.followPathCrossTrackKP, - drivetrainConfig.followPathCrossTrackKI, - drivetrainConfig.followPathCrossTrackKD - ) - ).withDefaultShouldFlip(); + this, + RobotState.getInstance()::getEstimatedPose, + RobotState.getInstance()::getRobotRelativeSpeeds, + this::driveRobotRelative, + new PIDController( + drivetrainConfig.followPathTranslationKP, + drivetrainConfig.followPathTranslationKI, + drivetrainConfig.followPathTranslationKD), + new PIDController( + drivetrainConfig.followPathRotationKP, + drivetrainConfig.followPathRotationKI, + drivetrainConfig.followPathRotationKD), + new PIDController( + drivetrainConfig.followPathCrossTrackKP, + drivetrainConfig.followPathCrossTrackKI, + drivetrainConfig.followPathCrossTrackKD)) + .withDefaultShouldFlip(); // Configure omega override PID controllers with velocity limiting omegaOverridePIDController = new PIDController( - drivetrainConfig.omegaOverrideKP, - drivetrainConfig.omegaOverrideKI, - drivetrainConfig.omegaOverrideKD - ); + drivetrainConfig.omegaOverrideKP, + drivetrainConfig.omegaOverrideKI, + drivetrainConfig.omegaOverrideKD); omegaOverridePIDController.enableContinuousInput(-Math.PI, Math.PI); omegaOverridePIDController.setTolerance(Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg)); snappedOmegaOverridePIDController = new PIDController( - drivetrainConfig.omegaOverrideKP, - drivetrainConfig.omegaOverrideKI, - drivetrainConfig.omegaOverrideKD - ); + drivetrainConfig.omegaOverrideKP, + drivetrainConfig.omegaOverrideKI, + drivetrainConfig.omegaOverrideKD); snappedOmegaOverridePIDController.enableContinuousInput(-Math.PI, Math.PI); snappedOmegaOverridePIDController.setTolerance(Math.toRadians(drivetrainConfig.snappedToleranceDeg)); @@ -349,7 +340,7 @@ private SwerveDrive() { @Override public void periodic() { - double dt = Timer.getTimestamp() - prevLoopTime; + double dt = Timer.getTimestamp() - prevLoopTime; prevLoopTime = Timer.getTimestamp(); Logger.recordOutput("SwerveDrive/dtPeriodic", dt); @@ -372,9 +363,8 @@ public void periodic() { for (int i = 0; i < 4; i++) { moduleStates[i] = new SwerveModuleState( - moduleInputs[i].driveVelocityMetersPerSec, - moduleInputs[i].steerPosition - ); + moduleInputs[i].driveVelocityMetersPerSec, + moduleInputs[i].steerPosition); } if (driveControlLoopConfigurator.hasChanged()) { @@ -394,27 +384,21 @@ public void periodic() { for (int i = 0; i < odometryTimestampsSeconds.length; i++) { for (int j = 0; j < 4; j++) { modulePositions[j] = new SwerveModulePosition( - moduleInputs[j].odometryDrivePositionsMeters[i], - moduleInputs[j].odometrySteerPositions[i] - ); + moduleInputs[j].odometryDrivePositionsMeters[i], + moduleInputs[j].odometrySteerPositions[i]); } - + RobotState.getInstance().addOdometryObservation( - new OdometryObservation( - odometryTimestampsSeconds[i], - gyroInputs.isConnected, - modulePositions, - moduleStates, - gyroInputs.isConnected ? - new Rotation3d( - gyroInputs.gyroOrientation.getX(), - gyroInputs.gyroOrientation.getY(), - gyroInputs.odometryYawPositions[i].getRadians() - ) : - new Rotation3d(), - gyroInputs.isConnected ? gyroInputs.yawVelocityRadPerSec : 0 - ) - ); + new OdometryObservation( + odometryTimestampsSeconds[i], + gyroInputs.isConnected, + modulePositions, + moduleStates, + gyroInputs.isConnected ? new Rotation3d( + gyroInputs.gyroOrientation.getX(), + gyroInputs.gyroOrientation.getY(), + gyroInputs.odometryYawPositions[i].getRadians()) : new Rotation3d(), + gyroInputs.isConnected ? gyroInputs.yawVelocityRadPerSec : 0)); updatedPoses.add(RobotState.getInstance().getEstimatedPose()); } @@ -427,7 +411,8 @@ public void periodic() { handleStateTransitions(); handleCurrentState(); - Logger.recordOutput("SwerveDrive/CurrentCommand", this.getCurrentCommand() == null ? "" : this.getCurrentCommand().toString()); + Logger.recordOutput("SwerveDrive/CurrentCommand", + this.getCurrentCommand() == null ? "" : this.getCurrentCommand().toString()); } /** @@ -453,7 +438,7 @@ private void handleStateTransitions() { currentSystemState = CurrentSystemState.FOLLOW_PATH; } break; - + case PREPARE_FOR_AUTO: if (currentPath != null) { modulesAlignmentTargetRotation = currentPath.getInitialModuleDirection(); @@ -562,8 +547,8 @@ private void handleCurrentState() { break; } - } + private void cancelPathCommand() { if (currentPathCommand != null && currentPathCommand.isScheduled()) { currentPathCommand.cancel(); @@ -576,7 +561,7 @@ private void handleDISABLEDSystemState() { setWheelCoast(true); } cancelPathCommand(); - + driveFieldRelative(new ChassisSpeeds(0, 0, 0)); previousSystemState = CurrentSystemState.DISABLED; @@ -589,7 +574,8 @@ private void handleIdleSystemState() { cancelPathCommand(); // Only cancel running commands, but don't null out finished commands - // This prevents re-scheduling when path completes while desired state is still FOLLOW_PATH + // This prevents re-scheduling when path completes while desired state is still + // FOLLOW_PATH if (currentPathCommand != null && currentPathCommand.isScheduled()) { currentPathCommand.cancel(); } @@ -607,10 +593,9 @@ private void handleTeleopSystemState() { invert = Constants.shouldFlipPath() ? -1 : 1; ChassisSpeeds desiredFieldRelativeSpeeds = new ChassisSpeeds( - vxNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, - vyNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, - omegaNormalizedSupplier.getAsDouble() * drivetrainConfig.maxAngularVelocityRadiansPerSec - ); + vxNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, + vyNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, + omegaNormalizedSupplier.getAsDouble() * drivetrainConfig.maxAngularVelocityRadiansPerSec); driveFieldRelative(desiredFieldRelativeSpeeds); previousSystemState = CurrentSystemState.TELEOP; @@ -620,9 +605,10 @@ private void handleFollowPathSystemState() { if (previousSystemState == CurrentSystemState.DISABLED) { setWheelCoast(false); } - + // Schedule path command if not already running - if (currentPath != null && (currentPathCommand == null || (!currentPathCommand.isScheduled() && !currentPathCommand.isFinished()))) { + if (currentPath != null && (currentPathCommand == null + || (!currentPathCommand.isScheduled() && !currentPathCommand.isFinished()))) { currentPathCommand = buildPathCommand(currentPath); CommandScheduler.getInstance().schedule(currentPathCommand); } @@ -632,7 +618,8 @@ private void handleFollowPathSystemState() { } private void prepareForAuto() { - if (previousSystemState != CurrentSystemState.PREPARE_FOR_AUTO && previousSystemState != CurrentSystemState.READY_FOR_AUTO) { + if (previousSystemState != CurrentSystemState.PREPARE_FOR_AUTO + && previousSystemState != CurrentSystemState.READY_FOR_AUTO) { setWheelCoast(false); } cancelPathCommand(); @@ -669,10 +656,11 @@ private void handleNoneOmegaOverrideState() { previousOmegaOverrideState = CurrentOmegaOverrideState.NONE; } - + private void handleRangedRotationOmegaOverrideState() { shouldOverrideOmega = true; - if (isWithinRotationRange(RANGED_ROTATION_BUFFER_RAD - Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg))) { + if (isWithinRotationRange( + RANGED_ROTATION_BUFFER_RAD - Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg))) { omegaOverride = limitOmegaForRange(lastUnoverriddenOmega); // TODO: BAD FIX } else { omegaOverride = calculateReturnToRangeOmega(); @@ -683,7 +671,7 @@ private void handleRangedRotationNominalOmegaOverrideState() { handleRangedRotationOmegaOverrideState(); previousOmegaOverrideState = CurrentOmegaOverrideState.RANGED_NOMINAL; } - + private void handleRangedRotationReturningOmegaOverrideState() { handleRangedRotationOmegaOverrideState(); previousOmegaOverrideState = CurrentOmegaOverrideState.RANGED_RETURNING; @@ -731,13 +719,15 @@ private void handleCappedTranslationOverrideState() { } /** - * Builds a path command using the followPathBuilder, applying pose reset if configured. + * Builds a path command using the followPathBuilder, applying pose reset if + * configured. */ private Command buildPathCommand(Path path) { if (shouldResetPose) { return followPathBuilder.withPoseReset(RobotState.getInstance()::resetPose).build(path); } - return followPathBuilder.withPoseReset((Pose2d pose) -> {}).build(path); + return followPathBuilder.withPoseReset((Pose2d pose) -> { + }).build(path); } /** @@ -754,17 +744,20 @@ private boolean isAtSnapTarget() { } /** - * Checks if robot rotation is within the specified range with an optional buffer. - * @param buffer The buffer in radians to constrict the range by (applied to both min and max) + * Checks if robot rotation is within the specified range with an optional + * buffer. + * + * @param buffer The buffer in radians to constrict the range by (applied to + * both min and max) */ private boolean isWithinRotationRange(double buffer) { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); - + // Normalize angles to -PI to PI for comparison double current = MathUtil.angleModulus(currentRotation.getRadians()); double min = MathUtil.angleModulus(rotationRangeMin.getRadians() + buffer); double max = MathUtil.angleModulus(rotationRangeMax.getRadians() - buffer); - + // Handle wrap-around case if (min <= max) { return current >= min && current <= max; @@ -775,34 +768,36 @@ private boolean isWithinRotationRange(double buffer) { } /** - * Limits omega using sqrt(2*a*d) formula to prevent exceeding the padded rotation bounds. + * Limits omega using sqrt(2*a*d) formula to prevent exceeding the padded + * rotation bounds. * Uses the padded range (user range constricted by buffer) as the boundary. - * Accounts for current robot angular velocity to be more conservative when already + * Accounts for current robot angular velocity to be more conservative when + * already * moving towards a boundary. */ private double limitOmegaForRange(double desiredOmega) { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); double currentOmega = RobotState.getInstance().getYawVelocityRadPerSec(); - + double current = currentRotation.getRadians(); // Use padded range bounds (constricted by buffer) double min = rotationRangeMin.getRadians() + RANGED_ROTATION_BUFFER_RAD; double max = rotationRangeMax.getRadians() - RANGED_ROTATION_BUFFER_RAD; - + // Calculate distance to padded bounds (using Rotation2d for proper wrapping) double distToMin = Math.abs(MathUtil.angleModulus(current - min)); double distToMax = Math.abs(MathUtil.angleModulus(max - current)); - + double maxAngularAccel = drivetrainConfig.maxAngularAccelerationRadiansPerSecSec; - + // Calculate stopping distance from current velocity: d = ω² / (2α) double stoppingDistFromCurrent = (currentOmega * currentOmega) / (2 * maxAngularAccel); - + // Adjust effective distances based on current velocity direction // If moving towards a boundary, reduce effective distance by stopping distance double effectiveDistToMax = distToMax; double effectiveDistToMin = distToMin; - + if (currentOmega > 0) { // Moving towards max, reduce effective distance to max effectiveDistToMax = Math.max(0, distToMax - stoppingDistFromCurrent); @@ -810,8 +805,9 @@ private double limitOmegaForRange(double desiredOmega) { // Moving towards min (negative omega), reduce effective distance to min effectiveDistToMin = Math.max(0, distToMin - stoppingDistFromCurrent); } - - // sqrt(2*a*d) formula for max velocity to stop at boundary using effective distances + + // sqrt(2*a*d) formula for max velocity to stop at boundary using effective + // distances double maxOmegaToMin = Math.sqrt(2 * maxAngularAccel * effectiveDistToMin); double maxOmegaToMax = Math.sqrt(2 * maxAngularAccel * effectiveDistToMax); @@ -820,7 +816,7 @@ private double limitOmegaForRange(double desiredOmega) { } if (currentRotation.getRadians() > max) { return Math.min(desiredOmega, 0); - } + } // Clamp omega based on direction if (desiredOmega > 0) { @@ -833,20 +829,22 @@ private double limitOmegaForRange(double desiredOmega) { } /** - * Calculates omega to return to rotation range using PID with velocity limiting. - * Uses an internal buffer to target slightly inside the range to prevent oscillation at boundaries. + * Calculates omega to return to rotation range using PID with velocity + * limiting. + * Uses an internal buffer to target slightly inside the range to prevent + * oscillation at boundaries. */ private double calculateReturnToRangeOmega() { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); - + double current = currentRotation.getRadians(); double min = rotationRangeMin.getRadians(); double max = rotationRangeMax.getRadians(); - + // Find closest bound double distToMin = Math.abs(MathUtil.angleModulus(current - min)); double distToMax = Math.abs(MathUtil.angleModulus(current - max)); - + double targetAngle; if (distToMin < distToMax) { // Target slightly inside the min boundary (add buffer) @@ -855,15 +853,14 @@ private double calculateReturnToRangeOmega() { // Target slightly inside the max boundary (subtract buffer) targetAngle = max - RANGED_ROTATION_BUFFER_RAD; } - + // Calculate PID output omegaOverridePIDController.setSetpoint(targetAngle); double pidOutput = omegaOverridePIDController.calculate(current); - + // Apply velocity limit (0.6 of max omega) double maxOmega = drivetrainConfig.maxAngularVelocityRadiansPerSec * RANGED_ROTATION_MAX_VELOCITY_FACTOR; - return MathUtil.clamp(pidOutput, -maxOmega, maxOmega); } @@ -881,10 +878,9 @@ private double calculateSnapOmega() { // Supplier setters public void setTeleopInputSuppliers( - DoubleSupplier vxNormalized, - DoubleSupplier vyNormalized, - DoubleSupplier omegaNormalized - ) { + DoubleSupplier vxNormalized, + DoubleSupplier vyNormalized, + DoubleSupplier omegaNormalized) { this.vxNormalizedSupplier = vxNormalized; this.vyNormalizedSupplier = vyNormalized; this.omegaNormalizedSupplier = omegaNormalized; @@ -968,23 +964,23 @@ public void setDesiredTranslationOverrideState(DesiredTranslationOverrideState d // Existing drive methods private ChassisSpeeds compensateRobotRelativeSpeeds(ChassisSpeeds speeds) { - Rotation2d angularVelocity = new Rotation2d(speeds.omegaRadiansPerSecond * drivetrainConfig.rotationCompensationCoefficient); + Rotation2d angularVelocity = new Rotation2d( + speeds.omegaRadiansPerSecond * drivetrainConfig.rotationCompensationCoefficient); if (angularVelocity.getRadians() != 0.0) { speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - ChassisSpeeds.fromRobotRelativeSpeeds( // why should this be split into two? - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond, - RobotState.getInstance().getEstimatedPose().getRotation().plus(angularVelocity) - ), - RobotState.getInstance().getEstimatedPose().getRotation() - ); + ChassisSpeeds.fromRobotRelativeSpeeds( // why should this be split into two? + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + RobotState.getInstance().getEstimatedPose().getRotation().plus(angularVelocity)), + RobotState.getInstance().getEstimatedPose().getRotation()); } return speeds; } - // return a supplier that is true if the modules are aligned within the tolerance + // return a supplier that is true if the modules are aligned within the + // tolerance private void alignModules(Rotation2d targetRotation, double toleranceDeg) { modulesAlignmentTargetRotation = targetRotation; modulesAlignmentToleranceDeg = toleranceDeg; @@ -997,8 +993,10 @@ private void alignModules(Rotation2d targetRotation, double toleranceDeg) { private boolean areModulesAligned() { for (int i = 0; i < 4; i++) { - if (Math.abs(moduleStates[i].angle.minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg && - Math.abs(moduleStates[i].angle.plus(Rotation2d.fromDegrees(180)).minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg) { + if (Math.abs(moduleStates[i].angle.minus(modulesAlignmentTargetRotation) + .getDegrees()) > modulesAlignmentToleranceDeg && + Math.abs(moduleStates[i].angle.plus(Rotation2d.fromDegrees(180)) + .minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg) { return false; } } @@ -1006,7 +1004,7 @@ private boolean areModulesAligned() { } public void driveRobotRelative(ChassisSpeeds speeds) { - double dt = Timer.getTimestamp() - prevDriveTime; + double dt = Timer.getTimestamp() - prevDriveTime; prevDriveTime = Timer.getTimestamp(); lastUnoverriddenOmega = speeds.omegaRadiansPerSecond; @@ -1014,61 +1012,64 @@ public void driveRobotRelative(ChassisSpeeds speeds) { if (shouldOverrideOmega) { desiredRobotRelativeSpeeds = new ChassisSpeeds( - desiredRobotRelativeSpeeds.vxMetersPerSecond, - desiredRobotRelativeSpeeds.vyMetersPerSecond, - omegaOverride - ); + desiredRobotRelativeSpeeds.vxMetersPerSecond, + desiredRobotRelativeSpeeds.vyMetersPerSecond, + omegaOverride); } if (shouldOverrideVelocityCap) { double maxVelocity = velocityCapMaxVelocityMetersPerSec; - double currentMagnitude = Math.hypot(desiredRobotRelativeSpeeds.vxMetersPerSecond, desiredRobotRelativeSpeeds.vyMetersPerSecond); + double currentMagnitude = Math.hypot(desiredRobotRelativeSpeeds.vxMetersPerSecond, + desiredRobotRelativeSpeeds.vyMetersPerSecond); if (currentMagnitude > maxVelocity && currentMagnitude > 0) { double scale = maxVelocity / currentMagnitude; desiredRobotRelativeSpeeds = new ChassisSpeeds( - desiredRobotRelativeSpeeds.vxMetersPerSecond * scale, - desiredRobotRelativeSpeeds.vyMetersPerSecond * scale, - desiredRobotRelativeSpeeds.omegaRadiansPerSecond - ); + desiredRobotRelativeSpeeds.vxMetersPerSecond * scale, + desiredRobotRelativeSpeeds.vyMetersPerSecond * scale, + desiredRobotRelativeSpeeds.omegaRadiansPerSecond); } } if (shouldOverrideTranslationalSpeedsFrozen) { ChassisSpeeds frozenFieldRelativeSpeeds = new ChassisSpeeds( - frozenVxMetersPerSec, - frozenVyMetersPerSec, - desiredRobotRelativeSpeeds.omegaRadiansPerSecond - ); + frozenVxMetersPerSec, + frozenVyMetersPerSec, + desiredRobotRelativeSpeeds.omegaRadiansPerSecond); - desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(frozenFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(frozenFieldRelativeSpeeds, + RobotState.getInstance().getEstimatedPose().getRotation()); } - ChassisSpeeds desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + ChassisSpeeds desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotRelativeSpeeds, + RobotState.getInstance().getEstimatedPose().getRotation()); Logger.recordOutput("SwerveDrive/desiredFieldRelativeSpeeds", desiredFieldRelativeSpeeds); + + // Discretize speeds to compensate for drift during the loop + desiredRobotRelativeSpeeds = ChassisSpeeds.discretize(desiredRobotRelativeSpeeds, dt); + Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); - + // Limit acceleration to prevent sudden changes in speed obtainableFieldRelativeSpeeds = ChassisRateLimiter.limit( - desiredFieldRelativeSpeeds, - obtainableFieldRelativeSpeeds, - dt, - drivetrainConfig.maxTranslationalAccelerationMetersPerSecSec, - drivetrainConfig.maxAngularAccelerationRadiansPerSecSec, - drivetrainConfig.maxTranslationalVelocityMetersPerSec, - drivetrainConfig.maxAngularVelocityRadiansPerSec - ); - + desiredFieldRelativeSpeeds, + obtainableFieldRelativeSpeeds, + dt, + drivetrainConfig.maxTranslationalAccelerationMetersPerSecSec, + drivetrainConfig.maxAngularAccelerationRadiansPerSecSec, + drivetrainConfig.maxTranslationalVelocityMetersPerSec, + drivetrainConfig.maxAngularVelocityRadiansPerSec); + Logger.recordOutput("SwerveDrive/obtainableFieldRelativeSpeeds", obtainableFieldRelativeSpeeds); - ChassisSpeeds obtainableRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(obtainableFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + ChassisSpeeds obtainableRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + obtainableFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); Logger.recordOutput("SwerveDrive/obtainableRobotRelativeSpeeds", obtainableRobotRelativeSpeeds); SwerveModuleState[] moduleSetpoints = kinematics.toSwerveModuleStates(obtainableRobotRelativeSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( - moduleSetpoints, - drivetrainConfig.maxModuleVelocity - ); + moduleSetpoints, + drivetrainConfig.maxModuleVelocity); Logger.recordOutput("SwerveDrive/desaturatedModuleSetpoints", moduleSetpoints); for (int i = 0; i < 4; i++) { @@ -1079,7 +1080,8 @@ public void driveRobotRelative(ChassisSpeeds speeds) { } public void driveFieldRelative(ChassisSpeeds speeds) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, RobotState.getInstance().getEstimatedPose().getRotation()); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, + RobotState.getInstance().getEstimatedPose().getRotation()); driveRobotRelative(speeds); } @@ -1180,4 +1182,12 @@ public Command getQuasistaticSteerCharacterizationSysIdRoutine(Direction directi public FollowPath.Builder getFollowPathBuilder() { return followPathBuilder; } + + public SwerveModulePosition[] getSwerveModulePositions() { + return modulePositions; + } + + public SwerveModuleState[] getSwerveModuleStates() { + return moduleStates; + } } diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 4f9a5d1..afac990 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-4", + "version": "v2026.2.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-4" + "version": "v2026.2.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-4" + "version": "v2026.2.1" } ] } \ No newline at end of file From 11916ffbd08e90b95a43242d77fea91725e5e8ea Mon Sep 17 00:00:00 2001 From: fishnos Date: Sat, 7 Feb 2026 13:25:10 -0500 Subject: [PATCH 2/3] overall robot subsystem functionality checker --- src/main/java/frc/robot/RobotContainer.java | 131 ++++++++---------- .../commands/check/CheckSystemCommand.java | 113 +++++++++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 4 + 3 files changed, 178 insertions(+), 70 deletions(-) create mode 100644 src/main/java/frc/robot/commands/check/CheckSystemCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 93cad7d..a55a8c7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,32 +3,20 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; -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.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.constants.AlignmentConstants; +import frc.robot.commands.check.CheckSystemCommand; // import frc.robot.commands.autos.tower.ScoreL1; import frc.robot.constants.Constants; import frc.robot.lib.BLine.FollowPath; import frc.robot.lib.BLine.Path; -import frc.robot.lib.BLine.Path.EventTrigger; -import frc.robot.lib.BLine.Path.PathConstraints; -import frc.robot.lib.BLine.Path.Waypoint; import frc.robot.lib.input.XboxController; import frc.robot.lib.util.ballistics.ProjectileVisualizer; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.Superstructure.DesiredState; -import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.SwerveDrive; -// import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.Vision; public class RobotContainer { public static RobotContainer instance = null; @@ -51,7 +39,6 @@ public static RobotContainer getInstance() { @SuppressWarnings("unused") private final ProjectileVisualizer projectileVisualizer = ProjectileVisualizer.getInstance(); - private RobotContainer() { registerEventTriggers(); @@ -62,18 +49,16 @@ private RobotContainer() { // Configure teleop input suppliers for SwerveDrive FSM // Using normalized inputs (-1 to 1) with deadband applied swerveDrive.setTeleopInputSuppliers( - () -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND), - () -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND), - () -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND) - ); + () -> -MathUtil.applyDeadband(xboxDriver.getLeftY(), Constants.OperatorConstants.LEFT_Y_DEADBAND), + () -> -MathUtil.applyDeadband(xboxDriver.getLeftX(), Constants.OperatorConstants.LEFT_X_DEADBAND), + () -> -MathUtil.applyDeadband(xboxDriver.getRightX(), Constants.OperatorConstants.RIGHT_X_DEADBAND)); // Set default state to TELEOP swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP); - + // Set default superstructure state to HOME superstructure.setDesiredState(Superstructure.DesiredState.HOME); - configureBindings(); } @@ -89,71 +74,77 @@ private void registerEventTriggers() { private void configureBindings() { // Path toClimb = new Path(new Waypoint(1,3, new Rotation2d(0))); // xboxDriver.getXButton().onTrue( - // new InstantCommand(() -> currentPath = toClimb).andThen( - // new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen( - // new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.IDLE)).andThen( - // new InstantCommand(() -> swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP)) - // ) - // ) + // new InstantCommand(() -> currentPath = toClimb).andThen( + // new InstantCommand(() -> + // swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen( + // new WaitUntilCommand(() -> swerveDrive.getCurrentSystemState() == + // SwerveDrive.CurrentSystemState.IDLE)).andThen( + // new InstantCommand(() -> + // swerveDrive.setDesiredSystemState(SwerveDrive.DesiredSystemState.TELEOP)) + // ) + // ) // ); - // xboxDriver.getXButton().onFalse(new InstantCommand(() -> robotState.resetPose(new Pose2d(0,0, new Rotation2d(0))))); + // xboxDriver.getXButton().onFalse(new InstantCommand(() -> + // robotState.resetPose(new Pose2d(0,0, new Rotation2d(0))))); // xboxDriver.getAButton().onTrue(new InstantCommand(() -> { - // ProjectileVisualizer.addProjectile( - // 0, // vx - // 0, // vy - // 10, // exitVelocity - // new Pose3d(0, 0, 2, new Rotation3d(0, Math.PI/4, 0)), // position & rotation - // 3 // hub height - // ); - // }) + // ProjectileVisualizer.addProjectile( + // 0, // vx + // 0, // vy + // 10, // exitVelocity + // new Pose3d(0, 0, 2, new Rotation3d(0, Math.PI/4, 0)), // position & rotation + // 3 // hub height + // ); + // }) // ); xboxDriver.getAButton().onTrue( - new InstantCommand(() -> superstructure.setDesiredState(DesiredState.SHOOTING)) - ); + new InstantCommand(() -> superstructure.setDesiredState(DesiredState.SHOOTING))); xboxDriver.getBButton().onTrue( - new InstantCommand(() -> superstructure.setDesiredState(DesiredState.TRACKING)) - ); + new InstantCommand(() -> superstructure.setDesiredState(DesiredState.TRACKING))); + + SmartDashboard.putData("System Check", new CheckSystemCommand()); // xboxDriver.getAButton().onTrue( - // new ConditionalCommand( - // new SequentialCommandGroup( - // followPath( - // new Path( - // new PathConstraints() - // .setMaxVelocityMetersPerSec(AlignmentConstants.Tower.INTERMEDIARY_MAX_VELOCITY_METERS_PER_SEC) - // .setEndTranslationToleranceMeters(AlignmentConstants.Tower.INTERMEDIARY_TRANSLATION_TOLERANCE_METERS) - // .setEndRotationToleranceDeg(AlignmentConstants.Tower.INTERMEDIARY_ROTATION_TOLERANCE_DEG), - // new Waypoint(AlignmentConstants.Tower.Left.INTERMEDIATE_WAYPOINT)), - // false - // ), - // followPath( - // new Path( - // new PathConstraints().setMaxVelocityMetersPerSec(AlignmentConstants.Tower.APPROACH_MAX_VELOCITY_METERS_PER_SEC), - // new Waypoint(AlignmentConstants.Tower.Left.FINAL_WAYPOINT)), - // false - // ) - // ), - // new InstantCommand(() -> {}), - // () -> AlignmentConstants.Tower.isWithinBounds(robotState.getEstimatedPose(), AlignmentConstants.Tower.Left.BOUNDS) - // ) + // new ConditionalCommand( + // new SequentialCommandGroup( + // followPath( + // new Path( + // new PathConstraints() + // .setMaxVelocityMetersPerSec(AlignmentConstants.Tower.INTERMEDIARY_MAX_VELOCITY_METERS_PER_SEC) + // .setEndTranslationToleranceMeters(AlignmentConstants.Tower.INTERMEDIARY_TRANSLATION_TOLERANCE_METERS) + // .setEndRotationToleranceDeg(AlignmentConstants.Tower.INTERMEDIARY_ROTATION_TOLERANCE_DEG), + // new Waypoint(AlignmentConstants.Tower.Left.INTERMEDIATE_WAYPOINT)), + // false + // ), + // followPath( + // new Path( + // new + // PathConstraints().setMaxVelocityMetersPerSec(AlignmentConstants.Tower.APPROACH_MAX_VELOCITY_METERS_PER_SEC), + // new Waypoint(AlignmentConstants.Tower.Left.FINAL_WAYPOINT)), + // false + // ) + // ), + // new InstantCommand(() -> {}), + // () -> AlignmentConstants.Tower.isWithinBounds(robotState.getEstimatedPose(), + // AlignmentConstants.Tower.Left.BOUNDS) + // ) // ); // Test snap-to-angle bindings - // xboxDriver.getAButton().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.DesiredState.BUMP))); - // xboxDriver.getAButton().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.DesiredState.HOME))); + // xboxDriver.getAButton().onTrue(new InstantCommand(() -> + // superstructure.setDesiredState(Superstructure.DesiredState.BUMP))); + // xboxDriver.getAButton().onFalse(new InstantCommand(() -> + // superstructure.setDesiredState(Superstructure.DesiredState.HOME))); } private Command followPath(Path path, boolean shouldResetPose) { - return - new InstantCommand(() -> { - swerveDrive.setCurrentPath(path, shouldResetPose); - }) - .andThen(setSwerveDriveState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)). - andThen(new WaitUntilCommand( - () -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.IDLE)); + return new InstantCommand(() -> { + swerveDrive.setCurrentPath(path, shouldResetPose); + }) + .andThen(setSwerveDriveState(SwerveDrive.DesiredSystemState.FOLLOW_PATH)).andThen(new WaitUntilCommand( + () -> swerveDrive.getCurrentSystemState() == SwerveDrive.CurrentSystemState.IDLE)); } private Command setSwerveDriveState(SwerveDrive.DesiredSystemState state) { diff --git a/src/main/java/frc/robot/commands/check/CheckSystemCommand.java b/src/main/java/frc/robot/commands/check/CheckSystemCommand.java new file mode 100644 index 0000000..ee18223 --- /dev/null +++ b/src/main/java/frc/robot/commands/check/CheckSystemCommand.java @@ -0,0 +1,113 @@ +package frc.robot.commands.check; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.hopper.Hopper; +import frc.robot.subsystems.hopper.Hopper.HopperSetpoint; +import frc.robot.subsystems.kicker.Kicker; +import frc.robot.subsystems.kicker.Kicker.KickerSetpoint; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.Shooter.FlywheelSetpoint; +import frc.robot.subsystems.shooter.Shooter.HoodSetpoint; +import frc.robot.subsystems.shooter.Shooter.TurretSetpoint; +import frc.robot.subsystems.swerve.SwerveDrive; +import frc.robot.subsystems.swerve.module.ModuleIOInputsAutoLogged; + +/** + * Checks the functionality of all subsystems on the robot.

+ * The checks are as follows:
+ * 1. Swerve Drive: Moves the robot forward at 1 m/s for 2 seconds and checks + * if the drive velocity is roughly 1 m/s.
+ * 2. Shooter: Sets the flywheel to dynamic, hood to home, and turret to home. + * Then checks if the flywheel velocity is roughly 40 RPS.
+ * 3. Hopper/Kicker: Sets the hopper and kicker to feeding and then checks if + * they are running.
+ */ +public class CheckSystemCommand extends SequentialCommandGroup { + private static final String LOG_ROOT = "SystemCheck/"; + + public CheckSystemCommand() { + SwerveDrive swerve = SwerveDrive.getInstance(); + Shooter shooter = Shooter.getInstance(); + Hopper hopper = Hopper.getInstance(); + Kicker kicker = Kicker.getInstance(); + + addRequirements(swerve, shooter, hopper, kicker); + + addCommands( + logStep("Starting System Check"), + + logStep("Checking Swerve Drive"), + new FunctionalCommand( + () -> {}, + () -> { + swerve.driveRobotRelative(new ChassisSpeeds(1.0, 0.0, 0.0)); + }, + (interrupted) -> { + swerve.driveRobotRelative(new ChassisSpeeds()); + }, + () -> false, swerve + ).withTimeout(2.0), + + Commands.runOnce(() -> { + boolean allGood = true; + + ModuleIOInputsAutoLogged[] inputs = swerve.getModuleInputs(); + for (int i = 0; i < 4; i++) { + double velocity = inputs[i].driveVelocityMetersPerSec; + + boolean velPass = Math.abs(velocity) > 0.5; + + check("SwerveModule" + i + "_DriveVelocity", velPass); + if (!velPass) + allGood = false; + } + check("Swerve_Drive_Overall", allGood); + }), + + logStep("Checking Shooter"), + Commands.runOnce(() -> { + shooter.setFlywheelSetpoint(FlywheelSetpoint.DYNAMIC); + shooter.setFlywheelRPSSupplier(() -> 40.0); + shooter.setHoodSetpoint(HoodSetpoint.HOME); + shooter.setTurretSetpoint(TurretSetpoint.HOME); + }, shooter), + + Commands.waitSeconds(1.5), + + Commands.runOnce(() -> { + double velocity = shooter.getFlywheelVelocityRotationsPerSec(); + check("Flywheel_SpinUp", velocity > 35.0); + + shooter.setFlywheelSetpoint(FlywheelSetpoint.OFF); + }), + + logStep("Checking Hopper/Kicker"), + Commands.runOnce(() -> { + hopper.setSetpoint(HopperSetpoint.FEEDING); + kicker.setSetpoint(KickerSetpoint.FEEDING); + }, hopper, kicker), + + Commands.waitSeconds(1.0), + + Commands.runOnce(() -> { + hopper.setSetpoint(HopperSetpoint.OFF); + kicker.setSetpoint(KickerSetpoint.OFF); + }), + + logStep("System Check Complete")); + } + + private Command logStep(String stepName) { + return Commands.runOnce(() -> Logger.recordOutput(LOG_ROOT + "CurrentStep", stepName)); + } + + private void check(String name, boolean condition) { + Logger.recordOutput(LOG_ROOT + "Checks/" + name, condition ? "PASS" : "FAIL"); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 3ef72ba..cefae42 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -169,6 +169,10 @@ public enum CurrentTranslationOverrideState { new ModuleIOInputsAutoLogged() }; + public ModuleIOInputsAutoLogged[] getModuleInputs() { + return moduleInputs; + } + private final GyroIO gyroIO; private GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); From 961644c403e5ffcc9073a94e0c4d90629f84faca Mon Sep 17 00:00:00 2001 From: fishnos Date: Thu, 12 Feb 2026 02:44:21 -0500 Subject: [PATCH 3/3] discretize swerve properly and fix a few logic errors. ready for merge --- .../commands/check/CheckSystemCommand.java | 39 +++++++++++-------- .../frc/robot/subsystems/shooter/Shooter.java | 26 ++++++------- .../robot/subsystems/swerve/SwerveDrive.java | 16 ++++---- 3 files changed, 44 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/commands/check/CheckSystemCommand.java b/src/main/java/frc/robot/commands/check/CheckSystemCommand.java index ee18223..42487f1 100644 --- a/src/main/java/frc/robot/commands/check/CheckSystemCommand.java +++ b/src/main/java/frc/robot/commands/check/CheckSystemCommand.java @@ -19,7 +19,8 @@ import frc.robot.subsystems.swerve.module.ModuleIOInputsAutoLogged; /** - * Checks the functionality of all subsystems on the robot.

+ * Checks the functionality of all subsystems on the robot.
+ *
* The checks are as follows:
* 1. Swerve Drive: Moves the robot forward at 1 m/s for 2 seconds and checks * if the drive velocity is roughly 1 m/s.
@@ -44,36 +45,42 @@ public CheckSystemCommand() { logStep("Checking Swerve Drive"), new FunctionalCommand( - () -> {}, () -> { - swerve.driveRobotRelative(new ChassisSpeeds(1.0, 0.0, 0.0)); }, - (interrupted) -> { - swerve.driveRobotRelative(new ChassisSpeeds()); - }, - () -> false, swerve - ).withTimeout(2.0), + () -> swerve.driveRobotRelative(new ChassisSpeeds(1.0, 0.0, 0.0)), + (interrupted) -> swerve.driveRobotRelative(new ChassisSpeeds()), + () -> false, + swerve).withTimeout(2.0), Commands.runOnce(() -> { boolean allGood = true; ModuleIOInputsAutoLogged[] inputs = swerve.getModuleInputs(); + double tolerance = 0.1; + + double sumAbsVel = 0.0; for (int i = 0; i < 4; i++) { double velocity = inputs[i].driveVelocityMetersPerSec; + sumAbsVel += Math.abs(velocity); - boolean velPass = Math.abs(velocity) > 0.5; - + boolean velPass = Math.abs(velocity - 1.0) < tolerance && velocity > 0.0; check("SwerveModule" + i + "_DriveVelocity", velPass); - if (!velPass) - allGood = false; + allGood &= velPass; } + + double avgAbsVel = sumAbsVel / 4.0; + boolean avgPass = Math.abs(avgAbsVel - 1.0) < tolerance; + check("Swerve_AvgAbsDriveVelocity", avgPass); + allGood &= avgPass; + check("Swerve_Drive_Overall", allGood); }), logStep("Checking Shooter"), Commands.runOnce(() -> { - shooter.setFlywheelSetpoint(FlywheelSetpoint.DYNAMIC); shooter.setFlywheelRPSSupplier(() -> 40.0); + shooter.setFlywheelSetpoint(FlywheelSetpoint.DYNAMIC); + shooter.setHoodSetpoint(HoodSetpoint.HOME); shooter.setTurretSetpoint(TurretSetpoint.HOME); }, shooter), @@ -85,7 +92,7 @@ public CheckSystemCommand() { check("Flywheel_SpinUp", velocity > 35.0); shooter.setFlywheelSetpoint(FlywheelSetpoint.OFF); - }), + }, shooter), logStep("Checking Hopper/Kicker"), Commands.runOnce(() -> { @@ -94,11 +101,11 @@ public CheckSystemCommand() { }, hopper, kicker), Commands.waitSeconds(1.0), - + Commands.runOnce(() -> { hopper.setSetpoint(HopperSetpoint.OFF); kicker.setSetpoint(KickerSetpoint.OFF); - }), + }, hopper, kicker), logStep("System Check Complete")); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2bba0ca..c9c6ab9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -127,6 +127,7 @@ private Shooter() { // Coast override private boolean coastOverride = false; + private boolean prevCoastOverride = false; @Override public void periodic() { @@ -144,9 +145,17 @@ public void periodic() { shooterIO.configureFlywheelControlLoop(flywheelControlLoopConfigurator.getConfig()); } - if (coastOverride) { + if (coastOverride && !prevCoastOverride) { shooterIO.stop(); } + prevCoastOverride = coastOverride; + + // Continuously apply the latest requested setpoints unless coasting + if (!coastOverride) { + shooterIO.setAngle(hoodSetpointRotations); + shooterIO.setTurretAngle(turretSetpointRotations); + shooterIO.setShotVelocity(flywheelSetpointRPS); + } } public void setCoastOverride(boolean override) { @@ -185,16 +194,14 @@ public void setFlywheelSetpoint(FlywheelSetpoint setpoint) { // Direct setters for mechanism control (now private, used internally by state // handlers) private void setHoodAngle(Rotation2d angle) { - double clampedAngle = MathUtil.clamp(angle.getRotations(), config.hoodMinAngleRotations, + double clampedAngle = MathUtil.clamp( + angle.getRotations(), + config.hoodMinAngleRotations, config.hoodMaxAngleRotations); hoodSetpointRotations = clampedAngle; Logger.recordOutput("Shooter/angleSetpointRotations", clampedAngle); Logger.recordOutput("Shooter/rawSetpointRotations", clampedAngle); - - if (!coastOverride) { - shooterIO.setAngle(clampedAngle); - } } private void setTurretAngle(Rotation2d angle) { @@ -221,18 +228,11 @@ private void setTurretAngle(Rotation2d angle) { double clampedAngleRotations = targetAngleDeg / 360.0; turretSetpointRotations = clampedAngleRotations; Logger.recordOutput("Shooter/turretAngleSetpointRotations", clampedAngleRotations); - - if (!coastOverride) { - shooterIO.setTurretAngle(clampedAngleRotations); - } } private void setShotVelocity(double velocityRotationsPerSec) { flywheelSetpointRPS = velocityRotationsPerSec; Logger.recordOutput("Shooter/shotVelocitySetpointRotationsPerSec", velocityRotationsPerSec); - if (!coastOverride) { - shooterIO.setShotVelocity(velocityRotationsPerSec); - } } // Mechanism getters diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index cefae42..8020b8c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -170,7 +170,7 @@ public enum CurrentTranslationOverrideState { }; public ModuleIOInputsAutoLogged[] getModuleInputs() { - return moduleInputs; + return moduleInputs.clone(); } private final GyroIO gyroIO; @@ -1044,15 +1044,13 @@ public void driveRobotRelative(ChassisSpeeds speeds) { RobotState.getInstance().getEstimatedPose().getRotation()); } + desiredRobotRelativeSpeeds = ChassisSpeeds.discretize(desiredRobotRelativeSpeeds, dt); + Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); + ChassisSpeeds desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); Logger.recordOutput("SwerveDrive/desiredFieldRelativeSpeeds", desiredFieldRelativeSpeeds); - // Discretize speeds to compensate for drift during the loop - desiredRobotRelativeSpeeds = ChassisSpeeds.discretize(desiredRobotRelativeSpeeds, dt); - - Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); - // Limit acceleration to prevent sudden changes in speed obtainableFieldRelativeSpeeds = ChassisRateLimiter.limit( desiredFieldRelativeSpeeds, @@ -1067,6 +1065,8 @@ public void driveRobotRelative(ChassisSpeeds speeds) { ChassisSpeeds obtainableRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( obtainableFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + + obtainableRobotRelativeSpeeds = ChassisSpeeds.discretize(obtainableRobotRelativeSpeeds, dt); Logger.recordOutput("SwerveDrive/obtainableRobotRelativeSpeeds", obtainableRobotRelativeSpeeds); SwerveModuleState[] moduleSetpoints = kinematics.toSwerveModuleStates(obtainableRobotRelativeSpeeds); @@ -1188,10 +1188,10 @@ public FollowPath.Builder getFollowPathBuilder() { } public SwerveModulePosition[] getSwerveModulePositions() { - return modulePositions; + return modulePositions.clone(); } public SwerveModuleState[] getSwerveModuleStates() { - return moduleStates; + return moduleStates.clone(); } }