Added flywheel subsystem

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

@@ -0,0 +1,67 @@
+package com.team4099.robot2023.subsystems.Shooter
+
+import com.team4099.lib.hal.Clock
+import com.team4099.lib.logging.LoggedTunableValue
+import org.team4099.lib.controller.SimpleMotorFeedforward
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.ctreAngularMechanismSensor
+import org.team4099.lib.units.derived.*
+
+class Flywheel (val io: FlywheelIO) {
+    var feedforward = SimpleMotorFeedforward<AngularVelocity, ElectricalPotential>()
+    val inputs = FlywheelIO.FlywheelIOInputs()
+    private val flywheelkS =
+        LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch }))
+    private val flywheelkkV =
+        LoggedTunableValue(
+            "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perInchSeconds })
+        )
+    private val flywheelkA =
+        LoggedTunableValue(
+            "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perInchPerSecond })
+        )
+
+    var flywheelTargetVoltage: ElectricalPotential = 0.0.volts
+    fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) {
+        io.setFlywheelVoltage(appliedVoltage) }
+
+    var lastFlywheelRunTime = 0.0.seconds
+    private var lastFlywheelVoltage = 0.0.volts
+    private var flywheelInitVoltage  = LoggedTunableValue ("Shooter/Initial flywheel Voltage", FlywheelConstants.FLYWHEEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts}))
+    private var hasNote:Boolean = true
+    var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED
+    fun periodic(){
+        io.updateInputs(inputs)
+        var nextState = currentState
+        when (currentState) {
+            Flywheel.Companion.FlywheelStates.UNINITIALIZED -> {
+                nextState = Flywheel.Companion.FlywheelStates.ZERO
+            }
+            Flywheel.Companion.FlywheelStates.OPEN_LOOP -> {
+                setFlywheelVoltage(flywheelTargetVoltage)
+                lastFlywheelRunTime = Clock.fpgaTime
+            }
+            Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{
+                if (flywheelTargetVoltage != lastFlywheelVoltage){
+                    if(hasNote){
+
+                    }
+                }
+            }
+            Flywheel.Companion.FlywheelStates.ZERO ->{
+
+            }
+        }
+
+    }
+
+    companion object {
+        enum class FlywheelStates {
+            UNINITIALIZED,
+            ZERO,
+            OPEN_LOOP,
+            TARGETING_VELOCITY,
+        }
+    }
+}

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

@@ -0,0 +1,44 @@
+package com.team4099.robot2023.subsystems.Shooter
+
+import org.team4099.lib.units.derived.volts
+
+object FlywheelConstants {
+    val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts
+    val FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts
+    val ROLLER_GEAR_RATIO = 0.0
+
+    object PID {
+
+        val AUTO_POS_KP: ProportionalGain<Meter, Velocity<Meter>>
+            get() {
+                if (RobotBase.isReal()) {
+                    return 8.0.meters.perSecond / 1.0.meters
+                } else {
+                    return 7.0.meters.perSecond / 1.0.meters
+                }
+            }
+        val AUTO_POS_KI: IntegralGain<Meter, Velocity<Meter>>
+            get() {
+                if (RobotBase.isReal()) {
+                    return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds)
+                } else {
+                    return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds)
+                }
+            }
+
+        val AUTO_POS_KD: DerivativeGain<Meter, Velocity<Meter>>
+            get() {
+                if (RobotBase.isReal()) {
+                    return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
+                } else {
+                    return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
+                }
+
+            }
+    }
+    val FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps
+    val FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps
+    val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds
+    val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps
+
+}

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

@@ -0,0 +1,63 @@
+package com.team4099.robot2023.subsystems.Shooter
+
+import org.littletonrobotics.junction.LogTable
+import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.celsius
+import org.team4099.lib.units.base.inAmperes
+import org.team4099.lib.units.base.inCelsius
+import org.team4099.lib.units.derived.*
+import org.team4099.lib.units.inRadiansPerSecond
+import org.team4099.lib.units.perMinute
+import org.team4099.lib.units.perSecond
+
+interface FlywheelIO {
+
+    class FlywheelIOInputs : LoggableInputs {
+        var rollerVelocity = 0.0.rotations.perMinute
+        var rollerAppliedVoltage = 0.volts
+        var rollerStatorCurrent = 0.amps
+        var rollerSupplyCurrent = 0.amps
+        var rollerTempreature = 0.celsius
+
+        override fun toLog(table: LogTable) {
+            table.put("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond)
+            table.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)
+            table.put("rollerStatorCurrent", rollerStatorCurrent.inAmperes)
+            table.put("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes)
+            table.put("rollerTempreature", rollerTempreature.inCelsius)
+        }
+        override fun fromLog(table: LogTable) {
+            //roller logs
+            table.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond).let {
+                rollerVelocity = it.radians.perSecond
+            }
+            table.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts).let {
+                rollerAppliedVoltage = it.volts
+            }
+            table.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes).let {
+                rollerStatorCurrent = it.amps
+            }
+            table.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes).let {
+                rollerSupplyCurrent = it.amps
+
+            }
+            table.get("rollerTempreature", rollerTempreature.inCelsius).let {
+                rollerTempreature = it.celsius
+            }
+        }
+    }
+    fun setFlywheelVoltage (voltage: ElectricalPotential){
+
+    }
+    fun setFlywheelBrakeMode (brake: Boolean){
+
+    }
+    fun updateInputs(inputs:FlywheelIOInputs){
+
+    }
+    fun zeroEncoder(){
+
+    }
+
+}

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

