Fixed some issues

team4099 · Jan 16, 2024 · a15439d · a15439d
2 parents c0a3c45 + 829a075
commit a15439d
Show file tree

Hide file tree

Showing 8 changed files with 86 additions and 18 deletions.
diff --git a/...3/subsystems/Shooter/FlywheelConstants.kt → ...023/config/constants/FlywheelConstants.kt b/...3/subsystems/Shooter/FlywheelConstants.kt → ...023/config/constants/FlywheelConstants.kt
@@ -1,5 +1,6 @@
-package com.team4099.robot2023.subsystems.Shooter
+package com.team4099.robot2023.config.constants
 
+import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.derived.volts
 
 object FlywheelConstants {

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt
@@ -2,27 +2,46 @@ package com.team4099.robot2023.subsystems.Shooter
 
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.config.constants.FlywheelConstants
 import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.seconds
-import org.team4099.lib.units.ctreAngularMechanismSensor
 import org.team4099.lib.units.derived.*
+<<<<<<< HEAD
 import org.team4099.lib.units.*
 
 class Flywheel (val io: FlywheelIO) {
     // TODO: Make shooter constants (kS, kV, kA)
     var feedforward = SimpleMotorFeedforward<Volt, AngularVelocity>()
+=======
+import org.team4099.lib.units.inRadiansPerSecond
+import org.team4099.lib.units.perMinute
+
+class Flywheel (val io: FlywheelIO) {
+
+>>>>>>> 829a07586e50bf71782342ae9623c560237b502b
     val inputs = FlywheelIO.FlywheelIOInputs()
     private val flywheelkS =
-        LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch }))
-    private val flywheelkkV =
+        LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian }))
+    private val flywheelkV =
         LoggedTunableValue(
+<<<<<<< HEAD
             "Flywheel/kV", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds })
         )
     private val flywheelkA =
         LoggedTunableValue(
             "Flywheel/kA", Pair({ it.inVoltsPerInchPerSecond}, { it.volts.perInchPerSecond })
+=======
+            "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perRadianSeconds })
+        )
+    private val flywheelkA =
+        LoggedTunableValue(
+            "Flywheel/kA", Pair({ it.inVolts.perRadianPerSecond}, { it.volts.perRadianPerSecond })
+>>>>>>> 829a07586e50bf71782342ae9623c560237b502b
         )
+    val flywheelFeedForward = SimpleMotorFeedforward<AngularVelocity, Volt>(flywheelkS, flywheelkV, flywheelkA)
+
 
     var flywheelTargetVoltage: ElectricalPotential = 0.0.volts
     fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) {
@@ -32,7 +51,9 @@ class Flywheel (val io: FlywheelIO) {
     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
+    val desiredVelocity: AngularVelocity = 1800.rotations.perMinute
     var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED
+
     fun periodic(){
         io.updateInputs(inputs)
         var nextState = currentState
@@ -46,9 +67,9 @@ class Flywheel (val io: FlywheelIO) {
             }
             Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{
                 if (flywheelTargetVoltage != lastFlywheelVoltage){
-                    if(hasNote){
+                    val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity)
 
-                    }
+                    io.setFlywheelVoltage(controlEffort)
                 }
             }
             Flywheel.Companion.FlywheelStates.ZERO ->{

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt
@@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.Shooter
 
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.celsius
 import org.team4099.lib.units.base.inAmperes
@@ -47,6 +48,7 @@ interface FlywheelIO {
             }
         }
     }
+
     fun setFlywheelVoltage (voltage: ElectricalPotential){
 
     }
@@ -59,5 +61,10 @@ interface FlywheelIO {
     fun zeroEncoder(){
 
     }
+    fun configPID(kP: ProportionalGain <Radian, Volt>,
+                  kI: IntegralGain <Radian, Volt>,
+                  kD: DerivativeGain <Radian, Volt>){
+
+    }
 
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt
@@ -2,16 +2,26 @@ package com.team4099.robot2023.subsystems.Shooter
 
 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.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.units.Velocity
+import org.team4099.lib.units.base.Meter
 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.ctreAngularMechanismSensor
+import org.team4099.lib.units.derived.*
 
 class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){
-private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration()
+    private val flywheelPIDController = TalonFX
+    private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration()
     private val flywheelSensor =
         ctreAngularMechanismSensor(
             flywheelFalcon,
@@ -22,18 +32,27 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration()
     val flywheelStatorCurrentSignal: StatusSignal<Double>
     val flywheelSupplyCurrentSignal: StatusSignal<Double>
     val flywheelTempSignal: StatusSignal<Double>
+    private val kP =
+        LoggedTunableValue("Flywheel/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch }))
+    private val kI =
+        LoggedTunableValue(
+            "Flywheel/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds })
+        )
+    private val kD =
+        LoggedTunableValue(
+            "Flywheel/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond })
     init {
         flywheelFalcon.configurator.apply(TalonFXConfiguration())
 
         flywheelFalcon.clearStickyFaults()
         flywheelFalcon.configurator.apply(flywheelConfiguration)
 //TODO fix PID
         flywheelConfiguration.Slot0.kP =
-            flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP)
+            flywheelSensor.proportionalVelocityGainToRawUnits(kP)
         flywheelConfiguration.Slot0.kI =
-            flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI)
+            flywheelSensor.integralVelocityGainToRawUnits(kI)
         flywheelConfiguration.Slot0.kD =
-            flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD)
+            flywheelSensor.derivativeVelocityGainToRawUnits(kD)
         flywheelConfiguration.Slot0.kV = 0.05425
         //      flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF)
         flywheelConfiguration.CurrentLimits.SupplyCurrentLimit =
@@ -65,9 +84,20 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration()
             )
         )
     }
