feeder sim stuff but unfinished

src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt

@@ -132,6 +132,10 @@ object Constants {
     const val REV_ENCODER_PORT = 7
   }
 
+  object Feeder {
+
+  }
+
   object Led {
     const val LED_CANDLE_ID = 61
     const val LED_BLINKEN_ID = 1

src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt

@@ -1,21 +1,17 @@
 package com.team4099.robot2023.config.constants
 
 import org.team4099.lib.units.Velocity
-import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.seconds
-import org.team4099.lib.units.derived.volts
-import org.team4099.lib.units.derived.rotations
-import org.team4099.lib.units.derived.perRotation
-import org.team4099.lib.units.derived.ProportionalGain
-import org.team4099.lib.units.derived.IntegralGain
-import org.team4099.lib.units.derived.DerivativeGain
-import org.team4099.lib.units.derived.Radian
-import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.*
 import org.team4099.lib.units.perMinute
 import org.team4099.lib.units.perSecond
 
 object FeederConstants {
     val FEEDER_INIT_VOLTAGE = 0.0.volts
+    val VOLTAGE_COMPENSATION = 0.0.volts
+
+    const val FEEDER_GEAR_RATIO = 0
+    const val FEEDER_INERTIA = 0
 
     // TODO: Add value to Feeder target velocity
     val FEED_NOTE_TARGET_VELOCITY = 0.rotations.perMinute

src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt

@@ -8,6 +8,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.controller.TrapezoidProfile
+import org.team4099.lib.units.Velocity
 
 import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.inSeconds
@@ -18,9 +19,22 @@ import org.team4099.lib.units.perSecond
 class Shooter (val io: ShooterIO){
     val inputs = ShooterIO.ShooterIOInputs()
     //TODO do feedforward
-    private var WristFeedforward: SimpleMotorFeedforward<Radian, Volt>()
+    private val wristkS =
+        LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts})
+        )
+    private val wristlkV =
+        LoggedTunableValue(
+            "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute })
+        )
+    private val wristkA =
+        LoggedTunableValue(
+            "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond })
+        )
+    val flywheelFeedForward = SimpleMotorFeedforward<Radian, Volt>(wristkS.get(), wristlkV.get(), wristkA.get())
+
+
+
 
-/*
     private val wristflywheelkP =
         LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch }))
     private val wristflywheelkI =
@@ -30,14 +44,14 @@ class Shooter (val io: ShooterIO){
     private val wristflywheelkD =
         LoggedTunableValue(
             "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond })
-        )*/
+        )
     var currentState = ShooterStates.UNINITIALIZED
     //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts
     var wristTargetVoltage : ElectricalPotential = 0.0.volts
     var feederTargetVoltage : ElectricalPotential = 0.0.volts
-  /*  fun setflywheelVoltage(appliedVoltage: ElectricalPotential){
-        io.setflywheelVoltage(appliedVoltage)
-    }*/
+    /*  fun setflywheelVoltage(appliedVoltage: ElectricalPotential){
+          io.setflywheelVoltage(appliedVoltage)
+      }*/
     fun setWristVoltage(appliedVoltage: ElectricalPotential){
         io.setWristVoltage(appliedVoltage)
     }
@@ -60,59 +74,63 @@ class Shooter (val io: ShooterIO){
         ShooterConstants.WRIST_INIT_VOLTAGE,
         //ShooterConstants.ROLLLER_INIT_VOLTAGE,
         //ShooterConstants.FEEDER_INIT_VOLTAGE
-        )
+    )
     private var wristProfile =
         TrapezoidProfile(
             wristConstraints,
             TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond),
             TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond)
         )
 
