From d28d2475a5d9a9e638b944bf9bac05d0e04bcad5 Mon Sep 17 00:00:00 2001 From: Joshua Reddick Date: Mon, 6 Apr 2026 10:08:28 -0400 Subject: [PATCH 1/4] swap intake to positionvoltage --- .../subsystems/intake/IntakeConstants.java | 36 +++++++++---------- .../subsystems/intake/IntakeSubsystem.java | 12 +++---- 2 files changed, 23 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 206d4e1..3ed7ace 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.intake; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -70,10 +71,10 @@ public static OpenLoopRampsConfigs createRollerMotorRampConfigs() { // Extension Motor public static final double ALLOWABLE_EXTENSION_ERROR = 1; - public static final double EXTENSION_KS = 0.468; - public static final double EXTENSION_KP = 7.5; - public static final double EXTENSION_KI = 1.75; - public static final double EXTENSION_KD = 0.3; + public static final double EXTENSION_KS = 0.47; + public static final double EXTENSION_KP = 5.0; + public static final double EXTENSION_KI = 0.0; + public static final double EXTENSION_KD = 0.5; public static Slot0Configs createExtensionMotorSlot0Configs() { Slot0Configs slot = new Slot0Configs(); @@ -125,24 +126,23 @@ public static MotorOutputConfigs createExtensionMotorOutputConfigs() { return newConfigs; } - public static final double EXTENSION_MM_CRUISE_VELOCITY = 40; - public static final double EXTENSION_MM_ACCELERATION = 60; - public static final double EXTENSION_MM_JERK = 400; - - public static MotionMagicConfigs createExtenstionMotionMagicConfigs() { - MotionMagicConfigs newConfigs = new MotionMagicConfigs(); - newConfigs.MotionMagicCruiseVelocity = 40; - newConfigs.MotionMagicAcceleration = 60; - newConfigs.MotionMagicJerk = 400; - return newConfigs; - } - - public static final double EXTENSION_CURRENT_LIMIT = 40; + public static final double EXTENSION_STATOR_CURRENT_LIMIT = 75; + public static final double EXTENSION_SUPPLY_CURRENT_LIMIT = 40; public static CurrentLimitsConfigs createExtenstionMotorCurrentLimitsConfigs() { CurrentLimitsConfigs config = new CurrentLimitsConfigs(); config.StatorCurrentLimitEnable = true; - config.StatorCurrentLimit = EXTENSION_CURRENT_LIMIT; + config.StatorCurrentLimit = EXTENSION_STATOR_CURRENT_LIMIT; + config.SupplyCurrentLimitEnable = true; + config.SupplyCurrentLimit = EXTENSION_SUPPLY_CURRENT_LIMIT; + return config; + } + + public static final double VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5; + + public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs(){ + ClosedLoopRampsConfigs config = new ClosedLoopRampsConfigs(); + config.VoltageClosedLoopRampPeriod = VOLTAGE_CLOSED_LOOP_RAMP_PERIOD; return config; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index baf3906..3d1d9a3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -36,9 +37,8 @@ public class IntakeSubsystem extends SubsystemBase { @Logged(name = "Extension Target", importance = Importance.CRITICAL) private Angle extensionTarget = Rotations.of(IntakeConstants.INTAKE_REVERSE_LIMIT); // Rotations - private PositionTorqueCurrentFOC extensionControl; + private PositionVoltage extensionControl; - private MotionMagicTorqueCurrentFOC mmExtenstionControl; @Logged(name = "Extension Is Compliant", importance = Importance.CRITICAL) private boolean isCompliantMode; @@ -81,15 +81,14 @@ public IntakeSubsystem() { .apply(IntakeConstants.createExtensionSoftwareLimitSwitchConfigs()); extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorOutputConfigs()); extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorSlot1Configs()); - extensionMotor.getConfigurator().apply(IntakeConstants.createExtenstionMotionMagicConfigs()); + extensionMotor.getConfigurator().apply(IntakeConstants.creatClosedLoopRampsConfigs()); extensionMotor .getConfigurator() .apply(IntakeConstants.createExtenstionMotorCurrentLimitsConfigs()); extensionMotor.setPosition(0); extensionTarget = Rotations.of(0); - extensionControl = new PositionTorqueCurrentFOC(0); - mmExtenstionControl = new MotionMagicTorqueCurrentFOC(0); + extensionControl = new PositionVoltage(0); isCompliantMode = false; } @@ -109,8 +108,7 @@ public void periodic() { extensionMotor.setControl( extensionControl .withSlot(isCompliantMode ? 1 : 0) - .withPosition(extensionTarget.in(Rotations)) - .withOverrideCoastDurNeutral(true)); + .withPosition(extensionTarget.in(Rotations))); } public Command spinRollerCommand() { From e9700c2d86b2602fec1eddd8a6a6d0bce24be60d Mon Sep 17 00:00:00 2001 From: Joshua Reddick Date: Mon, 6 Apr 2026 10:17:40 -0400 Subject: [PATCH 2/4] Update hood with ramping and new pid. --- .../subsystems/shooter/ShooterConstants.java | 45 ++++++++++--------- .../subsystems/shooter/ShooterSubsystem.java | 2 +- 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 1ae593b..578e04e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,8 +2,8 @@ import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; @@ -63,41 +63,35 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() return configs; } - public static final double HOOD_MAX_VELOCITY_RPS = 5; - public static final double HOOD_MAX_ACCELERATION_RPS2 = 10; - public static final double HOOD_MAX_JERK_RPS2 = 10; - - public static MotionMagicConfigs createHoodMotionMagicConfigs() { - MotionMagicConfigs configs = new MotionMagicConfigs(); - configs.MotionMagicCruiseVelocity = HOOD_MAX_VELOCITY_RPS; - configs.MotionMagicAcceleration = HOOD_MAX_ACCELERATION_RPS2; - configs.MotionMagicJerk = HOOD_MAX_JERK_RPS2; - - return configs; - } - - public static final double FLYWHEEL_CURRENT_LIMIT = 40; + + public static final double FLYWHEEL_STATOR_CURRENT_LIMIT = 100; + public static final double FLYWHEEL_SUPPLY_CURRENT_LIMIT = 40; public static CurrentLimitsConfigs createFlywheelCurrentLimitsConfigs() { CurrentLimitsConfigs configs = new CurrentLimitsConfigs(); - configs.StatorCurrentLimit = FLYWHEEL_CURRENT_LIMIT; + configs.StatorCurrentLimit = FLYWHEEL_STATOR_CURRENT_LIMIT; + configs.SupplyCurrentLimit = FLYWHEEL_SUPPLY_CURRENT_LIMIT; configs.StatorCurrentLimitEnable = true; + configs.SupplyCurrentLimitEnable = true; return configs; } - public static final double HOOD_CURRENT_LIMIT = 40; + public static final double HOOD_STATOR_CURRENT_LIMIT = 50; + public static final double HOOD_SUPPLY_CURRENT_LIMIT = 40; public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { CurrentLimitsConfigs configs = new CurrentLimitsConfigs(); - configs.StatorCurrentLimit = HOOD_CURRENT_LIMIT; + configs.StatorCurrentLimit = HOOD_STATOR_CURRENT_LIMIT; + configs.SupplyCurrentLimit = HOOD_SUPPLY_CURRENT_LIMIT; configs.StatorCurrentLimitEnable = true; + configs.SupplyCurrentLimitEnable = true; return configs; } - public static final double HOOD_KS = 4.8; - public static final double HOOD_KP = 9; - public static final double HOOD_KD = 0.1; - public static final double HOOD_KI = 1; + public static final double HOOD_KS = 0.47; + public static final double HOOD_KP = 5.5; + public static final double HOOD_KD = 0.266; + public static final double HOOD_KI = 0.0; public static Slot0Configs createHoodMotorSlot0Configs() { Slot0Configs slot = new Slot0Configs(); @@ -108,6 +102,13 @@ public static Slot0Configs createHoodMotorSlot0Configs() { return slot; } + public static final double HOOD_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5; + + public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs(){ + return new ClosedLoopRampsConfigs() + .withVoltageClosedLoopRampPeriod(HOOD_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD); + } + // TODO: Tune Flywheel and Hood Motor // Flywheel motor diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 144ed5e..fc452bd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -77,7 +77,7 @@ public ShooterSubsystem() { hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotionMagicConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.creatClosedLoopRampsConfigs()); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); From 5fedad5c10710e719b50b78de4173b278dca5d89 Mon Sep 17 00:00:00 2001 From: Joshua Reddick Date: Mon, 6 Apr 2026 10:50:14 -0400 Subject: [PATCH 3/4] spotless apply --- .../java/frc/robot/subsystems/intake/IntakeConstants.java | 3 +-- .../java/frc/robot/subsystems/intake/IntakeSubsystem.java | 3 --- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 5 ++--- 3 files changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 3ed7ace..c15fc5d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; @@ -140,7 +139,7 @@ public static CurrentLimitsConfigs createExtenstionMotorCurrentLimitsConfigs() { public static final double VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5; - public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs(){ + public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs() { ClosedLoopRampsConfigs config = new ClosedLoopRampsConfigs(); config.VoltageClosedLoopRampPeriod = VOLTAGE_CLOSED_LOOP_RAMP_PERIOD; return config; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 3d1d9a3..36e0663 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -6,8 +6,6 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -39,7 +37,6 @@ public class IntakeSubsystem extends SubsystemBase { private PositionVoltage extensionControl; - @Logged(name = "Extension Is Compliant", importance = Importance.CRITICAL) private boolean isCompliantMode; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 578e04e..15539e3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -63,7 +63,6 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() return configs; } - public static final double FLYWHEEL_STATOR_CURRENT_LIMIT = 100; public static final double FLYWHEEL_SUPPLY_CURRENT_LIMIT = 40; @@ -104,9 +103,9 @@ public static Slot0Configs createHoodMotorSlot0Configs() { public static final double HOOD_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.5; - public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs(){ + public static ClosedLoopRampsConfigs creatClosedLoopRampsConfigs() { return new ClosedLoopRampsConfigs() - .withVoltageClosedLoopRampPeriod(HOOD_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD); + .withVoltageClosedLoopRampPeriod(HOOD_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD); } // TODO: Tune Flywheel and Hood Motor From d369667235fce98282f84cd802eea0bd0c602624 Mon Sep 17 00:00:00 2001 From: Joshua Reddick Date: Mon, 6 Apr 2026 10:50:51 -0400 Subject: [PATCH 4/4] tuning resumes with last values. --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../subsystems/shooter/ShooterSubsystem.java | 33 ++++++++++++++++--- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 853e2f8..7113eb8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -186,8 +186,7 @@ public void configureTestBindings() { driverJoystick .a() - .whileTrue(driveAndLaunchCommand.alongWith(indexer.startFullIndexingNoPID())) - .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood())); + .whileTrue(indexer.startFullIndexingNoPID()); driverJoystick.b().onTrue(intake.testRollerNoPID()).onFalse(intake.stopRollerNoPID()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index fc452bd..856f96f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -34,6 +34,10 @@ public class ShooterSubsystem extends SubsystemBase { @Logged(name = "Hood Target (radians)", importance = Importance.CRITICAL) private Angle hoodTarget; // radians is the base unit. + // For testing + private double lastRPS = 0; + private double lastHoodRot = 0; + private PositionVoltage hoodControl; final SysIdRoutine m_sysIdRoutineFlywheel = @@ -129,6 +133,11 @@ public double getFlywheelTargetRPM() { return velocityTarget.in(RotationsPerSecond) * 60; } + @Logged(name = "Velocity Target RPS", importance = Importance.CRITICAL) + public double getFlywheelTargetRPMS() { + return velocityTarget.in(RotationsPerSecond); + } + public Command spinFlywheelCommand() { return runOnce( () -> { @@ -140,12 +149,20 @@ public Command spinFlywheelCommand() { } public Command stopFlywheelCommand() { - return runOnce(() -> velocityTarget = RotationsPerSecond.of(0)) + return runOnce( + () -> { + lastRPS = velocityTarget.in(RotationsPerSecond); + velocityTarget = RotationsPerSecond.of(0); + }) .withName("Stop Spinning Flywheel"); } public Command stowHood() { - return runOnce(() -> hoodTarget = Rotations.of(0)); + return runOnce( + () -> { + lastHoodRot = hoodTarget.in(Rotations); + hoodTarget = Rotations.of(0); + }); } public Command increaseFlywheelCommand() { @@ -189,11 +206,17 @@ public Command decreaseHoodCommand() { } public Command startShooterTuningCommand() { + return runOnce( () -> { - velocityTarget = - RotationsPerSecond.of(ShooterPreferences.tuningDefaultFlywheelRPS.getValue()); - hoodTarget = Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()); + double flywheelRPS = + lastRPS > 0 ? lastRPS : ShooterPreferences.tuningDefaultFlywheelRPS.getValue(); + double hoodRot = + lastHoodRot > 0 + ? lastHoodRot + : ShooterPreferences.tuningDefaultHoodRotations.getValue(); + velocityTarget = RotationsPerSecond.of(flywheelRPS); + hoodTarget = Rotations.of(hoodRot); }) .withName("Start Shooter Tuning"); }