feat: add elevator subsystem (#16)

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

@@ -1,5 +1,6 @@
 package com.frcteam3636.frc2025
 
+import com.ctre.phoenix6.hardware.CANcoder
 import com.ctre.phoenix6.hardware.Pigeon2
 import com.ctre.phoenix6.hardware.TalonFX
 import com.revrobotics.spark.SparkMax
@@ -35,8 +36,11 @@ enum class CTREDeviceId(val num: Int, val bus: String) {
     BackLeftDrivingMotor(2, "*"),
     BackRightDrivingMotor(3, "*"),
     FrontRightDrivingMotor(4, "*"),
+    ElevatorMotor(11, "*"),
     PigeonGyro(20, "*"),
+    ElevatorEncoder(30, "*"),
 }
 
+fun CANcoder(id: CTREDeviceId) = CANcoder(id.num, id.bus)
 fun TalonFX(id: CTREDeviceId) = TalonFX(id.num, id.bus)
 fun Pigeon2(id: CTREDeviceId) = Pigeon2(id.num, id.bus)

src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt

@@ -0,0 +1,46 @@
+package com.frcteam3636.frc2025.subsystems.elevator
+
+import com.frcteam3636.frc2025.Robot
+import edu.wpi.first.units.Units.Meters
+import edu.wpi.first.units.measure.Distance
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Subsystem
+import org.littletonrobotics.junction.Logger
+
+object Elevator: Subsystem {
+    private val io: ElevatorIO = when (Robot.model) {
+        Robot.Model.SIMULATION -> TODO()
+        Robot.Model.COMPETITION -> ElevatorIOReal()
+        Robot.Model.PROTOTYPE -> TODO()
+    }
+
+    var inputs = LoggedElevatorInputs()
+
+    override fun periodic() {
+        io.updateInputs(inputs)
+        Logger.processInputs("Elevator", inputs)
+    }
+
+    fun setTargetHeight(position: Position): Command =
+        startEnd({
+        io.runToHeight(position.height)
+    }, {
+        io.runToHeight(inputs.height)
+    })!!
+
+//    fun sysIdQuasistatic(direction: Direction) =
+//        sysID.quasistatic(direction)!!
+//
+//    fun sysIdDynamic(direction: Direction) =
+//        sysID.dynamic(direction)!!
+
+    enum class Position(val height: Distance) {
+        Stowed(Meters.of(0.0)),
+        Trough(Meters.of(0.0)),
+        LowBar(Meters.of(0.0)),
+        MidBar(Meters.of(0.0)),
+        HighBar(Meters.of(0.0)),
+        LowAlgae(Meters.of(0.0)),
+        HighAlgae(Meters.of(0.0)),
+    }
+}

src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt

@@ -0,0 +1,122 @@
+package com.frcteam3636.frc2025.subsystems.elevator
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration
+import com.ctre.phoenix6.configs.TalonFXConfiguration
+import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC
+import com.ctre.phoenix6.controls.VoltageOut
+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 com.frcteam3636.frc2025.CTREDeviceId
+import com.frcteam3636.frc2025.TalonFX
+import com.frcteam3636.frc2025.CANcoder
+import com.frcteam3636.frc2025.utils.math.MotorFFGains
+import com.frcteam3636.frc2025.utils.math.PIDGains
+import com.frcteam3636.frc2025.utils.math.motorFFGains
+import com.frcteam3636.frc2025.utils.math.pidGains
+import edu.wpi.first.units.Units.*
+import edu.wpi.first.units.measure.Angle
+import edu.wpi.first.units.measure.Distance
+import edu.wpi.first.units.measure.LinearVelocity
+import edu.wpi.first.units.measure.Voltage
+import org.littletonrobotics.junction.Logger
+import org.team9432.annotation.Logged
+
+@Logged
+open class ElevatorInputs {
+    var height = Meters.zero()!!
+    var rightCurrent = Volts.zero()!!
+    var leftCurrent = Volts.zero()!!
+    var velocity = MetersPerSecond.zero()!!
+}
+
+interface ElevatorIO{
+    fun updateInputs(inputs: ElevatorInputs)
+
+    fun runToHeight(height: Distance)
+
+    fun setVoltage(volts: Voltage)
+
+}
+
+class ElevatorIOReal: ElevatorIO {
+
+    private val encoder = CANcoder(CTREDeviceId.ElevatorEncoder).apply {
+        val config = CANcoderConfiguration().apply {
+            MagnetSensor.apply {
+                withAbsoluteSensorDiscontinuityPoint(Rotations.one())
+                SensorDirection = SensorDirectionValue.Clockwise_Positive
+            }
+        }
+        configurator.apply(config)
+    }
+
+    val config = TalonFXConfiguration().apply {
+        MotorOutput.apply {
+            NeutralMode = NeutralModeValue.Brake
+        }
+
+        Feedback.apply {
+            SensorToMechanismRatio = GEAR_RATIO
+            FeedbackRemoteSensorID = CTREDeviceId.ElevatorEncoder.num
+            FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder
+        }
+
+        Slot0.apply {
+            pidGains = PID_GAINS
+            motorFFGains = FF_GAINS
+            kG = GRAVITY_GAIN
+        }
+
+        MotionMagic.apply {
+            MotionMagicCruiseVelocity = PROFILE_VELOCITY
+            MotionMagicAcceleration = PROFILE_ACCELERATION
+            MotionMagicJerk = PROFILE_JERK
+        }
+    }
+
+    private val rightElevatorMotor = TalonFX(CTREDeviceId.ElevatorMotor).apply {
+        config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive
+        configurator.apply(config)
+    }
+
+    private val leftElevatorMotor = TalonFX(CTREDeviceId.ElevatorMotor).apply {
+        config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive
+        configurator.apply(config)
+    }
+
+    override fun updateInputs(inputs: ElevatorInputs) {
+        inputs.height = (DISTANCE_PER_TURN * encoder.position.value) as Distance
+        inputs.velocity = (DISTANCE_PER_TURN * encoder.velocity.value) as LinearVelocity
+        inputs.rightCurrent = rightElevatorMotor.motorVoltage.value
+        inputs.leftCurrent = leftElevatorMotor.motorVoltage.value
+    }
+
+    override fun runToHeight(height: Distance) {
+        Logger.recordOutput("Elevator/Height Setpoint", height)
+        var desiredMotorAngle = (height / DISTANCE_PER_TURN) as Angle
+        var controlRequest = MotionMagicTorqueCurrentFOC(desiredMotorAngle)
+        rightElevatorMotor.setControl(controlRequest)
+        leftElevatorMotor.setControl(controlRequest)
+    }
+
+    override fun setVoltage(volts: Voltage){
+        assert(volts in Volts.of(-12.0)..Volts.of(12.0))
+        val controlRequest = VoltageOut(volts.`in`(Volts))
+        rightElevatorMotor.setControl(controlRequest)
+        leftElevatorMotor.setControl(controlRequest)
+    }
+
+    internal companion object Constants {
+        val DISTANCE_PER_TURN = Meters.per(Rotation).of(0.0)!!
+        private const val GEAR_RATIO = 0.0
+        val PID_GAINS = PIDGains(0.0, 0.0, 0.0)
+        val FF_GAINS = MotorFFGains(0.0, 0.0, 0.0)
+        private const val GRAVITY_GAIN = 0.0
+        private const val PROFILE_ACCELERATION = 0.0
+        private const val PROFILE_JERK = 0.0
+        private const val PROFILE_VELOCITY = 0.0
+    }
+
+}