-fun periodic(){
-    io.updateInputs(inputs)
-    var nextState = currentState
-    when (currentState) {
-        ShooterStates.UNINITIALIZED -> {
-            nextState = ShooterStates.ZERO
-        }
-        ShooterStates.ZERO ->{
-        }
-
-        ShooterStates.OPEN_LOOP ->{
-            /*
-            setflywheelVoltage(flywheelTargetVoltage)
-            lastflywheelRunTime = Clock.fpgaTime
-*/
-            setWristVoltage(wristTargetVoltage)
-            lastWristRunTime = Clock.fpgaTime
-
-           /* setFeederVoltage(feederTargetVoltage)
-            lastFeederRunTime = Clock.fpgaTime*/
-            if (isZeroed == true ){
+    fun periodic(){
+        io.updateInputs(inputs)
+        var nextState = currentState
+        when (currentState) {
+            ShooterStates.UNINITIALIZED -> {
+                nextState = fromRequestToState(currentRequest)
+            }
+            ShooterStates.ZERO ->{
                 nextState = fromRequestToState(currentRequest)
             }
 
-        }
+            ShooterStates.OPEN_LOOP ->{
+                /*
+                setflywheelVoltage(flywheelTargetVoltage)
+                lastflywheelRunTime = Clock.fpgaTime
+    */
+                setWristVoltage(wristTargetVoltage)
+                lastWristRunTime = Clock.fpgaTime
+
+                /* setFeederVoltage(feederTargetVoltage)
+                 lastFeederRunTime = Clock.fpgaTime*/
+                if (isZeroed == true ){
+                    nextState = fromRequestToState(currentRequest)
+                }
+                nextState = fromRequestToState(currentRequest)
 
-        ShooterStates.TARGETING_POSITION ->{
-
-            if (wristPositionTarget!=lastWristPositionTarget){
-                val preProfileGenerate = Clock.fpgaTime
-                wristProfile = TrapezoidProfile(
-                    wristConstraints,
-                    TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond),
-                    TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity)
-                )
-                val postProfileGenerate = Clock.fpgaTime
-                Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds)
-                timeProfileGeneratedAt = Clock.fpgaTime
-                lastWristPositionTarget = wristPositionTarget
             }
-            val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt
-            setWristPosition( WristFeedforward, wristProfile.calculate(timeElapsed))
-            //TODO fix this error
-            Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed))
-        }
+
+            ShooterStates.TARGETING_POSITION ->{
+
+                if (wristPositionTarget!=lastWristPositionTarget){
+                    val preProfileGenerate = Clock.fpgaTime
+                    //TODO figure out how to implment feedforward here.
+                    wristProfile = TrapezoidProfile(
+                        wristConstraints,
+                        TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond),
+                        TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity)
+                    )
+                    val postProfileGenerate = Clock.fpgaTime
+                    Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds)
+                    timeProfileGeneratedAt = Clock.fpgaTime
+                    lastWristPositionTarget = wristPositionTarget
+                }
+                val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt
+                setWristPosition(wristProfile.calculate(timeElapsed))
+                //TODO fix this error
+                Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed))
+                nextState = fromRequestToState(currentRequest)
+            }
 
 
 
@@ -137,6 +155,4 @@ fun periodic(){
         }
     }
 
-}
-
-
+}

src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt

@@ -1,5 +1,6 @@
 package com.team4099.robot2023.subsystems.feeder
 
+import com.team4099.robot2023.subsystems.superstructure.Request
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
 import org.team4099.lib.units.*

src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt

@@ -0,0 +1,53 @@
+package com.team4099.robot2023.subsystems.feeder
+
+import edu.wpi.first.wpilibj.simulation.FlywheelSim
+import com.team4099.lib.math.clamp
+import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.FeederConstants
+import edu.wpi.first.math.system.plant.DCMotor
+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 FeederIOSim : FeederIO {
+    private val feederSim: FlywheelSim = FlywheelSim(
+        DCMotor.getNEO(1),
+        FeederConstants.FEEDER_GEAR_RATIO,
+        FeederConstants.FEEDER_INERTIA
+    )
+
+    override fun updateInputs(inputs: FeederIO.FeederIOInputs) {
+        feederSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
+
+        inputs.feederVelocity = feederSim.getAngularVelocityRPM().rotations.perMinute
+        inputs.feederAppliedVoltage = appliedVoltage
+        inputs.feederSupplyCurrent = 0.amps
+        inputs.feederStatorCurrent = feederSim.currentDrawAmps.amps
+        inputs.feederTemp = 0.0.celsius
+    }
+
+    override fun setRollerVoltage(voltage: ElectricalPotential) {
+        appliedVoltage = voltage
+        feederSim.setInputVoltage(
+            clamp(
+                voltage,
+                -FeederConstants.VOLTAGE_COMPENSATION,
+                FeederConstants.VOLTAGE_COMPENSATION
+            )
+                .inVolts
+        )
+    }
+
+    override fun setFeederVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
+
+    }
+
+    override fun configPID(kP: ProportionalGain<Velocity<Radian>, Volt>, kI: IntegralGain<Velocity<Radian>, Volt>, kD: DerivativeGain<Velocity<Radian>, Volt>) {
+
+    }
+}