From 393a1f3b77376c4148c8c86cdc904a8583afad7a Mon Sep 17 00:00:00 2001 From: rkris28 Date: Wed, 8 Oct 2025 20:34:44 -0400 Subject: [PATCH 01/11] Shooting Testing Shooting Testing --- src/main/java/frc/robot/Orchestrator.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 29 +++++++++++------ .../frc/robot/subsystems/shooter/Shooter.java | 31 +++++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 4 +-- 4 files changed, 51 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index f1d1b2d..9d0c90f 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -21,7 +21,7 @@ public Orchestrator(Indexer indexer, Shooter shooter) { public Command spinUp(BooleanSupplier fastMode) { return Commands.either( shooter.fullSpeed(), - shooter.speedUp(), + shooter.speedUp_80Percent(), fastMode); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5274408..428e94d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIOSparkMax; import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVision; @@ -31,7 +30,6 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Drive drive; // The robot's subsystems and commands are defined here... - private final Vision vision; private final Indexer indexer = new Indexer(new IndexerIOSparkMax()); private final Shooter shooter = new Shooter(new ShooterIOSparkMax()); private final Orchestrator orchestrator = new Orchestrator(indexer, shooter); @@ -44,7 +42,6 @@ public class RobotContainer { public RobotContainer() { // Configure the trigger bindings drive = new Drive(new DriveIOSparkMax()); - vision = new Vision(new VisionIOPhotonVision("VGA_USB_Camera"){}); configureBindings(); } @@ -75,16 +72,30 @@ private void configureBindings() { .rightTrigger() .whileTrue(orchestrator.shootCycle(() -> fastMode)) .onFalse(orchestrator.stopAll()); - driverController.y().whileTrue(shooter.speedUp()).onFalse(shooter.stop()); - driverController - .leftTrigger() - .whileTrue(orchestrator.reverseAll()) - .onFalse(orchestrator.stopAll()); - + driverController .rightBumper() .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); + + driverController.x().whileTrue(shooter.speedUp_20Percent()).onFalse(shooter.stop()); + + driverController.b().whileTrue(shooter.speedUp_40Percent()).onFalse(shooter.stop()); + + driverController.start().whileTrue(shooter.speedUp_60Percent()).onFalse(shooter.stop()); + + driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); + + driverController.y().onTrue(indexer.indexIntoShooter()); + + + driverController + .leftBumper() + .whileTrue(orchestrator.reverseAll()) + .onFalse(orchestrator.stopAll()); + + + } /** diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 52e56b4..4542335 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -37,10 +37,37 @@ public Command reverse() { } - public Command speedUp() { + public Command speedUp_60Percent() { return Commands.run( () -> { - io.setPercent(ShooterConstants.SPEED); + io.setPercent(0.6); + }, + this + ); + } + + public Command speedUp_80Percent() { + return Commands.run( + () -> { + io.setPercent(0.8); + }, + this + ); + } + + public Command speedUp_40Percent() { + return Commands.run( + () -> { + io.setPercent(0.4); + }, + this + ); + } + + public Command speedUp_20Percent() { + return Commands.run( + () -> { + io.setPercent(0.2); }, this ); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index de81527..2b28620 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,8 +2,6 @@ public class ShooterConstants { - public static final double SPEED = 0.8; - public static final double REVERSE_SHOOTER_PERCENT = -0.5; public static final int SHOOTER_MOTOR_ID = 8; @@ -12,5 +10,5 @@ public class ShooterConstants { public static final double ENCODER_VELOCITY_CONVERSION = 1.0; - public static final double SPINUP_SECONDS = 0.1; + public static final double SPINUP_SECONDS = 0.6; } \ No newline at end of file From 9e62d5ef89270ba5b5611714a2bd12fd56166cdc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Wed, 8 Oct 2025 20:50:48 -0400 Subject: [PATCH 02/11] regression line --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/Shooter.java | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 428e94d..f096a95 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,7 @@ private void configureBindings() { .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); - driverController.x().whileTrue(shooter.speedUp_20Percent()).onFalse(shooter.stop()); + driverController.x().whileTrue(shooter.speedUp_Distance(300.0)).onFalse(shooter.stop()); driverController.b().whileTrue(shooter.speedUp_40Percent()).onFalse(shooter.stop()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 4542335..56e13b7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -36,7 +36,15 @@ public Command reverse() { ); } - + public Command speedUp_Distance(Double distance) { + return Commands.run( + () -> { + double percent = (8.331 * Math.pow(10,-5)*Math.pow(distance,2) - 0.0734*distance + 15.709)/100; + io.setPercent(percent); + }, + this + ); + } public Command speedUp_60Percent() { return Commands.run( () -> { From 6ce40c4a4678c2d3ed613e87a20aa539e4358a2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Wed, 15 Oct 2025 22:05:24 -0400 Subject: [PATCH 03/11] fixed regression line, implemented shooting functions --- src/main/java/frc/robot/Orchestrator.java | 22 +++++++- src/main/java/frc/robot/RobotContainer.java | 6 +-- .../frc/robot/subsystems/shooter/Shooter.java | 51 +++---------------- 3 files changed, 28 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 9d0c90f..14cc9bb 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -7,21 +7,41 @@ import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Orchestrator { private final Indexer indexer; private final Shooter shooter; + @AutoLogOutput + private double Distance = 300.0; + @AutoLogOutput private double shootingPower = 0.0; public Orchestrator(Indexer indexer, Shooter shooter) { this.indexer = indexer; this.shooter = shooter; } + public Command spinUpDistance(Double distance) { + Distance = distance; + double percent = (8.331 * Math.pow(10, -5) * Math.pow(distance, 2) + 0.0734 * distance + 15.709) / 100; + Logger.recordOutput("Shooter/Percent", percent); + return shooter.runPercent(percent); + } + public Command shootCycleDistance() { + return Commands.parallel( + spinUpDistance(420.0), + Commands.sequence( + intakeIfNeeded(), + Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS), + indexer.indexIntoShooter()) + .repeatedly()); + } public Command spinUp(BooleanSupplier fastMode) { return Commands.either( shooter.fullSpeed(), - shooter.speedUp_80Percent(), + shooter.runPercent(0.8), fastMode); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f096a95..720d12d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,11 +78,7 @@ private void configureBindings() { .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); - driverController.x().whileTrue(shooter.speedUp_Distance(300.0)).onFalse(shooter.stop()); - - driverController.b().whileTrue(shooter.speedUp_40Percent()).onFalse(shooter.stop()); - - driverController.start().whileTrue(shooter.speedUp_60Percent()).onFalse(shooter.stop()); + driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 56e13b7..93fe0ff 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -36,51 +36,6 @@ public Command reverse() { ); } - public Command speedUp_Distance(Double distance) { - return Commands.run( - () -> { - double percent = (8.331 * Math.pow(10,-5)*Math.pow(distance,2) - 0.0734*distance + 15.709)/100; - io.setPercent(percent); - }, - this - ); - } - public Command speedUp_60Percent() { - return Commands.run( - () -> { - io.setPercent(0.6); - }, - this - ); - } - - public Command speedUp_80Percent() { - return Commands.run( - () -> { - io.setPercent(0.8); - }, - this - ); - } - - public Command speedUp_40Percent() { - return Commands.run( - () -> { - io.setPercent(0.4); - }, - this - ); - } - - public Command speedUp_20Percent() { - return Commands.run( - () -> { - io.setPercent(0.2); - }, - this - ); - } - public Command fullSpeed() { return Commands.run( () -> { @@ -89,6 +44,12 @@ public Command fullSpeed() { this ); } + public Command runPercent(double percent){ + return Commands.run(() -> { + io.setPercent(percent); + }, + this); + } } From 58ca07dd5b3e1671371533c3caa140a9abe0b345 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Thu, 16 Oct 2025 16:57:53 -0400 Subject: [PATCH 04/11] Added PID --- .../robot/subsystems/shooter/ShooterIO.java | 5 +++++ .../subsystems/shooter/ShooterIOSparkMax.java | 18 +++++++++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 2a6a301..338ae4a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -19,6 +19,8 @@ class ShooterIOInputs { public boolean readyToShoot = false; } + default void setPosition(double setpoint) {} + default void updateInputs(ShooterIOInputs inputs) {} default void setPercent(double percent) {} @@ -26,4 +28,7 @@ default void setPercent(double percent) {} default void setVoltage(double voltage) {} default void stop() {} + + default void goToSetpoint() {} + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 70c2267..ac36f45 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -3,9 +3,10 @@ import static frc.robot.subsystems.shooter.ShooterConstants.*; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.EncoderConfig; @@ -17,9 +18,14 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkMax motor; private final RelativeEncoder encoder; + private SparkClosedLoopController controller; + double setpoint = 0.0; + public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); + controller = motor.getClosedLoopController(); + var config = new SparkMaxConfig(); config.inverted(false) .idleMode(IdleMode.kBrake) @@ -45,6 +51,11 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.velocity = encoder.getVelocity(); } + @Override + public void setPosition(double setpoint) { + this.setpoint = setpoint; + } + public void setPercent(double percent) { motor.set(percent); } @@ -56,4 +67,9 @@ public void setVoltage(double volts) { public void stop() { motor.set(0); } + + @Override + public void goToSetpoint() { + controller.setReference(this.setpoint, SparkBase.ControlType.kPosition); + } } From cc2275b56d8747b38240ec04c3aa5ebcdac813b9 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Thu, 16 Oct 2025 17:05:40 -0400 Subject: [PATCH 05/11] Added a full PID system in the shooter subsystem. --- .../frc/robot/subsystems/shooter/Shooter.java | 33 +++++++++++++++---- .../subsystems/shooter/ShooterConstants.java | 2 ++ .../robot/subsystems/shooter/ShooterIO.java | 4 ++- .../subsystems/shooter/ShooterIOSparkMax.java | 5 +-- 4 files changed, 35 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 93fe0ff..d6d92b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -15,11 +15,13 @@ public class Shooter extends SubsystemBase { public Shooter(ShooterIO io) { this.io = io; this.inputs = new ShooterIOInputsAutoLogged(); + this.io.setSpeed(0.5); // TODO: Change the random value } @Override public void periodic() { io.updateInputs(inputs); + io.goToSetpoint(); Logger.processInputs("Shooter", inputs); } @@ -44,12 +46,31 @@ public Command fullSpeed() { this ); } - public Command runPercent(double percent){ - return Commands.run(() -> { - io.setPercent(percent); - }, - this); - } + public Command runPercent(double percent){ + return Commands.run(() -> { + io.setPercent(percent); + }, + this); + } + + public boolean atSetpoint() { + return inputs.atSetpoint; + } + + public Command toSetpoint(double setpointPercent) { + return Commands.run( + () -> { + io.setSpeed(setpointPercent); + }, + this); + } + public Command toSetpoint(DoubleSupplier setpointPercent) { + return Commands.run( + () -> { + io.setSpeed(setpointPercent.getAsDouble()); + }, + this); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 2b28620..14afbea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -11,4 +11,6 @@ public class ShooterConstants { public static final double ENCODER_VELOCITY_CONVERSION = 1.0; public static final double SPINUP_SECONDS = 0.6; + + public static final double TOLERANCE = 0.1; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 338ae4a..2f718ae 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -17,9 +17,11 @@ class ShooterIOInputs { public double temperature; public boolean readyToShoot = false; + + public boolean atSetpoint = false; } - default void setPosition(double setpoint) {} + default void setSpeed(double setpoint) {} default void updateInputs(ShooterIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index ac36f45..990e475 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -18,7 +18,7 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkMax motor; private final RelativeEncoder encoder; - private SparkClosedLoopController controller; + private final SparkClosedLoopController controller; double setpoint = 0.0; public ShooterIOSparkMax() { @@ -49,10 +49,11 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); inputs.velocity = encoder.getVelocity(); + inputs.atSetpoint = Math.abs(setpoint - inputs.velocity) < TOLERANCE; } @Override - public void setPosition(double setpoint) { + public void setSpeed(double setpoint) { this.setpoint = setpoint; } From 6a543b366a4d3d47322eece1ec9fae1575631b46 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Thu, 16 Oct 2025 17:12:14 -0400 Subject: [PATCH 06/11] Tracked angular velocity --- src/main/java/frc/robot/subsystems/shooter/ShooterIO.java | 2 ++ .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 1 + 2 files changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 2f718ae..e35f6be 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -16,6 +16,8 @@ class ShooterIOInputs { public double temperature; + public double angularVelocity; + public boolean readyToShoot = false; public boolean atSetpoint = false; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 990e475..ed264f1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -49,6 +49,7 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); inputs.velocity = encoder.getVelocity(); + inputs.angularVelocity = inputs.velocity * (2 * Math.PI / 60); inputs.atSetpoint = Math.abs(setpoint - inputs.velocity) < TOLERANCE; } From fd3ac5c8fc4da2914cf58b67936d6978713eafdb Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Thu, 16 Oct 2025 17:20:43 -0400 Subject: [PATCH 07/11] Added bindings and orchestrator methods to use the setpoint. --- src/main/java/frc/robot/Orchestrator.java | 16 ++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 10 ++++++++++ 2 files changed, 26 insertions(+) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 14cc9bb..3682af5 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -1,6 +1,7 @@ package frc.robot; import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -71,6 +72,21 @@ public Command shootCycle(BooleanSupplier fastMode) { .repeatedly()); } + public Command shootOnceSetpoint(DoubleSupplier setpoint) { + return Commands.parallel( + shooter.toSetpoint(setpoint) + .withTimeout(0.001) + .andThen(shooter.toSetpoint(setpoint).until(shooter::atSetpoint)), + Commands.sequence( + intakeIfNeeded(), + Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS), + indexer.indexIntoShooter())); + } + + public Command shootCycleSetpoint(DoubleSupplier setpoint) { + return shootOnceSetpoint(setpoint).repeatedly(); + } + public Command reverseAll() { return Commands.parallel(shooter.reverse(), indexer.reverse()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 720d12d..e7292c2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,6 +78,16 @@ private void configureBindings() { .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); + driverController + .b() + .whileTrue(orchestrator.shootCycleSetpoint(() -> 0.5)) + .onFalse(orchestrator.stopAll()); + + driverController + .b() + .onTrue(orchestrator.shootOnceSetpoint(() -> 0.5)) + .onFalse(orchestrator.stopAll()); + driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); From 87ebeb69431f0f466d83b924a2d7e3e0539378de Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Thu, 16 Oct 2025 17:50:07 -0400 Subject: [PATCH 08/11] Added config for the PID --- .../frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index ed264f1..9da1108 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -9,6 +9,7 @@ import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; @@ -31,6 +32,11 @@ public ShooterIOSparkMax() { .idleMode(IdleMode.kBrake) .voltageCompensation(12) .smartCurrentLimit(30); + config + .closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kAlternateOrExternalEncoder) + .positionWrappingEnabled(false) + .pidf(15, 0.0, 7, 0.0); // TODO: Fix arbitrary values EncoderConfig enc = new EncoderConfig(); enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); From f4f1258a1dd637c497ae71b9382a9c15b6694462 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 20 Oct 2025 17:59:32 -0400 Subject: [PATCH 09/11] Added constants for PID config. --- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 4 ++++ .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 14afbea..25e0727 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -13,4 +13,8 @@ public class ShooterConstants { public static final double SPINUP_SECONDS = 0.6; public static final double TOLERANCE = 0.1; + + public static final double PID_P = 15.0; + + public static final double PID_D = 7.0; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 9da1108..9a03fd4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -36,7 +36,7 @@ public ShooterIOSparkMax() { .closedLoop .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kAlternateOrExternalEncoder) .positionWrappingEnabled(false) - .pidf(15, 0.0, 7, 0.0); // TODO: Fix arbitrary values + .pidf(PID_P, 0.0, PID_D, 0.0); EncoderConfig enc = new EncoderConfig(); enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); From e6ddda57c7a537c416de060a059443653168bc16 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 20 Oct 2025 18:05:05 -0400 Subject: [PATCH 10/11] Fixed goToSetpoint method to update velocity, not position. --- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 9a03fd4..cb2967f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -78,6 +78,6 @@ public void stop() { @Override public void goToSetpoint() { - controller.setReference(this.setpoint, SparkBase.ControlType.kPosition); + controller.setReference(this.setpoint, SparkBase.ControlType.kVelocity); } } From 92837e7e5b9a814bac6c5cda948669d5f17f1f19 Mon Sep 17 00:00:00 2001 From: Ekansh Jain Date: Tue, 21 Oct 2025 13:19:10 -0400 Subject: [PATCH 11/11] Fix encoder type --- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index cb2967f..4c62494 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -34,7 +34,7 @@ public ShooterIOSparkMax() { .smartCurrentLimit(30); config .closedLoop - .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kAlternateOrExternalEncoder) + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder) .positionWrappingEnabled(false) .pidf(PID_P, 0.0, PID_D, 0.0);