Finshing up flywheel

src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt

@@ -0,0 +1,64 @@
+package com.team4099.lib.phoenix6
+
+import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
+import org.team4099.lib.units.AngularAcceleration
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inRotationsPerSecond
+import org.team4099.lib.units.inRotationsPerSecondPerSecond
+import org.team4099.lib.units.perSecond
+
+class VelocityVoltage(var velocity: AngularVelocity,
+                      var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
+                      var enableFOC:Boolean = true,
+                      var feedforward: ElectricalPotential = 0.0.volts,
+                      var slot:Int = 0,
+                      var overrideBrakeDurNeutral: Boolean = false,
+                      var limitForwardMotion: Boolean = false,
+                      var limitReverseMotion: Boolean = false){
+
+    val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+
+    fun setVelocity(new_velocity: AngularVelocity) {
+        velocity = new_velocity
+        velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
+    }
+
+    fun setAcceleration(new_accel: AngularAcceleration) {
+        acceleration = new_accel
+        velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
+    }
+
+    fun setEnableFOC(new_enableFOC: Boolean) {
+        enableFOC = new_enableFOC
+        velocityVoltagePhoenix6.EnableFOC = new_enableFOC
+    }
+
+    fun setFeedforward(new_feedforward: ElectricalPotential) {
+        feedforward = new_feedforward
+        velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+    }
+
+    fun setSlot(new_slot: Int) {
+        slot = new_slot
+        velocityVoltagePhoenix6.Slot = new_slot
+    }
+
+    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+        overrideBrakeDurNeutral = new_override
+        velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+    }
+
+    fun setLimitForwardMotion(new_limitForward: Boolean) {
+        limitForwardMotion = new_limitForward
+        velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
+    }
+
+    fun setLimitReverseMotion(new_limitReverse: Boolean) {
+        limitReverseMotion = new_limitReverse
+        velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+    }
+}

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

@@ -35,7 +35,7 @@ class Flywheel (val io: FlywheelIO) {
         )
     private val flywheelkV =
         LoggedTunableValue(
-            "Flywheel/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute })
+            "Flywheel/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotation.perMinute })
         )
     private val flywheelkA =
         LoggedTunableValue(
@@ -57,52 +57,49 @@ class Flywheel (val io: FlywheelIO) {
     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(){
         io.updateInputs(inputs)
+        if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) {
+            io.configPID(kP.get(), kI.get(), kD.get())
+        }
         var nextState = currentState
         when (currentState) {
             Flywheel.Companion.FlywheelStates.UNINITIALIZED -> {
-                nextState = Flywheel.Companion.FlywheelStates.ZERO
+                nextState = Flywheel.Companion.FlywheelStates.OPEN_LOOP
             }
             Flywheel.Companion.FlywheelStates.OPEN_LOOP -> {
                 setFlywheelVoltage(flywheelTargetVoltage)
                 lastFlywheelRunTime = Clock.fpgaTime
 
                 nextState = fromRequestToState(currentRequest)
             }
-            //TODO do sensor logic (mayb ask pranav)
-            //TODO add in the left flywheel to this function mayb ask ab dif ff for motors
+
             Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{
                 if (flywheelTargetVoltage != lastFlywheelVoltage){
                     val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity)
-                    io.setFlywheelVelocity(inputs.rightFlywheelVelocity, controlEffort)
+                    io.setFlywheelVelocity(inputs.rightFlywheelVelocity, inputs.leftFlywheelVelocity, controlEffort)
                     io.setFlywheelVoltage(controlEffort)
                     lastFlywheelRunTime = Clock.fpgaTime
                     nextState = fromRequestToState(currentRequest)
                 }
             }
-            Flywheel.Companion.FlywheelStates.ZERO ->{
-                nextState = fromRequestToState(currentRequest)
-            }
+
         }
 
     }
 
     companion object {
         enum class FlywheelStates {
             UNINITIALIZED,
-            ZERO,
             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/FlywheelIO.kt

@@ -77,7 +77,7 @@ interface FlywheelIO {
                 leftFlywheelTempreature = it.celsius
             }
         }
-
+    }
         fun setFlywheelVoltage(voltage: ElectricalPotential) {
 
         }
@@ -94,17 +94,11 @@ interface FlywheelIO {
 
         }
 
-        fun zeroEncoder() {
-
-        }
-
         fun configPID(
             kP: ProportionalGain<Velocity<Radian>, Volt>,
             kI: IntegralGain<Velocity<Radian>, Volt>,
             kD: DerivativeGain<Velocity<Radian>, Volt>
-        ) {
+        ) {}
 
-        }
 
-    }
 }

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