+    override fun configureDrivePID(
+        kP: ProportionalGain<Velocity<Radian>, Volt>,
+        kI: IntegralGain<Velocity<Radian>, Volt>,
+        kD: DerivativeGain<Velocity<Radian>, Volt>
+    ) {
+        val PIDConfig = Slot0Configs()
 
+        PIDConfig.kP = flywheelSensor.proportionalVelocityGainToRawUnits(kP)
+        PIDConfig.kI = flywheelSensor.integralVelocityGainToRawUnits(kI)
+        PIDConfig.kD = flywheelSensor.derivativeVelocityGainToRawUnits(kD)
+        PIDConfig.kV = 0.05425
 
-
+        flywheelFalcon.configurator.apply(PIDConfig)
+    }
 
 
     override fun setFlywheelBrakeMode(brake: Boolean) {

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt
@@ -18,7 +18,11 @@ import org.team4099.lib.units.perSecond
 class Shooter (val io: ShooterIO){
     val inputs = ShooterIO.ShooterIOInputs()
     //TODO do feedforward
+<<<<<<< HEAD
     private var wristFeedforward: SimpleMotorFeedforward<Meter, Volt>
+=======
+    private var WristFeedforward: SimpleMotorFeedforward<Meter, Volt>(kS,kV,kA)
+>>>>>>> 829a07586e50bf71782342ae9623c560237b502b
 
 /*
     private val wristflywheelkP =

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt
@@ -145,9 +145,9 @@ interface ShooterIO {
       }
 
     fun configWristPID(
-        kP: ProportionalGain <Meter, Volt>,
-        kI: IntegralGain <Meter, Volt>,
-        kD: DerivativeGain <Meter, Volt>,
+        kP: ProportionalGain <Radian, Volt>,
+        kI: IntegralGain <Radian, Volt>,
+        kD: DerivativeGain <Radian, Volt>
     ){}
     fun zeroEncoder(){
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt
@@ -85,11 +85,10 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID
         inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/
     }
     override fun configWristPID(
-        kP: ProportionalGain<Meter, Volt>,
-        kI: IntegralGain<Meter, Volt>,
-        kD: DerivativeGain<Meter, Volt>
+        kP: ProportionalGain<Radian, Volt>,
+        kI: IntegralGain<Radian, Volt>,
+        kD: DerivativeGain<Radian, Volt>
     ) {
-//TODO fix this please
         wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP)
         wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI)
         wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD)

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
@@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.superstructure
 
 import com.team4099.robot2023.config.constants.GamePiece
 import com.team4099.robot2023.config.constants.NodeTier
+import com.team4099.robot2023.subsystems.Shooter.Flywheel
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.LinearVelocity
@@ -94,4 +95,9 @@ sealed interface Request {
     class Zero () : ShooterRequest{}
 
   }
+  sealed interface FlywheelRequest : Request {
+    class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{}
+    class TargetingVelocity (flywheelVelocity: AngularVelocity)
+    class Zero ():FlywheelRequest{}
+  }
 }