88-climb-subsystem

src/main/java/frc/robot/Robot.kt

@@ -4,8 +4,10 @@ import edu.wpi.first.wpilibj.Joystick
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.PowerDistribution
 import edu.wpi.first.wpilibj2.command.CommandScheduler
+import edu.wpi.first.wpilibj2.command.Commands
 import edu.wpi.first.wpilibj2.command.Commands.*
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
+import frc.robot.subsystems.climb.Climb
 import frc.robot.subsystems.swerve.Drivebase
 import frc.robot.subsystems.intake.Intake
 import frc.robot.subsystems.pivot.Pivot
@@ -31,7 +33,7 @@ object Robot : LoggedRobot() {
     // This is so awful, but it's the best way to test DIO in simulation that I can think of
     val keyboard: Joystick by lazy { println("JOYSTICK INITIALIZED"); Joystick(5) }
 
-//    val climb = Climb()
+    val climb = Climb()
     val pivot = Pivot()
     val drivebase = Drivebase()
     val intake = Intake()
@@ -113,6 +115,9 @@ object Robot : LoggedRobot() {
         )
 
         driverController.y().onTrue(pivot.getHomeCommand())
+
+        driverController.b().and(climb.isPreclimb).onTrue(climb.getExtendCommand())
+        driverController.b().and(climb.isExtended).onTrue(climb.getRetractCommand())
     }
 
     /**

src/main/java/frc/robot/subsystems/climb/Climb.kt

@@ -1,85 +1,142 @@
 package frc.robot.subsystems.climb
 
-import com.revrobotics.CANSparkBase
-import com.revrobotics.CANSparkLowLevel
-import com.revrobotics.CANSparkMax
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab
+import edu.wpi.first.math.geometry.Pose3d
+import edu.wpi.first.math.geometry.Rotation3d
+import edu.wpi.first.math.geometry.Translation3d
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.util.Units
+import edu.wpi.first.units.Current
+import edu.wpi.first.units.Measure
+import edu.wpi.first.units.Units.*
+import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import frc.robot.Constants
-import java.util.function.Supplier
+import lib.math.units.into
+import org.littletonrobotics.junction.Logger
+import kotlin.math.sin
+import edu.wpi.first.wpilibj2.command.Commands.*
+import edu.wpi.first.wpilibj2.command.button.Trigger
+import java.util.function.BooleanSupplier
 
 class Climb : SubsystemBase() {
+    private val leftArmIO: ClimberArmIO = when(Constants.RobotConstants.mode){
+        Constants.RobotConstants.Mode.REAL -> ClimberArmNeo(
+            leftID,
+            false,
+            gearing,
+            drumRadius
+        )
+        Constants.RobotConstants.Mode.SIM -> ClimberArmIOSim(
+            DCMotor.getNEO(1),
+            gearing, load, drumRadius, maxExtension
+        )
+        Constants.RobotConstants.Mode.REPLAY -> object : ClimberArmIO {}
+    }
 
-    /** The spark max for the left motor */
-    val leftMotor = CANSparkMax(Constants.ClimbConstants.LEFT_CLIMB_PORT, CANSparkLowLevel.MotorType.kBrushless)
-
-    /** The spark max for the right motor */
-    val rightMotor = CANSparkMax(Constants.ClimbConstants.RIGHT_CLIMB_PORT, CANSparkLowLevel.MotorType.kBrushless)
-
-    init {
-        // Conversion ratio for encoders, so that the position is in rotations outside of the gearbox
-        leftMotor.encoder.setPositionConversionFactor(1.0 / 16.0)
-        rightMotor.encoder.setPositionConversionFactor(1.0 / 16.0)
-
-        // Logging for Shuffleboard, position and velocity of the motors
-        val driveTab: ShuffleboardTab = Shuffleboard.getTab("Climb Subsystem")
-        driveTab.addNumber("Left Climb Motor Velocity") { leftMotor.encoder.velocity }
-        driveTab.addNumber("Right Climb Motor Velocity") { rightMotor.encoder.velocity }
-        driveTab.addNumber("Left Climb Motor Position") { leftMotor.encoder.position }
-        driveTab.addNumber("Right Climb Motor Position") { rightMotor.encoder.position }
+    private val rightArmIO: ClimberArmIO = when(Constants.RobotConstants.mode){
+        Constants.RobotConstants.Mode.REAL -> ClimberArmNeo(
+            rightID,
+            true,
+            gearing,
+            drumRadius
+        )
+        Constants.RobotConstants.Mode.SIM -> ClimberArmIOSim(
+            DCMotor.getNEO(1),
+            gearing, load, drumRadius, maxExtension
+        )
+        Constants.RobotConstants.Mode.REPLAY -> object : ClimberArmIO {}
+    }
 
-        // Reset encoders to 0, since that only gets rest on power cycle not code deploy
-        rightMotor.encoder.setPosition(0.0)
-        leftMotor.encoder.setPosition(0.0)
+    val leftInputs: ClimberArmIO.ClimberArmInputs = ClimberArmIO.ClimberArmInputs()
+    val rightInputs: ClimberArmIO.ClimberArmInputs = ClimberArmIO.ClimberArmInputs()
 
-        // Set the motors to brake mode so that they don't move (as easily) when disabled
-        leftMotor.setIdleMode(CANSparkBase.IdleMode.kBrake)
-        rightMotor.setIdleMode(CANSparkBase.IdleMode.kBrake)
+    var state = ClimbState.PRECLIMB
+        private set
 
-        // "Burn" motor settings to the Spark Maxes, so that they persist across power cycles
-        rightMotor.burnFlash()
-        leftMotor.burnFlash()
-    }
+    val isPreclimb: Trigger = Trigger { state == ClimbState.PRECLIMB }
+    val isExtended: Trigger = Trigger { state == ClimbState.EXTENDED }
+    val isLatched: Trigger = Trigger { state == ClimbState.LATCHED }
 
     override fun periodic() {
+        leftArmIO.updateInputs(leftInputs)
+        rightArmIO.updateInputs(rightInputs)
+
+        Logger.processInputs("climb/leftArm", leftInputs)
+        Logger.processInputs("climb/rightArm", rightInputs)
+
+        Logger.recordOutput("climb/leftArmPose", Pose3d.struct, Pose3d(
+            Translation3d(
+                0.0,
+                Units.inchesToMeters(10.0),
+                (leftInputs.relativePosition into Meters) + Units.inchesToMeters(8.0)
+            ),
+            Rotation3d()
+        ))
+
+        Logger.recordOutput("climb/rightArmPose", Pose3d.struct, Pose3d(
+            Translation3d(
+                0.0,
+                Units.inchesToMeters(-10.0),
+                (rightInputs.relativePosition into Meters) + Units.inchesToMeters(8.0)
+            ),
+            Rotation3d()
+        ))
+
+        Logger.recordOutput("climb/state", state)
     }
 
