Merge pull request #1 from Galaxia5987/feature/elevator

Add elevator code

.gitignore

@@ -164,9 +164,10 @@ annotation/build/
 # Simulation GUI and other tools window save file
 *-window.json
 
+/src/main/kotlin/frc/robot/BuildConstants.java
+
 simgui*
 networktables.json
 /.idea
 /ctre_sim/
 /log-downloader/venv/
-/src/main/kotlin/frc/robot/BuildConstants.java

src/main/kotlin/frc/robot/generated/TunerConstants.java

@@ -0,0 +1 @@
+

src/main/kotlin/frc/robot/subsystems/elevator/Elevator.kt

@@ -0,0 +1,53 @@
+package frc.robot.subsystems.elevator
+
+import edu.wpi.first.units.Units
+import edu.wpi.first.units.measure.Distance
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import java.util.function.DoubleSupplier
+import org.littletonrobotics.junction.AutoLogOutput
+import org.littletonrobotics.junction.Logger
+import org.littletonrobotics.junction.mechanism.LoggedMechanism2d
+import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d
+
+class Elevator(private val io: ElevatorIO) : SubsystemBase() {
+    private val mechanism = LoggedMechanism2d(3.0, 3.0)
+    private val root = mechanism.getRoot("Elevator", 2.0, 0.0)
+    private val elevatorLigament =
+        root.append(LoggedMechanismLigament2d("ElevatorLigament", 5.0, 90.0))
+    @AutoLogOutput
+    private var setpointValue: Distance = Units.Millimeters.of(0.0)
+    @AutoLogOutput private var setpointName: Positions = Positions.ZERO
+
+    fun setPosition(position: Positions): Command =
+        runOnce {
+                setpointValue = position.value
+                setpointName = position
+                io.setHeight(position.value)
+            }
+            .withName(position.getLoggingName())
+
+    fun l1(): Command = setPosition(Positions.L1)
+    fun l2(): Command = setPosition(Positions.L2)
+    fun l3(): Command = setPosition(Positions.L3)
+    fun l4(): Command = setPosition(Positions.L4)
+    fun l2Algae(): Command = setPosition(Positions.L2_ALGAE)
+    fun l3Algae(): Command = setPosition(Positions.L3_ALGAE)
+    fun feeder(): Command = setPosition(Positions.FEEDER)
+    fun zero(): Command = setPosition(Positions.ZERO)
+
+    fun setPower(percentOutput: DoubleSupplier): Command = run {
+        io.setPower(percentOutput.asDouble)
+    }
+
+    fun reset(): Command = runOnce(io::resetAbsoluteEncoder)
+
+    override fun periodic() {
+        io.updateInputs()
+        Logger.processInputs(this.name, io.inputs)
+        Logger.recordOutput("Elevator/Mechanism2d", mechanism)
+        Logger.recordOutput("Elevator/Setpoint", setpointValue)
+
+        elevatorLigament.length = io.inputs.height.`in`(Units.Meters)
+    }
+}

src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt

