added feeder and wrist and did requests and started states

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

@@ -117,6 +117,9 @@ object Constants {
 
   object Shooter {
     const val ROLLER_MOTOR_ID = 51
+    //TODO find wrist motor id
+    const val SHOOTER_WRIST_MOTOR_ID = 999
+    const val FEEDER_MOTOR_ID = 999
   }
 
   object Alert {

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

@@ -2,10 +2,18 @@ package com.team4099.robot2023.config.constants
 
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.derived.volts
-import org.team4099.lib.units.derived.gearRatio
+
 
 object ShooterConstants {
-    val ROLLER_GEAR_RATIO = 0.0.gearRatio
+    val ROLLER_GEAR_RATIO = 0.0
     val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts
     val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps
+
+    val WRIST_GEAR_RATIO = 0.0
+    val WRIST_VOLTAGE_COMPENSATION = 0.0.volts
+    val WRIST_STATOR_CURRENT_LIMIT = 0.0.amps
+
+    val FEEDER_GEAR_RATIO = 0.0
+    val FEEDER_VOLTAGE_COMPENSATION = 0.0.volts
+    val FEEDER_STATOR_CURRENT_LIMIT = 0.0.amps
 }

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

@@ -0,0 +1,92 @@
+package com.team4099.robot2023.subsystems.Shooter
+
+import com.team4099.lib.hal.Clock
+import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.fromRequestToState
+import com.team4099.robot2023.subsystems.superstructure.Request
+import edu.wpi.first.units.Voltage
+import org.team4099.lib.controller.SimpleMotorFeedforward
+import org.team4099.lib.units.base.Meter
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.*
+
+class Shooter (val io: ShooterIO){
+    val inputs = ShooterIO.ShooterIOInputs()
+    private var RollerFeedforward: SimpleMotorFeedforward<Meter, Volt>
+    private var WristFeedforward: SimpleMotorFeedforward<Meter, Volt>
+
+
+    private val wristRollerkP =
+        LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch }))
+    private val wristRollerkI =
+        LoggedTunableValue(
+            "Wrist/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds })
+        )
+    private val wristRollerkD =
+        LoggedTunableValue(
+            "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond })
+        )
+    var currentState = ShooterStates.UNINITIALIZED
+    var rollerTargetVoltage : ElectricalPotential= 0.0.volts
+    var wristTargetVoltage : ElectricalPotential = 0.0.volts
+    var feederTargetVoltage : ElectricalPotential = 0.0.volts
+    fun setRollerVoltage(appliedVoltage: ElectricalPotential){
+        io.setRollerVoltage(appliedVoltage)
+    }
+    fun setWristVoltage(appliedVoltage: ElectricalPotential){
+        io.setWristVoltage(appliedVoltage)
+    }
+    fun setFeederVoltage(appliedVoltage: ElectricalPotential){
+        io.setFeederVoltage(appliedVoltage)
+    }
+    var lastRollerRunTime = 0.0.seconds
+    var lastWristRunTime = 0.0.seconds
+    var lastFeederRunTime = 0.0.seconds
+    var isZeroed : Boolean = false
+    var currentRequest = Request.ShooterRequest.OpenLoop
+fun periodic(){
+    io.updateInputs(inputs)
+    var nextState = currentState
+    when (currentState) {
+        ShooterStates.UNINITIALIZED -> {
+            nextState = ShooterStates.ZERO
+        }
+        ShooterStates.ZERO ->{
+//TODO write zero function for shooter
+        }
+        ShooterStates.IDLE ->{
+//TODO figure out if were
+        }
+        ShooterStates.OPEN_LOOP ->{
+            setRollerVoltage(rollerTargetVoltage)
+            lastRollerRunTime = Clock.fpgaTime
+
+            setWristVoltage(wristTargetVoltage)
+            lastWristRunTime = Clock.fpgaTime
+
+            setFeederVoltage(feederTargetVoltage)
+            lastFeederRunTime = Clock.fpgaTime
+            if (isZeroed == true ){
+                nextState = fromRequestToState(currentRequest)
+            }
+
+        }
+
+        ShooterStates.TARGETING_POSITION ->{
+
+
+        }
+
+    }
+
+}
+
+    companion object {
+        enum class ShooterStates{
+            UNINITIALIZED,
+            ZERO,
+            OPEN_LOOP,
+            TARGETING_POSITION,
+        }
+    }
+}

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

