From 76663ead981d831ae2e9aee77b145621f6aa4163 Mon Sep 17 00:00:00 2001 From: Brian <74162951+codeNinja-1@users.noreply.github.com> Date: Mon, 2 Mar 2026 12:07:36 -0800 Subject: [PATCH 1/3] Tune shooter hood and add backup zero button --- .../frc/robot/controls/TeleopControls.java | 3 +++ .../robot/subsystems/hood/ShooterHood.java | 19 ++++++++++++++++-- .../subsystems/hood/ShooterHoodConstants.java | 20 +++++++++---------- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index a3991890..ecb43add 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -183,6 +183,9 @@ public void configureBindings() { .getShooterHood() .moveAtVoltage(ShooterHoodConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); + // Backup zero + operator.x().onTrue(this.robot.getShooterHood().zero()); + operator .povLeft() .and(() -> fineControl) diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index 25d03a0f..e40c58be 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.Supplier; @@ -67,7 +68,7 @@ public ShooterHood() { positionSignal = hoodMotor.getPosition(); statorCurrentSignal = hoodMotor.getStatorCurrent(); supplyCurrentSignal = hoodMotor.getSupplyCurrent(); - hallSensorTriggeredSignal = candi.getS1Closed(); + hallSensorTriggeredSignal = candi.getS2Closed(); profileReferenceSignal = hoodMotor.getClosedLoopReference(); DogLog.tunable( @@ -197,6 +198,7 @@ public void periodic() { DogLog.log("Hood/SupplyCurrent", getSupplyCurrent()); DogLog.log("Hood/StatorCurrent", getStatorCurrent()); DogLog.log("Hood/HallSensorTriggered", isHallSensorTriggered()); + DogLog.log("Hood/IsZeroed", isZeroed); DogLog.log( "Hood/ProfileReferenceAngle", Rotations.of(profileReferenceSignal.getValue()).in(Degrees), @@ -346,7 +348,7 @@ public Command moveAtVoltage(Voltage voltage) { () -> { if (isZeroed) { hoodMotor.setControl( - new VoltageOut(voltage.plus(Volts.of(Math.cos(getPosition().in(Radians)) * kG)))); + new VoltageOut(voltage.plus(Volts.of(ShooterHoodConstants.MOTOR_CONFIGURATION.Slot0.kS).times(Math.signum(voltage.in(Volts)))).plus(Volts.of(Math.cos(getPosition().in(Radians)) * kG)))); } else { hoodMotor.setControl(new NeutralOut()); } @@ -372,4 +374,17 @@ private void setPositionControl(Angle position) { hoodMotor.setControl(new NeutralOut()); } } + + /** + * Zeroes the hood without the hall effect sensor by assuming the hood is at the minimum + * angle. + * + * @return A command that zeroes the hood + */ + public Command zero() { + return Commands.runOnce(() -> { + hoodMotor.setPosition(ShooterHoodConstants.MIN_ANGLE); + isZeroed = true; + }).ignoringDisable(true); + } } diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHoodConstants.java b/src/main/java/frc/robot/subsystems/hood/ShooterHoodConstants.java index 7f72a207..2325d7b0 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHoodConstants.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHoodConstants.java @@ -25,20 +25,20 @@ public final class ShooterHoodConstants { public static final int MOTOR_CAN_ID = 20; - public static final int CANDI_CAN_ID = 20; + public static final int CANDI_CAN_ID = 40; public static final String CANBUS = "subsystems"; public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44Foc(1); public static final MomentOfInertia MOMENT_OF_INERTIA = KilogramSquareMeters.of(0.04942); public static final Angle MIN_ANGLE = Degrees.of(17.95); - public static final Angle MAX_ANGLE = Degrees.of(50); + public static final Angle MAX_ANGLE = Degrees.of(41); /** Distance between the pivot point and the center of mass of the hood. */ public static final Distance ARM_LENGTH = Inches.of(6.85); /** Gravity compensation feedforward constant (volts). */ - public static final double kG = 0.22; + public static final double kG = 0.29; public static final Voltage FINE_CONTROL_VOLTAGE = Volts.of(0.5); @@ -47,16 +47,16 @@ public final class ShooterHoodConstants { .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(176 / 3)) .withMotionMagic( new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(30.0) - .withMotionMagicAcceleration(30.0) + .withMotionMagicCruiseVelocity(40.0) + .withMotionMagicAcceleration(20.0) .withMotionMagicJerk(0)) .withSlot0( new Slot0Configs() - .withKP(80.0) - .withKD(1.0) + .withKP(150.0) + .withKD(3.0) .withKV(12.0 / (7368.0 / 60.0) * 176.0 / 3.0) - .withKS(0.0) - .withKA(0.03) + .withKS(0.11) + .withKA(0.05) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign) // Don't add kG here, instead use ShooterHoodConstants.kG ) @@ -74,7 +74,7 @@ public final class ShooterHoodConstants { .withSupplyCurrentLimitEnable(true)) .withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) + .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake)); public static final CANdiConfiguration CANDI_CONFIGURATION = new CANdiConfiguration(); } From 9e55912af75f64bbd646318f9052d59ee7be2497 Mon Sep 17 00:00:00 2001 From: Brian <74162951+codeNinja-1@users.noreply.github.com> Date: Mon, 2 Mar 2026 12:08:22 -0800 Subject: [PATCH 2/3] Format code --- .../robot/subsystems/hood/ShooterHood.java | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java index e40c58be..c2778a67 100644 --- a/src/main/java/frc/robot/subsystems/hood/ShooterHood.java +++ b/src/main/java/frc/robot/subsystems/hood/ShooterHood.java @@ -348,7 +348,12 @@ public Command moveAtVoltage(Voltage voltage) { () -> { if (isZeroed) { hoodMotor.setControl( - new VoltageOut(voltage.plus(Volts.of(ShooterHoodConstants.MOTOR_CONFIGURATION.Slot0.kS).times(Math.signum(voltage.in(Volts)))).plus(Volts.of(Math.cos(getPosition().in(Radians)) * kG)))); + new VoltageOut( + voltage + .plus( + Volts.of(ShooterHoodConstants.MOTOR_CONFIGURATION.Slot0.kS) + .times(Math.signum(voltage.in(Volts)))) + .plus(Volts.of(Math.cos(getPosition().in(Radians)) * kG)))); } else { hoodMotor.setControl(new NeutralOut()); } @@ -376,15 +381,16 @@ private void setPositionControl(Angle position) { } /** - * Zeroes the hood without the hall effect sensor by assuming the hood is at the minimum - * angle. - * + * Zeroes the hood without the hall effect sensor by assuming the hood is at the minimum angle. + * * @return A command that zeroes the hood */ public Command zero() { - return Commands.runOnce(() -> { - hoodMotor.setPosition(ShooterHoodConstants.MIN_ANGLE); - isZeroed = true; - }).ignoringDisable(true); + return Commands.runOnce( + () -> { + hoodMotor.setPosition(ShooterHoodConstants.MIN_ANGLE); + isZeroed = true; + }) + .ignoringDisable(true); } } From 78c98afbadc860a23f247b957cedcca4d87ec237 Mon Sep 17 00:00:00 2001 From: Brian <74162951+codeNinja-1@users.noreply.github.com> Date: Mon, 2 Mar 2026 13:23:04 -0800 Subject: [PATCH 3/3] Add bang-bang control for shooter rollers Improves recovery and spin up time by running the motor at maximum output until it is near the target velocity. --- .../shooterrollers/ShooterRollers.java | 24 +++++++------------ .../ShooterRollersConstants.java | 1 + 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java index 30a33b22..8e31597d 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -82,17 +83,7 @@ public ShooterRollers() { * the build team */ public Command shoot(AngularVelocity targetVelocity) { - - return startEnd( - () -> { - // defines a local function to set motor voltage to make it go - shooterRollerMotor1.setControl( - new VelocityVoltage(targetVelocity.in(RotationsPerSecond))); - }, - () -> { - // defines a local function to stop motor - shooterRollerMotor1.setControl(new CoastOut()); - }); + return shoot(() -> targetVelocity); } /** @@ -105,12 +96,15 @@ public Command shoot(AngularVelocity targetVelocity) { * coasts the motor on end */ public Command shoot(Supplier targetVelocity) { - return runEnd( () -> { - // defines a local function to set motor voltage to make it go - shooterRollerMotor1.setControl( - new VelocityVoltage(targetVelocity.get().in(RotationsPerSecond))); + if (getAngularVelocity().plus(ShooterRollersConstants.BANG_BANG_TOLERANCE).lt(targetVelocity.get())) { + shooterRollerMotor1.setControl(new DutyCycleOut(1)); + } else { + // defines a local function to set motor voltage to make it go + shooterRollerMotor1.setControl( + new VelocityVoltage(targetVelocity.get().in(RotationsPerSecond))); + } }, () -> { // defines a local function to stop motor diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java index c9e1dcae..69f4df52 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java @@ -24,6 +24,7 @@ public final class ShooterRollersConstants { public static final String CANBUS_NAME = "subsystems"; public static final MomentOfInertia MOMENT_OF_INERTIA = KilogramSquareMeters.of(0.003072714); public static final AngularVelocity FIXED_FLYWHEEL_VELOCITY = RotationsPerSecond.of(24.25); + public static final AngularVelocity BANG_BANG_TOLERANCE = RotationsPerSecond.of(4); public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration() .withFeedback(new FeedbackConfigs())