@@ -0,0 +1,41 @@
+package frc.robot.subsystems.elevator
+
+import edu.wpi.first.units.*
+import edu.wpi.first.units.measure.Distance
+import frc.robot.lib.Gains
+import frc.robot.lib.selectGainsBasedOnMode
+import kotlin.math.PI
+import kotlin.time.times
+
+val MAX_HEIGHT: Distance = Units.Meters.of(1.3)
+const val GEAR_RATIO = (1.0 / 12.0) * (42.0 / 48.0)
+const val FIRST_STAGE_RATIO = 2.0
+const val ENCODER_OFFSET = 0.0
+const val ADJUSTED_GEAR_RATIO = FIRST_STAGE_RATIO * GEAR_RATIO
+val SPROCKET_RADIUS: Distance = Units.Millimeters.of(36.4 / 2)
+private val ROTATIONS_TO_CENTIMETERS_RATIO =
+    GEAR_RATIO *
+        FIRST_STAGE_RATIO *
+        (SPROCKET_RADIUS.`in`(Units.Centimeter) * 2 * PI)
+val ROTATIONS_TO_CENTIMETER: Measure<out PerUnit<DistanceUnit, AngleUnit>> =
+    Units.Centimeters.per(Units.Rotations).of(ROTATIONS_TO_CENTIMETERS_RATIO)
+val CENTIMETERS_TO_ROTATIONS: Measure<out PerUnit<AngleUnit, DistanceUnit>> =
+    Units.Rotations.per(Units.Centimeter).of(1 / ROTATIONS_TO_CENTIMETERS_RATIO)
+
+val GAINS = selectGainsBasedOnMode(Gains(kP = 20.0, kD = 1.0), Gains())
+
+enum class Positions(val value: Distance) {
+    L1(Units.Centimeters.of(25.0)),
+    L2(Units.Centimeters.of(35.0)),
+    L3(Units.Centimeters.of(45.0)),
+    L4(Units.Centimeters.of(110.0)),
+    L2_ALGAE(Units.Centimeters.of(55.0)),
+    L3_ALGAE(Units.Centimeters.of(79.0)),
+    FEEDER(Units.Centimeters.of(95.0)),
+    ZERO(Units.Centimeters.zero());
+
+    fun getLoggingName() =
+        name.split("_").joinToString(" ") {
+            it.lowercase().replaceFirstChar(Char::uppercase)
+        }
+}

src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIO.kt

@@ -0,0 +1,27 @@
+package frc.robot.subsystems.elevator
+
+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.Voltage
+import org.team9432.annotation.Logged
+
+interface ElevatorIO {
+    val inputs: LoggedElevatorInputs
+
+    fun setHeight(height: Distance) {}
+
+    fun setPower(percentOutput: Double) {}
+
+    fun resetAbsoluteEncoder() {}
+
+    fun updateInputs() {}
+
+    @Logged
+    open class ElevatorInputs {
+        var height: Distance = Units.Meters.of(0.0)
+        var appliedVoltage: Voltage = Units.Volts.of(0.0)
+        var noOffsetAbsoluteEncoderPosition: Angle = Units.Rotations.of(0.0)
+        var absoluteEncoderHeight: Distance = Units.Meters.of(0.0)
+    }
+}

src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt

