wrote update IO function fo flywheel and started feedforward for wrist

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

@@ -59,4 +59,6 @@ object FlywheelConstants {
     val FLYWHEEL_KS = 0.001.volts
     val FLYWHEEL_KV = 0.01.volts/ 1.rotations.perMinute
     val FLYWHEEL_KA = 0.01.volts/ 1.rotations.perMinute.perSecond
+
+    val FLYWHEEL_INIT_VOLTAGE = 0.0.volts
 }

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

@@ -7,6 +7,7 @@ import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.FlywheelConstants
 import com.team4099.robot2023.subsystems.falconspin.MotorChecker
+import com.team4099.robot2023.subsystems.superstructure.Request
 import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.Velocity
@@ -53,7 +54,9 @@ class Flywheel (val io: FlywheelIO) {
     private var hasNote:Boolean = true
     val desiredVelocity: AngularVelocity = 1800.rotations.perMinute
     var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED
+    var currentRequest = Request.FlywheelRequest.OpenLoop(FlywheelConstants.FLYWHEEL_INIT_VOLTAGE)
 init{
+    //TODO figure out what else needs to run in the init function
     io.configPID(FlywheelConstants.SHOOTER_FLYWHEEL_KP, FlywheelConstants.SHOOTER_FLYWHEEL_KI, FlywheelConstants.SHOOTER_FLYWHEEL_KD)
 }
     fun periodic(){
@@ -66,16 +69,21 @@ init{
             Flywheel.Companion.FlywheelStates.OPEN_LOOP -> {
                 setFlywheelVoltage(flywheelTargetVoltage)
                 lastFlywheelRunTime = Clock.fpgaTime
+
+                nextState = fromRequestToState(currentRequest)
             }
+            //TODO do sensor logic (mayb ask pranav)
             Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{
                 if (flywheelTargetVoltage != lastFlywheelVoltage){
                     val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity)
-                    io.setFlywheelVelocity(inputs.flywheelVelocity, controlEffort)//TODO talk to anshi ab a current velocity var
+                    io.setFlywheelVelocity(inputs.flywheelVelocity, controlEffort)
                     io.setFlywheelVoltage(controlEffort)
+                    lastFlywheelRunTime = Clock.fpgaTime
+                    nextState = fromRequestToState(currentRequest)
                 }
             }
             Flywheel.Companion.FlywheelStates.ZERO ->{
-
+                nextState = fromRequestToState(currentRequest)
             }
         }
 
@@ -88,5 +96,13 @@ init{
             OPEN_LOOP,
             TARGETING_VELOCITY,
         }
+        inline fun fromRequestToState(request: Request.FlywheelRequest): Flywheel.Companion.FlywheelStates {
+            return when (request) {
+                is Request.FlywheelRequest.OpenLoop -> Flywheel.Companion.FlywheelStates.OPEN_LOOP
+                is Request.FlywheelRequest.TargetingVelocity -> Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY
+                is Request.FlywheelRequest.Zero -> Flywheel.Companion.FlywheelStates.ZERO
+
+            }
+        }
     }
 }

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

@@ -31,6 +31,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{
     var flywheelStatorCurrentSignal: StatusSignal<Double>
     var flywheelSupplyCurrentSignal: StatusSignal<Double>
     var flywheelTempSignal: StatusSignal<Double>
+    var flywheelDutyCycle : StatusSignal<Double>
     init {
         flywheelFalcon.configurator.apply(TalonFXConfiguration())
 
@@ -62,6 +63,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{
         flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent
         flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent
         flywheelTempSignal = flywheelFalcon.deviceTemp
+        flywheelDutyCycle = flywheelFalcon.dutyCycle
 
         MotorChecker.add(
             "Shooter","Flywheel",
@@ -96,6 +98,13 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{
             )
     }
 
+    override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) {
+        inputs.flywheelVelocity = flywheelSensor.velocity
+        inputs.flywheelAppliedVoltage = flywheelDutyCycle.value.volts
+        inputs.flywheelStatorCurrent = flywheelStatorCurrentSignal.value.amps
+        inputs.flywheelSupplyCurrent = flywheelSupplyCurrentSignal.value.amps
+        inputs.flywheelTempreature = flywheelTempSignal.value.celsius
+    }
 
     override fun setFlywheelBrakeMode(brake: Boolean) {
         val motorOutputConfig = MotorOutputConfigs()

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,12 +19,22 @@ import org.team4099.lib.units.perSecond
 class Shooter (val io: ShooterIO){
     val inputs = ShooterIO.ShooterIOInputs()
     //TODO do feedforward
-    private var wristFeedforward: SimpleMotorFeedforward<Meter, 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 var WristFeedforward: SimpleMotorFeedforward<Meter, Volt>(kS,kV,kA)
 
 
-/*
     private val wristflywheelkP =
         LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch }))
     private val wristflywheelkI =
@@ -33,7 +44,7 @@ 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
@@ -76,9 +87,10 @@ fun periodic(){
     var nextState = currentState
     when (currentState) {
         ShooterStates.UNINITIALIZED -> {
-            nextState = ShooterStates.ZERO
+            nextState = fromRequestToState(currentRequest)
         }
         ShooterStates.ZERO ->{
+            nextState = fromRequestToState(currentRequest)
         }
 
         ShooterStates.OPEN_LOOP ->{
@@ -94,13 +106,15 @@ fun periodic(){
             if (isZeroed == true ){
                 nextState = fromRequestToState(currentRequest)
             }
+            nextState = fromRequestToState(currentRequest)
 
         }
 
         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),
@@ -115,6 +129,7 @@ fun periodic(){
             setWristPosition(wristProfile.calculate(timeElapsed))
             //TODO fix this error
             Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed))
+            nextState = fromRequestToState(currentRequest)
         }
 
 

src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt

@@ -97,7 +97,7 @@ sealed interface Request {
   }
   sealed interface FlywheelRequest : Request {
     class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{}
-    class TargetingVelocity (flywheelVelocity: AngularVelocity)
+    class TargetingVelocity (flywheelVelocity: AngularVelocity) : FlywheelRequest{}
     class Zero ():FlywheelRequest{}
   }
 }