@@ -16,48 +16,121 @@ interface ShooterIO {
         var rollerSupplyCurrent = 0.amps
         var rollerTempreature = 0.celsius
 
+        var wristPostion = 0.degrees
+        var wristAppliedVoltage = 0.volts
+        var wristStatorCurrent = 0.amps
+        var wristSupplyCurrent = 0.amps
+        var wristTemperature = 0.celsius
+
+        var feederVelocity = 0.rotations.perMinute
+        var feederAppliedVoltage = 0.volts
+        var feederStatorCurrent = 0.amps
+        var feederSupplyCurrent = 0.amps
+        var feederTemperature = 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)
+            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)
+
+            table.put("wristPostion", wristPostion.inDegrees)
+            table.put("wristAppliedVoltage", wristAppliedVoltage.inVolts)
+            table.put("wristStatorCurrent", wristStatorCurrent.inAmperes)
+            table.put("wristSupplyCurrent", wristSupplyCurrent.inAmperes)
+            table.put("wristTemperature", wristTemperature.inCelsius)
+
+            table.put("feederVelocity", feederVelocity.inRadiansPerSecond)
+            table.put("feederAppliedVoltage", feederAppliedVoltage.inVolts)
+            table.put("feederStatorCurrent", feederStatorCurrent.inAmperes)
+            table.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes)
+            table.put("feederTemperature", feederTemperature.inCelsius)
 
         }
 
         override fun fromLog(table: LogTable) {
-            table?.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond)?.let {
+            //roller logs
+            table.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond).let {
                 rollerVelocity = it.radians.perSecond
             }
-            table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let {
+            table.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts).let {
                 rollerAppliedVoltage = it.volts
             }
-            table?.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes)?.let {
+            table.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes).let {
                 rollerStatorCurrent = it.amps
             }
-            table?.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes)?.let {
+            table.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes).let {
                 rollerSupplyCurrent = it.amps
 
             }
-
-            table?.get("rollerTempreature", rollerTempreature.inCelsius)?.let {
+            table.get("rollerTempreature", rollerTempreature.inCelsius).let {
                 rollerTempreature = it.celsius
             }
+//wrist logs
+            table.get("wristPostion", wristPostion.inDegrees).let {
+                wristPostion = it.degrees
+            }
+            table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let {
+                wristAppliedVoltage = it.volts
+            }
+            table.get("wristStatorCurrent", wristStatorCurrent.inAmperes).let {
+                wristStatorCurrent = it.amps
+            }
+            table.get("wristSupplyCurrent", wristSupplyCurrent.inAmperes).let {
+                wristSupplyCurrent = it.amps
 
+            }
+            table.get("wristTemperature", wristTemperature.inCelsius).let {
+                wristTemperature = it.celsius
+            }
+            //feeder
+            table.get("feederVelocity", feederVelocity.inRadiansPerSecond).let {
+                feederVelocity = it.radians.perSecond
+            }
+            table.get("feederAppliedVoltage", feederAppliedVoltage.inVolts).let {
+                feederAppliedVoltage = it.volts
+            }
+            table.get("feederStatorCurrent", feederStatorCurrent.inAmperes).let {
+                feederStatorCurrent = it.amps
+            }
+            table.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes).let {
+                feederSupplyCurrent = it.amps
+
+            }
+            table.get("feederTemperature", feederTemperature.inCelsius).let {
+                feederTemperature = it.celsius
 
         }
     }
 
+
+}
+
     fun updateInputs(inputs: ShooterIOInputs){
 
     }