@@ -0,0 +1,89 @@
+package frc.robot.subsystems.elevator
+
+import com.ctre.phoenix6.configs.*
+import com.ctre.phoenix6.controls.Follower
+import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC
+import com.ctre.phoenix6.hardware.CANcoder
+import com.ctre.phoenix6.hardware.TalonFX
+import com.ctre.phoenix6.signals.InvertedValue
+import com.ctre.phoenix6.signals.NeutralModeValue
+import edu.wpi.first.units.measure.Distance
+import frc.robot.lib.toAngle
+import frc.robot.lib.toDistance
+
+class ElevatorIOReal : ElevatorIO {
+    override val inputs = LoggedElevatorInputs()
+    private val mainMotor = TalonFX(MAIN_ID)
+    private val auxMotor = TalonFX(AUX_ID)
+    private val encoder = CANcoder(ENCODER_ID)
+    private val mainMotorPositionRequest = MotionMagicTorqueCurrentFOC(0.0)
+
+    init {
+        val motorConfig =
+            TalonFXConfiguration().apply {
+                MotorOutput =
+                    MotorOutputConfigs().apply {
+                        NeutralMode = NeutralModeValue.Brake
+                        Inverted = InvertedValue.Clockwise_Positive
+                    }
+                Feedback = FeedbackConfigs().apply { RotorToSensorRatio = 1.0 }
+                Slot0 =
+                    Slot0Configs().apply {
+                        kP = GAINS.kP
+                        kI = GAINS.kI
+                        kD = GAINS.kD
+                    }
+                CurrentLimits =
+                    CurrentLimitsConfigs().apply {
+                        StatorCurrentLimitEnable = true
+                        SupplyCurrentLimitEnable = true
+                        StatorCurrentLimit = 80.0
+                        SupplyCurrentLimit = 40.0
+                    }
+            }
+
+        mainMotor.configurator.apply(motorConfig)
+        auxMotor.configurator.apply(motorConfig)
+
+        auxMotor.setControl(Follower(MAIN_ID, false))
+
+        val encoderConfig =
+            CANcoderConfiguration().apply {
+                MagnetSensor.MagnetOffset = ENCODER_OFFSET
+            }
+
+        encoder.configurator.apply(encoderConfig)
+    }
+
+    override fun setHeight(height: Distance) {
+        mainMotor.setControl(
+            mainMotorPositionRequest.withPosition(
+                height.toAngle(SPROCKET_RADIUS, ADJUSTED_GEAR_RATIO)
+            )
+        )
+    }
+
+    override fun setPower(percentOutput: Double) {
+        mainMotor.set(percentOutput)
+    }
+
+    override fun resetAbsoluteEncoder() {
+        mainMotor.setPosition(0.0)
+    }
+
+    override fun updateInputs() {
+        inputs.appliedVoltage = mainMotor.motorVoltage.value
+        inputs.height =
+            mainMotor.position.value.toDistance(
+                SPROCKET_RADIUS,
+                ADJUSTED_GEAR_RATIO
+            )
+        mainMotor.position.value.timesConversionFactor(ROTATIONS_TO_CENTIMETER)
+        inputs.noOffsetAbsoluteEncoderPosition = encoder.absolutePosition.value
+        inputs.absoluteEncoderHeight =
+            encoder.position.value.toDistance(
+                SPROCKET_RADIUS,
+                ADJUSTED_GEAR_RATIO
+            )
+    }
+}

src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOSim.kt

@@ -0,0 +1,44 @@
+package frc.robot.subsystems.elevator
+
+import com.ctre.phoenix6.controls.DutyCycleOut
+import com.ctre.phoenix6.controls.PositionVoltage
+import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.units.Units
+import edu.wpi.first.units.measure.Distance
+import edu.wpi.first.wpilibj.Timer
+import frc.robot.lib.motors.TalonFXSim
+import frc.robot.lib.motors.TalonType
+import frc.robot.lib.toAngle
+import frc.robot.lib.toDistance
+
+class ElevatorIOSim : ElevatorIO {
+    override val inputs = LoggedElevatorInputs()
+    private val motorPositionRequest = PositionVoltage(0.0)
+    private val dutyCycleRequest = DutyCycleOut(0.0)
+    private val angleController = PIDController(0.4, 0.0, 0.5)
+    private val motor = TalonFXSim(2, 1.0, 0.003, 1.0, TalonType.KRAKEN_FOC)
+
+    init {
+        motor.setController(angleController)
+    }
+
+    override fun setHeight(height: Distance) {
+        motor.setControl(
+            motorPositionRequest.withPosition(
+                height.toAngle(SPROCKET_RADIUS, ADJUSTED_GEAR_RATIO)
+            )
+        )
+    }
+
+    override fun setPower(percentOutput: Double) {
+        motor.setControl(dutyCycleRequest.withOutput(percentOutput))
+    }
+
+    override fun updateInputs() {
+        motor.update(Timer.getFPGATimestamp())
+        inputs.appliedVoltage = Units.Volts.of(motor.appliedVoltage)
+        inputs.height =
+            Units.Rotations.of(motor.position)
+                .toDistance(SPROCKET_RADIUS, ADJUSTED_GEAR_RATIO)
+    }
+}

src/main/kotlin/frc/robot/subsystems/elevator/ElevatorPorts.kt

@@ -0,0 +1,5 @@
+package frc.robot.subsystems.elevator
+
+const val MAIN_ID = 12
+const val AUX_ID = 11
+const val ENCODER_ID = 21