Completed Feeder and FeederIO

team4099 · Jan 21, 2024 · e815efe · e815efe
1 parent f309b35
commit e815efe
Show file tree

Hide file tree

Showing 3 changed files with 61 additions and 39 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt
@@ -17,12 +17,19 @@ import org.team4099.lib.units.perSecond
 object FeederConstants {
     val FEEDER_INIT_VOLTAGE = 0.0.volts
 
+    // TODO: Add value to Feeder target velocity
+    val FEED_NOTE_TARGET_VELOCITY = 0.rotations.perMinute
+
     // TODO: Tune PID variables
     val FEEDER_KS = 0.001.volts
     val FEEDER_KV = 0.01.volts.perRotation.perMinute
     val FEEDER_KA = 0.01.volts.perRotation.perMinute.perSecond
 
-    val FEEDER_KP: ProportionalGain<Velocity<Radian>, Volt> = 0.001.volts / 1.0.rotations.perMinute
-    val FEEDER_KI: IntegralGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
-    val FEEDER_KD: DerivativeGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds)
+    val FEEDER_REAL_KP: ProportionalGain<Velocity<Radian>, Volt> = 0.001.volts / 1.0.rotations.perMinute
+    val FEEDER_REAL_KI: IntegralGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
+    val FEEDER_REAL_KD: DerivativeGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds)
+
+    val FEEDER_SIM_KP: ProportionalGain<Velocity<Radian>, Volt> = 0.001.volts / 1.0.rotations.perMinute
+    val FEEDER_SIM_KI: IntegralGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
+    val FEEDER_SIM_KD: DerivativeGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds)
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt
@@ -3,83 +3,96 @@ package com.team4099.robot2023.subsystems.feeder
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.FeederConstants
-import com.team4099.robot2023.config.constants.FlywheelConstants
 import com.team4099.robot2023.subsystems.superstructure.Request
+import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.team4099.lib.controller.SimpleMotorFeedforward
-import org.team4099.lib.units.base.Meter
+import org.team4099.lib.units.*
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.*
+import kotlin.Pair
 
 class Feeder(val io: FeederIO): SubsystemBase() {
-    val inputs = FeederIO.FeederIOInputs()
-
-    var feederTargetVoltage : ElectricalPotential = 0.0.volts
-
-    var lastFeederRunTime = 0.0.seconds
-    var lastFeederVoltage = 0.0.volts
-
-    var isZeroed = false
+    val kP = LoggedTunableValue("Feeder/kP", Pair({it.inVoltsPerRotationPerMinute}, {it.volts / 1.0.rotations.perMinute}))
+    val kI = LoggedTunableValue("Feeder/kI", Pair({it.inVoltsPerRotations}, {it.volts / (1.0.rotations.perMinute * 1.0.seconds)}))
+    val kD = LoggedTunableValue("Feeder/kD", Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts / 1.0.rotations.perMinute.perSecond}))
 
-    private val kP = LoggedTunableValue("Feeder/kP", FlywheelConstants.SHOOTER_FLYWHEEL_KP)
-    private val kI = LoggedTunableValue("Feeder/kI", FlywheelConstants.SHOOTER_FLYWHEEL_KI)
-    private val kD = LoggedTunableValue("Feeder/kD", FlywheelConstants.SHOOTER_FLYWHEEL_KD)
+    val kS = LoggedTunableValue("Feeder/kS", FeederConstants.FEEDER_KS, Pair({it.inVolts}, {it.volts}))
+    val kV = LoggedTunableValue("Feeder/kV", FeederConstants.FEEDER_KV, Pair({it.inVoltsPerRotationPerMinute}, {it.volts.perRotation.perMinute}))
+    val kA = LoggedTunableValue("Feeder/kA", FeederConstants.FEEDER_KA, Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts.perRotation.perMinute.perSecond}))
 
-    private val kS = LoggedTunableValue("Feeder/kS", Pair({it.inVoltsPerInch}, {it.volts.perInch}))
-    private val kV = LoggedTunableValue("Feeder/kV", Pair({it.inVoltsPerInchSeconds}, {it.volts.perInchSeconds}))
-    private val kA = LoggedTunableValue("Feeder/kA", Pair({it.inVoltsPerInchPerSecond}, {it.volts.perInchPerSecond}))
-
-    var feederInitVoltage = LoggedTunableValue ("Shooter/Initial Feeder Voltage", FeederConstants.FEEDER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts}))
-    var timeProfileGeneratedAt = Clock.fpgaTime
+    val inputs = FeederIO.FeederIOInputs()
 