@@ -0,0 +1,87 @@
+package com.team4099.robot2023.subsystems.Shooter
+
+import com.ctre.phoenix6.StatusSignal
+import com.ctre.phoenix6.configs.MotorOutputConfigs
+import com.ctre.phoenix6.configs.TalonFXConfiguration
+import com.ctre.phoenix6.hardware.TalonFX
+import com.ctre.phoenix6.signals.NeutralModeValue
+import com.team4099.robot2023.subsystems.falconspin.Falcon500
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.celsius
+import org.team4099.lib.units.ctreAngularMechanismSensor
+
+class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){
+private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration()
+    private val flywheelSensor =
+        ctreAngularMechanismSensor(
+            flywheelFalcon,
+            FlywheelConstants.ROLLER_GEAR_RATIO,
+            FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION,
+
+            )
+    val flywheelStatorCurrentSignal: StatusSignal<Double>
+    val flywheelSupplyCurrentSignal: StatusSignal<Double>
+    val flywheelTempSignal: StatusSignal<Double>
+    init {
+        flywheelFalcon.configurator.apply(TalonFXConfiguration())
+
+        flywheelFalcon.clearStickyFaults()
+        flywheelFalcon.configurator.apply(flywheelConfiguration)
+//TODO fix PID
+        flywheelConfiguration.Slot0.kP =
+            flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP)
+        flywheelConfiguration.Slot0.kI =
+            flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI)
+        flywheelConfiguration.Slot0.kD =
+            flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD)
+        flywheelConfiguration.Slot0.kV = 0.05425
+        //      flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF)
+        flywheelConfiguration.CurrentLimits.SupplyCurrentLimit =
+            FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes
+        flywheelConfiguration.CurrentLimits.SupplyCurrentThreshold =
+            FlywheelConstants.FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes
+        flywheelConfiguration.CurrentLimits.SupplyTimeThreshold =
+            FlywheelConstants.flywheel_TRIGGER_THRESHOLD_TIME.inSeconds
+        flywheelConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
+        flywheelConfiguration.CurrentLimits.StatorCurrentLimit =
+            FlywheelConstants.FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes
+        flywheelConfiguration.CurrentLimits.StatorCurrentLimitEnable = false
+
+        flywheelConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
+        flywheelFalcon.configurator.apply(flywheelConfiguration)
+
+        flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent
+        flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent
+        flywheelTempSignal = flywheelFalcon.deviceTemp
+
+        MotorChecker.add(
+            "flywheel",
+            MotorCollection(
+                mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")),
+                FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT,
+                90.celsius,
+                FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps,
+                110.celsius
+            )
+        )
+    }
+
+
+
+
+
+    override fun setFlywheelBrakeMode(brake: Boolean) {
+        val motorOutputConfig = MotorOutputConfigs()
+
+        if (brake) {
+            motorOutputConfig.NeutralMode = NeutralModeValue.Brake
+        } else {
+            motorOutputConfig.NeutralMode = NeutralModeValue.Coast
+        }
+        flywheelFalcon.configurator.apply(motorOutputConfig)
+    }
+    override fun zeroEncoder(){
+        //TODO finish zero encoder fun (ask sumone what the encoder for falcon is)
+        flywheelFalcon
+    }
+}

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

@@ -32,7 +32,7 @@ class Shooter (val io: ShooterIO){
             "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond })
         )*/
     var currentState = ShooterStates.UNINITIALIZED
-    var flywheelTargetVoltage : ElectricalPotential= 0.0.volts
+    //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts
     var wristTargetVoltage : ElectricalPotential = 0.0.volts
     var feederTargetVoltage : ElectricalPotential = 0.0.volts
   /*  fun setflywheelVoltage(appliedVoltage: ElectricalPotential){
@@ -56,7 +56,6 @@ class Shooter (val io: ShooterIO){
     private var feederInitVoltage = LoggedTunableValue ("Shooter/Initial Feeder Voltage", ShooterConstants.FEEDER_INIT_VOLTAGE, Pair({it.inVolts},{it.volts}))
     */private var wristInitVoltage = LoggedTunableValue ("Shooter/Initial Wrist Voltage", ShooterConstants.WRIST_INIT_VOLTAGE, Pair({it.inVolts},{it.volts}))
     private var timeProfileGeneratedAt = 0.0.seconds
-    //TODO ask aanshi wtf to pass in for init parameters
     var currentRequest = Request.ShooterRequest.OpenLoop(
         ShooterConstants.WRIST_INIT_VOLTAGE,
         //ShooterConstants.ROLLLER_INIT_VOLTAGE,
@@ -77,7 +76,6 @@ fun periodic(){
             nextState = ShooterStates.ZERO
         }
         ShooterStates.ZERO ->{
-//TODO create zero encoder if we get one
         }
 
         ShooterStates.OPEN_LOOP ->{
@@ -112,7 +110,7 @@ fun periodic(){
             }
             val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt
             setWristPosition(wristProfile.calculate(timeElapsed))
-            //TODO implement set wrist pos function
+            //TODO fix this error
             Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed))
         }
 

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

@@ -13,7 +13,6 @@ import org.team4099.lib.units.base.inAmperes
 import org.team4099.lib.units.derived.*
 import org.team4099.lib.units.sparkMaxAngularMechanismSensor
 import kotlin.math.absoluteValue
-//TODO write a kraken file
 object ShooterIONeo : ShooterIO{
     //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless)
     //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax,