From a39b9c5d4f0e5208d6594cb1c0430ea0e0bec033 Mon Sep 17 00:00:00 2001 From: ronansoergel9 Date: Tue, 27 Jan 2026 23:50:57 -0800 Subject: [PATCH 1/3] 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/3] 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/3] 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); }); }