From a39b9c5d4f0e5208d6594cb1c0430ea0e0bec033 Mon Sep 17 00:00:00 2001 From: ronansoergel9 Date: Tue, 27 Jan 2026 23:50:57 -0800 Subject: [PATCH 1/7] First pass at climber subsystem --- .../robot/subsystems/climber/ClimberIO.java | 132 ++++++++++++++++++ .../subsystems/climber/ClimberSubsystem.java | 52 ++++++- 2 files changed, 178 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIO.java diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..c79ecd6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,132 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.geometry.Rotation2d; +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.Frequency; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; + +public class ClimberIO { + + @AutoLog + public static class ClimberIOInputs { + public Rotation2d motorPositionRotations = new Rotation2d(); + public double motorVelocityMetersPerSec = 0.0; + public double motorStatorCurrentAmps = 0.0; + public double motorSupplyCurrentAmps = 0.0; + public double motorVoltage = 0.0; + public double motorTempC = 0.0; + } + + protected final TalonFX climberMotor; + + private final StatusSignal motorPosition; + private final StatusSignal angularVelocityRotsPerSec; + private final StatusSignal statorCurrentAmps; + private final StatusSignal supplyCurrentAmps; + private final StatusSignal motorVoltage; + private final StatusSignal motorTemp; + + private VoltageOut voltageOut = new VoltageOut(0.0) .withEnableFOC(true); + private PositionVoltage positionVoltage = new PositionVoltage(0.0) .withEnableFOC(true); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0) .withEnableFOC(true); + + +public ClimberIO(CANBus canBus) { + //todo: set correct motor ID + climberMotor = new TalonFX(30, canBus); + climberMotor.getConfigurator().apply(ClimberIO.getConfigurator()); + + angularVelocityRotsPerSec = climberMotor.getVelocity(); + supplyCurrentAmps = climberMotor.getSupplyCurrent(); + motorVoltage = climberMotor.getMotorVoltage(); + statorCurrentAmps = climberMotor.getStatorCurrent(); + motorTemp = climberMotor.getDeviceTemp(); + motorPosition = climberMotor.getPosition(); + + //complaining about frequency syntax for some reason + BaseStatusSignal.setUpdateFrequencyForAll( + frequencyHz:50.0, + angularVelocityRotsPerSec, + supplyCurrentAmps, + motorVoltage, + statorCurrentAmps, + motorTemp, + motorPosition + climberMotor.optimizeBusUtilization()); + } + +public static TalonFXConfiguration getClimberConfiguration() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + //todo: find and make climber gear ratio variable + config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO; + + //todo: tune + config.Slot0.kS = 0.0; + config.Slot0.KG = 0.0; + config.Slot0.kV = 0.0; + config.Slot0.kP = 0.0; + config.Slot0.kD = 0.0; + + //todo: find actual current limits + config.CurrentLimits.StatorCurrentLimit = 50.00; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 40.00; + + return config; +} + +public void setClimberPosition(Rotation2d climberPosition) { + climberMotor.setControl(positionVoltage.withPosition(climberPosition.getRotations())); +} + +public void setClimberVoltage(double climberVoltage) { + climberMotor.setControl(voltageOut.withOutput(climberVoltage)); +} + +public void setClimberVelocity(double climberVelocity) { + climberMotor.setControl(velocityVoltage.withVelocity(climberVelocity)); +} + +public void updateInputs(ClimberIOInputs inputs) { + BaseStatusSignal.refreshAll( + motorPosition, + angularVelocityRotsPerSec, + statorCurrentAmps, + supplyCurrentAmps, + motorVoltage, + motorTemp); + + inputs.motorPositionRotations = + Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorVoltage = motorVoltage.getValueAsDouble(); + inputs.motorTempC = motorTempC.getValueAsDouble(); + inputs.motorSupplyCurrentAmps = motorSupplyCurrentAmps.getValueAsDouble(); + inputs.motorStatorCurrentAmps = motorStatorCurrentAmps.getValueAsDouble(); + inputs.motorVelocityMetersPerSec = motorVelocityMetersPerSec.getValueAsDouble(); +} +} + + + + diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 2b5d698..37cc0ef 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,17 +1,57 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems.climber; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.components.rollers.RollerIO; +import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import org.littletonrobotics.junction.Logger; + + public class ClimberSubsystem extends SubsystemBase { - /** Creates a new ClimberSubsystem. */ + //todo: find actual constants + public static final double GEAR_RATIO = (45.0 / 1.0); + public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(180); + public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); + public static final double MAX_ACCELERATION = 10.0; + public static final double MAX_VELOCITY = 2.0; + +ClimberIO climberIO; +ClimberIOInputsAutoLogged ClimberIOInputs = new ClimberIOInputsAutoLogged(); + public ClimberSubsystem() {} @Override public void periodic() { - // This method will be called once per scheduler run + climberIO.updateInputs(climberInputs); } + +//member variables here? + +public ClimberSubsystem(ClimberIO climberIO) { + this.climberIO = climberIO; +} + +//not sure about these implementations, some issues with "static reference to non-static method" +public Command climbUp() { + return this.run( + () -> { + ClimberIO.setClimberPosition(MAX_ANGLE) + }); + +} + +public Command climbDown() { + return this.run( + () -> { + ClimberIO.setClimberPosition(MIN_ANGLE) + }); + +} + } From bf552b2b2f80ad11d5c85367934399e837bd8cd8 Mon Sep 17 00:00:00 2001 From: ronansoergel9 Date: Wed, 28 Jan 2026 16:37:07 -0800 Subject: [PATCH 2/7] un-chopped --- .../robot/subsystems/climber/ClimberIO.java | 35 +++++++++++-------- .../subsystems/climber/ClimberSubsystem.java | 17 ++++----- 2 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index c79ecd6..d1ce283 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -12,14 +12,17 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import org.littletonrobotics.junction.AutoLogOutput; import edu.wpi.first.math.geometry.Rotation2d; 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.Frequency; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Frequency; +import frc.robot.subsystems.shooter.HoodIO; public class ClimberIO { @@ -35,7 +38,7 @@ public static class ClimberIOInputs { protected final TalonFX climberMotor; - private final StatusSignal motorPosition; + private final StatusSignal motorPositionRotations; private final StatusSignal angularVelocityRotsPerSec; private final StatusSignal statorCurrentAmps; private final StatusSignal supplyCurrentAmps; @@ -46,29 +49,30 @@ public static class ClimberIOInputs { private PositionVoltage positionVoltage = new PositionVoltage(0.0) .withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0) .withEnableFOC(true); + private Rotation2d climberSetpoint = Rotation2d.kZero; + public ClimberIO(CANBus canBus) { //todo: set correct motor ID climberMotor = new TalonFX(30, canBus); - climberMotor.getConfigurator().apply(ClimberIO.getConfigurator()); + climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration()); angularVelocityRotsPerSec = climberMotor.getVelocity(); supplyCurrentAmps = climberMotor.getSupplyCurrent(); motorVoltage = climberMotor.getMotorVoltage(); statorCurrentAmps = climberMotor.getStatorCurrent(); motorTemp = climberMotor.getDeviceTemp(); - motorPosition = climberMotor.getPosition(); + motorPositionRotations = climberMotor.getPosition(); - //complaining about frequency syntax for some reason BaseStatusSignal.setUpdateFrequencyForAll( - frequencyHz:50.0, + 50.0, angularVelocityRotsPerSec, supplyCurrentAmps, motorVoltage, statorCurrentAmps, motorTemp, - motorPosition - climberMotor.optimizeBusUtilization()); + motorPositionRotations); + climberMotor.optimizeBusUtilization(); } public static TalonFXConfiguration getClimberConfiguration() { @@ -83,7 +87,7 @@ public static TalonFXConfiguration getClimberConfiguration() { //todo: tune config.Slot0.kS = 0.0; - config.Slot0.KG = 0.0; + config.Slot0.kG = 0.0; config.Slot0.kV = 0.0; config.Slot0.kP = 0.0; config.Slot0.kD = 0.0; @@ -97,6 +101,7 @@ public static TalonFXConfiguration getClimberConfiguration() { } public void setClimberPosition(Rotation2d climberPosition) { + climberSetpoint = climberPosition; climberMotor.setControl(positionVoltage.withPosition(climberPosition.getRotations())); } @@ -110,7 +115,7 @@ public void setClimberVelocity(double climberVelocity) { public void updateInputs(ClimberIOInputs inputs) { BaseStatusSignal.refreshAll( - motorPosition, + motorPositionRotations, angularVelocityRotsPerSec, statorCurrentAmps, supplyCurrentAmps, @@ -120,10 +125,10 @@ public void updateInputs(ClimberIOInputs inputs) { inputs.motorPositionRotations = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.motorVoltage = motorVoltage.getValueAsDouble(); - inputs.motorTempC = motorTempC.getValueAsDouble(); - inputs.motorSupplyCurrentAmps = motorSupplyCurrentAmps.getValueAsDouble(); - inputs.motorStatorCurrentAmps = motorStatorCurrentAmps.getValueAsDouble(); - inputs.motorVelocityMetersPerSec = motorVelocityMetersPerSec.getValueAsDouble(); + inputs.motorTempC = motorTemp.getValueAsDouble(); + inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); + inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble(); + inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble(); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 37cc0ef..b5c473c 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -15,20 +15,21 @@ public class ClimberSubsystem extends SubsystemBase { //todo: find actual constants - public static final double GEAR_RATIO = (45.0 / 1.0); - public static final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(180); - public static final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); - public static final double MAX_ACCELERATION = 10.0; - public static final double MAX_VELOCITY = 2.0; + public static double GEAR_RATIO = (45.0 / 1.0); + public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(180); + public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); + public static double MAX_ACCELERATION = 10.0; + public static double MAX_VELOCITY = 2.0; ClimberIO climberIO; -ClimberIOInputsAutoLogged ClimberIOInputs = new ClimberIOInputsAutoLogged(); +ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged(); public ClimberSubsystem() {} @Override public void periodic() { climberIO.updateInputs(climberInputs); + Logger.processInputs("Climber", climberInputs); } //member variables here? @@ -41,7 +42,7 @@ public ClimberSubsystem(ClimberIO climberIO) { public Command climbUp() { return this.run( () -> { - ClimberIO.setClimberPosition(MAX_ANGLE) + ClimberIO.setClimberPosition(MAX_ANGLE); }); } @@ -49,7 +50,7 @@ public Command climbUp() { public Command climbDown() { return this.run( () -> { - ClimberIO.setClimberPosition(MIN_ANGLE) + ClimberIO.setClimberPosition(MIN_ANGLE); }); } From 94d32436f5f760d1a22366cb8bb98231ca6a69d3 Mon Sep 17 00:00:00 2001 From: ronansoergel9 Date: Thu, 29 Jan 2026 15:55:38 -0800 Subject: [PATCH 3/7] fix static reference issue --- src/main/java/frc/robot/Robot.java | 5 +++-- .../frc/robot/subsystems/climber/ClimberSubsystem.java | 7 ++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..ee77706 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; +import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.LindexerSubsystem; @@ -260,7 +261,7 @@ public Robot() { indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); shooter = new TurretSubsystem(); - climber = new ClimberSubsystem(); // TODO climber + climber = new ClimberSubsystem(); break; } // now that we've assigned the correct subsystems based on robot edition, we can pass them into @@ -270,7 +271,7 @@ public Robot() { // this creates a placeholder "no-operation" climber that will just not do anything, but is not // null (and we need it to be not null) if (climber == null) - climber = new ClimberSubsystem(); // TODO new ClimberSubsystem(new ClimberIO() {}) and such + climber = new ClimberSubsystem(new ClimberIO(canivore) {}); DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index b5c473c..eb75606 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -24,8 +24,6 @@ public class ClimberSubsystem extends SubsystemBase { ClimberIO climberIO; ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged(); - public ClimberSubsystem() {} - @Override public void periodic() { climberIO.updateInputs(climberInputs); @@ -38,11 +36,10 @@ public ClimberSubsystem(ClimberIO climberIO) { this.climberIO = climberIO; } -//not sure about these implementations, some issues with "static reference to non-static method" public Command climbUp() { return this.run( () -> { - ClimberIO.setClimberPosition(MAX_ANGLE); + climberIO.setClimberPosition(MAX_ANGLE); }); } @@ -50,7 +47,7 @@ public Command climbUp() { public Command climbDown() { return this.run( () -> { - ClimberIO.setClimberPosition(MIN_ANGLE); + climberIO.setClimberPosition(MIN_ANGLE); }); } From e95044c8270dd62d1eca8257d303dc89fd594bc0 Mon Sep 17 00:00:00 2001 From: ronansoergel9 Date: Tue, 3 Feb 2026 00:18:26 -0800 Subject: [PATCH 4/7] Empty climber class + other updates --- src/main/java/frc/robot/Robot.java | 4 +- .../robot/subsystems/climber/ClimberIO.java | 8 ++-- .../subsystems/climber/ClimberIOSim.java | 38 +++++++++++++++++++ .../subsystems/climber/ClimberSubsystem.java | 15 +++++--- .../climber/EmptyClimberSubsystem.java | 23 +++++++++++ 5 files changed, 77 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ee77706..aa067cc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -261,7 +261,7 @@ public Robot() { indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); shooter = new TurretSubsystem(); - climber = new ClimberSubsystem(); + climber = new ClimberSubsystem(new ClimberIO(canivore)); break; } // now that we've assigned the correct subsystems based on robot edition, we can pass them into @@ -271,7 +271,7 @@ public Robot() { // this creates a placeholder "no-operation" climber that will just not do anything, but is not // null (and we need it to be not null) if (climber == null) - climber = new ClimberSubsystem(new ClimberIO(canivore) {}); + //climber = new EmptyClimberSubsystem(new ClimberIO(canivore) {}); DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index d1ce283..4a6be34 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -28,7 +28,7 @@ public class ClimberIO { @AutoLog public static class ClimberIOInputs { - public Rotation2d motorPositionRotations = new Rotation2d(); + public double climberPosition = 0.0; public double motorVelocityMetersPerSec = 0.0; public double motorStatorCurrentAmps = 0.0; public double motorSupplyCurrentAmps = 0.0; @@ -49,7 +49,7 @@ public static class ClimberIOInputs { private PositionVoltage positionVoltage = new PositionVoltage(0.0) .withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0) .withEnableFOC(true); - private Rotation2d climberSetpoint = Rotation2d.kZero; + private double climberSetpoint = 0.0; public ClimberIO(CANBus canBus) { @@ -100,9 +100,9 @@ public static TalonFXConfiguration getClimberConfiguration() { return config; } -public void setClimberPosition(Rotation2d climberPosition) { +public void setClimberPosition(double climberPosition) { climberSetpoint = climberPosition; - climberMotor.setControl(positionVoltage.withPosition(climberPosition.getRotations())); + climberMotor.setControl(positionVoltage.withPosition(climberSetpoint)); } public void setClimberVoltage(double climberVoltage) { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java new file mode 100644 index 0000000..274b8e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +//unfinished + +public class ClimberIOSim extends ClimberIO { + TalonFXSimState climberMotorSim; + +private final DCMotorSim climberPhysicsSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.01, ClimberSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)); + +private final double simLoopPeriod = 0.002; +private Notifier simNotifier = null; +private double lastSimTime = 0.0; + +public HoodIOSim(CANBus canbus) { + super(ClimberIO.getClimberConfiguration(), canbus); + climberMotorSim = climberMotor.getSimState(); + climberMotorSim.setMotorType(MotorType.KrakenX60); + + +} + + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index eb75606..82a0bee 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -16,8 +16,7 @@ public class ClimberSubsystem extends SubsystemBase { //todo: find actual constants public static double GEAR_RATIO = (45.0 / 1.0); - public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(180); - public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); + public static double MAX_EXTENSION_METERS = 0.2413; public static double MAX_ACCELERATION = 10.0; public static double MAX_VELOCITY = 2.0; @@ -39,15 +38,21 @@ public ClimberSubsystem(ClimberIO climberIO) { public Command climbUp() { return this.run( () -> { - climberIO.setClimberPosition(MAX_ANGLE); + climberIO.setClimberPosition(MAX_EXTENSION_METERS); + wait(1000); + climberIO.setClimberPosition(0.0); + // TODO figure out how to correctly implement }); } public Command climbDown() { - return this.run( + return this.run( () -> { - climberIO.setClimberPosition(MIN_ANGLE); + climberIO.setClimberPosition(MAX_EXTENSION_METERS); + wait(1000); + climberIO.setClimberPosition(0.0); + // TODO figure out how to correctly implement }); } diff --git a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java new file mode 100644 index 0000000..0eb60bb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.Command; + +import com.ctre.phoenix6.CANBus; + +public class EmptyClimberSubsystem extends ClimberSubsystem{ + + @Override + public void periodic(){} + +public EmptyClimberSubsystem(CANBus canbus) { + super(new ClimberIO(canbus)); +} + +public Command climbUp() { + return this.idle(); + } + +public Command climbDown() { + return this.idle(); + } +} From d82dcb5ff0fd5120ec07cd4d2b281b925276a706 Mon Sep 17 00:00:00 2001 From: ronansoergel9 Date: Sat, 7 Feb 2026 16:40:29 -0800 Subject: [PATCH 5/7] some more minor edits --- src/main/java/frc/robot/Robot.java | 4 +- .../robot/subsystems/climber/ClimberIO.java | 129 ++++++++---------- .../subsystems/climber/ClimberIOSim.java | 57 +++----- .../subsystems/climber/ClimberSubsystem.java | 60 +++----- .../climber/EmptyClimberSubsystem.java | 19 ++- 5 files changed, 112 insertions(+), 157 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aa067cc..4affc09 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -271,9 +271,9 @@ public Robot() { // this creates a placeholder "no-operation" climber that will just not do anything, but is not // null (and we need it to be not null) if (climber == null) - //climber = new EmptyClimberSubsystem(new ClimberIO(canivore) {}); + // climber = new EmptyClimberSubsystem(new ClimberIO(canivore) {}); - DriverStation.silenceJoystickConnectionWarning(true); + DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); RobotController.setBrownoutVoltage(6.0); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 4a6be34..88b1b93 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.climber; -import org.littletonrobotics.junction.AutoLog; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; @@ -12,51 +10,46 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.GravityTypeValue; -import org.littletonrobotics.junction.AutoLogOutput; - import edu.wpi.first.math.geometry.Rotation2d; 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 edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Frequency; -import frc.robot.subsystems.shooter.HoodIO; +import org.littletonrobotics.junction.AutoLog; public class ClimberIO { - @AutoLog - public static class ClimberIOInputs { - public double climberPosition = 0.0; - public double motorVelocityMetersPerSec = 0.0; - public double motorStatorCurrentAmps = 0.0; - public double motorSupplyCurrentAmps = 0.0; - public double motorVoltage = 0.0; - public double motorTempC = 0.0; - } - - protected final TalonFX climberMotor; - - private final StatusSignal motorPositionRotations; - private final StatusSignal angularVelocityRotsPerSec; - private final StatusSignal statorCurrentAmps; - private final StatusSignal supplyCurrentAmps; - private final StatusSignal motorVoltage; - private final StatusSignal motorTemp; - - private VoltageOut voltageOut = new VoltageOut(0.0) .withEnableFOC(true); - private PositionVoltage positionVoltage = new PositionVoltage(0.0) .withEnableFOC(true); - private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0) .withEnableFOC(true); - - private double climberSetpoint = 0.0; - - -public ClimberIO(CANBus canBus) { - //todo: set correct motor ID + @AutoLog + public static class ClimberIOInputs { + public Rotation2d motorPositionRotations = new Rotation2d(); + public double motorVelocityMetersPerSec = 0.0; + public double motorStatorCurrentAmps = 0.0; + public double motorSupplyCurrentAmps = 0.0; + public double motorVoltage = 0.0; + public double motorTempC = 0.0; + } + + protected final TalonFX climberMotor; + + private final StatusSignal motorPositionRotations; + private final StatusSignal angularVelocityRotsPerSec; + private final StatusSignal statorCurrentAmps; + private final StatusSignal supplyCurrentAmps; + private final StatusSignal motorVoltage; + private final StatusSignal motorTemp; + + private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); + private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); + private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + + private double climberSetpoint = 0.0; + + public ClimberIO(CANBus canBus) { + // todo: set correct motor ID climberMotor = new TalonFX(30, canBus); climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration()); - + angularVelocityRotsPerSec = climberMotor.getVelocity(); supplyCurrentAmps = climberMotor.getSupplyCurrent(); motorVoltage = climberMotor.getMotorVoltage(); @@ -73,65 +66,61 @@ public ClimberIO(CANBus canBus) { motorTemp, motorPositionRotations); climberMotor.optimizeBusUtilization(); - } + } -public static TalonFXConfiguration getClimberConfiguration() { + public static TalonFXConfiguration getClimberConfiguration() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - //todo: find and make climber gear ratio variable + // todo: find and make climber gear ratio variable config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO; - //todo: tune + // todo: tune config.Slot0.kS = 0.0; config.Slot0.kG = 0.0; config.Slot0.kV = 0.0; config.Slot0.kP = 0.0; config.Slot0.kD = 0.0; - //todo: find actual current limits + // todo: find actual current limits config.CurrentLimits.StatorCurrentLimit = 50.00; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 40.00; return config; -} + } -public void setClimberPosition(double climberPosition) { + public void setClimberPosition(double climberPosition) { climberSetpoint = climberPosition; climberMotor.setControl(positionVoltage.withPosition(climberSetpoint)); -} + } -public void setClimberVoltage(double climberVoltage) { + public void setClimberVoltage(double climberVoltage) { climberMotor.setControl(voltageOut.withOutput(climberVoltage)); -} + } -public void setClimberVelocity(double climberVelocity) { + public void setClimberVelocity(double climberVelocity) { climberMotor.setControl(velocityVoltage.withVelocity(climberVelocity)); -} + } -public void updateInputs(ClimberIOInputs inputs) { - BaseStatusSignal.refreshAll( - motorPositionRotations, - angularVelocityRotsPerSec, - statorCurrentAmps, - supplyCurrentAmps, - motorVoltage, - motorTemp); - - inputs.motorPositionRotations = - Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); - inputs.motorVoltage = motorVoltage.getValueAsDouble(); - inputs.motorTempC = motorTemp.getValueAsDouble(); - inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); - inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble(); - inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble(); -} + public void updateInputs(ClimberIOInputs inputs) { + BaseStatusSignal.refreshAll( + motorPositionRotations, + angularVelocityRotsPerSec, + statorCurrentAmps, + supplyCurrentAmps, + motorVoltage, + motorTemp); + + inputs.motorPositionRotations = + Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorVoltage = motorVoltage.getValueAsDouble(); + inputs.motorTempC = motorTemp.getValueAsDouble(); + inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); + inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble(); + inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble(); + } } - - - - diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 274b8e2..315d344 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -1,38 +1,23 @@ package frc.robot.subsystems.climber; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.sim.ChassisReference; -import com.ctre.phoenix6.sim.TalonFXSimState; -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; - -//unfinished - -public class ClimberIOSim extends ClimberIO { - TalonFXSimState climberMotorSim; - -private final DCMotorSim climberPhysicsSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.01, ClimberSubsystem.GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)); - -private final double simLoopPeriod = 0.002; -private Notifier simNotifier = null; -private double lastSimTime = 0.0; - -public HoodIOSim(CANBus canbus) { - super(ClimberIO.getClimberConfiguration(), canbus); - climberMotorSim = climberMotor.getSimState(); - climberMotorSim.setMotorType(MotorType.KrakenX60); - - -} - - -} +// unfinished + +// public class ClimberIOSim extends ClimberIO { +// TalonFXSimState climberMotorSim; + +// private final DCMotorSim climberPhysicsSim = +// new DCMotorSim( +// LinearSystemId.createDCMotorSystem( +// DCMotor.getKrakenX44Foc(1), 0.01, ClimberSubsystem.GEAR_RATIO), +// DCMotor.getKrakenX44Foc(1)); + +// private final double simLoopPeriod = 0.002; +// private Notifier simNotifier = null; +// private double lastSimTime = 0.0; + +// public ClimberIOSim(CANBus canbus) { +// super(ClimberIO.getClimberConfiguration(), canbus); +// climberMotorSim = climberMotor.getSimState(); +// climberMotorSim.setMotorType(MotorType.KrakenX60); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 82a0bee..57c1c46 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,60 +1,42 @@ package frc.robot.subsystems.climber; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.components.rollers.RollerIO; -import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; - - public class ClimberSubsystem extends SubsystemBase { - //todo: find actual constants + // todo: find actual constants public static double GEAR_RATIO = (45.0 / 1.0); public static double MAX_EXTENSION_METERS = 0.2413; public static double MAX_ACCELERATION = 10.0; public static double MAX_VELOCITY = 2.0; -ClimberIO climberIO; -ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged(); + ClimberIO climberIO; + ClimberIOInputsAutoLogged climberInputs = new ClimberIOInputsAutoLogged(); @Override public void periodic() { climberIO.updateInputs(climberInputs); - Logger.processInputs("Climber", climberInputs); + Logger.processInputs("Climber", climberInputs); } -//member variables here? - -public ClimberSubsystem(ClimberIO climberIO) { - this.climberIO = climberIO; -} + // member variables here? -public Command climbUp() { - return this.run( - () -> { - climberIO.setClimberPosition(MAX_EXTENSION_METERS); - wait(1000); - climberIO.setClimberPosition(0.0); - // TODO figure out how to correctly implement - }); - -} + public ClimberSubsystem(ClimberIO climberIO) { + this.climberIO = climberIO; + } -public Command climbDown() { - return this.run( - () -> { - climberIO.setClimberPosition(MAX_EXTENSION_METERS); - wait(1000); - climberIO.setClimberPosition(0.0); - // TODO figure out how to correctly implement - }); - -} + public Command extendClimber() { + return this.run( + () -> { + climberIO.setClimberPosition(MAX_EXTENSION_METERS); + }); + } + public Command retractClimber() { + return this.run( + () -> { + climberIO.setClimberPosition(0.0); + }); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java index 0eb60bb..2044cf4 100644 --- a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java @@ -1,23 +1,22 @@ package frc.robot.subsystems.climber; -import edu.wpi.first.wpilibj2.command.Command; - import com.ctre.phoenix6.CANBus; +import edu.wpi.first.wpilibj2.command.Command; -public class EmptyClimberSubsystem extends ClimberSubsystem{ +public class EmptyClimberSubsystem extends ClimberSubsystem { @Override - public void periodic(){} + public void periodic() {} -public EmptyClimberSubsystem(CANBus canbus) { + public EmptyClimberSubsystem(CANBus canbus) { super(new ClimberIO(canbus)); -} + } -public Command climbUp() { + public Command climbUp() { return this.idle(); - } + } -public Command climbDown() { + public Command climbDown() { return this.idle(); - } + } } From 555493f8701095622a827ff3aa237422a1621978 Mon Sep 17 00:00:00 2001 From: ronansoergel9 Date: Sun, 8 Feb 2026 12:29:56 -0800 Subject: [PATCH 6/7] update command names + add bindings --- src/main/java/frc/robot/Robot.java | 11 +++++++---- .../subsystems/climber/EmptyClimberSubsystem.java | 6 ++++-- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4affc09..03c310f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,6 +32,7 @@ import frc.robot.components.rollers.RollerIOSim; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberSubsystem; +import frc.robot.subsystems.climber.EmptyClimberSubsystem; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.LindexerSubsystem; import frc.robot.subsystems.indexer.SpindexerSubsystem; @@ -270,10 +271,9 @@ public Robot() { // if this is alpha, we won't have assigned a climber yet // this creates a placeholder "no-operation" climber that will just not do anything, but is not // null (and we need it to be not null) - if (climber == null) - // climber = new EmptyClimberSubsystem(new ClimberIO(canivore) {}); + if (climber == null) climber = new EmptyClimberSubsystem(canivore); - DriverStation.silenceJoystickConnectionWarning(true); + DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); RobotController.setBrownoutVoltage(6.0); @@ -436,7 +436,10 @@ private void addControllerBindings() { () -> modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); - // TODO add binding for climb + + // TODO ACTUAL BUTTON BINDING FOR CLIMBER + driver.x().onTrue(climber.extendClimber()); + driver.y().onTrue(climber.retractClimber()); // ---zeroing stuff--- diff --git a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java index 2044cf4..1dbe0c2 100644 --- a/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/EmptyClimberSubsystem.java @@ -12,11 +12,13 @@ public EmptyClimberSubsystem(CANBus canbus) { super(new ClimberIO(canbus)); } - public Command climbUp() { + @Override + public Command extendClimber() { return this.idle(); } - public Command climbDown() { + @Override + public Command retractClimber() { return this.idle(); } } From 2558c339244fe73c0c1ce31ab560e13850acf646 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Sun, 8 Feb 2026 12:51:08 -0800 Subject: [PATCH 7/7] fmt --- src/main/java/frc/robot/Robot.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f511ffc..1d7f329 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -329,7 +329,10 @@ public Robot() { canivore, HoodIO.getCompHood(), TurretSubsystem.HOOD_GEAR_RATIO, 11)); break; } - climber = ROBOT_EDITION == RobotEdition.ALPHA ? new EmptyClimberSubsystem(canivore) : new ClimberSubsystem(new ClimberIO(canivore)); + climber = + ROBOT_EDITION == RobotEdition.ALPHA + ? new EmptyClimberSubsystem(canivore) + : new ClimberSubsystem(new ClimberIO(canivore)); // now that we've assigned the correct subsystems based on robot edition, we can pass them into // the superstructure superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator);