Climb subsystem

src/main/kotlin/com/frcteam3636/frc2026/CAN.kt

@@ -24,6 +24,8 @@ enum class CTREDeviceId(val num: Int, val bus: CANBus) {
     FrontRightTurningEncoder(12, canivoreBus),
 
     PigeonGyro(20, canivoreBus),
+
+    ClimbMotor(30, canivoreBus),
 }
 
 fun CANcoder(id: CTREDeviceId) = CANcoder(id.num, id.bus)

src/main/kotlin/com/frcteam3636/frc2026/subsystems/climb/Climb.kt

@@ -0,0 +1,72 @@
+package com.frcteam3636.frc2026.subsystems.climb
+
+import com.frcteam3636.frc2026.Robot
+import com.frcteam3636.frc2026.utils.math.degrees
+import com.frcteam3636.frc2026.utils.math.inMeters
+import com.frcteam3636.frc2026.utils.math.inMetersPerSecond
+import com.frcteam3636.frc2026.utils.math.inches
+import com.frcteam3636.frc2026.utils.math.meters
+import edu.wpi.first.networktables.DoubleTopic
+import edu.wpi.first.networktables.NetworkTableInstance
+import edu.wpi.first.units.measure.Distance
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Commands
+import edu.wpi.first.wpilibj2.command.Subsystem
+import org.littletonrobotics.junction.Logger
+import kotlin.math.abs
+
+class Climb : Subsystem {
+    internal companion object Constants {
+        private val CLIMBER_TOLERANCE = 0.2.inches
+    }
+
+    enum class Position(val height: Distance) {
+        STOWED(0.meters),
+        GROUND_L1(0.5.meters),
+        // ... Might be more complicated than just set heights, we'll see.
+    }
+
+    private val io: ClimbIO = when (Robot.model) {
+        Robot.Model.COMPETITION -> ClimbIOReal()
+        Robot.Model.SIMULATION -> ClimbIOSim()
+    }
+
+    val inputs = LoggedClimbInputs()
+
+    var position = Position.STOWED
+    var targetHeight = 0.meters
+
+    val rgbTable = NetworkTableInstance.getDefault().getTable("RGB")
+    val climbPosTopic = DoubleTopic(rgbTable.getTopic("/ClimbHeight")).publish()
+    val climbVelTopic = DoubleTopic(rgbTable.getTopic("/ClimbVelocity")).publish()
+    val climbTargetTopic = DoubleTopic(rgbTable.getTopic("/ClimbTarget")).publish()
+
+    override fun periodic() {
+        io.updateInputs(inputs)
+        Logger.processInputs("Climb", inputs)
+        climbPosTopic.set(inputs.height.inMeters())
+        climbVelTopic.set(inputs.velocity.inMetersPerSecond())
+        climbTargetTopic.set(targetHeight.inMeters())
+    }
+
+    fun goToHeight(setPosition: Position, slow: Boolean = false): Command = startEnd(
+        {
+            position = setPosition
+            targetHeight = position.height
+            io.goToHeight(position.height, slow)
+        },
+        {
+
+        }
+    ).until { isAtTarget() }
+
+    // Pulls down and hooks onto the L1 bar.
+    fun climbL1(): Command = Commands.sequence(
+        goToHeight(Position.GROUND_L1),
+        // TODO: Wait until the robot is in the right position to climb. Ideally it should already be in that position, but just in case.
+        // This is dependant on the auto system which to my knowledge isn't implemented yet.
+        goToHeight(Position.STOWED, true),
+    )
+
+    fun isAtTarget(): Boolean = abs((inputs.height - targetHeight).inMeters()) < CLIMBER_TOLERANCE.inMeters()
+}

src/main/kotlin/com/frcteam3636/frc2026/subsystems/climb/ClimbIO.kt

