Initial commit for IntakeIOSim

plus syntax changes stake --> state and OPEN_LOOP_REQUEST --> OPEN_LOOP
team4099 · Jan 14, 2024 · da6d21a · da6d21a
1 parent fdb3aa1
commit da6d21a
Show file tree

Hide file tree

Showing 4 changed files with 64 additions and 13 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
@@ -1,11 +1,11 @@
 package com.team4099.robot2023.config.constants
 
 import org.team4099.lib.units.base.amps
-import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.volts
 
 object IntakeConstants {
-  val VOLTAGE_COMPENSATION = 12.0.volts
+    val ROLLER_INERTIA = 0.0
+    val VOLTAGE_COMPENSATION = 12.0.volts
 
   // TODO: Change gear ratio according to robot
   val ROLLER_CURRENT_LIMIT = 50.0.amps

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt
@@ -14,7 +14,7 @@ class Intake(val io: IntakeIO) {
     val inputs = IntakeIO.IntakeIOInputs()
     var rollerVoltageTarget: ElectricalPotential = 0.0.volts
     var isZeroed = false
-    var currentState: IntakeStake = IntakeStake.UNINITIALIZED
+    var currentState: IntakeState = IntakeState.UNINITIALIZED
 
     var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle()
         set(value) {
@@ -49,21 +49,21 @@ class Intake(val io: IntakeIO) {
 
         var nextState = currentState
         when (currentState) {
-            IntakeStake.UNINITIALIZED -> {
+            IntakeState.UNINITIALIZED -> {
                 // Outputs
                 // No designated output functionality because targeting position will take care of it next
                 // loop cycle
 
                 // Transitions
-                nextState = IntakeStake.IDLE
+                nextState = IntakeState.IDLE
             }
-            IntakeStake.IDLE -> {
+            IntakeState.IDLE -> {
                 setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE)
 
                 // Transitions
                 nextState = fromRequestToState(currentRequest)
             }
-            IntakeStake.OPEN_LOOP_REQUEST -> {
+            IntakeState.OPEN_LOOP -> {
                 setRollerVoltage(rollerVoltageTarget)
 
                 // Transitions
@@ -83,23 +83,23 @@ class Intake(val io: IntakeIO) {
     }
 
     companion object {
-        enum class IntakeStake {
+        enum class IntakeState {
             UNINITIALIZED,
             IDLE,
-            OPEN_LOOP_REQUEST;
+            OPEN_LOOP;
 
             fun equivalentToRequest(request: Request.IntakeRequest): Boolean {
                 return (
-                        (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) ||
+                        (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) ||
                                 (request is Request.IntakeRequest.Idle && this == IDLE)
                         )
             }
         }
 
-        fun fromRequestToState(request: Request.IntakeRequest): IntakeStake {
+        fun fromRequestToState(request: Request.IntakeRequest): IntakeState {
             return when (request) {
-                is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST
-                is Request.IntakeRequest.Idle -> IntakeStake.IDLE
+                is Request.IntakeRequest.OpenLoop -> IntakeState.OPEN_LOOP
+                is Request.IntakeRequest.Idle -> IntakeState.IDLE
             }
         }
     }

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt
@@ -24,6 +24,8 @@ interface IntakeIO {
 
     var rollerTemp = 0.0.celsius
 
+    var isSimulated = false
+
     override fun toLog(table: LogTable?) {
       table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)
       table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt
@@ -0,0 +1,49 @@
+package com.team4099.robot2023.subsystems.intake
+
+import com.team4099.lib.math.clamp
+import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.IntakeConstants
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.wpilibj.simulation.FlywheelSim
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.celsius
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.perMinute
+
+class IntakeIOSim : IntakeIO {
+    private val rollerSim: FlywheelSim = FlywheelSim(
+        DCMotor.getNEO(1),
+        IntakeConstants.ROLLER_GEAR_RATIO,
+        IntakeConstants.ROLLER_INERTIA
+    )
+
+    init{}
+
+    override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) {
+
+        rollerSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
+
+        inputs.rollerVelocity = rollerSim.getAngularVelocityRPM().rotations.perMinute
+        inputs.rollerAppliedVoltage = 0.volts
+        inputs.rollerSupplyCurrent = 0.amps
+        inputs.rollerStatorCurrent = rollerSim.currentDrawAmps.amps
+        inputs.rollerTemp = 0.0.celsius
+
+        inputs.isSimulated = true
+    }
+
+    override fun setRollerVoltage(voltage: ElectricalPotential) {
+        rollerSim.setInputVoltage(
+            clamp(
+                voltage,
+                -IntakeConstants.VOLTAGE_COMPENSATION,
+                IntakeConstants.VOLTAGE_COMPENSATION
+            )
+                .inVolts
+        )
+    }
+}