fix(Shooter): make commands require subsystem

FRC3636 · Jan 29, 2024 · 4165e0d · 4165e0d
1 parent 65f5235
commit 4165e0d
Show file tree

Hide file tree

Showing 2 changed files with 78 additions and 59 deletions.
diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt
@@ -1,25 +1,25 @@
 package com.frcteam3636.frc2024.subsystems.shooter
 
+import com.ctre.phoenix6.controls.DynamicMotionMagicTorqueCurrentFOC
 import com.frcteam3636.frc2024.utils.math.*
 import edu.wpi.first.math.filter.SlewRateLimiter
+import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.InstantCommand
 import edu.wpi.first.wpilibj2.command.PIDCommand
 import edu.wpi.first.wpilibj2.command.Subsystem
 import org.littletonrobotics.junction.Logger
-import com.ctre.phoenix6.controls.DynamicMotionMagicTorqueCurrentFOC
-import edu.wpi.first.wpilibj2.command.InstantCommand
-import edu.wpi.first.math.geometry.Rotation2d
 
 object Shooter : Subsystem {
 
     const val ACCELERATION_PROFILE = 0.0
     const val VELOCITY_PROFILE = 0.0
     const val JERK_PROFILE = 0.0
-    
-    private val io: ShooterIO  =
+
+    private val io: ShooterIO =
             if (RobotBase.isReal()) {
                 ShooterIOReal()
             } else {
@@ -33,20 +33,18 @@ object Shooter : Subsystem {
     val tab = Shuffleboard.getTab("Shooter")
     val shouldSpin = tab.add("Should Spin", true).withWidget(BuiltInWidgets.kToggleSwitch).entry
 
-    val dynamicMotionMagicTorqueCurrentFOCRequest = 
-        DynamicMotionMagicTorqueCurrentFOC(
-            0.0,
-            ACCELERATION_PROFILE,
-            VELOCITY_PROFILE,
-            JERK_PROFILE
-        )
+    val dynamicMotionMagicTorqueCurrentFOCRequest =
+            DynamicMotionMagicTorqueCurrentFOC(
+                    0.0,
+                    ACCELERATION_PROFILE,
+                    VELOCITY_PROFILE,
+                    JERK_PROFILE
+            )
 
     val targetVelocity =
             tab.add("Target Velocity", 0.0)
                     .withWidget(BuiltInWidgets.kNumberSlider)
-                    .withProperties(
-                            mapOf(Pair("min", 0.0), Pair("max", 6000.0))
-                    ) 
+                    .withProperties(mapOf(Pair("min", 0.0), Pair("max", 6000.0)))
                     .entry
 
     private val rateLimiter = SlewRateLimiter(1.0)
@@ -69,14 +67,28 @@ object Shooter : Subsystem {
                 .also { it.addRequirements(this) }
     }
 
-    fun runWithSetpoint(setpoint: Rotation2d): Command {
+    fun startPivotingTo(setpoint: Rotation2d): Command {
         return InstantCommand({
-            io.setPivotControlRequest(dynamicMotionMagicTorqueCurrentFOCRequest.withPosition(setpoint.rotations))
-        })
+                    io.setPivotControlRequest(
+                            dynamicMotionMagicTorqueCurrentFOCRequest.withPosition(
+                                    setpoint.rotations
+                            )
+                    )
+                })
+                .also { it.addRequirements(this) }
+    }
+
+    fun pivotTo(setpoint: Rotation2d): Command {
+        return run {
+            io.setPivotControlRequest(
+                    dynamicMotionMagicTorqueCurrentFOCRequest.withPosition(setpoint.rotations)
+            )
+        }
+                .until(io::doneWithMotionProfile)
+                .also { it.addRequirements(this) }
     }
 
     fun intakeCommand(): Command {
         return startEnd({ io.intake(1.0) }, { io.intake(0.0) })
     }
-
 }
diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/ShooterIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/ShooterIO.kt
@@ -1,7 +1,10 @@
 package com.frcteam3636.frc2024.subsystems.shooter
 
+import com.ctre.phoenix6.configs.MotionMagicConfigs
+import com.ctre.phoenix6.configs.TalonFXConfiguration
 import com.ctre.phoenix6.controls.ControlRequest
 import com.ctre.phoenix6.hardware.TalonFX
+import com.ctre.phoenix6.signals.MotionMagicIsRunningValue
 import com.frcteam3636.frc2024.CANSparkMax
 import com.frcteam3636.frc2024.CTREMotorControllerId
 import com.frcteam3636.frc2024.REVMotorControllerId
@@ -12,8 +15,6 @@ import edu.wpi.first.math.util.Units
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.Logger
 import org.littletonrobotics.junction.inputs.LoggableInputs
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.configs.MotionMagicConfigs
 
 interface ShooterIO {
     class ShooterIOInputs : LoggableInputs {
@@ -48,11 +49,13 @@ interface ShooterIO {
     fun intake(speed: Double) {}
 
     fun setPivotControlRequest(control: ControlRequest) {}
+
+    fun doneWithMotionProfile(): Boolean { return false}
 }
 
 class ShooterIOReal : ShooterIO {
 
-    internal companion object Constants{
+    internal companion object Constants {
         const val FLYWHEEL_GEAR_RATIO = 1.0
         const val PIVOT_GEAR_RATIO = 1 / 90.0
         const val kS = 0.0
@@ -67,8 +70,6 @@ class ShooterIOReal : ShooterIO {
         const val MOTION_MAGIC_JERK = 4000.0
     }
 
-
-
     private val left =
             CANSparkMax(
                             REVMotorControllerId.LeftShooterFlywheel,
@@ -91,47 +92,53 @@ class ShooterIOReal : ShooterIO {
                         inverted = false
                     }
 
-    val pivotMotorConfigs = TalonFXConfiguration()
-    .apply{
-
-        Feedback.SensorToMechanismRatio = Constants.PIVOT_GEAR_RATIO
-
-    }
-
-    val slot0Configs = pivotMotorConfigs.Slot0
-        .apply { 
-            kS = Constants.kS
-            kV = Constants.kV
-            kA = Constants.kA
-            kG = Constants.kG
-            kP = Constants.kP
-            kI = Constants.kI
-            kD = Constants.kD
-        }
-
-    val motionProfileConfigs: MotionMagicConfigs = pivotMotorConfigs.MotionMagic
-        .apply {
-            MotionMagicAcceleration = Constants.MOTION_MAGIC_ACCELERATION
-            MotionMagicJerk = Constants.MOTION_MAGIC_JERK
-        }
-
-    private val pivotLeftKraken = TalonFX(CTREMotorControllerId.LeftPivotMotor)
-        .apply { 
-            configurator.apply(pivotMotorConfigs)
-         }
-
-    private val pivotRightKraken = TalonFX(CTREMotorControllerId.RightPivotMotor)
-        .apply { 
-            configurator.apply(pivotMotorConfigs)
-         }
+    val pivotMotorConfigs =
+            TalonFXConfiguration().apply {
+                Feedback.SensorToMechanismRatio = Constants.PIVOT_GEAR_RATIO
+            }
+
+    val slot0Configs =
+            pivotMotorConfigs.Slot0.apply {
+                kS = Constants.kS
+                kV = Constants.kV
+                kA = Constants.kA
+                kG = Constants.kG
+                kP = Constants.kP
+                kI = Constants.kI
+                kD = Constants.kD
+            }
+
+    val motionProfileConfigs: MotionMagicConfigs =
+            pivotMotorConfigs.MotionMagic.apply {
+                MotionMagicAcceleration = Constants.MOTION_MAGIC_ACCELERATION
+                MotionMagicJerk = Constants.MOTION_MAGIC_JERK
+            }
+
+    private val pivotLeftKraken =
+            TalonFX(CTREMotorControllerId.LeftPivotMotor).apply {
+                configurator.apply(pivotMotorConfigs)
+            }
+
+    private val pivotRightKraken =
+            TalonFX(CTREMotorControllerId.RightPivotMotor).apply {
+                configurator.apply(pivotMotorConfigs)
+            }
 
     override fun updateInputs(inputs: ShooterIO.ShooterIOInputs) {
         inputs.leftSpeed = Rotation2d(left.encoder.velocity)
         inputs.rightSpeed = Rotation2d(right.encoder.velocity)
 
         inputs.position = Rotation2d(pivotLeftKraken.position.value * Constants.PIVOT_GEAR_RATIO)
-        inputs.pivotAngularVelocity = Rotation2d(pivotLeftKraken.velocity.value * Constants.PIVOT_GEAR_RATIO)
-        inputs.pivotAcceleration = Rotation2d(pivotLeftKraken.acceleration.value * Constants.PIVOT_GEAR_RATIO)
+        inputs.pivotAngularVelocity =
+                Rotation2d(pivotLeftKraken.velocity.value * Constants.PIVOT_GEAR_RATIO)
+        inputs.pivotAcceleration =
+                Rotation2d(pivotLeftKraken.acceleration.value * Constants.PIVOT_GEAR_RATIO)
+    }
+
+    override fun doneWithMotionProfile(): Boolean {
+        return pivotRightKraken.motionMagicIsRunning.value.equals(
+                MotionMagicIsRunningValue.Enabled
+        ) || pivotLeftKraken.motionMagicIsRunning.value.equals(MotionMagicIsRunningValue.Enabled)
     }
 
     override fun shoot(speed: Double, spin: Boolean) {