@@ -0,0 +1,144 @@
+package com.frcteam3636.frc2026.subsystems.climb
+
+import com.ctre.phoenix6.BaseStatusSignal
+import com.ctre.phoenix6.configs.TalonFXConfiguration
+import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage
+import com.ctre.phoenix6.signals.NeutralModeValue
+import com.frcteam3636.frc2026.CTREDeviceId
+import com.frcteam3636.frc2026.TalonFX
+import com.frcteam3636.frc2026.utils.math.PIDController
+import com.frcteam3636.frc2026.utils.math.PIDGains
+import com.frcteam3636.frc2026.utils.math.amps
+import com.frcteam3636.frc2026.utils.math.inMeters
+import com.frcteam3636.frc2026.utils.math.inRotationsPerSecond
+import com.frcteam3636.frc2026.utils.math.inVolts
+import com.frcteam3636.frc2026.utils.math.inches
+import com.frcteam3636.frc2026.utils.math.inchesPerSecond
+import com.frcteam3636.frc2026.utils.math.meters
+import com.frcteam3636.frc2026.utils.math.metersPerSecond
+import com.frcteam3636.frc2026.utils.math.pidGains
+import com.frcteam3636.frc2026.utils.math.toAngular
+import com.frcteam3636.frc2026.utils.math.toLinear
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.system.plant.LinearSystemId
+import edu.wpi.first.units.measure.Distance
+import edu.wpi.first.units.measure.Voltage
+import edu.wpi.first.wpilibj.simulation.DCMotorSim
+import org.littletonrobotics.junction.Logger
+import org.team9432.annotation.Logged
+
+@Logged
+open class ClimbInputs {
+    var height = 0.meters
+    var current = 0.amps
+    var velocity = 0.metersPerSecond
+}
+
+interface ClimbIO {
+    fun setVoltage(volts: Voltage)
+    fun goToHeight(height: Distance, slow: Boolean = false)
+    fun setEncoderPosition(position: Distance)
+    fun updateInputs(inputs: ClimbInputs)
+}
+
+class ClimbIOReal : ClimbIO {
+    internal companion object Constants {
+        private val SPOOL_RADIUS = 1.0.inches // Not measured, approximation
+        private val PID_GAINS = PIDGains(30.0, 0.0, 0.0) // Not measured, approximation
+        private val ROTOR_TO_MECHANISM_GEAR_RATIO = 8.0 // Not sure what this does, taken from ElevatorIO@frc-2025
+
+        // Below values taken from frc-2025, again, need to be tuned.
+        private val FAST_PROFILE_VELOCITY = 100.inchesPerSecond.toAngular(SPOOL_RADIUS)
+        private val FAST_PROFILE_ACCELERATION = 50.0
+        private val FAST_PROFILE_JERK = 0.0
+        private val SLOW_PROFILE_VELOCITY = 10.inchesPerSecond.toAngular(SPOOL_RADIUS)
+        private val SLOW_PROFILE_ACCELERATION = 2.0
+        private val SLOW_PROFILE_JERK = 0.0
+    }
+
+    private val motorConfig = TalonFXConfiguration()
+    private val motor = TalonFX(CTREDeviceId.ClimbMotor)
+
+    init {
+        motorConfig.apply {
+            MotorOutput.apply {
+                NeutralMode = NeutralModeValue.Brake
+            }
+
+            Slot0.apply {
+                pidGains = PID_GAINS
+                // Gravity and FF gains?
+            }
+
+            Feedback.apply {
+                SensorToMechanismRatio = ROTOR_TO_MECHANISM_GEAR_RATIO
+            }
+        }
+        motor.configurator.apply(motorConfig)
+
+        BaseStatusSignal.setUpdateFrequencyForAll(
+            100.0,
+            motor.position,
+            motor.velocity,
+            motor.supplyCurrent
+        )
+        motor.optimizeBusUtilization()
+    }
+
+    override fun setVoltage(volts: Voltage) {
+        motor.setVoltage(volts.inVolts())
+    }
+
+    override fun goToHeight(height: Distance, slow: Boolean) {
+        Logger.recordOutput("Climb/Height Setpoint", height)
+        motor.setControl(getMotionMagicVoltage(slow).withPosition(height.toAngular(SPOOL_RADIUS)))
+    }
+
+    override fun setEncoderPosition(position: Distance) {
+        motor.setPosition(position.toAngular(SPOOL_RADIUS))
+    }
+
+    override fun updateInputs(inputs: ClimbInputs) {
+        inputs.height = motor.getPosition(false).value.toLinear(SPOOL_RADIUS)
+        inputs.velocity = motor.getVelocity(false).value.toLinear(SPOOL_RADIUS)
+        inputs.current = motor.getSupplyCurrent(false).value
+    }
+
+    private fun getMotionMagicVoltage(slow: Boolean): DynamicMotionMagicVoltage {
+        if (slow) {
+            return DynamicMotionMagicVoltage(0.0, SLOW_PROFILE_VELOCITY.inRotationsPerSecond(), SLOW_PROFILE_ACCELERATION).withJerk(SLOW_PROFILE_JERK)
+        }
+        return DynamicMotionMagicVoltage(0.0, FAST_PROFILE_VELOCITY.inRotationsPerSecond(), FAST_PROFILE_ACCELERATION).withJerk(FAST_PROFILE_JERK)
+    }
+}
+
+class ClimbIOSim : ClimbIO {
+    internal companion object Constants {
+        private val SPOOL_RADIUS = 1.0.inches // Not measured, approximation
+        private val PID_GAINS = PIDGains(30.0, 0.0, 0.0) // Not measured, approximation
+    }
+
+    private val motor = DCMotor.getKrakenX60(1)
+    private val system = LinearSystemId.createDCMotorSystem(motor, 1.0,1.0)
+    private val sim = DCMotorSim(system, motor)
+
+    private val pid = PIDController(PID_GAINS)
+
+    override fun setVoltage(volts: Voltage) {
+        sim.inputVoltage = volts.inVolts()
+    }
+
+    override fun goToHeight(height: Distance, slow: Boolean) {
+        sim.inputVoltage = pid.calculate(sim.angularPosition.toLinear(SPOOL_RADIUS).inMeters())
+    }
+
+    override fun setEncoderPosition(position: Distance) {
+        TODO("Not yet implemented")
+    }
+
+    override fun updateInputs(inputs: ClimbInputs) {
+        inputs.height = sim.angularPosition.toLinear(SPOOL_RADIUS)
+        inputs.velocity = sim.angularVelocity.toLinear(SPOOL_RADIUS)
+        inputs.current = sim.currentDrawAmps.amps
+    }
+}