Removed zero state

team4099 · Jan 14, 2024 · fdb3aa1 · fdb3aa1
1 parent 265511d
commit fdb3aa1
Show file tree

Hide file tree

Showing 2 changed files with 87 additions and 94 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt
@@ -1,110 +1,106 @@
 package com.team4099.robot2023.subsystems.intake
 
+import com.team4099.lib.hal.Clock
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.IntakeConstants
 import com.team4099.robot2023.subsystems.superstructure.Request
 import org.littletonrobotics.junction.Logger
+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.volts
 
-class Intake(private val io: IntakeIO) {
-  val inputs = IntakeIO.IntakeIOInputs()
-
-  var rollerVoltageTarget: ElectricalPotential = 0.0.volts
-
-  var isZeroed = false
-
-  var currentState: IntakeStake = IntakeStake.UNINITIALIZED
-
-  var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle()
-    set(value) {
-      rollerVoltageTarget =
-        when (value) {
-          is Request.IntakeRequest.OpenLoop -> {
-            value.rollerVoltage
-          }
-          is Request.IntakeRequest.Idle -> {
-            IntakeConstants.IDLE_ROLLER_VOLTAGE
-          }
+class Intake(val io: IntakeIO) {
+    val inputs = IntakeIO.IntakeIOInputs()
+    var rollerVoltageTarget: ElectricalPotential = 0.0.volts
+    var isZeroed = false
+    var currentState: IntakeStake = IntakeStake.UNINITIALIZED
+
+    var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle()
+        set(value) {
+            rollerVoltageTarget = when (value) {
+                is Request.IntakeRequest.OpenLoop -> {
+                    value.rollerVoltage
+                }
+
+                is Request.IntakeRequest.Idle -> {
+                    IntakeConstants.IDLE_ROLLER_VOLTAGE
+                }
+            }
+
+            field = value
         }
 
-      field = value
-    }
-
-  fun periodic() {
-    io.updateInputs(inputs)
+    private var timeProfileGeneratedAt = Clock.fpgaTime
 
-    Logger.processInputs("Intake", inputs)
+    fun periodic() {
+        io.updateInputs(inputs)
 
-    Logger.recordOutput("Intake/currentState", currentState.name)
+        Logger.processInputs("GroundIntake", inputs)
+        Logger.recordOutput("GroundIntake/currentState", currentState.name)
+        Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName)
+        Logger.recordOutput("GroundIntake/isZeroed", isZeroed)
 
-    Logger.recordOutput("Intake/requestedState", currentRequest.javaClass.simpleName)
-
-    Logger.recordOutput("Intake/isZeroed", isZeroed)
+        if (Constants.Tuning.DEBUGING_MODE) {
+            Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest))
+            Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds)
+            Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts)
+        }
 
-    if (Constants.Tuning.DEBUGING_MODE) {
-      Logger.recordOutput(
-        "Intake/isAtCommandedState", currentState.equivalentToRequest(currentRequest)
-      )
+        var nextState = currentState
+        when (currentState) {
+            IntakeStake.UNINITIALIZED -> {
+                // Outputs
+                // No designated output functionality because targeting position will take care of it next
+                // loop cycle
+
+                // Transitions
+                nextState = IntakeStake.IDLE
+            }
+            IntakeStake.IDLE -> {
+                setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE)
+
+                // Transitions
+                nextState = fromRequestToState(currentRequest)
+            }
+            IntakeStake.OPEN_LOOP_REQUEST -> {
+                setRollerVoltage(rollerVoltageTarget)
+
+                // Transitions
+                nextState = fromRequestToState(currentRequest)
+            }
+        }
 
-      Logger.recordOutput("Intake/rollerVoltageTarget", rollerVoltageTarget.inVolts)
+        // The next loop cycle, we want to run ground intake at the state that was requested. setting
+        // current state to the next state ensures that we run the logic for the state we want in the
+        // next loop cycle.
+        currentState = nextState
     }
 
-    var nextState = currentState
-    when (currentState) {
-      IntakeStake.UNINITIALIZED -> {
-        // Outputs
-        // No designated output functionality because targeting position will take care of it next
-        // loop cycle
-
-        // Transitions
-        nextState = IntakeStake.IDLE
-      }
-      IntakeStake.IDLE -> {
-        setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE)
-
-        // Transitions
-        nextState = fromRequestToState(currentRequest)
-      }
-      IntakeStake.OPEN_LOOP -> {
-        setRollerVoltage(rollerVoltageTarget)
-
-        // Transitions
-        nextState = fromRequestToState(currentRequest)
-      }
+    /** @param appliedVoltage Represents the applied voltage of the roller motor */
+    fun setRollerVoltage(appliedVoltage: ElectricalPotential) {
+        io.setRollerVoltage(appliedVoltage)
     }
 
-    // The next loop cycle, we want to run ground intake at the state that was requested. setting
-    // current state to the next state ensures that we run the logic for the state we want in the
-    // next loop cycle.
-    currentState = nextState
-  }
-
-  /** @param appliedVoltage Represents the applied voltage of the roller motor */
-  fun setRollerVoltage(appliedVoltage: ElectricalPotential) {
-    io.setRollerVoltage(appliedVoltage)
-  }
-
-  companion object {
-    enum class IntakeStake {
-      UNINITIALIZED,
-      IDLE,
-      OPEN_LOOP;
-
-      inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean {
-        return (
-          (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) ||
-            (request is Request.IntakeRequest.Idle && this == IDLE)
-          )
-      }
-    }
+    companion object {
+        enum class IntakeStake {
+            UNINITIALIZED,
+            IDLE,
+            OPEN_LOOP_REQUEST;
+
+            fun equivalentToRequest(request: Request.IntakeRequest): Boolean {
+                return (
+                        (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) ||
+                                (request is Request.IntakeRequest.Idle && this == IDLE)
+                        )
+            }
+        }
 
-    inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake {
-      return when (request) {
-        is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP
-        is Request.IntakeRequest.Idle -> IntakeStake.IDLE
-      }
+        fun fromRequestToState(request: Request.IntakeRequest): IntakeStake {
+            return when (request) {
+                is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST
+                is Request.IntakeRequest.Idle -> IntakeStake.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
@@ -21,13 +21,10 @@ interface IntakeIO {
 
     var rollerSupplyCurrent = 0.0.amps
     var rollerStatorCurrent = 0.0.amps
-    var rollerTemp = 0.0.celsius
 
-    var isSimulated = false
+    var rollerTemp = 0.0.celsius
 
     override fun toLog(table: LogTable?) {
-      table?.put("isSimulated", isSimulated)
-
       table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)
       table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)
       table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)
@@ -56,9 +53,9 @@ interface IntakeIO {
     }
   }
 
-  fun updateInputs(io: IntakeIOInputs)
+  fun updateInputs(io: IntakeIOInputs) {}
 
-  fun setRollerVoltage(voltage: ElectricalPotential)
+  fun setRollerVoltage(voltage: ElectricalPotential) {}
 
-  fun setRollerBrakeMode(brake: Boolean)
+  fun setRollerBrakeMode(brake: Boolean) {}
 }