-    override fun simulationPeriodic() {
+    fun getSineCommand() = run {
+        val voltage = sin(Timer.getFPGATimestamp()) * 12.0
+        leftArmIO.setVoltage(Volts.of(voltage), false)
+        rightArmIO.setVoltage(Volts.of(voltage), false)
     }
 
-    /**
-     * Raise the arms at a constant speed
-     */
-    fun armsUp() {
-        leftMotor.set(Constants.ClimbConstants.MOTOR_SPEED_UP)
-        rightMotor.set(Constants.ClimbConstants.MOTOR_SPEED_UP)
+    fun getExtendCommand() = sequence(
+        runOnce {
+            leftArmIO.setVoltage(extensionVoltage, false)
+            rightArmIO.setVoltage(extensionVoltage, false)
+        },
+        waitSeconds(2.5),
+        runOnce {
+            leftArmIO.stop()
+            rightArmIO.stop()
+            state = ClimbState.EXTENDED
+        }
+    )
+
+    fun getRetractCommand() = sequence(
+        runOnce {
+            leftArmIO.setVoltage(retractionVoltage, false)
+            rightArmIO.setVoltage(retractionVoltage, false)
+        },
+        waitUntil {
+            leftInputs.appliedCurrent > currentThreshold || rightInputs.appliedCurrent > currentThreshold
+        },
+        runOnce {
+            leftArmIO.stop()
+            rightArmIO.stop()
+            state = ClimbState.LATCHED
+        }
+    )
+
+    enum class ClimbState {
+        PRECLIMB,
+        EXTENDED,
+        LATCHED
     }
 
-    /**
-     * Lower the arms at a constant speed
-     */
-    fun armsDown() {
-        leftMotor.set(Constants.ClimbConstants.MOTOR_SPEED_DOWN)
-        rightMotor.set(Constants.ClimbConstants.MOTOR_SPEED_DOWN)
-    }
+    companion object {
+        val leftID = 18
+        val rightID = 15
 
-    /**
-     * Stop the motors
-     */
-    fun stop() {
-        leftMotor.set(0.0)
-        rightMotor.set(0.0)
-    }
+        val maxExtension = Inches.of(48.0)
+        val drumRadius = Inches.of(0.325)
 
-    /**
-     * Set the raw speeds of the motors
-     * @param speed The speed to set the motors to
-     */
-    fun setRawSpeeds(speed: Double) {
-        leftMotor.set(speed)
-        rightMotor.set(speed)
-    }
+        val load = Constants.RobotConstants.robotWeight.divide(2.0)
+        val gearing = 16/1.0
 
-    fun climbCommand(speed: Supplier<Double>) = run { setRawSpeeds(speed.get()) }
+        val extensionVoltage = Volts.of(12.0)
+        val retractionVoltage = Volts.of(-12.0)
+        val currentThreshold: Measure<Current> = Amps.of(25.0)
+    }
 }

src/main/java/frc/robot/subsystems/climb/ClimberArmIO.kt

@@ -15,12 +15,12 @@ interface ClimberArmIO {
         /**
          * The relative position as measured by the motor's encoder.
          */
-        val relativePosition: MutableMeasure<Angle> = MutableMeasure.zero(Radians)
+        val relativePosition: MutableMeasure<Distance> = MutableMeasure.zero(Inches)
 
         /**
          * The angular velocity as measured by the motor's encoder.
          */
-        val velocity: MutableMeasure<Velocity<Angle>> = MutableMeasure.zero(RadiansPerSecond)
+        val velocity: MutableMeasure<Velocity<Distance>> = MutableMeasure.zero(InchesPerSecond)
 
         /**
          * The voltage applied to the motor.
@@ -62,4 +62,9 @@ interface ClimberArmIO {
      * @param voltage The voltage to supply
      */
     fun setVoltage(voltage: Measure<Voltage>, isClosedLoop: Boolean = false) {}
+
+    /**
+     * Stops the motor.
+     */
+    fun stop() {}
 }

src/main/java/frc/robot/subsystems/climb/ClimberArmIOSim.kt

@@ -20,21 +20,17 @@ class ClimberArmIOSim(
         drumRadius into Meters,
         0.0, // Minimum extension
         maxExtension into Meters,
-        true, // Simulate gravity,
+        false, // Simulate gravity,
         0.0 // Initial position
     )
 
     private val cachedVoltage = MutableMeasure.zero(Volts)
 
     override fun updateInputs(inputs: ClimberArmIO.ClimberArmInputs) {
-        inputs.velocity.mut_replace(
-            (elevatorSim.velocityMetersPerSecond) / (drumRadius.divide(2.0) into Meters),
-            RadiansPerSecond
-        )
-        inputs.relativePosition.mut_replace(
-            (elevatorSim.positionMeters) / (drumRadius.divide(2.0) into Meters),
-            Radians
-        )
+        elevatorSim.update(0.02)
+
+        inputs.velocity.mut_replace(elevatorSim.velocityMetersPerSecond, MetersPerSecond)
+        inputs.relativePosition.mut_replace(elevatorSim.positionMeters, Meters)
         inputs.appliedCurrent.mut_replace(Amps.of(elevatorSim.currentDrawAmps))
         inputs.appliedVoltage.mut_replace(cachedVoltage)
     }
@@ -43,4 +39,8 @@ class ClimberArmIOSim(
         cachedVoltage.mut_replace(voltage)
         elevatorSim.setInputVoltage(voltage into Volts)
     }
+
+    override fun stop() {
+        elevatorSim.setInputVoltage(0.0)
+    }
 }

