From 4165e0d3fc87f6cc4a2cff305f2db3419ead86a7 Mon Sep 17 00:00:00 2001 From: rotatingcow Date: Sun, 28 Jan 2024 17:41:25 -0800 Subject: [PATCH] fix(Shooter): make commands require subsystem --- .../frc2024/subsystems/shooter/Shooter.kt | 50 +++++++---- .../frc2024/subsystems/shooter/ShooterIO.kt | 87 ++++++++++--------- 2 files changed, 78 insertions(+), 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 index 6878e19..87c0201 100644 --- 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 index aef01ac..d3036ac 100644 --- 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) {