-    var currentState = FeederStates.UNINITIALIZED
+    var feederFeedForward: SimpleMotorFeedforward<Radian, Volt> = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get())
+    var feederTargetVoltage: ElectricalPotential = 0.0.volts
+    var feederTargetVelocity: AngularVelocity = FeederConstants.FEED_NOTE_TARGET_VELOCITY
+    var lastFeederRunTime = 0.0.seconds
 
+    var currentState: FeederStates = FeederStates.UNINITIALIZED
     var currentRequest: Request.FeederRequest = Request.FeederRequest.Idle()
-        set (value) {
-            feederTargetVoltage = when (value) {
-                is Request.FeederRequest.OpenLoop -> {
-                    value.feederVoltage
-                }
-
-                is Request.FeederRequest.Idle -> {
-                    FeederConstants.FEEDER_INIT_VOLTAGE
-                }
-            }
 
-            field = value
+    init {
+        if (RobotBase.isReal()) {
+            kP.initDefault(FeederConstants.FEEDER_REAL_KP)
+            kI.initDefault(FeederConstants.FEEDER_REAL_KI)
+            kD.initDefault(FeederConstants.FEEDER_REAL_KD)
+        } else {
+            kP.initDefault(FeederConstants.FEEDER_SIM_KP)
+            kI.initDefault(FeederConstants.FEEDER_SIM_KI)
+            kD.initDefault(FeederConstants.FEEDER_SIM_KD)
         }
+    }
 
     fun setFeederVoltage(appliedVoltage: ElectricalPotential){
         io.setFeederVoltage(appliedVoltage)
     }
 
+    fun setFeederVelocity(velocity: AngularVelocity) {
+        io.setFeederVelocity(velocity, feederFeedForward.calculate(velocity))
+    }
+
     override fun periodic() {
         io.updateInputs(inputs)
 
-        val nextState = when (currentState) {
+        if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) {
+            io.configPID(kP.get(), kI.get(), kD.get())
+        }
+
+        if (kS.hasChanged() || kV.hasChanged() || kA.hasChanged()) {
+            feederFeedForward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get())
+        }
+
+        val nextState: FeederStates = when (currentState) {
             FeederStates.UNINITIALIZED -> {
                 FeederStates.IDLE
             }
 
             FeederStates.IDLE -> {
                 setFeederVoltage(FeederConstants.FEEDER_INIT_VOLTAGE)
+                lastFeederRunTime = Clock.fpgaTime
                 fromRequestToState(currentRequest)
             }
 
             FeederStates.OPEN_LOOP -> {
                 setFeederVoltage(feederTargetVoltage)
+                lastFeederRunTime = Clock.fpgaTime
                 fromRequestToState(currentRequest)
             }
-        }
 
-        currentState = nextState
+            FeederStates.TARGETING_VELOCITY -> {
+                setFeederVelocity(feederTargetVelocity)
+                lastFeederRunTime = Clock.fpgaTime
+                fromRequestToState(currentRequest)
+            }
+        }
     }
 
     companion object {
         enum class FeederStates {
             UNINITIALIZED,
             IDLE,
-            OPEN_LOOP;
+            OPEN_LOOP,
+            TARGETING_VELOCITY;
 
             fun equivalentToRequest(request: Request.FeederRequest): Boolean {
                 return((request is Request.FeederRequest.OpenLoop && this == OPEN_LOOP) || (request is Request.FeederRequest.Idle && this == IDLE))
@@ -90,6 +103,7 @@ class Feeder(val io: FeederIO): SubsystemBase() {
             return when (request) {
                 is Request.FeederRequest.Idle -> FeederStates.IDLE
                 is Request.FeederRequest.OpenLoop -> FeederStates.OPEN_LOOP
+                is Request.FeederRequest.TargetingVelocity -> FeederStates.TARGETING_VELOCITY
             }
         }
     }

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
@@ -84,7 +84,8 @@ sealed interface Request {
   }
 
   sealed interface FeederRequest : Request {
-    class OpenLoop(val feederVoltage: ElectricalPotential, val flywheelVoltage: ElectricalPotential): FeederRequest {}
     class Idle(): FeederRequest {}
+    class OpenLoop(val feederVoltage: ElectricalPotential): FeederRequest {}
+    class TargetingVelocity(val feederVelocity: AngularVelocity): FeederRequest {}
   }
 }