@@ -4,36 +4,36 @@ import com.ctre.phoenix6.StatusSignal
 import com.ctre.phoenix6.configs.MotorOutputConfigs
 import com.ctre.phoenix6.configs.Slot0Configs
 import com.ctre.phoenix6.configs.TalonFXConfiguration
+import com.team4099.lib.phoenix6.VelocityVoltage
 import com.ctre.phoenix6.hardware.TalonFX
 import com.ctre.phoenix6.signals.NeutralModeValue
-import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.FlywheelConstants
 import com.team4099.robot2023.subsystems.falconspin.Falcon500
 import com.team4099.robot2023.subsystems.falconspin.MotorChecker
 import com.team4099.robot2023.subsystems.falconspin.MotorCollection
-import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.*
 import org.team4099.lib.units.ctreAngularMechanismSensor
 import org.team4099.lib.units.derived.*
+import org.team4099.lib.units.inRotationsPerSecond
 
-class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val flywheelLeftFalcon: TalonFX) : FlywheelIO{
+class FlywheelIOTalon (private val flywheelRightTalon: TalonFX, private val flywheelLeftTalon: TalonFX) : FlywheelIO{
 
     private val flywheelRightConfiguration: TalonFXConfiguration = TalonFXConfiguration()
     private val flywheelLeftConfiguration: TalonFXConfiguration = TalonFXConfiguration()
 
     private val flywheelRightSensor =
         ctreAngularMechanismSensor(
-            flywheelRightFalcon,
+            flywheelRightTalon,
             FlywheelConstants.ROLLER_GEAR_RATIO,
             FlywheelConstants.RIGHT_FLYWHEEL_VOLTAGE_COMPENSATION,
 
             )
 
     private val flywheelLeftSensor =
         ctreAngularMechanismSensor(
-            flywheelLeftFalcon,
+            flywheelLeftTalon,
             FlywheelConstants.ROLLER_GEAR_RATIO,
             FlywheelConstants.LEFT_FLYWHEEL_VOLTAGE_COMPENSATION,
 
@@ -49,14 +49,14 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl
     var leftFlywheelTempSignal: StatusSignal<Double>
     var leftFlywheelDutyCycle : StatusSignal<Double>
     init {
-        flywheelRightFalcon.configurator.apply(TalonFXConfiguration())
-        flywheelLeftFalcon.configurator.apply(TalonFXConfiguration())
+        flywheelRightTalon.configurator.apply(TalonFXConfiguration())
+        flywheelLeftTalon.configurator.apply(TalonFXConfiguration())
 
-        flywheelRightFalcon.clearStickyFaults()
-        flywheelRightFalcon.configurator.apply(flywheelRightConfiguration)
+        flywheelRightTalon.clearStickyFaults()
+        flywheelRightTalon.configurator.apply(flywheelRightConfiguration)
 
-        flywheelLeftFalcon.clearStickyFaults()
-        flywheelLeftFalcon.configurator.apply(flywheelLeftConfiguration)
+        flywheelLeftTalon.clearStickyFaults()
+        flywheelLeftTalon.configurator.apply(flywheelLeftConfiguration)
 
         flywheelRightConfiguration.Slot0.kP =
             flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP)
@@ -101,23 +101,23 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl
         flywheelRightConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
         flywheelLeftConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
 
-        flywheelLeftFalcon.configurator.apply(flywheelRightConfiguration)
-        flywheelRightFalcon.configurator.apply(flywheelLeftConfiguration)
+        flywheelLeftTalon.configurator.apply(flywheelRightConfiguration)
+        flywheelRightTalon.configurator.apply(flywheelLeftConfiguration)
 
-        rightFlywheelStatorCurrentSignal = flywheelRightFalcon.statorCurrent
-        rightFlywheelSupplyCurrentSignal = flywheelRightFalcon.supplyCurrent
-        rightFlywheelTempSignal = flywheelRightFalcon.deviceTemp
-        rightFlywheelDutyCycle = flywheelRightFalcon.dutyCycle
+        rightFlywheelStatorCurrentSignal = flywheelRightTalon.statorCurrent
+        rightFlywheelSupplyCurrentSignal = flywheelRightTalon.supplyCurrent
+        rightFlywheelTempSignal = flywheelRightTalon.deviceTemp
+        rightFlywheelDutyCycle = flywheelRightTalon.dutyCycle
 
-        leftFlywheelStatorCurrentSignal = flywheelLeftFalcon.statorCurrent
-        leftFlywheelSupplyCurrentSignal = flywheelLeftFalcon.supplyCurrent
-        leftFlywheelTempSignal = flywheelLeftFalcon.deviceTemp
-        leftFlywheelDutyCycle = flywheelLeftFalcon.dutyCycle
+        leftFlywheelStatorCurrentSignal = flywheelLeftTalon.statorCurrent
+        leftFlywheelSupplyCurrentSignal = flywheelLeftTalon.supplyCurrent
+        leftFlywheelTempSignal = flywheelLeftTalon.deviceTemp
+        leftFlywheelDutyCycle = flywheelLeftTalon.dutyCycle
 
         MotorChecker.add(
             "Shooter","Flywheel",
             MotorCollection(
-                mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")),
+                mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")),
                 FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
                 90.celsius,
                 FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
@@ -128,7 +128,7 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl
         MotorChecker.add(
             "Shooter","Flywheel",
             MotorCollection(
-                mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")),
+                mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")),
                 FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
                 90.celsius,
                 FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
@@ -139,7 +139,7 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl
         MotorChecker.add(
             "Shooter","Flywheel",
             MotorCollection(
-                mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")),
+                mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")),
                 FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT,
                 90.celsius,
                 FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
@@ -169,21 +169,14 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl
         PIDConfig.kD = flywheelLeftSensor.derivativeVelocityGainToRawUnits(leftkD)
         PIDConfig.kV = 0.05425
 
-        flywheelRightFalcon.configurator.apply(PIDConfig)
-        flywheelLeftFalcon.configurator.apply(PIDConfig)
+        flywheelRightTalon.configurator.apply(PIDConfig)
+        flywheelLeftTalon.configurator.apply(PIDConfig)
     }
     //TODO add left flywheel to this
     override fun setFlywheelVelocity(rightAngularVelocity: AngularVelocity, leftAngularVelocity: AngularVelocity, feedforward: ElectricalPotential){
-        flywheelRightFalcon.setControl(0,
-            flywheelRightSensor.velocityToRawUnits(rightAngularVelocity),
-            DemandType.ArbitraryFeedForward,
-            feedforward.inVolts
-            )
-        flywheelLeftFalcon.setControl(0,
-            flywheelRightSensor.velocityToRawUnits(leftAngularVelocity),
-            DemandType.ArbitraryFeedForward,
-            feedforward.inVolts
-        )
+        flywheelRightTalon.setControl(VelocityVoltage(rightAngularVelocity, slot = 0, feedforward = feedforward).velocityVoltagePhoenix6)
+
+        flywheelLeftTalon.setControl(VelocityVoltage(leftAngularVelocity, slot = 0, feedforward = feedforward).velocityVoltagePhoenix6)
     }
     override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) {
         inputs.rightFlywheelVelocity = flywheelRightSensor.velocity
@@ -207,11 +200,7 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl
         } else {
             motorOutputConfig.NeutralMode = NeutralModeValue.Coast
         }
-        flywheelRightFalcon.configurator.apply(motorOutputConfig)
-        flywheelRightFalcon.configurator.apply(motorOutputConfig)
-    }
-    override fun zeroEncoder(){
-        //TODO finish zero encoder fun (ask sumone what the encoder for falcon is)
-
+        flywheelRightTalon.configurator.apply(motorOutputConfig)
+        flywheelRightTalon.configurator.apply(motorOutputConfig)
     }
 }

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

@@ -19,12 +19,23 @@ import org.team4099.lib.units.perSecond
 class Wrist (val io: WristIO) {
     val inputs = WristIO.ShooterIOInputs()
 
-    var wristFeedForward: ArmFeedforward =
-        ArmFeedforward(
-            WristConstants.WRIST_KS,
-            WristConstants.WRRIST_KG,
-            WristConstants.WRIST_KV,
-            WristConstants.WRIST_KA
+    private val wristkS =
+        LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts})
+        )
+    private val wristkV =
+        LoggedTunableValue(
+            "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute })
+        )
+    private val wristkA =
+        LoggedTunableValue(
+            "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond }))
+    private val wristkG = LoggedTunableValue("Wrist/kG", Pair({ it.inVolts }, { it.volts} ))
+
+    var wristFeedForward: ArmFeedforward = ArmFeedforward(
+            wristkS.get(),
+            wristkG.get(),
+            wristkV.get(),
+            wristkA.get()
         )
 
 
@@ -125,6 +136,14 @@ class Wrist (val io: WristIO) {
         if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) {
             io.configWristPID(wristkP.get(), wristkI.get(), wristkD.get())
         }
+        if(wristkA.hasChanged()||wristkV.hasChanged()||wristkG.hasChanged()||wristkS.hasChanged()){
+            wristFeedForward = ArmFeedforward(
+                wristkS.get(),
+                wristkG.get(),
+                wristkV.get(),
+                wristkA.get()
+            )
+        }
         Logger.processInputs("Wrist", inputs)
 
         Logger.recordOutput("Wrist/currentState", currentState.name)

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

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