Add elevator code

.gitignore

@@ -164,7 +164,7 @@ annotation/build/
 # Simulation GUI and other tools window save file
 *-window.json
 
-.\src\main\kotlin\frc\robot\BuildConstants.java
+/src/main/kotlin/frc/robot/BuildConstants.java
 
 simgui*
 networktables.json

files/cad/config.json

@@ -0,0 +1,119 @@
+{
+  "name":"Andromeda",
+  "rotations": [
+    {
+      "axis": "x",
+      "degrees": 90
+    },
+    {
+      "axis": "z",
+      "degrees": -90
+    }
+  ],
+  "position": [
+    -0.651,
+    0,
+    0
+  ],
+  "cameras":[],
+  "components":[
+    {
+      "zeroedRotations":[
+        {
+          "axis": "x",
+          "degrees": 90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        }
+      ],
+      "zeroedPosition":[
+        -0.61,
+        0,
+        0
+      ]
+    },
+    {
+      "zeroedRotations":[
+      {
+          "axis": "y",
+          "degrees": -90
+        },
+        {
+          "axis": "x",
+          "degrees": -90
+        }
+      ],
+      "zeroedPosition":[
+        -0.44,
+        0,
+        0.265
+      ]
+    },
+    {
+      "zeroedRotations":[
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition":[
+        -0.80,
+        0,
+        -0.12
+      ]
+    },
+    {
+      "zeroedRotations":[
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition":[
+        -0.80,
+        0,
+        -0.15
+      ]
+    },
+    {
+      "zeroedRotations":[
+        {
+          "axis": "x",
+          "degrees": -90
+        },
+        {
+          "axis": "z",
+          "degrees": -90
+        },
+        {
+          "axis": "y",
+          "degrees": 180
+        }
+      ],
+      "zeroedPosition":[
+        -0.80,
+        0,
+        -0.80
+      ]
+    }
+
+  ]
+}

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

@@ -0,0 +1 @@
+

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

@@ -0,0 +1,37 @@
+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 elevator = root.append(LoggedMechanismLigament2d("ElevatorLigament", 1.0, 90.0))
+    @AutoLogOutput private var positionSetpoint: Distance = Units.Millimeters.of(0.0)
+
+    fun setPosition(position: Distance): Command = runOnce {
+        positionSetpoint = position
+        io.setHeight(position)
+    }
+
+    fun setPower(percentOutput: DoubleSupplier): Command = run {
+        io.setPower(percentOutput.asDouble)
+    }
+
+    fun resetAbsoluteEncoder(): Command = runOnce(io::resetAbsoluteEncoder)
+
+    override fun periodic() {
+        io.updateInputs()
+        Logger.processInputs(this.name, io.inputs)
+        Logger.recordOutput("Elevator/Mechanism2d", mechanism)
+
+        elevator.length = io.inputs.height.`in`(Units.Meters)
+    }
+}

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

@@ -0,0 +1,24 @@
+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
+
+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 WEIGHT = 0.0
+const val ENCODER_OFSET = 0.0
+private 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())

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

@@ -0,0 +1,28 @@
+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(position: Distance) {}
+
+    fun setPower(percentOutput: Double) {}
+
+    fun resetAbsoluteEncoder() {}
+
+    fun updateInputs() {}
+
+    @Logged
+    open class ElevatorInputs {
+        var height: Distance = Units.Meters.of(0.0)
+        var setpoint: 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,79 @@
+package frc.robot.subsystems.elevator
+
+import ENCODER_ID
+import MOTOR_ID
+import com.ctre.phoenix6.configs.*
+import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC
+import com.ctre.phoenix6.controls.PositionVoltage
+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
+
+class ElevatorIOReal : ElevatorIO {
+    override val inputs = LoggedElevatorInputs()
+    private val motor = TalonFX(MOTOR_ID)
+    private val encoder = CANcoder(ENCODER_ID)
+    private val motorPositionRequest = 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
+                    }
+            }
+        motor.configurator.apply(motorConfig)
+
+        val encoderConfig =
+            CANcoderConfiguration().apply {
+                MagnetSensor.MagnetOffset = ENCODER_OFSET
+            }
+
+        encoder.configurator.apply(encoderConfig)
+    }
+
+    override fun setHeight(height: Distance) {
+        motor.setControl(
+            motorPositionRequest.withPosition(
+                height.timesConversionFactor(CENTIMETERS_TO_ROTATIONS)
+            )
+        )
+    }
+
+    override fun setPower(percentOutput: Double) {
+        motor.set(percentOutput)
+    }
+
+    override fun resetAbsoluteEncoder() {
+        motor.setPosition(0.0)
+    }
+
+    override fun updateInputs() {
+        inputs.appliedVoltage = motor.motorVoltage.value
+        inputs.height =
+            motor.position.value.timesConversionFactor(ROTATIONS_TO_CENTIMETER)
+        inputs.noOffsetAbsoluteEncoderPosition = encoder.absolutePosition.value
+        inputs.absoluteEncoderHeight =
+            encoder.position.value.timesConversionFactor(
+                ROTATIONS_TO_CENTIMETER
+            )
+    }
+}

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

@@ -0,0 +1,48 @@
+package frc.robot.subsystems.elevator
+
+import com.ctre.phoenix6.controls.DutyCycleOut
+import com.ctre.phoenix6.controls.PositionVoltage
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.system.plant.LinearSystemId
+import edu.wpi.first.units.Units
+import edu.wpi.first.units.measure.Distance
+import frc.robot.lib.motors.TalonFXSim
+import frc.robot.lib.motors.TalonType
+
+class ElevatorIOSim : ElevatorIO {
+    override val inputs = LoggedElevatorInputs()
+    private val motorPosititonRequest = PositionVoltage(0.0)
+    private val dutyCycleRequest = DutyCycleOut(0.0)
+    private val motor =
+        TalonFXSim(
+            LinearSystemId.createElevatorSystem(
+                DCMotor.getKrakenX60(2),
+                WEIGHT,
+                36.4 / 2,
+                GEAR_RATIO
+            ),
+            2,
+            GEAR_RATIO,
+            1.0,
+            TalonType.KRAKEN
+        )
+
+    override fun setHeight(position: Distance) {
+        motor.setControl(
+            motorPosititonRequest.withPosition(
+                position.timesConversionFactor(CENTIMETERS_TO_ROTATIONS)
+            )
+        )
+    }
+
+    override fun setPower(percentOutput: Double) {
+        motor.setControl(dutyCycleRequest.withOutput(percentOutput))
+    }
+
+    override fun updateInputs() {
+        inputs.appliedVoltage = Units.Volts.of(motor.appliedVoltage)
+        inputs.height =
+            Units.Rotations.of(motor.position)
+                .timesConversionFactor(ROTATIONS_TO_CENTIMETER)
+    }
+}

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

@@ -0,0 +1,2 @@
+const val MOTOR_ID = 12
+const val ENCODER_ID = 21