-    fun setRollerPower (voltage: ElectricalPotential){
+    fun setRollerVoltage (voltage: ElectricalPotential){
+
+    }
+    fun setWristVoltage (voltage: ElectricalPotential){
+
+    }
+    fun setFeederVoltage (voltage: ElectricalPotential){
+
+    }
+    fun setWristPosition (voltage: ElectricalPotential){
 
     }
     fun setRollerBrakeMode (brake: Boolean){
 
     }
-    fun configPID(
+    fun setFeederBrakeMode (brake: Boolean){
+
+    }
+
+
+    fun configWristPID(
         kP: ProportionalGain <Meter, Volt>,
         kI: IntegralGain <Meter, Volt>,
         kD: DerivativeGain <Meter, Volt>,

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

@@ -7,25 +7,49 @@ import com.team4099.robot2023.config.constants.ShooterConstants
 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.derived.inDegrees
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.volts
 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,
-        ShooterConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving,
+        ShooterConstants.ROLLER_GEAR_RATIO,
         ShooterConstants.ROLLER_VOLTAGE_COMPENSATION
         )
+
+private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless)
+    private val wristSensor = sparkMaxAngularMechanismSensor( wristSparkMax,
+        ShooterConstants.WRIST_GEAR_RATIO,
+        ShooterConstants.WRIST_VOLTAGE_COMPENSATION
+    )
+    private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless)
+    private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax,
+        ShooterConstants.FEEDER_GEAR_RATIO,
+        ShooterConstants.FEEDER_VOLTAGE_COMPENSATION
+    )
     init{
         //reset the motors
         rollerSparkMax.restoreFactoryDefaults()
         rollerSparkMax.clearFaults()
 
+        wristSparkMax.restoreFactoryDefaults()
+        wristSparkMax.clearFaults()
+
+        feederSparkMax.restoreFactoryDefaults()
+        feederSparkMax.clearFaults()
+
         //voltage and current limits
         rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts)
         rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt())
+
+        wristSparkMax.enableVoltageCompensation(ShooterConstants.WRIST_VOLTAGE_COMPENSATION.inVolts)
+        wristSparkMax.setSmartCurrentLimit(ShooterConstants.WRIST_STATOR_CURRENT_LIMIT.inAmperes.toInt())
+
+        feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts)
+        feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_STATOR_CURRENT_LIMIT.inAmperes.toInt())
     }
 
     override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){
@@ -38,6 +62,26 @@ object ShooterIONeo : ShooterIO{
         // percentOutput * statorCurrent
         inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue
         inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius
+
+        inputs.wristPostion = wristSensor.position
+        inputs.wristAppliedVoltage = wristSparkMax.busVoltage.volts * wristSparkMax.appliedOutput
+        inputs.wristStatorCurrent = wristSparkMax.outputCurrent.amps
+        // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent
+        // AppliedVoltage = percentOutput * BatteryVoltage
+        // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent =
+        // percentOutput * statorCurrent
+        inputs.wristSupplyCurrent = inputs.wristStatorCurrent * wristSparkMax.appliedOutput.absoluteValue
+        inputs.wristTemperature = wristSparkMax.motorTemperature.celsius
+
+        inputs.feederVelocity = feederSensor.velocity
+        inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput
+        inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps
+        // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent
+        // AppliedVoltage = percentOutput * BatteryVoltage
+        // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent =
+        // percentOutput * statorCurrent
+        inputs.feederSupplyCurrent = inputs.wristStatorCurrent * feederSparkMax.appliedOutput.absoluteValue
+        inputs.feederTemperature = feederSparkMax.motorTemperature.celsius
     }
 
 }

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

@@ -5,6 +5,7 @@ import com.team4099.robot2023.config.constants.NodeTier
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.LinearVelocity
+import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.Length
 import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.derived.Angle
@@ -81,4 +82,14 @@ sealed interface Request {
     class ZeroSensors : DrivetrainRequest
     class Idle : DrivetrainRequest
   }
+  sealed interface ShooterRequest : Request {
+    class OpenLoop(wristVoltage : ElectricalPotential,
+                   rollerVoltage: ElectricalPotential,
+                   feederVoltage: ElectricalPotential):ShooterRequest{}
+    class TargetingPosition (val wristPosition : Angle,
+                             val rollerVelocity: AngularVelocity,
+                              val feederVelocity: AngularVelocity
+                                                        ):ShooterRequest{}
+
+  }
 }