elevatorSim (#46)

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

@@ -2,6 +2,7 @@ package com.frcteam3636.frc2025
 
 import com.ctre.phoenix6.StatusSignal
 import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain
+import com.frcteam3636.frc2025.subsystems.elevator.Elevator
 import com.frcteam3636.frc2025.utils.Elastic
 import com.frcteam3636.frc2025.utils.ElasticNotification
 import com.frcteam3636.frc2025.utils.NotificationLevel
@@ -127,6 +128,7 @@ object Robot : LoggedRobot() {
     /** Start robot subsystems so that their periodic tasks are run */
     private fun configureSubsystems() {
         Drivetrain.register()
+        Elevator.register()
     }
 
     /** Expose commands for autonomous routines to use and display an auto picker in Shuffleboard. */

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

@@ -9,7 +9,7 @@ import org.littletonrobotics.junction.Logger
 
 object Elevator: Subsystem {
     private val io: ElevatorIO = when (Robot.model) {
-        Robot.Model.SIMULATION -> TODO()
+        Robot.Model.SIMULATION -> ElevatorIOSim()
         Robot.Model.COMPETITION -> ElevatorIOReal()
         Robot.Model.PROTOTYPE -> TODO()
     }
@@ -35,12 +35,12 @@ object Elevator: Subsystem {
 //        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)),
+        Stowed(Meters.of(0.254000)),
+        Trough(Meters.of(0.254000)),
+        LowBar(Meters.of(0.050800)),
+        MidBar(Meters.of(0.254000)),
+        HighBar(Meters.of(1.219200)),
+//        LowAlgae(Meters.of(0.0)),
+//        HighAlgae(Meters.of(0.0)),
     }
 }

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

@@ -8,26 +8,41 @@ 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.ctre.phoenix6.sim.TalonFXSimState
+import com.frcteam3636.frc2025.CANcoder
 import com.frcteam3636.frc2025.CTREDeviceId
+import com.frcteam3636.frc2025.Robot
+import com.frcteam3636.frc2025.Robot.controller
 import com.frcteam3636.frc2025.TalonFX
-import com.frcteam3636.frc2025.CANcoder
+import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain
+import com.frcteam3636.frc2025.subsystems.elevator.ElevatorIOReal.Constants
 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.math.controller.ElevatorFeedforward
+import edu.wpi.first.math.controller.ProfiledPIDController
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.system.plant.LinearSystemId
+import edu.wpi.first.math.trajectory.TrapezoidProfile
 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 edu.wpi.first.wpilibj.Encoder
+import edu.wpi.first.wpilibj.RobotController
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax
+import edu.wpi.first.wpilibj.simulation.*
 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 rightCurrent = Amps.zero()!!
+    var leftCurrent = Amps.zero()!!
     var velocity = MetersPerSecond.zero()!!
 }
 
@@ -89,8 +104,8 @@ class ElevatorIOReal: ElevatorIO {
     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
+        inputs.rightCurrent = rightElevatorMotor.torqueCurrent.value
+        inputs.leftCurrent = leftElevatorMotor.torqueCurrent.value
     }
 
     override fun runToHeight(height: Distance) {
@@ -120,3 +135,64 @@ class ElevatorIOReal: ElevatorIO {
     }
 
 }
+
+class ElevatorIOSim: ElevatorIO {
+    // Simulation classes help us simulate what's going on, including gravity.
+    private var motor: DCMotor = DCMotor.getKrakenX60Foc(2)
+
+    private val elevatorSim: ElevatorSim = ElevatorSim(
+        motor,
+        GEAR_RATIO,
+        CARRIAGE_MASS,
+        DRUM_RADIUS,
+        MIN_HEIGHT,
+        MAX_HEIGHT,
+        true,
+        0.01,
+        0.0
+    )
+
+    private var controller = ProfiledPIDController(
+        PID_GAINS.p,
+        PID_GAINS.i,
+        PID_GAINS.d,
+        TRAPEZOID_CONSTRAINTS
+    )
+
+    private var feedforward = ElevatorFeedforward(
+        FF_GAINS.s,
+        FF_GAINS.v,
+        FF_GAINS.a
+    )
+
+    override fun updateInputs(inputs: ElevatorInputs) {
+        elevatorSim.update(Robot.period)
+        inputs.height = Meters.of(elevatorSim.positionMeters)
+        inputs.velocity = MetersPerSecond.of(elevatorSim.velocityMetersPerSecond)
+        inputs.leftCurrent = Amps.of(elevatorSim.currentDrawAmps)
+        inputs.rightCurrent = Amps.of(elevatorSim.currentDrawAmps)
+        RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps))
+    }
+
+    override fun runToHeight(height: Distance) {
+        val pidOutput = controller.calculate(elevatorSim.positionMeters)
+        val feedforwardOutput = feedforward.calculate(controller.setpoint.velocity)
+        elevatorSim.setInputVoltage(pidOutput + feedforwardOutput)
+    }
+
+    override fun setVoltage(volts: Voltage) {
+        elevatorSim.setInputVoltage(volts.`in`(Volts))
+        Logger.recordOutput("/Elevator/OutVolt", volts)
+    }
+
+    internal companion object Constants {
+        private const val GEAR_RATIO = 1.0
+        private const val CARRIAGE_MASS = 1.62519701308126
+        private const val MIN_HEIGHT = 0.254000
+        private const val MAX_HEIGHT = 2.298700
+        private const val DRUM_RADIUS = 0.028575/2
+        val PID_GAINS = PIDGains(0.0, 0.0, 0.0)
+        val FF_GAINS = MotorFFGains(0.0, 2.77, 0.05)
+        var TRAPEZOID_CONSTRAINTS = TrapezoidProfile.Constraints(4.16, 8.2119724)
+    }
+}