src/main/java/frc/robot/subsystems/climb/ClimberArmNeo.kt

@@ -1,10 +1,10 @@
 package frc.robot.subsystems.climb
 
+import com.revrobotics.CANSparkBase
 import com.revrobotics.CANSparkLowLevel
 import com.revrobotics.CANSparkMax
 import edu.wpi.first.units.*
-import edu.wpi.first.units.Units.Amps
-import edu.wpi.first.units.Units.Volts
+import edu.wpi.first.units.Units.*
 import lib.ControllerGains
 import lib.math.units.into
 import lib.math.units.rotations
@@ -14,22 +14,33 @@ class ClimberArmNeo(
     private val id: Int,
     private val isInverted: Boolean,
     private val gearing: Double,
-    private val gains: ControllerGains
+    private val drumRadius: Measure<Distance>,
 ): ClimberArmIO {
     private val motor: CANSparkMax = CANSparkMax(id, CANSparkLowLevel.MotorType.kBrushless).apply {
         inverted = true
         encoder.positionConversionFactor = 1/gearing
         encoder.velocityConversionFactor = 1/gearing
+        idleMode = CANSparkBase.IdleMode.kBrake
     }
 
     override fun updateInputs(inputs: ClimberArmIO.ClimberArmInputs) {
-        inputs.velocity.mut_replace(motor.encoder.velocity.rpm)
-        inputs.relativePosition.mut_replace(motor.encoder.position.rotations)
+        inputs.velocity.mut_replace(
+            (motor.encoder.velocity / 60) * (drumRadius into Meters) * 2 * Math.PI,
+            MetersPerSecond
+        )
+        inputs.relativePosition.mut_replace(
+            motor.encoder.position * (drumRadius into Meters) * 2 * Math.PI,
+            Meters
+        )
         inputs.appliedCurrent.mut_replace(Amps.of(motor.outputCurrent))
         inputs.appliedVoltage.mut_replace(Volts.of(motor.appliedOutput * motor.busVoltage))
     }
 
     override fun setVoltage(voltage: Measure<Voltage>, isClosedLoop: Boolean) {
         motor.setVoltage(voltage into Volts)
     }
+
+    override fun stop() {
+        motor.stopMotor()
+    }
 }