From c4c3d3006ffd711ec0be195f0f351b299ac70380 Mon Sep 17 00:00:00 2001
From: Nikola Jenkins <naojoy5@gmail.com>
Date: Tue, 21 Jan 2025 20:06:43 -0800
Subject: [PATCH] elevator simulation

---
 .../kotlin/com/frcteam3636/frc2025/Robot.kt   |  2 +-
 .../frc2025/subsystems/elevator/ElevatorIO.kt | 89 ++++++++++++++++++-
 2 files changed, 89 insertions(+), 2 deletions(-)

diff --git a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt
index 545e244..fb36751 100644
--- a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt
+++ b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt
@@ -43,7 +43,7 @@ import kotlin.io.path.exists
  * renaming the object or package, it will get changed everywhere.)
  */
 object Robot : LoggedRobot() {
-    private val controller = CommandXboxController(2)
+    val controller = CommandXboxController(2)
     private val joystickLeft = Joystick(0)
     private val joystickRight = Joystick(1)
 
diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt
index 4c003d7..73312ba 100644
--- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt
+++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt
@@ -8,21 +8,36 @@ 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()!!
@@ -120,3 +135,75 @@ class ElevatorIOReal: ElevatorIO {
     }
 
 }
+
+class ElevatorIOSim: ElevatorIO {
+    // Simulation classes help us simulate what's going on, including gravity.
+    var motor = DCMotor.getKrakenX60Foc(2)
+    var motorSim = DCMotorSim(
+        LinearSystemId.createDCMotorSystem(motor,0.0000763789,25.0),
+        motor,
+        0.0
+    )
+
+    private val elevatorSim: ElevatorSim = ElevatorSim(
+        motor,
+        GEAR_RATIO,
+        CARRIAGE_MASS,
+        DRUM_RADIUS,
+        MIN_HEIGHT,
+        MAX_HEIGHT,
+        true,
+        0.01,
+        0.0
+    )
+
+    var controller = ProfiledPIDController(
+        PID_GAINS.p,
+        PID_GAINS.i,
+        PID_GAINS.d,
+        TRAPEZOID_CONSTRAINTS
+    )
+
+    var feedforward = ElevatorFeedforward(
+        FF_GAINS.s,
+        FF_GAINS.v,
+        FF_GAINS.a
+    )
+
+    var encoder = Encoder(0,1)
+    var encoderSim = EncoderSim(encoder)
+
+    override fun updateInputs(inputs: ElevatorInputs) {
+        elevatorSim.setInput(motorSim.angularVelocity * RobotController.getBatteryVoltage())
+        elevatorSim.update(.020)
+        encoderSim.setDistance(elevatorSim.positionMeters)
+        RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps))
+    }
+
+    override fun runToHeight(height: Distance) {
+        var pidOutput = controller.calculate(encoder.distance)
+        var feedforwardOutput = feedforward.calculate(controller.setpoint.velocity)
+        motor.setVoltage(pidOutput + feedforwardOutput)
+    }
+
+    override fun setVoltage(volts: Voltage) {
+        elevatorSim.setInputVoltage(volts.`in`(Volts))
+        Logger.recordOutput("/Elevator/OutVolt", volts)
+    }
+
+    internal companion object Constants {
+        val DISTANCE_PER_TURN = Meters.per(Rotation).of(0.0)!!
+        private const val GEAR_RATIO = 0.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, 0.0, 0.0)
+        var TRAPEZOID_CONSTRAINTS = TrapezoidProfile.Constraints(1.0, 1.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
+    }
+}
\ No newline at end of file