From 66a017db6076fc9efa9f8cedfd842d77d315f1b4 Mon Sep 17 00:00:00 2001 From: Taylor Hartnett Date: Thu, 22 Jan 2026 18:08:14 -0800 Subject: [PATCH 01/39] add basic climb system --- .../java/org/team5924/frc2026/Constants.java | 39 +++++++++++++++++++ .../java/org/team5924/frc2026/RobotState.java | 4 ++ .../frc2026/subsystems/drive/GyroIOSim.java | 4 +- .../exampleSystem/ExampleSystem.java | 18 +++++---- .../team5924/frc2026/util/PhoenixUtil.java | 7 +++- 5 files changed, 59 insertions(+), 13 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 74b280ce..da27c0b4 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -124,4 +124,43 @@ public final class ExampleRoller { .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); } + public final class Climb { + public static final int CAN_ID = 0; + public static final String BUS = "rio"; + public static final double REDUCTION = 1.0; + public static final double SIM_MOI = 0.001; + + public static final TalonFXConfiguration CONFIG = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(35) + .withStatorCurrentLimit(35)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); + + public static final CANdiConfiguration CANDI_CONFIG = + new CANdiConfiguration() + .withDigitalInputs( + new DigitalInputsConfigs() + .withS1CloseState(S1CloseStateValue.CloseWhenLow) + .withS2CloseState(S2CloseStateValue.CloseWhenLow)); + + public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(0.02) + .withTorqueOpenLoopRampPeriod(0.02) + .withVoltageOpenLoopRampPeriod(0.02); + + public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS = + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(0.02) + .withTorqueClosedLoopRampPeriod(0.02) + .withVoltageClosedLoopRampPeriod(0.02); + } + } + + diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index ac5c1e02..80f5f7b1 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -21,6 +21,7 @@ import lombok.Getter; import lombok.Setter; import org.littletonrobotics.junction.AutoLogOutput; +import org.team5924.frc2026.subsystems.climb.Climb.ClimbState; import org.team5924.frc2026.subsystems.exampleSystem.ExampleSystem.ExampleSystemState; import org.team5924.frc2026.subsystems.rollers.exampleRoller.ExampleRoller.ExampleRollerState; @@ -47,4 +48,7 @@ public static RobotState getInstance() { /* ### Example Roller ### */ @Getter @Setter private ExampleRollerState exampleRollerState = ExampleRollerState.IDLE; + + /* ### Example Subsystem ### */ + @Getter @Setter private ClimbState climbState = ClimbState.STOW; } diff --git a/src/main/java/org/team5924/frc2026/subsystems/drive/GyroIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/drive/GyroIOSim.java index 3dd6bf14..c19d4823 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/drive/GyroIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/drive/GyroIOSim.java @@ -18,7 +18,6 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; -import edu.wpi.first.math.util.Units; import org.ironmaple.simulation.drivesims.GyroSimulation; import org.team5924.frc2026.util.PhoenixUtil; @@ -33,8 +32,7 @@ public GyroIOSim(GyroSimulation gyroSimulation) { public void updateInputs(GyroIOInputs inputs) { inputs.connected = true; inputs.yawPosition = gyroSimulation.getGyroReading(); - inputs.yawVelocityRadPerSec = - gyroSimulation.getMeasuredAngularVelocity().in(RadiansPerSecond); + inputs.yawVelocityRadPerSec = gyroSimulation.getMeasuredAngularVelocity().in(RadiansPerSecond); inputs.odometryYawTimestamps = PhoenixUtil.getSimulationOdometryTimeStamps(); inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings(); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java b/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java index a2b05d35..cdbbe6c4 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java +++ b/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java @@ -30,7 +30,8 @@ public class ExampleSystem extends SubsystemBase { private final ExampleSystemIO io; - private final ExampleSystemIOInputsAutoLogged inputs = new ExampleSystemIOInputsAutoLogged(); + + // private final ExampleSystemIOInputsAutoLogged inputs = new ExampleSystemIOInputsAutoLogged(); public enum ExampleSystemState { STOW(new LoggedTunableNumber("ExampleSystem/Stow", Math.toRadians(0))), @@ -64,23 +65,24 @@ public ExampleSystem(ExampleSystemIO io) { @Override public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("ExampleSystem", inputs); + // io.updateInputs(inputs); + // Logger.processInputs("ExampleSystem", inputs); Logger.recordOutput("ExampleSystem/GoalState", goalState.toString()); Logger.recordOutput( "ExampleSystem/CurrentState", RobotState.getInstance().getExampleSystemState()); Logger.recordOutput("ExampleSystem/TargetRads", goalState.rads); - exampleMotorDisconnected.set(!inputs.exampleMotorConnected); + // exampleMotorDisconnected.set(!inputs.exampleMotorConnected); // prevents error spam - if (!inputs.exampleMotorConnected && wasExampleMotorConnected) { - Elastic.sendNotification(exampleMotorDisconnectedNotification); - } - wasExampleMotorConnected = inputs.exampleMotorConnected; + // if (!inputs.exampleMotorConnected && wasExampleMotorConnected) { + Elastic.sendNotification(exampleMotorDisconnectedNotification); } + // wasExampleMotorConnected = inputs.exampleMotorConnected; + // } + public void runVolts(double volts) { io.runVolts(volts); } diff --git a/src/main/java/org/team5924/frc2026/util/PhoenixUtil.java b/src/main/java/org/team5924/frc2026/util/PhoenixUtil.java index a5a4d789..1a732d98 100644 --- a/src/main/java/org/team5924/frc2026/util/PhoenixUtil.java +++ b/src/main/java/org/team5924/frc2026/util/PhoenixUtil.java @@ -96,10 +96,13 @@ public Voltage updateControlSignal( public static double[] getSimulationOdometryTimeStamps() { final double[] odometryTimeStamps = new double[SimulatedArena.getSimulationSubTicksIn1Period()]; final double periodSeconds = - SimulatedArena.getSimulationSubTicksIn1Period() * SimulatedArena.getSimulationDt().in(Seconds); + SimulatedArena.getSimulationSubTicksIn1Period() + * SimulatedArena.getSimulationDt().in(Seconds); for (int i = 0; i < odometryTimeStamps.length; i++) { odometryTimeStamps[i] = - Timer.getFPGATimestamp() - periodSeconds + i * SimulatedArena.getSimulationDt().in(Seconds); + Timer.getFPGATimestamp() + - periodSeconds + + i * SimulatedArena.getSimulationDt().in(Seconds); } return odometryTimeStamps; From f91b0128db648f58c9234171143655922d73fbfa Mon Sep 17 00:00:00 2001 From: Taylor Hartnett Date: Thu, 22 Jan 2026 19:31:24 -0800 Subject: [PATCH 02/39] implement basic climb --- .../frc2026/subsystems/climb/Climb.java | 105 ++++++++++++++++++ .../frc2026/subsystems/climb/ClimbIO.java | 51 +++++++++ .../subsystems/climb/ClimbIOInputs.java | 28 +++++ .../subsystems/climb/ClimbIOTalonFX.java | 102 +++++++++++++++++ 4 files changed, 286 insertions(+) create mode 100644 src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java create mode 100644 src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java create mode 100644 src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java create mode 100644 src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java new file mode 100644 index 00000000..3521a712 --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -0,0 +1,105 @@ +/* + * Climb.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.climb; + +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lombok.Getter; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2026.RobotState; +import org.team5924.frc2026.util.Elastic.Notification; +import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; +import org.team5924.frc2026.util.LoggedTunableNumber; + +public class Climb extends SubsystemBase { + + private final ClimbIO io; + private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + + public enum ClimbState { + STOW(new LoggedTunableNumber("Climb/Stow", 0)), + LEVEL_ONE(new LoggedTunableNumber("Climb/LevelOne", 0)), + LEVEL_TWO(new LoggedTunableNumber("Climb/LevelTwo", 0)), + LEVEL_THREE(new LoggedTunableNumber("Climb/LevelThree", 0)), + CLIMB_DOWN(new LoggedTunableNumber("Climb/ClimbDown", 0)), + DEPLOY(new LoggedTunableNumber("Climb/Deploy", 0)), + DROP(new LoggedTunableNumber("Climb/Drop", 0)), + // voltage at which the climb subsystem motor moves when controlled by the operator + OPERATOR_CONTROL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); + + private final LoggedTunableNumber rads; + + ClimbState(LoggedTunableNumber rads) { + this.rads = rads; + } + } + + @Getter private ClimbState goalState; + + private final Alert climbMotorDisconnected; + private final Notification climbMotorDisconnectedNotification; + private boolean wasClimbMotorConnected = true; + + public Climb(ClimbIO io) { + this.io = io; + this.goalState = ClimbState.STOW; + this.climbMotorDisconnected = + new Alert("Climb System Motor Disconnected!", Alert.AlertType.kWarning); + this.climbMotorDisconnectedNotification = + new Notification(NotificationLevel.WARNING, "Climb System Motor Disconnected", ""); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Climb", inputs); + + Logger.recordOutput("Climb/GoalState", goalState.toString()); + Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState()); + Logger.recordOutput("Climb/TargetRads", goalState.rads); + + climbMotorDisconnected.set(!inputs.climbMotorConnected); + + // prevents error spam + if (!inputs.climbMotorConnected && wasClimbMotorConnected) + ; + + wasClimbMotorConnected = inputs.climbMotorConnected; + } + + public void runVolts(double volts) { + io.runVolts(volts); + } + + public void setGoalState(ClimbState goalState) { + this.goalState = goalState; + switch (goalState) { + case OPERATOR_CONTROL: + RobotState.getInstance().setClimbState(ClimbState.OPERATOR_CONTROL); + break; + case STOW: + DriverStation.reportError( + "climb Subsystem: MOVING is an invalid goal state; it is a transition state!!", null); + break; + default: + RobotState.getInstance().setClimbState(ClimbState.STOW); + io.setPosition(goalState.rads.getAsDouble()); + break; + } + } +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java new file mode 100644 index 00000000..cc249523 --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java @@ -0,0 +1,51 @@ +/* + * ClimbIO.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.climb; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimbIO { + @AutoLog + public static class ClimbIOInputs { + public boolean climbMotorConnected = true; + public double climbPositionRads = 0.0; + public double climbVelocityRadsPerSec = 0.0; + public double climbAppliedVoltage = 0.0; + public double climbSupplyCurrentAmps = 0.0; + public double climbTorqueCurrentAmps = 0.0; + public double climbTempCelsius = 0.0; + } + + /** + * Updates the inputs object with the latest data from hardware + * + * @param inputs Inputs to update + */ + public default void updateInputs(ClimbIOInputs inputs) {} + + /** + * Sets the subsystem motor to the specified voltage + * + * @param volts number of volts + */ + public default void runVolts(double volts) {} + + public default void setPosition(double rads) {} + + /** stops the motor */ + default void stop() {} +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java new file mode 100644 index 00000000..555d07bc --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java @@ -0,0 +1,28 @@ +/* + * ClimbIOInputs.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.climb; + +public class ClimbIOInputs { + + public boolean climbMotorConnected; + public Object climbVelocityRadsPerSec; + public double climbAppliedVoltage; + public double climbSupplyCurrentAmps; + public double climbTorqueCurrentAmps; + public double climbTempCelsius; + public Object climbPositionRads; +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java new file mode 100644 index 00000000..a3cebe0d --- /dev/null +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -0,0 +1,102 @@ +/* + * ClimbIOTalonFX.java + */ + +/* + * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2026.subsystems.climb; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +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.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; +import org.team5924.frc2026.Constants; + +public class ClimbIOTalonFX implements ClimbIO { + + private final TalonFX climbTalon; + private final StatusSignal climbPosition; + private final StatusSignal climbVelocity; + private final StatusSignal climbAppliedVoltage; + private final StatusSignal climbSupplyCurrent; + private final StatusSignal climbTorqueCurrent; + private final StatusSignal climbTempCelsius; + + // Single shot for voltage mode, robot loop will call continuously + private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); + private final PositionVoltage positionOut = + new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); + + public ClimbIOTalonFX() { + climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); + climbTalon.getConfigurator().apply(Constants.Climb.CONFIG); + + // Get select status signals and set update frequency + climbPosition = climbTalon.getPosition(); + climbVelocity = climbTalon.getVelocity(); + climbAppliedVoltage = climbTalon.getMotorVoltage(); + climbSupplyCurrent = climbTalon.getSupplyCurrent(); + climbTorqueCurrent = climbTalon.getTorqueCurrent(); + climbTempCelsius = climbTalon.getDeviceTemp(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + climbPosition, + climbVelocity, + climbAppliedVoltage, + climbSupplyCurrent, + climbTorqueCurrent, + climbTempCelsius); + + climbTalon.setPosition(0); + } + + @Override + public void updateInputs(ClimbIOInputs inputs) { + inputs.climbMotorConnected = + BaseStatusSignal.refreshAll( + climbPosition, + climbVelocity, + climbAppliedVoltage, + climbSupplyCurrent, + climbTorqueCurrent, + climbTempCelsius) + .isOK(); + inputs.climbPositionRads = + Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climb.REDUCTION; + inputs.climbVelocityRadsPerSec = + Units.rotationsToRadians(climbVelocity.getValueAsDouble()) / Constants.Climb.REDUCTION; + inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); + inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); + inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); + inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); + } + + @Override + public void runVolts(double volts) { + climbTalon.setControl(voltageOut.withOutput(volts)); + } + + @Override + public void stop() { + climbTalon.stopMotor(); + } +} From edd7ab4c257bc364faa90a96fdeab7f30299a740 Mon Sep 17 00:00:00 2001 From: Taylor Hartnett Date: Thu, 22 Jan 2026 20:22:42 -0800 Subject: [PATCH 03/39] cleaned code and fixed if statement --- .../org/team5924/frc2026/subsystems/climb/Climb.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 3521a712..0991437d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -22,6 +22,7 @@ import lombok.Getter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.RobotState; +import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -39,6 +40,7 @@ public enum ClimbState { CLIMB_DOWN(new LoggedTunableNumber("Climb/ClimbDown", 0)), DEPLOY(new LoggedTunableNumber("Climb/Deploy", 0)), DROP(new LoggedTunableNumber("Climb/Drop", 0)), + MOVING(new LoggedTunableNumber("Climb/Moving", 0)), // voltage at which the climb subsystem motor moves when controlled by the operator OPERATOR_CONTROL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); @@ -76,8 +78,9 @@ public void periodic() { climbMotorDisconnected.set(!inputs.climbMotorConnected); // prevents error spam - if (!inputs.climbMotorConnected && wasClimbMotorConnected) - ; + if (!inputs.climbMotorConnected && wasClimbMotorConnected) { + Elastic.sendNotification(climbMotorDisconnectedNotification); + } wasClimbMotorConnected = inputs.climbMotorConnected; } @@ -92,9 +95,9 @@ public void setGoalState(ClimbState goalState) { case OPERATOR_CONTROL: RobotState.getInstance().setClimbState(ClimbState.OPERATOR_CONTROL); break; - case STOW: + case MOVING: DriverStation.reportError( - "climb Subsystem: MOVING is an invalid goal state; it is a transition state!!", null); + "Climb: MOVING is an invalid goal state; it is a transition state!!", null); break; default: RobotState.getInstance().setClimbState(ClimbState.STOW); From 99ef66b2836aaceb0ad0b71d297e14185e9af20c Mon Sep 17 00:00:00 2001 From: Leo Teague Date: Sat, 24 Jan 2026 16:31:36 -0800 Subject: [PATCH 04/39] added setPosition in hardware implementation file --- .../team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index a3cebe0d..2710eaea 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -99,4 +99,9 @@ public void runVolts(double volts) { public void stop() { climbTalon.stopMotor(); } + + @Override + public void setPosition(double rads) { + climbTalon.setControl(positionOut.withPosition(rads * Constants.Climb.REDUCTION)); + } } From ffd1ba7a7d12bcf5fa7ba557dd0a646e1b1fd09c Mon Sep 17 00:00:00 2001 From: Leo Teague Date: Sat, 24 Jan 2026 16:45:55 -0800 Subject: [PATCH 05/39] added conversion from rads to motor rotations to setPosition (in hardware implementation) --- .../org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 2710eaea..93ef2f26 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -102,6 +102,6 @@ public void stop() { @Override public void setPosition(double rads) { - climbTalon.setControl(positionOut.withPosition(rads * Constants.Climb.REDUCTION)); + climbTalon.setControl(positionOut.withPosition(rads * Constants.Climb.REDUCTION * Units.radiansToRotations(1.0))); } } From 79250acfc30d7c3673a7ea1d907af73b71dd0f40 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 24 Jan 2026 18:01:09 -0800 Subject: [PATCH 06/39] double supplier update --- .../org/team5924/frc2026/subsystems/climb/Climb.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 0991437d..515d2357 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -20,6 +20,9 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; + +import java.util.function.DoubleSupplier; + import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.Elastic; @@ -40,13 +43,13 @@ public enum ClimbState { CLIMB_DOWN(new LoggedTunableNumber("Climb/ClimbDown", 0)), DEPLOY(new LoggedTunableNumber("Climb/Deploy", 0)), DROP(new LoggedTunableNumber("Climb/Drop", 0)), - MOVING(new LoggedTunableNumber("Climb/Moving", 0)), + MOVING(() -> 0.0), // voltage at which the climb subsystem motor moves when controlled by the operator OPERATOR_CONTROL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); - private final LoggedTunableNumber rads; + private final DoubleSupplier rads; - ClimbState(LoggedTunableNumber rads) { + ClimbState(DoubleSupplier rads) { this.rads = rads; } } From 8c80c60a31bff009ca98440e4c69862c3bd89859 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sat, 24 Jan 2026 18:03:06 -0800 Subject: [PATCH 07/39] fix logging --- src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 515d2357..3c6442fb 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -76,7 +76,7 @@ public void periodic() { Logger.recordOutput("Climb/GoalState", goalState.toString()); Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState()); - Logger.recordOutput("Climb/TargetRads", goalState.rads); + Logger.recordOutput("Climb/TargetRads", goalState.rads.getAsDouble()); climbMotorDisconnected.set(!inputs.climbMotorConnected); From 4ff4ff4798ac884c3987e14274bff6af12264a1d Mon Sep 17 00:00:00 2001 From: Gio Bueno Date: Sat, 24 Jan 2026 19:29:24 -0800 Subject: [PATCH 08/39] fixed default setGoalState only set ClimbState to Stow --- src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 0991437d..7db19e89 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -100,7 +100,7 @@ public void setGoalState(ClimbState goalState) { "Climb: MOVING is an invalid goal state; it is a transition state!!", null); break; default: - RobotState.getInstance().setClimbState(ClimbState.STOW); + RobotState.getInstance().setClimbState(goalState); io.setPosition(goalState.rads.getAsDouble()); break; } From 06091cf7099491b19c1ab9dc10f2ae68b5135ed9 Mon Sep 17 00:00:00 2001 From: Gio Bueno Date: Sat, 24 Jan 2026 19:37:55 -0800 Subject: [PATCH 09/39] fixed Climb State Label in RobotState to be consistent --- src/main/java/org/team5924/frc2026/RobotState.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 80f5f7b1..0ea14aa7 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -49,6 +49,6 @@ public static RobotState getInstance() { /* ### Example Roller ### */ @Getter @Setter private ExampleRollerState exampleRollerState = ExampleRollerState.IDLE; - /* ### Example Subsystem ### */ + /* ### Climb ### */ @Getter @Setter private ClimbState climbState = ClimbState.STOW; } From 80aacef7c6758a9dd84419d3e4710c1b75e904d4 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 31 Jan 2026 13:42:56 -0800 Subject: [PATCH 10/39] fixed constants bug - code bui;ds now --- .../java/org/team5924/frc2026/Constants.java | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 065fb84f..ad20e244 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -193,9 +193,21 @@ public final class ShooterRoller { } public final class Climb { public static final int CAN_ID = 0; + public static final String BUS = "rio"; + public static final double REDUCTION = 1.0; + public static final TalonFXConfiguration CONFIG = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(35) + .withStatorCurrentLimit(35)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); - + } public final class Indexer { //TODO: update these later public final static int CAN_ID = 0; public final static int BEAM_BREAK_ID = 0; @@ -234,7 +246,8 @@ public final class Indexer { //TODO: update these later .withVoltageClosedLoopRampPeriod(0.02); } - } + + } From 61a3246b7511d17274bd62382d09c43b210b70bd Mon Sep 17 00:00:00 2001 From: Tracy Thai Date: Sat, 31 Jan 2026 14:51:16 -0800 Subject: [PATCH 11/39] fixed IO inputs and Autolog --- .../java/org/team5924/frc2026/Constants.java | 18 ++++++++++-- .../java/org/team5924/frc2026/RobotState.java | 2 +- .../frc2026/subsystems/climb/Climb.java | 7 ++--- .../subsystems/climb/ClimbIOInputs.java | 28 ------------------- .../subsystems/climb/ClimbIOTalonFX.java | 12 ++++---- .../rollers/exampleRoller/ExampleRoller.java | 4 +-- .../rollers/generic/GenericRollerSystem.java | 5 +--- .../rollers/hopperAgitator/Hopper.java | 4 +-- .../subsystems/rollers/indexer/Indexer.java | 4 +-- .../rollers/shooterRoller/ShooterRoller.java | 4 +-- 10 files changed, 32 insertions(+), 56 deletions(-) delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index ad20e244..fe8e410e 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -245,9 +245,23 @@ public final class Indexer { //TODO: update these later .withTorqueClosedLoopRampPeriod(0.02) .withVoltageClosedLoopRampPeriod(0.02); } + public final class Climber{ //TODO: Update Values + public static final int CAN_ID = 0; + public static final String BUS = "rio"; + public static final double REDUCTION = 1.0; + public static final double SIM_MOI = 0.001; - - + public static final TalonFXConfiguration CONFIG = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60)) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake)); + } } diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 0a5b9b71..bd11a411 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -24,8 +24,8 @@ import org.team5924.frc2026.subsystems.climb.Climb.ClimbState; import org.team5924.frc2026.subsystems.exampleSystem.ExampleSystem.ExampleSystemState; import org.team5924.frc2026.subsystems.rollers.exampleRoller.ExampleRoller.ExampleRollerState; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; import org.team5924.frc2026.subsystems.rollers.hopperAgitator.Hopper.HopperState; +import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; import org.team5924.frc2026.subsystems.rollers.shooterRoller.ShooterRoller.ShooterRollerState; import org.team5924.frc2026.subsystems.shooterHood.ShooterHood.ShooterHoodState; import org.team5924.frc2026.subsystems.superShooter.SuperShooter.ShooterState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index b7a8838f..4d31b1d2 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -19,11 +19,10 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import lombok.Getter; - import java.util.function.DoubleSupplier; - +import lombok.Getter; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; @@ -72,7 +71,7 @@ public Climb(ClimbIO io) { @Override public void periodic() { io.updateInputs(inputs); - Logger.processInputs("Climb", inputs); + Logger.processInputs("Climb", (LoggableInputs) inputs); Logger.recordOutput("Climb/GoalState", goalState.toString()); Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState()); diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java deleted file mode 100644 index 555d07bc..00000000 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOInputs.java +++ /dev/null @@ -1,28 +0,0 @@ -/* - * ClimbIOInputs.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.climb; - -public class ClimbIOInputs { - - public boolean climbMotorConnected; - public Object climbVelocityRadsPerSec; - public double climbAppliedVoltage; - public double climbSupplyCurrentAmps; - public double climbTorqueCurrentAmps; - public double climbTempCelsius; - public Object climbPositionRads; -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 93ef2f26..f0020a7d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -46,8 +46,8 @@ public class ClimbIOTalonFX implements ClimbIO { new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); public ClimbIOTalonFX() { - climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); - climbTalon.getConfigurator().apply(Constants.Climb.CONFIG); + climbTalon = new TalonFX(Constants.Climber.CAN_ID, new CANBus(Constants.Climber.BUS)); + climbTalon.getConfigurator().apply(Constants.Climber.CONFIG); // Get select status signals and set update frequency climbPosition = climbTalon.getPosition(); @@ -81,9 +81,9 @@ public void updateInputs(ClimbIOInputs inputs) { climbTempCelsius) .isOK(); inputs.climbPositionRads = - Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climb.REDUCTION; + Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climber.REDUCTION; inputs.climbVelocityRadsPerSec = - Units.rotationsToRadians(climbVelocity.getValueAsDouble()) / Constants.Climb.REDUCTION; + Units.rotationsToRadians(climbVelocity.getValueAsDouble()) / Constants.Climber.REDUCTION; inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); @@ -102,6 +102,8 @@ public void stop() { @Override public void setPosition(double rads) { - climbTalon.setControl(positionOut.withPosition(rads * Constants.Climb.REDUCTION * Units.radiansToRotations(1.0))); + climbTalon.setControl( + positionOut.withPosition( + rads * Constants.Climber.REDUCTION * Units.radiansToRotations(1.0))); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/exampleRoller/ExampleRoller.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/exampleRoller/ExampleRoller.java index 0cbd82db..4baa5b4c 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/exampleRoller/ExampleRoller.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/exampleRoller/ExampleRoller.java @@ -16,11 +16,9 @@ package org.team5924.frc2026.subsystems.rollers.exampleRoller; +import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; - -import java.util.function.DoubleSupplier; - import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem.VoltageState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerSystem.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerSystem.java index 45b75c95..bf0568f3 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerSystem.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/generic/GenericRollerSystem.java @@ -19,15 +19,12 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import lombok.RequiredArgsConstructor; - import java.util.function.DoubleSupplier; - +import lombok.RequiredArgsConstructor; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.inputs.LoggableInputs; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystemIO.GenericRollerSystemIOInputs; import org.team5924.frc2026.util.Elastic; -import org.team5924.frc2026.util.LoggedTunableNumber; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopperAgitator/Hopper.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopperAgitator/Hopper.java index 1562f19a..c1281e57 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopperAgitator/Hopper.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopperAgitator/Hopper.java @@ -16,11 +16,9 @@ package org.team5924.frc2026.subsystems.rollers.hopperAgitator; +import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; - -import java.util.function.DoubleSupplier; - import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem.VoltageState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java index 25917632..c9d66ac9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java @@ -16,11 +16,9 @@ package org.team5924.frc2026.subsystems.rollers.indexer; +import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; - -import java.util.function.DoubleSupplier; - import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem.VoltageState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/shooterRoller/ShooterRoller.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/shooterRoller/ShooterRoller.java index 4cc747ed..0da6805a 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/shooterRoller/ShooterRoller.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/shooterRoller/ShooterRoller.java @@ -16,11 +16,9 @@ package org.team5924.frc2026.subsystems.rollers.shooterRoller; +import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; - -import java.util.function.DoubleSupplier; - import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem; import org.team5924.frc2026.subsystems.rollers.generic.GenericRollerSystem.VoltageState; From 54a48b89d2e88835a8148b661816454b1c9400bd Mon Sep 17 00:00:00 2001 From: Tracy Thai Date: Thu, 5 Feb 2026 19:43:12 -0800 Subject: [PATCH 12/39] Fixed duplicate climb constants and useless constants --- .../java/org/team5924/frc2026/Constants.java | 74 +++---------------- .../frc2026/subsystems/climb/Climb.java | 3 +- .../subsystems/climb/ClimbIOTalonFX.java | 11 ++- 3 files changed, 16 insertions(+), 72 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index fdde6e50..90d5c369 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -1,6 +1,6 @@ -/* - * Constants.java - */ + +// * Constants.java +// */ /* * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. @@ -103,24 +103,7 @@ public final class Example { .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); - public static final CANdiConfiguration CANDI_CONFIG = - new CANdiConfiguration() - .withDigitalInputs( - new DigitalInputsConfigs() - .withS1CloseState(S1CloseStateValue.CloseWhenLow) - .withS2CloseState(S2CloseStateValue.CloseWhenLow)); - - public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(0.02) - .withTorqueOpenLoopRampPeriod(0.02) - .withVoltageOpenLoopRampPeriod(0.02); - public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS = - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(0.02) - .withTorqueClosedLoopRampPeriod(0.02) - .withVoltageClosedLoopRampPeriod(0.02); } public final class GenericRollerSystem { @@ -210,23 +193,6 @@ public final class Intake { .withNeutralMode(NeutralModeValue.Brake)); } - public final class Climb { - public static final int CAN_ID = 0; - public static final String BUS = "rio"; - public static final double REDUCTION = 1.0; - public static final TalonFXConfiguration CONFIG = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(35) - .withStatorCurrentLimit(35)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)); - - - } public final class Indexer { //TODO: update these later public final static int CAN_ID = 0; public final static int CAN_ID_INVERSE = 0; @@ -246,43 +212,23 @@ public final class Indexer { //TODO: update these later new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); - - public static final CANdiConfiguration CANDI_CONFIG = - new CANdiConfiguration() - .withDigitalInputs( - new DigitalInputsConfigs() - .withS1CloseState(S1CloseStateValue.CloseWhenLow) - .withS2CloseState(S2CloseStateValue.CloseWhenLow)); - - public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = - new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(0.02) - .withTorqueOpenLoopRampPeriod(0.02) - .withVoltageOpenLoopRampPeriod(0.02); - - public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS = - new ClosedLoopRampsConfigs() - .withDutyCycleClosedLoopRampPeriod(0.02) - .withTorqueClosedLoopRampPeriod(0.02) - .withVoltageClosedLoopRampPeriod(0.02); } - public final class Climber{ //TODO: Update Values + + public final class Climb { public static final int CAN_ID = 0; public static final String BUS = "rio"; public static final double REDUCTION = 1.0; - public static final double SIM_MOI = 0.001; - - public static final TalonFXConfiguration CONFIG = + public static final TalonFXConfiguration CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withSupplyCurrentLimit(60) - .withStatorCurrentLimit(60)) + .withSupplyCurrentLimit(35) + .withStatorCurrentLimit(35)) .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); - } -} + } +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 4d31b1d2..92505e6e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -22,7 +22,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.inputs.LoggableInputs; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; @@ -71,7 +70,7 @@ public Climb(ClimbIO io) { @Override public void periodic() { io.updateInputs(inputs); - Logger.processInputs("Climb", (LoggableInputs) inputs); + Logger.processInputs("Climb", inputs); Logger.recordOutput("Climb/GoalState", goalState.toString()); Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState()); diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index f0020a7d..db07bcce 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -46,8 +46,8 @@ public class ClimbIOTalonFX implements ClimbIO { new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); public ClimbIOTalonFX() { - climbTalon = new TalonFX(Constants.Climber.CAN_ID, new CANBus(Constants.Climber.BUS)); - climbTalon.getConfigurator().apply(Constants.Climber.CONFIG); + climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); + climbTalon.getConfigurator().apply(Constants.Climb.CONFIG); // Get select status signals and set update frequency climbPosition = climbTalon.getPosition(); @@ -81,9 +81,9 @@ public void updateInputs(ClimbIOInputs inputs) { climbTempCelsius) .isOK(); inputs.climbPositionRads = - Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climber.REDUCTION; + Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climb.REDUCTION; inputs.climbVelocityRadsPerSec = - Units.rotationsToRadians(climbVelocity.getValueAsDouble()) / Constants.Climber.REDUCTION; + Units.rotationsToRadians(climbVelocity.getValueAsDouble()) / Constants.Climb.REDUCTION; inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); @@ -103,7 +103,6 @@ public void stop() { @Override public void setPosition(double rads) { climbTalon.setControl( - positionOut.withPosition( - rads * Constants.Climber.REDUCTION * Units.radiansToRotations(1.0))); + positionOut.withPosition(rads * Constants.Climb.REDUCTION * Units.radiansToRotations(1.0))); } } From 6f9623c2544c8cb97f227a87dddb4f7d635a8a46 Mon Sep 17 00:00:00 2001 From: Tracy Thai Date: Thu, 5 Feb 2026 20:17:02 -0800 Subject: [PATCH 13/39] implement automatic voltage application in the periodic() method when goalState == OPERATOR_CONTROL --- .../java/org/team5924/frc2026/Constants.java | 6 ------ .../frc2026/subsystems/climb/Climb.java | 18 ++++++++++++++++++ 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 90d5c369..0a21678e 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -16,18 +16,12 @@ package org.team5924.frc2026; -import com.ctre.phoenix6.configs.CANdiConfiguration; -import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.DigitalInputsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.S1CloseStateValue; -import com.ctre.phoenix6.signals.S2CloseStateValue; import edu.wpi.first.wpilibj.RobotBase; /** diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 92505e6e..53723605 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -78,6 +78,8 @@ public void periodic() { climbMotorDisconnected.set(!inputs.climbMotorConnected); + handleCurrentState(); + // prevents error spam if (!inputs.climbMotorConnected && wasClimbMotorConnected) { Elastic.sendNotification(climbMotorDisconnectedNotification); @@ -106,4 +108,20 @@ public void setGoalState(ClimbState goalState) { break; } } + + public void handleCurrentState() { + switch (goalState) { + case OPERATOR_CONTROL: + io.runVolts(goalState.rads.getAsDouble()); + break; + + case MOVING: + // Transition state - no direct motor command + break; + + default: + // Closed-loop position handdled by io.setPosition(...) + break; + } + } } From d729b7612266b1e9befb70147f66951205e6d51c Mon Sep 17 00:00:00 2001 From: Tracy Thai Date: Thu, 5 Feb 2026 20:32:17 -0800 Subject: [PATCH 14/39] added .toString and io.setPosition(goalState.rads.getAsDouble()); --- src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 53723605..594951e3 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -73,7 +73,7 @@ public void periodic() { Logger.processInputs("Climb", inputs); Logger.recordOutput("Climb/GoalState", goalState.toString()); - Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState()); + Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState().toString()); Logger.recordOutput("Climb/TargetRads", goalState.rads.getAsDouble()); climbMotorDisconnected.set(!inputs.climbMotorConnected); @@ -121,6 +121,7 @@ public void handleCurrentState() { default: // Closed-loop position handdled by io.setPosition(...) + io.setPosition(goalState.rads.getAsDouble()); break; } } From 5318cc906d62e33d11b6cf5b84d9baab5d309140 Mon Sep 17 00:00:00 2001 From: thartnett27-sudo Date: Thu, 12 Feb 2026 16:54:57 -0800 Subject: [PATCH 15/39] Integrate CANcoder for climb motor feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Added CANcoder for position feedback in ClimbIOTalonFX. (I did this on my ipad and I can’t really tell what I actually did) --- .../subsystems/climb/ClimbIOTalonFX.java | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index db07bcce..b140f148 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -22,6 +22,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -69,6 +70,27 @@ public ClimbIOTalonFX() { climbTalon.setPosition(0); } + private final TalonFX motor; + private final CANcoder cancoder; + + public ClimbIOTalonFX() { + motor = new TalonFX(0); + cancoder = new CANcoder(0); + + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Feedback.FeedbackRemoteSensorID = 0; + config.Feedback.FeedbackSensorSource = + FeedbackSensorSourceValue.RemoteCANcoder; + + motor.getConfigurator().apply(config); + } + + @Override + public void updateInputs(ClimbIOInputs inputs) { + inputs.positionRad = motor.getPosition().getValueAsDouble(); + inputs.velocityRadPerSec = motor.getVelocity().getValueAsDouble(); + } + @Override public void updateInputs(ClimbIOInputs inputs) { inputs.climbMotorConnected = From 19a88be59d0435ae584d31e8730332f7040d651d Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Tue, 17 Feb 2026 19:41:11 -0800 Subject: [PATCH 16/39] fixed duplicate constructers, code will build now --- .../java/org/team5924/frc2026/Constants.java | 9 ++++++-- .../org/team5924/frc2026/RobotContainer.java | 10 +++------ .../java/org/team5924/frc2026/RobotState.java | 2 +- .../subsystems/climb/ClimbIOTalonFX.java | 22 ------------------- .../exampleSystem/ExampleSystem.java | 18 +++++++-------- 5 files changed, 20 insertions(+), 41 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 8743dbac..f9243c33 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -209,7 +209,7 @@ public final class Indexer { //TODO: update these later .withNeutralMode(NeutralModeValue.Brake)); } - public final class Climb { + public final class Climb { // TODO: update these values public static final int CAN_ID = 0; public static final String BUS = "rio"; public static final double REDUCTION = 1.0; @@ -222,7 +222,12 @@ public final class Climb { .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)); + .withNeutralMode(NeutralModeValue.Brake)) + .withSlot0( + new Slot0Configs() // TODO: Tune PID gains for climb + .withKP(1.0) + .withKI(0) + .withKD(0)); } diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index b3b0a0bf..4db492dc 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -99,8 +99,7 @@ public RobotContainer() { new BeamBreakIOHardware(Constants.ShooterRoller.BEAM_BREAK_PORT)); intake = new Intake(new IntakeIOKrakenFOC()); shooter = new SuperShooter(shooterRoller, shooterHood); - hopper = - new Hopper(new HopperKrakenFOC()); + hopper = new Hopper(new HopperKrakenFOC()); break; case SIM: @@ -128,8 +127,7 @@ public RobotContainer() { shooterRoller = new ShooterRoller(new ShooterRollerIOSim(), new BeamBreakIO() {}); intake = new Intake(new IntakeIOSim()); shooter = new SuperShooter(shooterRoller, shooterHood); - hopper = - new Hopper(new HopperIO() {}); // TODO: Hopper sim implementation + hopper = new Hopper(new HopperIO() {}); // TODO: Hopper sim implementation break; default: @@ -147,9 +145,7 @@ public RobotContainer() { shooterRoller = new ShooterRoller(new ShooterRollerIO() {}, new BeamBreakIO() {}); intake = new Intake(new IntakeIO() {}); shooter = new SuperShooter(shooterRoller, shooterHood); - hopper = - new Hopper( - new HopperIO() {}); // TODO: Add replay IO implementation + hopper = new Hopper(new HopperIO() {}); // TODO: Add replay IO implementation // vision = new Vision(drive, new VisionIO() {}, new VisionIO() {}); break; } diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index e7a0516d..d750a0fc 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -21,8 +21,8 @@ import lombok.Getter; import lombok.Setter; import org.littletonrobotics.junction.AutoLogOutput; -import org.team5924.frc2026.subsystems.climb.Climb.ClimbState; import org.team5924.frc2026.subsystems.SuperShooter.ShooterState; +import org.team5924.frc2026.subsystems.climb.Climb.ClimbState; import org.team5924.frc2026.subsystems.exampleSystem.ExampleSystem.ExampleSystemState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; import org.team5924.frc2026.subsystems.rollers.exampleRoller.ExampleRoller.ExampleRollerState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index b140f148..db07bcce 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -22,7 +22,6 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -70,27 +69,6 @@ public ClimbIOTalonFX() { climbTalon.setPosition(0); } - private final TalonFX motor; - private final CANcoder cancoder; - - public ClimbIOTalonFX() { - motor = new TalonFX(0); - cancoder = new CANcoder(0); - - TalonFXConfiguration config = new TalonFXConfiguration(); - config.Feedback.FeedbackRemoteSensorID = 0; - config.Feedback.FeedbackSensorSource = - FeedbackSensorSourceValue.RemoteCANcoder; - - motor.getConfigurator().apply(config); - } - - @Override - public void updateInputs(ClimbIOInputs inputs) { - inputs.positionRad = motor.getPosition().getValueAsDouble(); - inputs.velocityRadPerSec = motor.getVelocity().getValueAsDouble(); - } - @Override public void updateInputs(ClimbIOInputs inputs) { inputs.climbMotorConnected = diff --git a/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java b/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java index cdbbe6c4..2d05d887 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java +++ b/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java @@ -31,7 +31,7 @@ public class ExampleSystem extends SubsystemBase { private final ExampleSystemIO io; - // private final ExampleSystemIOInputsAutoLogged inputs = new ExampleSystemIOInputsAutoLogged(); + private final ExampleSystemIOInputsAutoLogged inputs = new ExampleSystemIOInputsAutoLogged(); public enum ExampleSystemState { STOW(new LoggedTunableNumber("ExampleSystem/Stow", Math.toRadians(0))), @@ -65,23 +65,23 @@ public ExampleSystem(ExampleSystemIO io) { @Override public void periodic() { - // io.updateInputs(inputs); - // Logger.processInputs("ExampleSystem", inputs); + io.updateInputs(inputs); + Logger.processInputs("ExampleSystem", inputs); Logger.recordOutput("ExampleSystem/GoalState", goalState.toString()); Logger.recordOutput( "ExampleSystem/CurrentState", RobotState.getInstance().getExampleSystemState()); Logger.recordOutput("ExampleSystem/TargetRads", goalState.rads); - // exampleMotorDisconnected.set(!inputs.exampleMotorConnected); + exampleMotorDisconnected.set(!inputs.exampleMotorConnected); // prevents error spam - // if (!inputs.exampleMotorConnected && wasExampleMotorConnected) { - Elastic.sendNotification(exampleMotorDisconnectedNotification); - } + if (!inputs.exampleMotorConnected && wasExampleMotorConnected) { + Elastic.sendNotification(exampleMotorDisconnectedNotification); + } - // wasExampleMotorConnected = inputs.exampleMotorConnected; - // } + wasExampleMotorConnected = inputs.exampleMotorConnected; + } public void runVolts(double volts) { io.runVolts(volts); From ae56d9b1865c600a3b0dd1a0e8e14ae07ef93de0 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Tue, 17 Feb 2026 19:55:13 -0800 Subject: [PATCH 17/39] climb beam break implementation --- .../team5924/frc2026/subsystems/climb/Climb.java | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 594951e3..97a5b9d1 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -23,6 +23,8 @@ import lombok.Getter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.RobotState; +import org.team5924.frc2026.subsystems.sensors.BeamBreakIO; +import org.team5924.frc2026.subsystems.sensors.BeamBreakIOInputsAutoLogged; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -58,19 +60,31 @@ public enum ClimbState { private final Notification climbMotorDisconnectedNotification; private boolean wasClimbMotorConnected = true; - public Climb(ClimbIO io) { + // Climb Beam Breaks + private final BeamBreakIO grabBeamBreakIO; + private final BeamBreakIO latchBeamBreakIO; + private final BeamBreakIOInputsAutoLogged grabBeamBreakInputs = new BeamBreakIOInputsAutoLogged(); + private final BeamBreakIOInputsAutoLogged latchBeamBreakInputs = new BeamBreakIOInputsAutoLogged(); + + public Climb(ClimbIO io, BeamBreakIO grabBeamBreakIO, BeamBreakIO latchBeamBreakIO) { this.io = io; this.goalState = ClimbState.STOW; this.climbMotorDisconnected = new Alert("Climb System Motor Disconnected!", Alert.AlertType.kWarning); this.climbMotorDisconnectedNotification = new Notification(NotificationLevel.WARNING, "Climb System Motor Disconnected", ""); + this.grabBeamBreakIO = grabBeamBreakIO; + this.latchBeamBreakIO = latchBeamBreakIO; } @Override public void periodic() { io.updateInputs(inputs); + grabBeamBreakIO.updateInputs(grabBeamBreakInputs); + latchBeamBreakIO.updateInputs(latchBeamBreakInputs); Logger.processInputs("Climb", inputs); + Logger.processInputs("Climb/GrabBeamBreak", grabBeamBreakInputs); + Logger.processInputs("Climb/LatchBeamBreak", latchBeamBreakInputs); Logger.recordOutput("Climb/GoalState", goalState.toString()); Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState().toString()); From e081e9b7eaf1c76255f7778877050cc09db89282 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Tue, 17 Feb 2026 20:19:46 -0800 Subject: [PATCH 18/39] nitpick fixes --- src/main/java/org/team5924/frc2026/Constants.java | 13 +++++++------ .../team5924/frc2026/subsystems/climb/Climb.java | 3 +-- .../frc2026/subsystems/climb/ClimbIOTalonFX.java | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index f9243c33..e245bd30 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -1,6 +1,7 @@ -// * Constants.java -// */ +/* + * Constants.java +*/ /* * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. @@ -224,10 +225,10 @@ public final class Climb { // TODO: update these values .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)) .withSlot0( - new Slot0Configs() // TODO: Tune PID gains for climb - .withKP(1.0) - .withKI(0) - .withKD(0)); + new Slot0Configs() // TODO: Tune PID gains for climb + .withKP(1.0) + .withKI(0) + .withKD(0)); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 97a5b9d1..f561e4d4 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -118,12 +118,11 @@ public void setGoalState(ClimbState goalState) { break; default: RobotState.getInstance().setClimbState(goalState); - io.setPosition(goalState.rads.getAsDouble()); break; } } - public void handleCurrentState() { + private void handleCurrentState() { switch (goalState) { case OPERATOR_CONTROL: io.runVolts(goalState.rads.getAsDouble()); diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index db07bcce..7555cc67 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -103,6 +103,6 @@ public void stop() { @Override public void setPosition(double rads) { climbTalon.setControl( - positionOut.withPosition(rads * Constants.Climb.REDUCTION * Units.radiansToRotations(1.0))); + positionOut.withPosition(Constants.Climb.REDUCTION * Units.radiansToRotations(rads))); } } From 32ef32e7005729e04e16ab34d20f5f41dab01c40 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Tue, 17 Feb 2026 20:36:35 -0800 Subject: [PATCH 19/39] updated to just one beambreak --- .../frc2026/subsystems/climb/Climb.java | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index f561e4d4..53f89155 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -60,31 +60,26 @@ public enum ClimbState { private final Notification climbMotorDisconnectedNotification; private boolean wasClimbMotorConnected = true; - // Climb Beam Breaks - private final BeamBreakIO grabBeamBreakIO; - private final BeamBreakIO latchBeamBreakIO; - private final BeamBreakIOInputsAutoLogged grabBeamBreakInputs = new BeamBreakIOInputsAutoLogged(); - private final BeamBreakIOInputsAutoLogged latchBeamBreakInputs = new BeamBreakIOInputsAutoLogged(); + // Climb Beam Break + private final BeamBreakIO beamBreakIO; + private final BeamBreakIOInputsAutoLogged beamBreakInputs = new BeamBreakIOInputsAutoLogged(); - public Climb(ClimbIO io, BeamBreakIO grabBeamBreakIO, BeamBreakIO latchBeamBreakIO) { + public Climb(ClimbIO io, BeamBreakIO beamBreakIO) { this.io = io; this.goalState = ClimbState.STOW; this.climbMotorDisconnected = new Alert("Climb System Motor Disconnected!", Alert.AlertType.kWarning); this.climbMotorDisconnectedNotification = new Notification(NotificationLevel.WARNING, "Climb System Motor Disconnected", ""); - this.grabBeamBreakIO = grabBeamBreakIO; - this.latchBeamBreakIO = latchBeamBreakIO; + this.beamBreakIO = beamBreakIO; } @Override public void periodic() { io.updateInputs(inputs); - grabBeamBreakIO.updateInputs(grabBeamBreakInputs); - latchBeamBreakIO.updateInputs(latchBeamBreakInputs); + beamBreakIO.updateInputs(beamBreakInputs); Logger.processInputs("Climb", inputs); - Logger.processInputs("Climb/GrabBeamBreak", grabBeamBreakInputs); - Logger.processInputs("Climb/LatchBeamBreak", latchBeamBreakInputs); + Logger.processInputs("Climb/BeamBreak", beamBreakInputs); Logger.recordOutput("Climb/GoalState", goalState.toString()); Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState().toString()); From db422e1e8a9707fab3ebcb0446d1f3e9c04cfc36 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 11:36:51 -0800 Subject: [PATCH 20/39] implemented candoder --- .../java/org/team5924/frc2026/Constants.java | 2 ++ .../frc2026/subsystems/climb/ClimbIO.java | 5 ++++ .../subsystems/climb/ClimbIOTalonFX.java | 24 +++++++++++++++++++ 3 files changed, 31 insertions(+) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index e245bd30..504ed91c 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -229,6 +229,8 @@ public final class Climb { // TODO: update these values .withKP(1.0) .withKI(0) .withKD(0)); + + public static final int CANCODER_ID = 0; // TODO: update id } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java index cc249523..cbd5114f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java @@ -28,6 +28,11 @@ public static class ClimbIOInputs { public double climbSupplyCurrentAmps = 0.0; public double climbTorqueCurrentAmps = 0.0; public double climbTempCelsius = 0.0; + + public boolean cancoderConnected = true; + public double cancoderPosition = 0.0; + public double cancoderVelocity = 0.0; + public double cancoderSupplyVoltage = 0.0; } /** diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 7555cc67..06d07001 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -21,6 +21,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; @@ -40,6 +41,11 @@ public class ClimbIOTalonFX implements ClimbIO { private final StatusSignal climbTorqueCurrent; private final StatusSignal climbTempCelsius; + private final CANcoder cancoder; + private final StatusSignal cancoderPosition; + private final StatusSignal cancoderVelocity; + private final StatusSignal cancoderSupplyVoltage; + // Single shot for voltage mode, robot loop will call continuously private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); private final PositionVoltage positionOut = @@ -49,6 +55,8 @@ public ClimbIOTalonFX() { climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); climbTalon.getConfigurator().apply(Constants.Climb.CONFIG); + cancoder = new CANcoder(Constants.Climb.CANCODER_ID); + // Get select status signals and set update frequency climbPosition = climbTalon.getPosition(); climbVelocity = climbTalon.getVelocity(); @@ -57,6 +65,10 @@ public ClimbIOTalonFX() { climbTorqueCurrent = climbTalon.getTorqueCurrent(); climbTempCelsius = climbTalon.getDeviceTemp(); + cancoderPosition = cancoder.getAbsolutePosition(); + cancoderVelocity = cancoder.getVelocity(); + cancoderSupplyVoltage = cancoder.getSupplyVoltage(); + BaseStatusSignal.setUpdateFrequencyForAll( 50.0, climbPosition, @@ -66,6 +78,9 @@ public ClimbIOTalonFX() { climbTorqueCurrent, climbTempCelsius); + BaseStatusSignal.setUpdateFrequencyForAll( + 250.0, cancoderPosition, cancoderVelocity, cancoderSupplyVoltage); + climbTalon.setPosition(0); } @@ -80,6 +95,11 @@ public void updateInputs(ClimbIOInputs inputs) { climbTorqueCurrent, climbTempCelsius) .isOK(); + + inputs.cancoderConnected = + BaseStatusSignal.refreshAll(cancoderPosition, cancoderVelocity, cancoderSupplyVoltage) + .isOK(); + inputs.climbPositionRads = Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climb.REDUCTION; inputs.climbVelocityRadsPerSec = @@ -88,6 +108,10 @@ public void updateInputs(ClimbIOInputs inputs) { inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); + + inputs.cancoderPosition = cancoderPosition.getValueAsDouble(); + inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); + inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); } @Override From 4c4901ecf9d68cd55d8b7d7c6fabc70c5402ea10 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 11:51:39 -0800 Subject: [PATCH 21/39] implemented PID --- .../subsystems/climb/ClimbIOTalonFX.java | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 06d07001..81fc756b 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -19,6 +19,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; @@ -30,6 +31,7 @@ import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.team5924.frc2026.Constants; +import org.team5924.frc2026.util.LoggedTunableNumber; public class ClimbIOTalonFX implements ClimbIO { @@ -46,6 +48,16 @@ public class ClimbIOTalonFX implements ClimbIO { private final StatusSignal cancoderVelocity; private final StatusSignal cancoderSupplyVoltage; + private final Slot0Configs slot0Configs; + + private final LoggedTunableNumber kA = new LoggedTunableNumber("Climb/kA", 0.00); + private final LoggedTunableNumber kS = new LoggedTunableNumber("Climb/kS", 0.13); + private final LoggedTunableNumber kV = new LoggedTunableNumber("Climb/kV", 0.4); + private final LoggedTunableNumber kP = new LoggedTunableNumber("Climb/kP", 6.0); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Climb/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Climb/kD", 0.07); + private final LoggedTunableNumber kG = new LoggedTunableNumber("Climb/kG", 0.33); + // Single shot for voltage mode, robot loop will call continuously private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); private final PositionVoltage positionOut = @@ -53,6 +65,16 @@ public class ClimbIOTalonFX implements ClimbIO { public ClimbIOTalonFX() { climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); + + slot0Configs = new Slot0Configs(); + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); + slot0Configs.kG = kG.get(); + climbTalon.getConfigurator().apply(Constants.Climb.CONFIG); cancoder = new CANcoder(Constants.Climb.CANCODER_ID); @@ -112,6 +134,18 @@ public void updateInputs(ClimbIOInputs inputs) { inputs.cancoderPosition = cancoderPosition.getValueAsDouble(); inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); + + updateLoggedTunableNumbers(); + } + + private void updateLoggedTunableNumbers() { + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); + slot0Configs.kG = kG.get(); } @Override From b39149dcd6ad6c39798e3f118408453e7093dcc3 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 12:09:05 -0800 Subject: [PATCH 22/39] fixed pid updating --- .../java/org/team5924/frc2026/Constants.java | 9 ++----- .../subsystems/climb/ClimbIOTalonFX.java | 27 ++++++++++++++----- 2 files changed, 22 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 504ed91c..634568b6 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -223,13 +223,8 @@ public final class Climb { // TODO: update these values .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)) - .withSlot0( - new Slot0Configs() // TODO: Tune PID gains for climb - .withKP(1.0) - .withKI(0) - .withKD(0)); - + .withNeutralMode(NeutralModeValue.Brake)); + public static final int CANCODER_ID = 0; // TODO: update id diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 81fc756b..d9a95eeb 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -76,6 +76,7 @@ public ClimbIOTalonFX() { slot0Configs.kG = kG.get(); climbTalon.getConfigurator().apply(Constants.Climb.CONFIG); + climbTalon.getConfigurator().apply(slot0Configs); cancoder = new CANcoder(Constants.Climb.CANCODER_ID); @@ -139,13 +140,25 @@ public void updateInputs(ClimbIOInputs inputs) { } private void updateLoggedTunableNumbers() { - slot0Configs.kP = kP.get(); - slot0Configs.kI = kI.get(); - slot0Configs.kD = kD.get(); - slot0Configs.kS = kS.get(); - slot0Configs.kV = kV.get(); - slot0Configs.kA = kA.get(); - slot0Configs.kG = kG.get(); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); + slot0Configs.kG = kG.get(); + climbTalon.getConfigurator().apply(slot0Configs); + }, + kP, + kI, + kD, + kS, + kV, + kA, + kG); } @Override From b17c61a53da5a8c4b8f7231c7c6801d27316a6a3 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 12:22:51 -0800 Subject: [PATCH 23/39] added cancoder feedback --- src/main/java/org/team5924/frc2026/Constants.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 634568b6..4d45592d 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -18,9 +18,11 @@ package org.team5924.frc2026; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.RobotBase; @@ -223,7 +225,11 @@ public final class Climb { // TODO: update these values .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)); + .withNeutralMode(NeutralModeValue.Brake)) + .withFeedback( + new FeedbackConfigs() + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) + .withFeedbackRemoteSensorID(Constants.Climb.CANCODER_ID)); public static final int CANCODER_ID = 0; // TODO: update id From b2c5e8119f8ed4c53aa864f7123bc979752ae3fd Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 14:23:08 -0800 Subject: [PATCH 24/39] updated constants and configs --- .../java/org/team5924/frc2026/Constants.java | 65 +++++++++++++++++-- .../subsystems/climb/ClimbIOTalonFX.java | 10 +-- .../exampleSystem/ExampleSystem.java | 4 +- 3 files changed, 67 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 4d45592d..cdd7522e 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -17,14 +17,20 @@ package org.team5924.frc2026; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; + +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; /** @@ -101,7 +107,17 @@ public final class Example { .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); + public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(0.02) + .withTorqueOpenLoopRampPeriod(0.02) + .withVoltageOpenLoopRampPeriod(0.02); + public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS = + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(0.02) + .withTorqueClosedLoopRampPeriod(0.02) + .withVoltageClosedLoopRampPeriod(0.02); } public final class GenericRollerSystem { @@ -213,15 +229,32 @@ public final class Indexer { //TODO: update these later } public final class Climb { // TODO: update these values - public static final int CAN_ID = 0; + public static final int CAN_ID = 60; public static final String BUS = "rio"; - public static final double REDUCTION = 1.0; + public static final double MOTOR_TO_CANCODER = (7.0 / 1.0); // TODO: Update Reductions + public static final double CANCODER_TO_MECHANISM = (7.0 / 1.0); + public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM; + public static final double SIM_MOI = 0.001; + + public static final double MIN_POSITION_MULTI = 0.8; // rotations + public static final double MAX_POSITION_MULTI = 0.8; // rotations + + public static final double MIN_POSITION_RADS = -Math.PI * MIN_POSITION_MULTI; + public static final double MAX_POSITION_RADS = Math.PI * MAX_POSITION_MULTI; + + public static final double JOYSTICK_DEADZONE = 0.01; + + public static final double STATE_TIMEOUT = 5.0; + + public static final int CANCODER_ID = 0; // TODO: update id + public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; + public static final TalonFXConfiguration CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withSupplyCurrentLimit(35) - .withStatorCurrentLimit(35)) + .withSupplyCurrentLimit(60) + .withStatorCurrentLimit(60)) .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) @@ -230,9 +263,31 @@ public final class Climb { // TODO: update these values new FeedbackConfigs() .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) .withFeedbackRemoteSensorID(Constants.Climb.CANCODER_ID)); + + public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(0.02) + .withTorqueOpenLoopRampPeriod(0.02) + .withVoltageOpenLoopRampPeriod(0.02); - public static final int CANCODER_ID = 0; // TODO: update id + public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS = + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(0.02) + .withTorqueClosedLoopRampPeriod(0.02) + .withVoltageClosedLoopRampPeriod(0.02); + public static final FeedbackConfigs FEEDBACK_CONFIGS = + new FeedbackConfigs() + .withFeedbackRemoteSensorID(CANCODER_ID) + .withFeedbackRotorOffset(CANCODER_ABSOLUTE_OFFSET) + .withSensorToMechanismRatio(1.0 / CANCODER_TO_MECHANISM) + .withRotorToSensorRatio(1.0 / MOTOR_TO_CANCODER) + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); + public static final MagnetSensorConfigs CANCODER_CONFIG = + new MagnetSensorConfigs() + .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET) + .withAbsoluteSensorDiscontinuityPoint(0.5) + .withSensorDirection(SensorDirectionValue.Clockwise_Positive); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index d9a95eeb..a663c332 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -124,10 +124,11 @@ public void updateInputs(ClimbIOInputs inputs) { .isOK(); inputs.climbPositionRads = - Units.rotationsToRadians(climbPosition.getValueAsDouble()) / Constants.Climb.REDUCTION; + Units.rotationsToRadians(climbPosition.getValueAsDouble()) + / Constants.Climb.MOTOR_TO_MECHANISM; inputs.climbVelocityRadsPerSec = - Units.rotationsToRadians(climbVelocity.getValueAsDouble()) / Constants.Climb.REDUCTION; - inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); + Units.rotationsToRadians(climbVelocity.getValueAsDouble()) + / Constants.Climb.MOTOR_TO_MECHANISM; inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); @@ -174,6 +175,7 @@ public void stop() { @Override public void setPosition(double rads) { climbTalon.setControl( - positionOut.withPosition(Constants.Climb.REDUCTION * Units.radiansToRotations(rads))); + positionOut.withPosition( + Constants.Climb.MOTOR_TO_MECHANISM * Units.radiansToRotations(rads))); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java b/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java index 2d05d887..73b87f12 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java +++ b/src/main/java/org/team5924/frc2026/subsystems/exampleSystem/ExampleSystem.java @@ -30,13 +30,12 @@ public class ExampleSystem extends SubsystemBase { private final ExampleSystemIO io; - private final ExampleSystemIOInputsAutoLogged inputs = new ExampleSystemIOInputsAutoLogged(); public enum ExampleSystemState { STOW(new LoggedTunableNumber("ExampleSystem/Stow", Math.toRadians(0))), MOVING(new LoggedTunableNumber("ExampleSystem/Moving", 0)), - UP(new LoggedTunableNumber("ExampleSystem/Stow", Math.toRadians(90))), + UP(new LoggedTunableNumber("ExampleSystem/Up", Math.toRadians(90))), // voltage at which the example subsystem motor moves when controlled by the operator OPERATOR_CONTROL(new LoggedTunableNumber("ExampleSystem/OperatorVoltage", 4.5)); @@ -79,7 +78,6 @@ public void periodic() { if (!inputs.exampleMotorConnected && wasExampleMotorConnected) { Elastic.sendNotification(exampleMotorDisconnectedNotification); } - wasExampleMotorConnected = inputs.exampleMotorConnected; } From e9340d95ec67ac366654b547285193e6ba826930 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 14:50:40 -0800 Subject: [PATCH 25/39] updated configs and cancoder --- .../frc2026/subsystems/climb/Climb.java | 1 + .../frc2026/subsystems/climb/ClimbIO.java | 7 +- .../subsystems/climb/ClimbIOTalonFX.java | 89 ++++++++++++++----- 3 files changed, 73 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 53f89155..6c86a6d9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -76,6 +76,7 @@ public Climb(ClimbIO io, BeamBreakIO beamBreakIO) { @Override public void periodic() { + io.periodicUpdates(); io.updateInputs(inputs); beamBreakIO.updateInputs(beamBreakInputs); Logger.processInputs("Climb", inputs); diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java index cbd5114f..7603effe 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java @@ -23,6 +23,7 @@ public interface ClimbIO { public static class ClimbIOInputs { public boolean climbMotorConnected = true; public double climbPositionRads = 0.0; + public double climbPositionCancoder = 0.0; public double climbVelocityRadsPerSec = 0.0; public double climbAppliedVoltage = 0.0; public double climbSupplyCurrentAmps = 0.0; @@ -30,9 +31,10 @@ public static class ClimbIOInputs { public double climbTempCelsius = 0.0; public boolean cancoderConnected = true; - public double cancoderPosition = 0.0; + public double cancoderAbsolutePosition = 0.0; public double cancoderVelocity = 0.0; public double cancoderSupplyVoltage = 0.0; + public double cancoderPositionRotations = 0.0; } /** @@ -42,6 +44,9 @@ public static class ClimbIOInputs { */ public default void updateInputs(ClimbIOInputs inputs) {} + /** Updates that are be called in climb periodic */ + public default void periodicUpdates() {} + /** * Sets the subsystem motor to the specified voltage * diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index a663c332..d981a2da 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -18,8 +18,10 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; @@ -31,22 +33,16 @@ import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.team5924.frc2026.Constants; +import org.team5924.frc2026.util.Elastic; +import org.team5924.frc2026.util.Elastic.Notification; +import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; import org.team5924.frc2026.util.LoggedTunableNumber; public class ClimbIOTalonFX implements ClimbIO { - private final TalonFX climbTalon; - private final StatusSignal climbPosition; - private final StatusSignal climbVelocity; - private final StatusSignal climbAppliedVoltage; - private final StatusSignal climbSupplyCurrent; - private final StatusSignal climbTorqueCurrent; - private final StatusSignal climbTempCelsius; + private final CANcoder climbCANCoder; - private final CANcoder cancoder; - private final StatusSignal cancoderPosition; - private final StatusSignal cancoderVelocity; - private final StatusSignal cancoderSupplyVoltage; + private TalonFXConfigurator climbTalonConfig; private final Slot0Configs slot0Configs; @@ -58,6 +54,20 @@ public class ClimbIOTalonFX implements ClimbIO { private final LoggedTunableNumber kD = new LoggedTunableNumber("Climb/kD", 0.07); private final LoggedTunableNumber kG = new LoggedTunableNumber("Climb/kG", 0.33); + private final StatusSignal climbPosition; + private final StatusSignal climbVelocity; + private final StatusSignal climbAppliedVoltage; + private final StatusSignal climbSupplyCurrent; + private final StatusSignal climbTorqueCurrent; + private final StatusSignal climbTempCelsius; + + private final StatusSignal cancoderAbsolutePosition; + private final StatusSignal cancoderVelocity; + private final StatusSignal cancoderSupplyVoltage; + private final StatusSignal cancoderPositionRotations; + + private final StatusSignal closedLoopReferenceSlope; + // Single shot for voltage mode, robot loop will call continuously private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); private final PositionVoltage positionOut = @@ -65,6 +75,7 @@ public class ClimbIOTalonFX implements ClimbIO { public ClimbIOTalonFX() { climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); + climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID); slot0Configs = new Slot0Configs(); slot0Configs.kP = kP.get(); @@ -75,10 +86,24 @@ public ClimbIOTalonFX() { slot0Configs.kA = kA.get(); slot0Configs.kG = kG.get(); - climbTalon.getConfigurator().apply(Constants.Climb.CONFIG); - climbTalon.getConfigurator().apply(slot0Configs); + climbTalonConfig = climbTalon.getConfigurator(); + + // Apply Configs + StatusCode[] statusArray = new StatusCode[6]; - cancoder = new CANcoder(Constants.Climb.CANCODER_ID); + statusArray[0] = climbTalonConfig.apply(Constants.Climb.CONFIG); + statusArray[1] = climbTalonConfig.apply(slot0Configs); + statusArray[2] = climbTalonConfig.apply(Constants.Climb.OPEN_LOOP_RAMPS_CONFIGS); + statusArray[3] = climbTalonConfig.apply(Constants.Climb.CLOSED_LOOP_RAMPS_CONFIGS); + statusArray[4] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); + statusArray[5] = climbCANCoder.getConfigurator().apply(Constants.Climb.CANCODER_CONFIG); + + boolean isErrorPresent = false; + for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; + + if (isErrorPresent) + Elastic.sendNotification( + new Notification(NotificationLevel.WARNING, "Climb Configs", "Error in climb configs!")); // Get select status signals and set update frequency climbPosition = climbTalon.getPosition(); @@ -88,21 +113,29 @@ public ClimbIOTalonFX() { climbTorqueCurrent = climbTalon.getTorqueCurrent(); climbTempCelsius = climbTalon.getDeviceTemp(); - cancoderPosition = cancoder.getAbsolutePosition(); - cancoderVelocity = cancoder.getVelocity(); - cancoderSupplyVoltage = cancoder.getSupplyVoltage(); + cancoderAbsolutePosition = climbCANCoder.getAbsolutePosition(); + cancoderVelocity = climbCANCoder.getVelocity(); + cancoderSupplyVoltage = climbCANCoder.getSupplyVoltage(); + cancoderPositionRotations = climbCANCoder.getPosition(); + + closedLoopReferenceSlope = climbTalon.getClosedLoopReferenceSlope(); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, + 100.0, climbPosition, climbVelocity, climbAppliedVoltage, climbSupplyCurrent, climbTorqueCurrent, - climbTempCelsius); + climbTempCelsius, + cancoderAbsolutePosition, + cancoderVelocity, + cancoderSupplyVoltage, + cancoderPositionRotations, + closedLoopReferenceSlope); BaseStatusSignal.setUpdateFrequencyForAll( - 250.0, cancoderPosition, cancoderVelocity, cancoderSupplyVoltage); + 250.0, cancoderAbsolutePosition, cancoderVelocity, cancoderSupplyVoltage); climbTalon.setPosition(0); } @@ -120,7 +153,11 @@ public void updateInputs(ClimbIOInputs inputs) { .isOK(); inputs.cancoderConnected = - BaseStatusSignal.refreshAll(cancoderPosition, cancoderVelocity, cancoderSupplyVoltage) + BaseStatusSignal.refreshAll( + cancoderAbsolutePosition, + cancoderVelocity, + cancoderSupplyVoltage, + cancoderPositionRotations) .isOK(); inputs.climbPositionRads = @@ -133,16 +170,22 @@ public void updateInputs(ClimbIOInputs inputs) { inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); - inputs.cancoderPosition = cancoderPosition.getValueAsDouble(); + inputs.cancoderAbsolutePosition = cancoderAbsolutePosition.getValueAsDouble(); inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); + inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); + + inputs.climbPositionCancoder = + (inputs.cancoderPositionRotations) / Constants.Climb.CANCODER_TO_MECHANISM; + } + public void periodicUpdates() { updateLoggedTunableNumbers(); } private void updateLoggedTunableNumbers() { LoggedTunableNumber.ifChanged( - hashCode(), + 0, () -> { slot0Configs.kP = kP.get(); slot0Configs.kI = kI.get(); From 3909a28863fac75d4e6b042b8726d40b598363c5 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 15:11:25 -0800 Subject: [PATCH 26/39] coderabbit fixes --- src/main/java/org/team5924/frc2026/Constants.java | 10 +++------- .../frc2026/subsystems/climb/ClimbIOTalonFX.java | 10 +++------- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index cdd7522e..e6360fec 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -258,11 +258,7 @@ public final class Climb { // TODO: update these values .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)) - .withFeedback( - new FeedbackConfigs() - .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder) - .withFeedbackRemoteSensorID(Constants.Climb.CANCODER_ID)); + .withNeutralMode(NeutralModeValue.Brake)); public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = new OpenLoopRampsConfigs() @@ -280,8 +276,8 @@ public final class Climb { // TODO: update these values new FeedbackConfigs() .withFeedbackRemoteSensorID(CANCODER_ID) .withFeedbackRotorOffset(CANCODER_ABSOLUTE_OFFSET) - .withSensorToMechanismRatio(1.0 / CANCODER_TO_MECHANISM) - .withRotorToSensorRatio(1.0 / MOTOR_TO_CANCODER) + .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) + .withRotorToSensorRatio(MOTOR_TO_CANCODER) .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); public static final MagnetSensorConfigs CANCODER_CONFIG = diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index d981a2da..18857035 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.PositionVoltage; @@ -66,8 +67,6 @@ public class ClimbIOTalonFX implements ClimbIO { private final StatusSignal cancoderSupplyVoltage; private final StatusSignal cancoderPositionRotations; - private final StatusSignal closedLoopReferenceSlope; - // Single shot for voltage mode, robot loop will call continuously private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); private final PositionVoltage positionOut = @@ -96,7 +95,7 @@ public ClimbIOTalonFX() { statusArray[2] = climbTalonConfig.apply(Constants.Climb.OPEN_LOOP_RAMPS_CONFIGS); statusArray[3] = climbTalonConfig.apply(Constants.Climb.CLOSED_LOOP_RAMPS_CONFIGS); statusArray[4] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); - statusArray[5] = climbCANCoder.getConfigurator().apply(Constants.Climb.CANCODER_CONFIG); + statusArray[5] = climbCANCoder.getConfigurator().apply(new CANcoderConfiguration().withMagnetSensor(Constants.Climb.CANCODER_CONFIG)); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -118,8 +117,6 @@ public ClimbIOTalonFX() { cancoderSupplyVoltage = climbCANCoder.getSupplyVoltage(); cancoderPositionRotations = climbCANCoder.getPosition(); - closedLoopReferenceSlope = climbTalon.getClosedLoopReferenceSlope(); - BaseStatusSignal.setUpdateFrequencyForAll( 100.0, climbPosition, @@ -131,8 +128,7 @@ public ClimbIOTalonFX() { cancoderAbsolutePosition, cancoderVelocity, cancoderSupplyVoltage, - cancoderPositionRotations, - closedLoopReferenceSlope); + cancoderPositionRotations); BaseStatusSignal.setUpdateFrequencyForAll( 250.0, cancoderAbsolutePosition, cancoderVelocity, cancoderSupplyVoltage); From cc7aaf57d06124df629fe7a8bc92c760f41226a8 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 15:22:40 -0800 Subject: [PATCH 27/39] fixed units --- .../frc2026/subsystems/climb/ClimbIOTalonFX.java | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 18857035..383ad051 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -157,11 +157,9 @@ public void updateInputs(ClimbIOInputs inputs) { .isOK(); inputs.climbPositionRads = - Units.rotationsToRadians(climbPosition.getValueAsDouble()) - / Constants.Climb.MOTOR_TO_MECHANISM; + Units.rotationsToRadians(climbPosition.getValueAsDouble()); inputs.climbVelocityRadsPerSec = - Units.rotationsToRadians(climbVelocity.getValueAsDouble()) - / Constants.Climb.MOTOR_TO_MECHANISM; + Units.rotationsToRadians(climbVelocity.getValueAsDouble()); inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); @@ -172,7 +170,7 @@ public void updateInputs(ClimbIOInputs inputs) { inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); inputs.climbPositionCancoder = - (inputs.cancoderPositionRotations) / Constants.Climb.CANCODER_TO_MECHANISM; + Units.rotationsToRadians(inputs.cancoderPositionRotations) / Constants.Climb.CANCODER_TO_MECHANISM; } public void periodicUpdates() { @@ -214,7 +212,6 @@ public void stop() { @Override public void setPosition(double rads) { climbTalon.setControl( - positionOut.withPosition( - Constants.Climb.MOTOR_TO_MECHANISM * Units.radiansToRotations(rads))); + positionOut.withPosition(Units.radiansToRotations(rads))); } } From 6e497b9408aff4910139772acd3c8ad5f05bc8bd Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 15:37:33 -0800 Subject: [PATCH 28/39] updated inputs --- .../org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 383ad051..dfd2efe8 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -160,6 +160,7 @@ public void updateInputs(ClimbIOInputs inputs) { Units.rotationsToRadians(climbPosition.getValueAsDouble()); inputs.climbVelocityRadsPerSec = Units.rotationsToRadians(climbVelocity.getValueAsDouble()); + inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); From 7f9416c1ea385493a39215ce799112c521e610b4 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 16:32:59 -0800 Subject: [PATCH 29/39] nitpick fixes --- .../subsystems/climb/ClimbIOTalonFX.java | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index dfd2efe8..a6902c27 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -43,7 +43,7 @@ public class ClimbIOTalonFX implements ClimbIO { private final TalonFX climbTalon; private final CANcoder climbCANCoder; - private TalonFXConfigurator climbTalonConfig; + private final TalonFXConfigurator climbTalonConfig; private final Slot0Configs slot0Configs; @@ -95,7 +95,10 @@ public ClimbIOTalonFX() { statusArray[2] = climbTalonConfig.apply(Constants.Climb.OPEN_LOOP_RAMPS_CONFIGS); statusArray[3] = climbTalonConfig.apply(Constants.Climb.CLOSED_LOOP_RAMPS_CONFIGS); statusArray[4] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); - statusArray[5] = climbCANCoder.getConfigurator().apply(new CANcoderConfiguration().withMagnetSensor(Constants.Climb.CANCODER_CONFIG)); + statusArray[5] = + climbCANCoder + .getConfigurator() + .apply(new CANcoderConfiguration().withMagnetSensor(Constants.Climb.CANCODER_CONFIG)); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -130,9 +133,6 @@ public ClimbIOTalonFX() { cancoderSupplyVoltage, cancoderPositionRotations); - BaseStatusSignal.setUpdateFrequencyForAll( - 250.0, cancoderAbsolutePosition, cancoderVelocity, cancoderSupplyVoltage); - climbTalon.setPosition(0); } @@ -156,10 +156,8 @@ public void updateInputs(ClimbIOInputs inputs) { cancoderPositionRotations) .isOK(); - inputs.climbPositionRads = - Units.rotationsToRadians(climbPosition.getValueAsDouble()); - inputs.climbVelocityRadsPerSec = - Units.rotationsToRadians(climbVelocity.getValueAsDouble()); + inputs.climbPositionRads = Units.rotationsToRadians(climbPosition.getValueAsDouble()); + inputs.climbVelocityRadsPerSec = Units.rotationsToRadians(climbVelocity.getValueAsDouble()); inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); inputs.climbSupplyCurrentAmps = climbSupplyCurrent.getValueAsDouble(); inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); @@ -171,7 +169,8 @@ public void updateInputs(ClimbIOInputs inputs) { inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); inputs.climbPositionCancoder = - Units.rotationsToRadians(inputs.cancoderPositionRotations) / Constants.Climb.CANCODER_TO_MECHANISM; + Units.rotationsToRadians(inputs.cancoderPositionRotations) + / Constants.Climb.CANCODER_TO_MECHANISM; } public void periodicUpdates() { @@ -212,7 +211,6 @@ public void stop() { @Override public void setPosition(double rads) { - climbTalon.setControl( - positionOut.withPosition(Units.radiansToRotations(rads))); + climbTalon.setControl(positionOut.withPosition(Units.radiansToRotations(rads))); } } From 99958f97b1d2620af558fb43fa522c45134ffdd2 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 16:39:35 -0800 Subject: [PATCH 30/39] nitpick fixes --- .../org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index a6902c27..8f0a3707 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -173,13 +173,14 @@ public void updateInputs(ClimbIOInputs inputs) { / Constants.Climb.CANCODER_TO_MECHANISM; } + @Override public void periodicUpdates() { updateLoggedTunableNumbers(); } private void updateLoggedTunableNumbers() { LoggedTunableNumber.ifChanged( - 0, + hashCode(), () -> { slot0Configs.kP = kP.get(); slot0Configs.kI = kI.get(); From d1f3275d070f9e0c33897f976746889cd7498878 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 21 Feb 2026 16:47:32 -0800 Subject: [PATCH 31/39] nitpick fixes again --- .../org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 8f0a3707..d8ebbe40 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -74,7 +74,7 @@ public class ClimbIOTalonFX implements ClimbIO { public ClimbIOTalonFX() { climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); - climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID); + climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID, new CANBus(Constants.Climb.BUS)); slot0Configs = new Slot0Configs(); slot0Configs.kP = kP.get(); From 6eb6fd63c474c980991a616bb1e1b9c1d28da032 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Thu, 26 Feb 2026 19:59:13 -0800 Subject: [PATCH 32/39] added todos --- src/main/java/org/team5924/frc2026/Constants.java | 4 ++-- .../java/org/team5924/frc2026/subsystems/climb/Climb.java | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index e6360fec..3ea6b30c 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -231,7 +231,7 @@ public final class Indexer { //TODO: update these later public final class Climb { // TODO: update these values public static final int CAN_ID = 60; public static final String BUS = "rio"; - public static final double MOTOR_TO_CANCODER = (7.0 / 1.0); // TODO: Update Reductions + public static final double MOTOR_TO_CANCODER = (7.0 / 1.0); // TODO: Update Reductions and add hook to reduction public static final double CANCODER_TO_MECHANISM = (7.0 / 1.0); public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM; public static final double SIM_MOI = 0.001; @@ -282,7 +282,7 @@ public final class Climb { // TODO: update these values public static final MagnetSensorConfigs CANCODER_CONFIG = new MagnetSensorConfigs() - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET) + .withMagnetOffset(1.0 * CANCODER_ABSOLUTE_OFFSET) .withAbsoluteSensorDiscontinuityPoint(0.5) .withSensorDirection(SensorDirectionValue.Clockwise_Positive); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 6c86a6d9..e33f7a5b 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -35,7 +35,7 @@ public class Climb extends SubsystemBase { private final ClimbIO io; private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); - public enum ClimbState { + public enum ClimbState { // TODO: Update climb level values - distance not rads!!! STOW(new LoggedTunableNumber("Climb/Stow", 0)), LEVEL_ONE(new LoggedTunableNumber("Climb/LevelOne", 0)), LEVEL_TWO(new LoggedTunableNumber("Climb/LevelTwo", 0)), @@ -47,6 +47,8 @@ public enum ClimbState { // voltage at which the climb subsystem motor moves when controlled by the operator OPERATOR_CONTROL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); + // TODO: add distance to rads method + private final DoubleSupplier rads; ClimbState(DoubleSupplier rads) { @@ -90,7 +92,6 @@ public void periodic() { handleCurrentState(); - // prevents error spam if (!inputs.climbMotorConnected && wasClimbMotorConnected) { Elastic.sendNotification(climbMotorDisconnectedNotification); } From 3e39ec9c9b3e96c5c0e5ebb0c65097deb9d879a7 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Thu, 26 Feb 2026 20:10:27 -0800 Subject: [PATCH 33/39] merge conflicts --- .../java/org/team5924/frc2026/Constants.java | 34 +++++++++++++++++++ .../java/org/team5924/frc2026/RobotState.java | 1 - .../frc2026/generated/TunerConstants.java | 2 +- .../shooterHood/ShooterHoodIOTalonFX.java | 4 +-- 4 files changed, 37 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 80a1d0ee..a161d780 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -429,6 +429,40 @@ public final class TurretRight { new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); + public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(0.02) + .withTorqueOpenLoopRampPeriod(0.02) + .withVoltageOpenLoopRampPeriod(0.02); + + public static final ClosedLoopRampsConfigs CLOSED_LOOP_RAMPS_CONFIGS = + new ClosedLoopRampsConfigs() + .withDutyCycleClosedLoopRampPeriod(0.02) + .withTorqueClosedLoopRampPeriod(0.02) + .withVoltageClosedLoopRampPeriod(0.02); + + public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold( + MIN_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor? rotations + .withReverseSoftLimitThreshold( + MAX_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor? rotations + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true); + + public static final FeedbackConfigs FEEDBACK_CONFIGS = + new FeedbackConfigs() + .withFeedbackRemoteSensorID(CANCODER_ID) + .withFeedbackRotorOffset(CANCODER_ABSOLUTE_OFFSET) + .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) + .withRotorToSensorRatio(MOTOR_TO_CANCODER) + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); + + public static final MagnetSensorConfigs CANCODER_CONFIG = + new MagnetSensorConfigs() + .withMagnetOffset(-CANCODER_ABSOLUTE_OFFSET) // TODO: update offset -> when the turret is facing forward (units: rotations) + .withAbsoluteSensorDiscontinuityPoint(0.5) + .withSensorDirection(SensorDirectionValue.Clockwise_Positive); } public final class Climb { // TODO: update these values diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 249d3970..688c27c2 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -24,7 +24,6 @@ import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.subsystems.SuperShooter.ShooterState; import org.team5924.frc2026.subsystems.climb.Climb.ClimbState; -import org.team5924.frc2026.subsystems.exampleSystem.ExampleSystem.ExampleSystemState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; diff --git a/src/main/java/org/team5924/frc2026/generated/TunerConstants.java b/src/main/java/org/team5924/frc2026/generated/TunerConstants.java index 9e6eb0e2..70a45b75 100644 --- a/src/main/java/org/team5924/frc2026/generated/TunerConstants.java +++ b/src/main/java/org/team5924/frc2026/generated/TunerConstants.java @@ -336,4 +336,4 @@ public TunerSwerveDrivetrain( modules); } } -} \ No newline at end of file +} diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 7f47e9e5..7f81b861 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -49,8 +49,8 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { // Single shot for voltage mode, robot loop will call continuously private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final PositionVoltage positionOut = - new PositionVoltage(0).withEnableFOC(true); + private final PositionVoltage positionOut = new PositionVoltage(0).withEnableFOC(true); + public ShooterHoodIOTalonFX(boolean isLeft) { reduction = isLeft ? Constants.ShooterHoodLeft.REDUCTION : Constants.ShooterHoodRight.REDUCTION; // cancoderToMechanism = isLeft ? Constants.ShooterHoodLeft.CANCODER_TO_MECHANISM : From 6e3eed08425e0bf0720fca32482f6070d892f840 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 28 Feb 2026 12:11:21 -0800 Subject: [PATCH 34/39] fixed stuff and configs and handle curent state --- .../java/org/team5924/frc2026/Constants.java | 9 ++- .../frc2026/subsystems/climb/Climb.java | 63 ++++++++++++------- .../frc2026/subsystems/climb/ClimbIO.java | 3 + .../subsystems/climb/ClimbIOTalonFX.java | 32 +++++++++- 4 files changed, 77 insertions(+), 30 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index a161d780..eb4bb4c4 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -21,15 +21,12 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; @@ -479,7 +476,9 @@ public final class Climb { // TODO: update these values public static final double MIN_POSITION_RADS = -Math.PI * MIN_POSITION_MULTI; public static final double MAX_POSITION_RADS = Math.PI * MAX_POSITION_MULTI; - public static final double JOYSTICK_DEADZONE = 0.01; + + public static final double EPSILON_RADS = Units.degreesToRadians(2.0); + public static final double JOYSTICK_DEADZONE = 0.05; public static final double STATE_TIMEOUT = 5.0; @@ -497,7 +496,7 @@ public final class Climb { // TODO: update these values .withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); - public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = + public static final OpenLoopRampsConfigs OPEN_LOOP_RAMPS_CONFIGS = new OpenLoopRampsConfigs() .withDutyCycleOpenLoopRampPeriod(0.02) .withTorqueOpenLoopRampPeriod(0.02) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index e33f7a5b..177c1a73 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -21,13 +21,18 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import lombok.Getter; +import lombok.Setter; + import org.littletonrobotics.junction.Logger; +import org.team5924.frc2026.Constants; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.sensors.BeamBreakIO; import org.team5924.frc2026.subsystems.sensors.BeamBreakIOInputsAutoLogged; +import org.team5924.frc2026.subsystems.turret.Turret.TurretState; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; +import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; public class Climb extends SubsystemBase { @@ -35,8 +40,11 @@ public class Climb extends SubsystemBase { private final ClimbIO io; private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + @Setter private double input; + public enum ClimbState { // TODO: Update climb level values - distance not rads!!! STOW(new LoggedTunableNumber("Climb/Stow", 0)), + OFF(() -> 0.0), LEVEL_ONE(new LoggedTunableNumber("Climb/LevelOne", 0)), LEVEL_TWO(new LoggedTunableNumber("Climb/LevelTwo", 0)), LEVEL_THREE(new LoggedTunableNumber("Climb/LevelThree", 0)), @@ -45,11 +53,11 @@ public enum ClimbState { // TODO: Update climb level values - distance not rads! DROP(new LoggedTunableNumber("Climb/Drop", 0)), MOVING(() -> 0.0), // voltage at which the climb subsystem motor moves when controlled by the operator - OPERATOR_CONTROL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); + MANUAL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); // TODO: add distance to rads method - private final DoubleSupplier rads; + @Getter private final DoubleSupplier rads; ClimbState(DoubleSupplier rads) { this.rads = rads; @@ -62,6 +70,8 @@ public enum ClimbState { // TODO: Update climb level values - distance not rads! private final Notification climbMotorDisconnectedNotification; private boolean wasClimbMotorConnected = true; + private double lastStateChange = 0.0; + // Climb Beam Break private final BeamBreakIO beamBreakIO; private final BeamBreakIOInputsAutoLogged beamBreakInputs = new BeamBreakIOInputsAutoLogged(); @@ -99,15 +109,39 @@ public void periodic() { wasClimbMotorConnected = inputs.climbMotorConnected; } - public void runVolts(double volts) { - io.runVolts(volts); + public boolean isAtSetpoint() { + return RobotState.getTime() - lastStateChange < Constants.Climb.STATE_TIMEOUT + || EqualsUtil.epsilonEquals( + inputs.setpointRads, inputs.climbPositionRads, Constants.Climb.EPSILON_RADS); } + private void handleCurrentState() { + switch (RobotState.getInstance().getClimbState()) { + case MOVING -> { + if (isAtSetpoint()) RobotState.getInstance().setClimbState(goalState); + } + case MANUAL -> handleManualState(); + case OFF -> io.stop(); + default -> io.setPosition(goalState.rads.getAsDouble()); + } + } + + private void handleManualState() { + if (!goalState.equals(TurretState.MANUAL)) return; + + if (Math.abs(input) <= Constants.Climb.JOYSTICK_DEADZONE) { + io.runVolts(0); + return; + } + + io.runVolts(ClimbState.MANUAL.getRads().getAsDouble() * input); + } + public void setGoalState(ClimbState goalState) { this.goalState = goalState; switch (goalState) { - case OPERATOR_CONTROL: - RobotState.getInstance().setClimbState(ClimbState.OPERATOR_CONTROL); + case MANUAL: + RobotState.getInstance().setClimbState(ClimbState.MANUAL); break; case MOVING: DriverStation.reportError( @@ -117,22 +151,7 @@ public void setGoalState(ClimbState goalState) { RobotState.getInstance().setClimbState(goalState); break; } - } - - private void handleCurrentState() { - switch (goalState) { - case OPERATOR_CONTROL: - io.runVolts(goalState.rads.getAsDouble()); - break; - case MOVING: - // Transition state - no direct motor command - break; - - default: - // Closed-loop position handdled by io.setPosition(...) - io.setPosition(goalState.rads.getAsDouble()); - break; - } + lastStateChange = RobotState.getTime(); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java index 7603effe..dfc277b4 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java @@ -30,6 +30,9 @@ public static class ClimbIOInputs { public double climbTorqueCurrentAmps = 0.0; public double climbTempCelsius = 0.0; + public double setpointRads = 0.0; + public double acceleration = 0.0; + public boolean cancoderConnected = true; public double cancoderAbsolutePosition = 0.0; public double cancoderVelocity = 0.0; diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index d8ebbe40..a03f9d44 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -27,6 +27,7 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -46,6 +47,7 @@ public class ClimbIOTalonFX implements ClimbIO { private final TalonFXConfigurator climbTalonConfig; private final Slot0Configs slot0Configs; + private double setpointRads; private final LoggedTunableNumber kA = new LoggedTunableNumber("Climb/kA", 0.00); private final LoggedTunableNumber kS = new LoggedTunableNumber("Climb/kS", 0.13); @@ -67,12 +69,22 @@ public class ClimbIOTalonFX implements ClimbIO { private final StatusSignal cancoderSupplyVoltage; private final StatusSignal cancoderPositionRotations; + private final double cancoderToMechanism; + private final double motorToMechanism; + private final double minPositionRads; + private final double maxPositionRads; + // Single shot for voltage mode, robot loop will call continuously private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); private final PositionVoltage positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); public ClimbIOTalonFX() { + cancoderToMechanism = Constants.Climb.CANCODER_TO_MECHANISM; + motorToMechanism = Constants.Climb.MOTOR_TO_MECHANISM; + minPositionRads = Constants.Climb.MIN_POSITION_MULTI; + maxPositionRads = Constants.Climb.MAX_POSITION_MULTI; + climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID, new CANBus(Constants.Climb.BUS)); @@ -163,14 +175,15 @@ public void updateInputs(ClimbIOInputs inputs) { inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); + inputs.setpointRads = setpointRads; + inputs.cancoderAbsolutePosition = cancoderAbsolutePosition.getValueAsDouble(); inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); inputs.climbPositionCancoder = - Units.rotationsToRadians(inputs.cancoderPositionRotations) - / Constants.Climb.CANCODER_TO_MECHANISM; + Units.rotationsToRadians(inputs.cancoderPositionRotations) / cancoderToMechanism; } @Override @@ -212,6 +225,19 @@ public void stop() { @Override public void setPosition(double rads) { - climbTalon.setControl(positionOut.withPosition(Units.radiansToRotations(rads))); + setpointRads = clampRads(rads); + climbTalon.setControl(positionOut.withPosition(Units.radiansToRotations(setpointRads))); + } + + private double clampRads(double rads) { + return MathUtil.clamp(rads, minPositionRads, maxPositionRads); + } + + private double radsToMotorPosition(double rads) { + return Units.radiansToRotations(rads * motorToMechanism); + } + + private double motorPositionToRads(double motorPosition) { + return Units.rotationsToRadians(motorPosition / motorToMechanism); } } From 2b316549a15410ac5a9ec7492a2155c9a3c2fc3c Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 28 Feb 2026 13:12:57 -0800 Subject: [PATCH 35/39] implemented motion magic and fixed inputs --- .../java/org/team5924/frc2026/Constants.java | 6 +- .../frc2026/subsystems/climb/ClimbIO.java | 6 + .../subsystems/climb/ClimbIOTalonFX.java | 114 +++++++++++++++--- 3 files changed, 109 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index a7fd516d..672ed976 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -577,8 +577,8 @@ public final class Climb { // TODO: update these values public static final double MIN_POSITION_MULTI = 0.8; // rotations public static final double MAX_POSITION_MULTI = 0.8; // rotations - public static final double MIN_POSITION_RADS = -Math.PI * MIN_POSITION_MULTI; - public static final double MAX_POSITION_RADS = Math.PI * MAX_POSITION_MULTI; + public static final double MIN_POSITION_RADS = -2 * Math.PI * MIN_POSITION_MULTI; + public static final double MAX_POSITION_RADS = 2 * Math.PI * MAX_POSITION_MULTI; public static final double EPSILON_RADS = Units.degreesToRadians(2.0); @@ -623,7 +623,7 @@ public final class Climb { // TODO: update these values public static final MagnetSensorConfigs CANCODER_CONFIG = new MagnetSensorConfigs() .withMagnetOffset(1.0 * CANCODER_ABSOLUTE_OFFSET) - .withAbsoluteSensorDiscontinuityPoint(0.5) + .withAbsoluteSensorDiscontinuityPoint(1.0) .withSensorDirection(SensorDirectionValue.Clockwise_Positive); } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java index dfc277b4..545a101f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIO.java @@ -22,6 +22,7 @@ public interface ClimbIO { @AutoLog public static class ClimbIOInputs { public boolean climbMotorConnected = true; + public double climbPosition = 0.0; public double climbPositionRads = 0.0; public double climbPositionCancoder = 0.0; public double climbVelocityRadsPerSec = 0.0; @@ -30,6 +31,9 @@ public static class ClimbIOInputs { public double climbTorqueCurrentAmps = 0.0; public double climbTempCelsius = 0.0; + public double motionMagicVelocityTarget = 0.0; + public double motionMagicPositionTarget = 0.0; + public double setpointRads = 0.0; public double acceleration = 0.0; @@ -59,6 +63,8 @@ public default void runVolts(double volts) {} public default void setPosition(double rads) {} + public default void holdPosition(double rads) {} + /** stops the motor */ default void stop() {} } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index a03f9d44..0e5cfd91 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -21,8 +21,10 @@ import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.CANcoder; @@ -34,6 +36,8 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; + +import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; @@ -47,6 +51,7 @@ public class ClimbIOTalonFX implements ClimbIO { private final TalonFXConfigurator climbTalonConfig; private final Slot0Configs slot0Configs; + private final MotionMagicConfigs motionMagicConfigs; private double setpointRads; private final LoggedTunableNumber kA = new LoggedTunableNumber("Climb/kA", 0.00); @@ -57,6 +62,12 @@ public class ClimbIOTalonFX implements ClimbIO { private final LoggedTunableNumber kD = new LoggedTunableNumber("Climb/kD", 0.07); private final LoggedTunableNumber kG = new LoggedTunableNumber("Climb/kG", 0.33); + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("Climb/MotionCruiseVelocity", 90.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("Climb/MotionAcceleration", 900.0); + private final LoggedTunableNumber motionJerk = new LoggedTunableNumber("Turret/MotionJerk", 0.0); + private final StatusSignal climbPosition; private final StatusSignal climbVelocity; private final StatusSignal climbAppliedVoltage; @@ -69,15 +80,19 @@ public class ClimbIOTalonFX implements ClimbIO { private final StatusSignal cancoderSupplyVoltage; private final StatusSignal cancoderPositionRotations; + private final StatusSignal closedLoopReferenceSlope; + private double prevClosedLoopReferenceSlope = 0.0; + private double prevReferenceSlopeTimestamp = 0.0; + private final double cancoderToMechanism; private final double motorToMechanism; private final double minPositionRads; private final double maxPositionRads; // Single shot for voltage mode, robot loop will call continuously - private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); - private final PositionVoltage positionOut = - new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); + private final VoltageOut voltageOut; + private final PositionVoltage positionOut; + private final MotionMagicVoltage motionMagicVoltage; public ClimbIOTalonFX() { cancoderToMechanism = Constants.Climb.CANCODER_TO_MECHANISM; @@ -87,6 +102,8 @@ public ClimbIOTalonFX() { climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID, new CANBus(Constants.Climb.BUS)); + + climbTalonConfig = climbTalon.getConfigurator(); slot0Configs = new Slot0Configs(); slot0Configs.kP = kP.get(); @@ -97,17 +114,22 @@ public ClimbIOTalonFX() { slot0Configs.kA = kA.get(); slot0Configs.kG = kG.get(); - climbTalonConfig = climbTalon.getConfigurator(); + motionMagicConfigs = new MotionMagicConfigs(); + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); // Apply Configs - StatusCode[] statusArray = new StatusCode[6]; + StatusCode[] statusArray = new StatusCode[8]; statusArray[0] = climbTalonConfig.apply(Constants.Climb.CONFIG); - statusArray[1] = climbTalonConfig.apply(slot0Configs); - statusArray[2] = climbTalonConfig.apply(Constants.Climb.OPEN_LOOP_RAMPS_CONFIGS); - statusArray[3] = climbTalonConfig.apply(Constants.Climb.CLOSED_LOOP_RAMPS_CONFIGS); + statusArray[1] = climbTalonConfig.apply(Constants.Climb.OPEN_LOOP_RAMPS_CONFIGS); + statusArray[2] = climbTalonConfig.apply(Constants.Climb.CLOSED_LOOP_RAMPS_CONFIGS); + statusArray[3] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); statusArray[4] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); - statusArray[5] = + statusArray[5] = climbTalonConfig.apply(motionMagicConfigs); + statusArray[6] = climbTalonConfig.apply(slot0Configs); + statusArray[7] = climbCANCoder .getConfigurator() .apply(new CANcoderConfiguration().withMagnetSensor(Constants.Climb.CANCODER_CONFIG)); @@ -119,6 +141,8 @@ public ClimbIOTalonFX() { Elastic.sendNotification( new Notification(NotificationLevel.WARNING, "Climb Configs", "Error in climb configs!")); + Logger.recordOutput("Turret/InitConfReport", statusArray); + // Get select status signals and set update frequency climbPosition = climbTalon.getPosition(); climbVelocity = climbTalon.getVelocity(); @@ -132,6 +156,8 @@ public ClimbIOTalonFX() { cancoderSupplyVoltage = climbCANCoder.getSupplyVoltage(); cancoderPositionRotations = climbCANCoder.getPosition(); + closedLoopReferenceSlope = climbTalon.getClosedLoopReferenceSlope(); + BaseStatusSignal.setUpdateFrequencyForAll( 100.0, climbPosition, @@ -143,9 +169,16 @@ public ClimbIOTalonFX() { cancoderAbsolutePosition, cancoderVelocity, cancoderSupplyVoltage, - cancoderPositionRotations); + cancoderPositionRotations, + closedLoopReferenceSlope); + + voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); + positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); + motionMagicVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true).withSlot(0); + BaseStatusSignal.waitForAll(0.5, cancoderAbsolutePosition); climbTalon.setPosition(0); + climbCANCoder.setPosition(0.0); // TODO: Check if this value will always be zero } @Override @@ -157,7 +190,8 @@ public void updateInputs(ClimbIOInputs inputs) { climbAppliedVoltage, climbSupplyCurrent, climbTorqueCurrent, - climbTempCelsius) + climbTempCelsius, + closedLoopReferenceSlope) .isOK(); inputs.cancoderConnected = @@ -167,7 +201,8 @@ public void updateInputs(ClimbIOInputs inputs) { cancoderSupplyVoltage, cancoderPositionRotations) .isOK(); - + inputs.climbPosition = + BaseStatusSignal.getLatencyCompensatedValueAsDouble(climbPosition, climbVelocity); inputs.climbPositionRads = Units.rotationsToRadians(climbPosition.getValueAsDouble()); inputs.climbVelocityRadsPerSec = Units.rotationsToRadians(climbVelocity.getValueAsDouble()); inputs.climbAppliedVoltage = climbAppliedVoltage.getValueAsDouble(); @@ -175,8 +210,22 @@ public void updateInputs(ClimbIOInputs inputs) { inputs.climbTorqueCurrentAmps = climbTorqueCurrent.getValueAsDouble(); inputs.climbTempCelsius = climbTempCelsius.getValueAsDouble(); + inputs.motionMagicVelocityTarget = + motorPositionToRads(climbTalon.getClosedLoopReferenceSlope().getValueAsDouble()); + inputs.motionMagicPositionTarget = + motorPositionToRads(climbTalon.getClosedLoopReference().getValueAsDouble()); + inputs.setpointRads = setpointRads; + double currentTime = closedLoopReferenceSlope.getTimestamp().getTime(); + double timeDiff = currentTime - prevReferenceSlopeTimestamp; + if (timeDiff > 0.0) { + inputs.acceleration = + (inputs.motionMagicVelocityTarget - prevClosedLoopReferenceSlope) / timeDiff; + } + prevClosedLoopReferenceSlope = inputs.motionMagicVelocityTarget; + prevReferenceSlopeTimestamp = currentTime; + inputs.cancoderAbsolutePosition = cancoderAbsolutePosition.getValueAsDouble(); inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); @@ -202,7 +251,17 @@ private void updateLoggedTunableNumbers() { slot0Configs.kV = kV.get(); slot0Configs.kA = kA.get(); slot0Configs.kG = kG.get(); - climbTalon.getConfigurator().apply(slot0Configs); + + StatusCode statusCode = climbTalon.getConfigurator().apply(slot0Configs); + if (!statusCode.isOK()) { + Elastic.sendNotification( + new Notification( + NotificationLevel.WARNING, + "Climb Slot 0 Configs", + "Error in periodically updating climb Slot0 configs!")); + + Logger.recordOutput("Climb/UpdateSlot0Report", statusCode); + } }, kP, kI, @@ -211,6 +270,28 @@ private void updateLoggedTunableNumbers() { kV, kA, kG); + + LoggedTunableNumber.ifChanged( + 0, + () -> { + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); + + StatusCode statusCode = climbTalon.getConfigurator().apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Elastic.sendNotification( + new Notification( + NotificationLevel.WARNING, + "Climb Motion Magic Configs", + "Error in periodically updating climb MotionMagic configs!")); + + Logger.recordOutput("Climb/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); } @Override @@ -226,7 +307,12 @@ public void stop() { @Override public void setPosition(double rads) { setpointRads = clampRads(rads); - climbTalon.setControl(positionOut.withPosition(Units.radiansToRotations(setpointRads))); + climbTalon.setControl(motionMagicVoltage.withPosition(radsToMotorPosition(setpointRads))); + } + + @Override + public void holdPosition(double rads) { + climbTalon.setControl(positionOut.withPosition(radsToMotorPosition(rads))); } private double clampRads(double rads) { From 4cb8d0dde0013a4358004e38d54c0c24fb271153 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 28 Feb 2026 14:13:15 -0800 Subject: [PATCH 36/39] implemented distance to radian conversion --- .../java/org/team5924/frc2026/Constants.java | 13 +++++++ .../frc2026/subsystems/climb/Climb.java | 39 ++++++++++++------- .../subsystems/climb/ClimbIOTalonFX.java | 6 +-- 3 files changed, 42 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 672ed976..288d75ee 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -572,6 +572,10 @@ public final class Climb { // TODO: update these values public static final double MOTOR_TO_CANCODER = (7.0 / 1.0); // TODO: Update Reductions and add hook to reduction public static final double CANCODER_TO_MECHANISM = (7.0 / 1.0); public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM; + + public static final double DRUM_CORE_RADIUS_METERS = 0.02; //TODO: Update with physical values from cad/robot + public static final double ROPE_THICKNESS_METERS = 0.005; + public static final double SIM_MOI = 0.001; public static final double MIN_POSITION_MULTI = 0.8; // rotations @@ -612,6 +616,15 @@ public final class Climb { // TODO: update these values .withTorqueClosedLoopRampPeriod(0.02) .withVoltageClosedLoopRampPeriod(0.02); + public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitThreshold( + MIN_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor? rotations + .withReverseSoftLimitThreshold( + MAX_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor? rotations + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true); + public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() .withFeedbackRemoteSensorID(CANCODER_ID) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 177c1a73..99e1adfb 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -54,13 +54,11 @@ public enum ClimbState { // TODO: Update climb level values - distance not rads! MOVING(() -> 0.0), // voltage at which the climb subsystem motor moves when controlled by the operator MANUAL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); + + @Getter private final DoubleSupplier distance; - // TODO: add distance to rads method - - @Getter private final DoubleSupplier rads; - - ClimbState(DoubleSupplier rads) { - this.rads = rads; + ClimbState(DoubleSupplier distance) { + this.distance = distance; } } @@ -96,7 +94,7 @@ public void periodic() { Logger.recordOutput("Climb/GoalState", goalState.toString()); Logger.recordOutput("Climb/CurrentState", RobotState.getInstance().getClimbState().toString()); - Logger.recordOutput("Climb/TargetRads", goalState.rads.getAsDouble()); + Logger.recordOutput("Climb/TargetRads", distanceToRadians(goalState.distance.getAsDouble())); climbMotorDisconnected.set(!inputs.climbMotorConnected); @@ -110,7 +108,7 @@ public void periodic() { } public boolean isAtSetpoint() { - return RobotState.getTime() - lastStateChange < Constants.Climb.STATE_TIMEOUT + return RobotState.getTime() - lastStateChange > Constants.Climb.STATE_TIMEOUT || EqualsUtil.epsilonEquals( inputs.setpointRads, inputs.climbPositionRads, Constants.Climb.EPSILON_RADS); } @@ -122,23 +120,22 @@ private void handleCurrentState() { } case MANUAL -> handleManualState(); case OFF -> io.stop(); - default -> io.setPosition(goalState.rads.getAsDouble()); + default -> io.setPosition(distanceToRadians(goalState.distance.getAsDouble())); } } private void handleManualState() { - if (!goalState.equals(TurretState.MANUAL)) return; + if (!goalState.equals(ClimbState.MANUAL)) return; if (Math.abs(input) <= Constants.Climb.JOYSTICK_DEADZONE) { io.runVolts(0); return; } - io.runVolts(ClimbState.MANUAL.getRads().getAsDouble() * input); + io.runVolts(distanceToRadians(ClimbState.MANUAL.getDistance().getAsDouble()) * input); } public void setGoalState(ClimbState goalState) { - this.goalState = goalState; switch (goalState) { case MANUAL: RobotState.getInstance().setClimbState(ClimbState.MANUAL); @@ -146,12 +143,28 @@ public void setGoalState(ClimbState goalState) { case MOVING: DriverStation.reportError( "Climb: MOVING is an invalid goal state; it is a transition state!!", null); - break; + return; default: RobotState.getInstance().setClimbState(goalState); break; } + this.goalState = goalState; lastStateChange = RobotState.getTime(); } + + private double distanceToRadians(double distance) { + double r = Constants.Climb.DRUM_CORE_RADIUS_METERS; + double t = Constants.Climb.ROPE_THICKNESS_METERS; + + double a = t / (4.0 * Math.PI); + double b = r; + double c = -distance; + + double discriminant = b * b - 4 * a * c; + + if (discriminant < 0) return 0; + + return (-b + Math.sqrt(discriminant)) / (2 * a); + } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 0e5cfd91..2bcd3fd6 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -97,8 +97,8 @@ public class ClimbIOTalonFX implements ClimbIO { public ClimbIOTalonFX() { cancoderToMechanism = Constants.Climb.CANCODER_TO_MECHANISM; motorToMechanism = Constants.Climb.MOTOR_TO_MECHANISM; - minPositionRads = Constants.Climb.MIN_POSITION_MULTI; - maxPositionRads = Constants.Climb.MAX_POSITION_MULTI; + minPositionRads = Constants.Climb.MIN_POSITION_RADS; + maxPositionRads = Constants.Climb.MAX_POSITION_RADS; climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID, new CANBus(Constants.Climb.BUS)); @@ -125,7 +125,7 @@ public ClimbIOTalonFX() { statusArray[0] = climbTalonConfig.apply(Constants.Climb.CONFIG); statusArray[1] = climbTalonConfig.apply(Constants.Climb.OPEN_LOOP_RAMPS_CONFIGS); statusArray[2] = climbTalonConfig.apply(Constants.Climb.CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[3] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); + statusArray[3] = climbTalonConfig.apply(Constants.Climb.SOFTWARE_LIMIT_CONFIGS); statusArray[4] = climbTalonConfig.apply(Constants.Climb.FEEDBACK_CONFIGS); statusArray[5] = climbTalonConfig.apply(motionMagicConfigs); statusArray[6] = climbTalonConfig.apply(slot0Configs); From 6ef202025c140047dc749d8e2fe73b8a0da7c3d9 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 28 Feb 2026 14:30:10 -0800 Subject: [PATCH 37/39] fixes to distance --- src/main/java/org/team5924/frc2026/Constants.java | 4 ++-- .../java/org/team5924/frc2026/subsystems/climb/Climb.java | 2 +- .../org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 288d75ee..1933ecc7 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -619,9 +619,9 @@ public final class Climb { // TODO: update these values public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitThreshold( - MIN_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor? rotations + MAX_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor rotations .withReverseSoftLimitThreshold( - MAX_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor? rotations + -MIN_POSITION_MULTI * MOTOR_TO_MECHANISM) // motor rotations .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true); diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 99e1adfb..c267f1bf 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -132,7 +132,7 @@ private void handleManualState() { return; } - io.runVolts(distanceToRadians(ClimbState.MANUAL.getDistance().getAsDouble()) * input); + io.runVolts(ClimbState.MANUAL.getDistance().getAsDouble() * input); } public void setGoalState(ClimbState goalState) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 2bcd3fd6..4e6d93e7 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -66,7 +66,7 @@ public class ClimbIOTalonFX implements ClimbIO { new LoggedTunableNumber("Climb/MotionCruiseVelocity", 90.0); private final LoggedTunableNumber motionAcceleration = new LoggedTunableNumber("Climb/MotionAcceleration", 900.0); - private final LoggedTunableNumber motionJerk = new LoggedTunableNumber("Turret/MotionJerk", 0.0); + private final LoggedTunableNumber motionJerk = new LoggedTunableNumber("Climb/MotionJerk", 0.0); private final StatusSignal climbPosition; private final StatusSignal climbVelocity; @@ -272,7 +272,7 @@ private void updateLoggedTunableNumbers() { kG); LoggedTunableNumber.ifChanged( - 0, + hashCode(), () -> { motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); From bf7e3ea92b64d1ae568d2a50f0d686bb9bbe92f7 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 28 Feb 2026 17:31:42 -0800 Subject: [PATCH 38/39] nitpick fixes --- src/main/java/org/team5924/frc2026/Constants.java | 4 +++- .../java/org/team5924/frc2026/subsystems/climb/Climb.java | 3 ++- .../org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 1933ecc7..02bcf758 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -598,7 +598,9 @@ public final class Climb { // TODO: update these values .withCurrentLimits( new CurrentLimitsConfigs() .withSupplyCurrentLimit(60) - .withStatorCurrentLimit(60)) + .withStatorCurrentLimit(60) + .withSupplyCurrentLimitEnable(true) + .withStatorCurrentLimitEnable(true)) .withMotorOutput( new MotorOutputConfigs() .withInverted(InvertedValue.CounterClockwise_Positive) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index c267f1bf..720b969a 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -116,6 +116,7 @@ public boolean isAtSetpoint() { private void handleCurrentState() { switch (RobotState.getInstance().getClimbState()) { case MOVING -> { + io.setPosition(distanceToRadians(goalState.distance.getAsDouble())); if (isAtSetpoint()) RobotState.getInstance().setClimbState(goalState); } case MANUAL -> handleManualState(); @@ -145,7 +146,7 @@ public void setGoalState(ClimbState goalState) { "Climb: MOVING is an invalid goal state; it is a transition state!!", null); return; default: - RobotState.getInstance().setClimbState(goalState); + RobotState.getInstance().setClimbState(ClimbState.MOVING); break; } this.goalState = goalState; diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 4e6d93e7..317b57e5 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -173,7 +173,7 @@ public ClimbIOTalonFX() { closedLoopReferenceSlope); voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); - positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); + positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true).withSlot(0); motionMagicVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true).withSlot(0); BaseStatusSignal.waitForAll(0.5, cancoderAbsolutePosition); From 3a2406ba8692fdf051dc5b2460b7041885743410 Mon Sep 17 00:00:00 2001 From: Michael Willson Date: Sat, 28 Feb 2026 17:47:48 -0800 Subject: [PATCH 39/39] fixed off mode --- .../org/team5924/frc2026/subsystems/climb/Climb.java | 9 +++++---- .../frc2026/subsystems/climb/ClimbIOTalonFX.java | 7 +++---- .../pivots/shooterHood/ShooterHoodIOTalonFX.java | 1 - 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java index 720b969a..27790f8e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/Climb.java @@ -22,13 +22,11 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.Setter; - import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.sensors.BeamBreakIO; import org.team5924.frc2026.subsystems.sensors.BeamBreakIOInputsAutoLogged; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -54,7 +52,7 @@ public enum ClimbState { // TODO: Update climb level values - distance not rads! MOVING(() -> 0.0), // voltage at which the climb subsystem motor moves when controlled by the operator MANUAL(new LoggedTunableNumber("Climb/OperatorVoltage", 4.5)); - + @Getter private final DoubleSupplier distance; ClimbState(DoubleSupplier distance) { @@ -135,12 +133,15 @@ private void handleManualState() { io.runVolts(ClimbState.MANUAL.getDistance().getAsDouble() * input); } - + public void setGoalState(ClimbState goalState) { switch (goalState) { case MANUAL: RobotState.getInstance().setClimbState(ClimbState.MANUAL); break; + case OFF: + RobotState.getInstance().setClimbState(ClimbState.OFF); + break; case MOVING: DriverStation.reportError( "Climb: MOVING is an invalid goal state; it is a transition state!!", null); diff --git a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java index 317b57e5..303f8d0f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/climb/ClimbIOTalonFX.java @@ -36,7 +36,6 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; - import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.util.Elastic; @@ -102,7 +101,7 @@ public ClimbIOTalonFX() { climbTalon = new TalonFX(Constants.Climb.CAN_ID, new CANBus(Constants.Climb.BUS)); climbCANCoder = new CANcoder(Constants.Climb.CANCODER_ID, new CANBus(Constants.Climb.BUS)); - + climbTalonConfig = climbTalon.getConfigurator(); slot0Configs = new Slot0Configs(); @@ -141,7 +140,7 @@ public ClimbIOTalonFX() { Elastic.sendNotification( new Notification(NotificationLevel.WARNING, "Climb Configs", "Error in climb configs!")); - Logger.recordOutput("Turret/InitConfReport", statusArray); + Logger.recordOutput("Climb/InitConfReport", statusArray); // Get select status signals and set update frequency climbPosition = climbTalon.getPosition(); @@ -271,7 +270,7 @@ private void updateLoggedTunableNumbers() { kA, kG); - LoggedTunableNumber.ifChanged( + LoggedTunableNumber.ifChanged( hashCode(), () -> { motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 1f6ce905..45a9376c 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -384,5 +384,4 @@ private double radsToMotorPosition(double rads) { private double motorPositionToRads(double motorPosition) { return Units.rotationsToRadians(motorPosition) / motorToMechanism / mechanismRangePercent; } - }