Did more targeting position state and open loop state

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

@@ -1,6 +1,7 @@
 package com.team4099.robot2023.config.constants
 
 import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.volts
 
 
@@ -16,4 +17,11 @@ object ShooterConstants {
     val FEEDER_GEAR_RATIO = 0.0
     val FEEDER_VOLTAGE_COMPENSATION = 0.0.volts
     val FEEDER_STATOR_CURRENT_LIMIT = 0.0.amps
+
+    val ROLLLER_INIT_VOLTAGE = 0.0.volts
+    val FEEDER_INIT_VOLTAGE = 0.0.volts
+    val WRIST_INIT_VOLTAGE = 0.0.volts
+
+    val ROLLER_SHOOTING_VOLTAGE = 0.0.volts
+
 }

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

@@ -2,13 +2,20 @@ package com.team4099.robot2023.subsystems.Shooter
 
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.config.constants.ShooterConstants
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 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.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.SimpleMotorFeedforward
+import org.team4099.lib.controller.TrapezoidProfile
 import org.team4099.lib.units.base.Meter
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.*
+import org.team4099.lib.units.perSecond
 
 class Shooter (val io: ShooterIO){
     val inputs = ShooterIO.ShooterIOInputs()
@@ -43,7 +50,26 @@ class Shooter (val io: ShooterIO){
     var lastWristRunTime = 0.0.seconds
     var lastFeederRunTime = 0.0.seconds
     var isZeroed : Boolean = false
-    var currentRequest = Request.ShooterRequest.OpenLoop
+    private var lastRollerVoltage = 0.0.volts
+    //TODO ask what to set this too
+    private var wristPositionTarget = 0.0.degrees
+    private var lastWristPositionTarget = 0.0.degrees
+    private var rollerInitVoltage  = LoggedTunableValue ("Shooter/Initial Roller Voltage", ShooterConstants.ROLLLER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts}))
+    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,
+        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
@@ -52,11 +78,9 @@ fun periodic(){
             nextState = ShooterStates.ZERO
         }
         ShooterStates.ZERO ->{
-//TODO write zero function for shooter
-        }
-        ShooterStates.IDLE ->{
-//TODO figure out if were
+//TODO create zero encoder if we get one
         }
+
         ShooterStates.OPEN_LOOP ->{
             setRollerVoltage(rollerTargetVoltage)
             lastRollerRunTime = Clock.fpgaTime
@@ -74,19 +98,47 @@ fun periodic(){
 
         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(wristProfile.calculate(timeElapsed))
+            //TODO implement set wrist pos function
+            Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed))
         }
 
-    }
 
-}
 
-    companion object {
+
+        }
+
+    }
+    companion object{
         enum class ShooterStates{
             UNINITIALIZED,
             ZERO,
             OPEN_LOOP,
             TARGETING_POSITION,
         }
+        inline fun fromRequestToState(request: Request.ShooterRequest): ShooterStates {
+            return when (request) {
+                is Request.ShooterRequest.OpenLoop -> ShooterStates.OPEN_LOOP
+                is Request.ShooterRequest.TargetingPosition -> ShooterStates.TARGETING_POSITION
+                is Request.ShooterRequest.Zero -> ShooterStates.ZERO
+
+            }
+        }
     }
+
 }
+
+

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

@@ -4,6 +4,7 @@ import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
 import org.team4099.lib.units.base.*
 import org.team4099.lib.units.derived.*
+import org.team4099.lib.units.inInchesPerSecond
 import org.team4099.lib.units.inRadiansPerSecond
 import org.team4099.lib.units.perMinute
 import org.team4099.lib.units.perSecond
@@ -17,6 +18,7 @@ interface ShooterIO {
         var rollerTempreature = 0.celsius
 
         var wristPostion = 0.degrees
+        var wristVelocity = 0.radians.perSecond
         var wristAppliedVoltage = 0.volts
         var wristStatorCurrent = 0.amps
         var wristSupplyCurrent = 0.amps
@@ -36,12 +38,13 @@ interface ShooterIO {
             table.put("rollerTempreature", rollerTempreature.inCelsius)
 
             table.put("wristPostion", wristPostion.inDegrees)
+            table.put("wristVelocityRPM", wristVelocity.inRadiansPerSecond)
             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("feederVelocityRPM", feederVelocity.inRadiansPerSecond)
             table.put("feederAppliedVoltage", feederAppliedVoltage.inVolts)
             table.put("feederStatorCurrent", feederStatorCurrent.inAmperes)
             table.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes)
@@ -67,10 +70,16 @@ interface ShooterIO {
             table.get("rollerTempreature", rollerTempreature.inCelsius).let {
                 rollerTempreature = it.celsius
             }
+
+
+
 //wrist logs
             table.get("wristPostion", wristPostion.inDegrees).let {
                 wristPostion = it.degrees
             }
+            table.get("wristVelocity", wristVelocity.inRadiansPerSecond).let {
+                wristVelocity = it.radians.perSecond
+            }
             table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let {
                 wristAppliedVoltage = it.volts
             }
@@ -84,6 +93,9 @@ interface ShooterIO {
             table.get("wristTemperature", wristTemperature.inCelsius).let {
                 wristTemperature = it.celsius
             }
+
+
+
             //feeder
             table.get("feederVelocity", feederVelocity.inRadiansPerSecond).let {
                 feederVelocity = it.radians.perSecond

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

@@ -84,4 +84,5 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID
         inputs.feederTemperature = feederSparkMax.motorTemperature.celsius
     }
 
+
 }

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

@@ -90,6 +90,7 @@ sealed interface Request {
                              val rollerVelocity: AngularVelocity,
                               val feederVelocity: AngularVelocity
                                                         ):ShooterRequest{}
+    class Zero () : ShooterRequest{}
 
   }
 }