From 393a1f3b77376c4148c8c86cdc904a8583afad7a Mon Sep 17 00:00:00 2001 From: rkris28 Date: Wed, 8 Oct 2025 20:34:44 -0400 Subject: [PATCH 01/43] 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/43] 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/43] 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 a68fea9ef00af8cde34cf938fe2123ea829edc22 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:00:40 -0400 Subject: [PATCH 04/43] added PID velocity control --- .../frc/robot/subsystems/shooter/Shooter.java | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 93fe0ff..81401b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -21,13 +21,13 @@ public Shooter(ShooterIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); + io.goToSetpoint(); } public Command stop() { return Commands.runOnce(() ->{io.stop();}, this); } - public Command reverse() { return Commands.startEnd( () -> io.setPercent(ShooterConstants.REVERSE_SHOOTER_PERCENT), @@ -36,6 +36,27 @@ public Command reverse() { ); } + public boolean atSetpoint() { + return inputs.atSetpoint; + } + + public Command toSetpoint(double setpointRPM) { + return Commands.run( + () -> { + io.setVelocity(setpointRPM); + }, + this + ); + } + + public Command toSetpoint(DoubleSupplier setpointRPM) { + return Commands.run( + () -> { + io.setVelocity(setpointRPM.getAsDouble()); + }, + this); + } + public Command fullSpeed() { return Commands.run( () -> { From d4752d08178469334d7ea779ae6625301b9eed3d Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:02:13 -0400 Subject: [PATCH 05/43] added tolerance constant --- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 2b28620..97179fc 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 = 1.0; // TODO: change tolerance } \ No newline at end of file From 31f28038037e607d39712d0c41d69f65293132f0 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:06:26 -0400 Subject: [PATCH 06/43] setpointRPM and velocity control for PID --- .../java/frc/robot/subsystems/shooter/ShooterIO.java | 9 ++++++++- 1 file changed, 8 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..729379d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.shooter; -import org.ejml.dense.row.factory.DecompositionFactory_CDRM; import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { @@ -16,7 +15,11 @@ class ShooterIOInputs { public double temperature; + public double setpointRPM = 0.0; + public boolean readyToShoot = false; + + public boolean atSetpoint = false; } default void updateInputs(ShooterIOInputs inputs) {} @@ -25,5 +28,9 @@ default void setPercent(double percent) {} default void setVoltage(double voltage) {} + default void setVelocity(double RPM) {} + + default void goToSetpoint() {} + default void stop() {} } \ No newline at end of file From 661c835004b593ef6cfb1d6c772200da9da88ed4 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:08:11 -0400 Subject: [PATCH 07/43] config for PID (placeholder vals) --- .../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 70c2267..13ec4c7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -26,6 +26,12 @@ public ShooterIOSparkMax() { .voltageCompensation(12) .smartCurrentLimit(30); + config + .closedLoop + .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kAlternateOrExternalEncoder) + .positionWrappingEnabled(false) + .pidf(0,0,0,0); // TODO: tune PIDF values + EncoderConfig enc = new EncoderConfig(); enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); From 5326df58f5f8c11883b5e9c3e3d39b5bcceb769e Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:08:44 -0400 Subject: [PATCH 08/43] Update inputs with setpointRPM and atSetpoint calculation --- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 13ec4c7..d229e21 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -49,6 +49,8 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); inputs.velocity = encoder.getVelocity(); + inputs.setpointRPM = setpointRPM; + inputs.atSetpoint = Math.abs(setpointRPM - inputs.velocity) < TOLERANCE; // TODO: change tolerance } public void setPercent(double percent) { From 914bb63dd5a2a4c64ca04d6902e92d223381313c Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:09:56 -0400 Subject: [PATCH 09/43] Implement setVelocity and goToSetpoint --- .../robot/subsystems/shooter/ShooterIOSparkMax.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index d229e21..13b60f2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -61,6 +61,16 @@ public void setVoltage(double volts) { motor.setVoltage(volts); } + @Override + public void setVelocity(double setpointRPM) { + this.setpointRPM = setpointRPM; + } + + @Override + public void goToSetpoint() { + controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); + } + public void stop() { motor.set(0); } From f2f44392089cbf77bb3bdb0cae079a8edd09de32 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:11:27 -0400 Subject: [PATCH 10/43] Initialize SparkClosedLoopController --- .../robot/subsystems/shooter/ShooterIOSparkMax.java | 10 +++++++++- 1 file changed, 9 insertions(+), 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 13b60f2..fb06b9a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -3,11 +3,13 @@ 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.ClosedLoopConfig; import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; @@ -17,9 +19,15 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkMax motor; private final RelativeEncoder encoder; + private final SparkClosedLoopController controller; + + private double setpointRPM; + public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); + controller = motor.getClosedLoopController(); + var config = new SparkMaxConfig(); config.inverted(false) .idleMode(IdleMode.kBrake) From 2b4c12f7e0380230278c6be17f8d41c6666206fc Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:28:03 -0400 Subject: [PATCH 11/43] Added angularVelocity --- src/main/java/frc/robot/subsystems/shooter/ShooterIO.java | 4 +++- 1 file changed, 3 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 729379d..f7317f6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -7,7 +7,9 @@ public interface ShooterIO { @AutoLog class ShooterIOInputs { - public double velocity; + public double velocity; // RPM + + public double angularVelocity; // Rad/s public double appliedVolts; From 97b9033421bc91117b7fa9dfa93a2b74e1b95dd5 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 18:28:41 -0400 Subject: [PATCH 12/43] convert velocity (rpm) to angular velocity rad/s --- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 3 ++- 1 file changed, 2 insertions(+), 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 fb06b9a..b98f77e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -56,7 +56,8 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.temperature = motor.getMotorTemperature(); inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); - inputs.velocity = encoder.getVelocity(); + inputs.velocity = encoder.getVelocity(); // RPM + inputs.angularVelocity = encoder.getVelocity() * (2 * Math.PI / 60); // Rad/s inputs.setpointRPM = setpointRPM; inputs.atSetpoint = Math.abs(setpointRPM - inputs.velocity) < TOLERANCE; // TODO: change tolerance } From 919609902d2ba1d37afc986885e12fb6f2ee0fe2 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 19:48:03 -0400 Subject: [PATCH 13/43] Added PID based shootOnce and shootCycle --- src/main/java/frc/robot/Orchestrator.java | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 14cc9bb..dabf726 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -71,6 +71,26 @@ public Command shootCycle(BooleanSupplier fastMode) { .repeatedly()); } + public Command shootOncePID(double setpointRPM) { + return Commands.parallel( + shooter.toSetpoint(setpointRPM), + Commands.sequence( + intakeIfNeeded(), + Commands.waitUntil(shooter::atSetpoint), + indexer.indexIntoShooter())); + } + + public Command shootCyclePID(double setpointRPM) { + return Commands.parallel( + shooter.toSetpoint(setpointRPM), + Commands.sequence( + intakeIfNeeded(), + Commands.waitUntil(shooter::atSetpoint), + indexer.indexIntoShooter()) + .repeatedly()); + } + + public Command reverseAll() { return Commands.parallel(shooter.reverse(), indexer.reverse()); } From 1e599612106e15220ccc7d307e79b717af3c49b7 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 19:55:31 -0400 Subject: [PATCH 14/43] removed uneeded angular velocity --- src/main/java/frc/robot/subsystems/shooter/ShooterIO.java | 2 -- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 1 - 2 files changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index f7317f6..6f84dc5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -9,8 +9,6 @@ class ShooterIOInputs { public double velocity; // RPM - public double angularVelocity; // Rad/s - public double appliedVolts; public double currentAmps; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index b98f77e..6ce67f2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -57,7 +57,6 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); inputs.velocity = encoder.getVelocity(); // RPM - inputs.angularVelocity = encoder.getVelocity() * (2 * Math.PI / 60); // Rad/s inputs.setpointRPM = setpointRPM; inputs.atSetpoint = Math.abs(setpointRPM - inputs.velocity) < TOLERANCE; // TODO: change tolerance } From f6e074306dca6cf56cb3432a40e921f12f9bab17 Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 20:11:43 -0400 Subject: [PATCH 15/43] Added getVelocity method --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 81401b1..6e06cf9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,6 +40,10 @@ public boolean atSetpoint() { return inputs.atSetpoint; } + public double getVelocity() { + return inputs.velocity; + } + public Command toSetpoint(double setpointRPM) { return Commands.run( () -> { From 12740aaa5b0fddcc73af1956023880063f5f052a Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 20:12:08 -0400 Subject: [PATCH 16/43] Log shooter velocity before indexing --- src/main/java/frc/robot/Orchestrator.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index dabf726..3b5dfc6 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -77,6 +77,7 @@ public Command shootOncePID(double setpointRPM) { Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), + Commands.runOnce(() -> Logger.recordOutput("Shooter/VelocityBeforeShoot", shooter.getVelocity())), indexer.indexIntoShooter())); } @@ -86,6 +87,7 @@ public Command shootCyclePID(double setpointRPM) { Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), + Commands.runOnce(() -> Logger.recordOutput("Shooter/VelocityBeforeShoot", shooter.getVelocity())), indexer.indexIntoShooter()) .repeatedly()); } From 7b3b96587bcb0c75e82ffb845aca32d09a43139e Mon Sep 17 00:00:00 2001 From: Niti Date: Sun, 19 Oct 2025 20:28:03 -0400 Subject: [PATCH 17/43] x button to use shootCyclePID() --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 720d12d..62b8d44 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,7 +78,8 @@ private void configureBindings() { .onTrue(orchestrator.shootOnce(() -> fastMode)) .onFalse(orchestrator.stopAll()); - driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); + //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); + driverController.x().whileTrue(orchestrator.shootCyclePID(0)).onFalse(shooter.stop()); // TODO: change the RPM driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); From f78e0a1102924a7e71a08baa8bfeb201ff0fb311 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Sun, 26 Oct 2025 23:41:16 -0400 Subject: [PATCH 18/43] tuned PIDS --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/shooter/ShooterConstants.java | 9 +++++---- .../subsystems/shooter/ShooterIOSparkMax.java | 15 ++++++++------- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62b8d44..07f37b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(0)).onFalse(shooter.stop()); // TODO: change the RPM + driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 97179fc..092a2e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,16 +1,17 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.math.MathShared; +import edu.wpi.first.math.MathUtil; + public class ShooterConstants { public static final double REVERSE_SHOOTER_PERCENT = -0.5; public static final int SHOOTER_MOTOR_ID = 8; - public static final double ENCODER_POSITION_CONVERSION = 1.0; - - public static final double ENCODER_VELOCITY_CONVERSION = 1.0; + public static final double ENCODER_VELOCITY_CONVERSION = 1; public static final double SPINUP_SECONDS = 0.6; - public static final double TOLERANCE = 1.0; // TODO: change tolerance + public static final double TOLERANCE = 0.1; // TODO: change tolerance } \ 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 6ce67f2..d2312d9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import static com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder; import static frc.robot.subsystems.shooter.ShooterConstants.*; import com.revrobotics.RelativeEncoder; @@ -34,19 +35,18 @@ public ShooterIOSparkMax() { .voltageCompensation(12) .smartCurrentLimit(30); - config - .closedLoop - .feedbackSensor(ClosedLoopConfig.FeedbackSensor.kAlternateOrExternalEncoder) + var loopConfig = new ClosedLoopConfig(); + loopConfig + .feedbackSensor(kPrimaryEncoder) .positionWrappingEnabled(false) - .pidf(0,0,0,0); // TODO: tune PIDF values + .pid(0.000009,0.0000003,0.0001); // TODO: tune PIDF values EncoderConfig enc = new EncoderConfig(); - enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); config.apply(enc); + config.apply(loopConfig); motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - encoder = motor.getEncoder(); } @@ -56,9 +56,10 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.temperature = motor.getMotorTemperature(); inputs.appliedVolts = motor.getBusVoltage() * motor.getAppliedOutput(); inputs.currentAmps = motor.getOutputCurrent(); - inputs.velocity = encoder.getVelocity(); // RPM + inputs.velocity = encoder.getVelocity();// RPM inputs.setpointRPM = setpointRPM; inputs.atSetpoint = Math.abs(setpointRPM - inputs.velocity) < TOLERANCE; // TODO: change tolerance + } public void setPercent(double percent) { From ff59ac3fde0b1f3317ca152514ce5d4d50d8a359 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 20:04:29 -0500 Subject: [PATCH 19/43] Random change to revert PID --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6e06cf9..0e1f046 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -74,7 +74,7 @@ public Command runPercent(double percent){ io.setPercent(percent); }, this); - } + } // } From 26397eb66020023a3f19b53ea882810b6d1bd7eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Mon, 27 Oct 2025 17:32:58 -0400 Subject: [PATCH 20/43] added Ks characterization plus feedforward that updates with rpm needed. --- .../subsystems/shooter/ShooterIOSparkMax.java | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index d2312d9..37fbe9d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class ShooterIOSparkMax implements ShooterIO { @@ -23,13 +24,15 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkClosedLoopController controller; private double setpointRPM; + private SimpleMotorFeedforward feedforward; + + public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); - + feedforward = new SimpleMotorFeedforward(0,0.39,0.52); controller = motor.getClosedLoopController(); - - var config = new SparkMaxConfig(); + var config = new SparkMaxConfig(); config.inverted(false) .idleMode(IdleMode.kBrake) .voltageCompensation(12) @@ -39,7 +42,8 @@ public ShooterIOSparkMax() { loopConfig .feedbackSensor(kPrimaryEncoder) .positionWrappingEnabled(false) - .pid(0.000009,0.0000003,0.0001); // TODO: tune PIDF values + .pid(0.000009,0.0000003,0.0001);// TODO: tune PIDF values + EncoderConfig enc = new EncoderConfig(); enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); @@ -69,6 +73,9 @@ public void setPercent(double percent) { public void setVoltage(double volts) { motor.setVoltage(volts); } + public double returnVelocity(){ + return this.encoder.getVelocity(); + } @Override public void setVelocity(double setpointRPM) { @@ -77,6 +84,7 @@ public void setVelocity(double setpointRPM) { @Override public void goToSetpoint() { + motor.setVoltage(feedforward.calculateWithVelocities(this.encoder.getVelocity(), this.setpointRPM)); controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } From 08887ebbe5237a8ecc3ed5befa5b66c19c6a0667 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Mon, 27 Oct 2025 19:21:34 -0400 Subject: [PATCH 21/43] added Ks characterization plus feedforward that updates with rpm needed. --- src/main/java/frc/robot/Orchestrator.java | 1 + src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 4 +--- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 3b5dfc6..4f74d5a 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -83,6 +83,7 @@ public Command shootOncePID(double setpointRPM) { public Command shootCyclePID(double setpointRPM) { return Commands.parallel( + shooter.runFF(), shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07f37b0..61e9031 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,7 +80,7 @@ private void configureBindings() { //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - + driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 37fbe9d..2ad3428 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -30,7 +30,6 @@ public class ShooterIOSparkMax implements ShooterIO { public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); - feedforward = new SimpleMotorFeedforward(0,0.39,0.52); controller = motor.getClosedLoopController(); var config = new SparkMaxConfig(); config.inverted(false) @@ -83,8 +82,7 @@ public void setVelocity(double setpointRPM) { } @Override - public void goToSetpoint() { - motor.setVoltage(feedforward.calculateWithVelocities(this.encoder.getVelocity(), this.setpointRPM)); + public void goToSetpoint(){ controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } From e0d9fc6cf8c3210462341dd48fcb5a3663570957 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Mon, 27 Oct 2025 20:05:22 -0400 Subject: [PATCH 22/43] added Ks characterization plus feedforward that updates with rpm needed. --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61e9031..b6eafc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,8 +79,9 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); + driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM + driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); + driverController.b().onTrue(shooter.runFF()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 092a2e7..72e5d83 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,7 +9,7 @@ public class ShooterConstants { public static final int SHOOTER_MOTOR_ID = 8; - public static final double ENCODER_VELOCITY_CONVERSION = 1; + public static final double RPM_TO_RADS = (2*Math.PI)/60; public static final double SPINUP_SECONDS = 0.6; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 2ad3428..3f72e31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -45,7 +45,7 @@ public ShooterIOSparkMax() { EncoderConfig enc = new EncoderConfig(); - enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); + enc.velocityConversionFactor(RPM_TO_RADS); config.apply(enc); config.apply(loopConfig); From 128b5dfd0d3b4c19bf03b5bb5f5731e656dd26b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Mon, 27 Oct 2025 20:51:22 -0400 Subject: [PATCH 23/43] switch to ff --- src/main/java/frc/robot/Orchestrator.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 4f74d5a..20d9d20 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -82,9 +82,9 @@ public Command shootOncePID(double setpointRPM) { } public Command shootCyclePID(double setpointRPM) { - return Commands.parallel( - shooter.runFF(), - shooter.toSetpoint(setpointRPM), + return Commands.sequence( + Commands.sequence(shooter.toSetpoint((setpointRPM)), + shooter.runFF()), Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b6eafc8..99193db 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM + driverController.x().whileTrue(orchestrator.shootCyclePID((2000))).onFalse(shooter.stop()); // TODO: change the RPM driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); driverController.b().onTrue(shooter.runFF()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 3f72e31..664b6f9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -83,7 +83,7 @@ public void setVelocity(double setpointRPM) { @Override public void goToSetpoint(){ - controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); + //controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } public void stop() { From ae18ac1800f5d4e38b9b07c63a5ecbd53b7e9242 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:46:13 -0500 Subject: [PATCH 24/43] Revert "switch to ff" This reverts commit a61686d7bf8c3a57a28f0baf9861a0fa07d3826a. --- src/main/java/frc/robot/Orchestrator.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 20d9d20..4f74d5a 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -82,9 +82,9 @@ public Command shootOncePID(double setpointRPM) { } public Command shootCyclePID(double setpointRPM) { - return Commands.sequence( - Commands.sequence(shooter.toSetpoint((setpointRPM)), - shooter.runFF()), + return Commands.parallel( + shooter.runFF(), + shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 99193db..b6eafc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID((2000))).onFalse(shooter.stop()); // TODO: change the RPM + driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); driverController.b().onTrue(shooter.runFF()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 664b6f9..3f72e31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -83,7 +83,7 @@ public void setVelocity(double setpointRPM) { @Override public void goToSetpoint(){ - //controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); + controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } public void stop() { From e546e310e0f70e87786ca7ebe1b61cae021bd7ba Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:46:27 -0500 Subject: [PATCH 25/43] Revert "added Ks characterization plus feedforward that updates with rpm needed." This reverts commit 7cec5a1a9a021dac6ee63a9f91171d8207eca32e. --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b6eafc8..61e9031 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,9 +79,8 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM - driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); - driverController.b().onTrue(shooter.runFF()); + driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM + driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 72e5d83..092a2e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,7 +9,7 @@ public class ShooterConstants { public static final int SHOOTER_MOTOR_ID = 8; - public static final double RPM_TO_RADS = (2*Math.PI)/60; + public static final double ENCODER_VELOCITY_CONVERSION = 1; public static final double SPINUP_SECONDS = 0.6; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 3f72e31..2ad3428 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -45,7 +45,7 @@ public ShooterIOSparkMax() { EncoderConfig enc = new EncoderConfig(); - enc.velocityConversionFactor(RPM_TO_RADS); + enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); config.apply(enc); config.apply(loopConfig); From a33a541a8e251106ea499c2619cc15b8640886ce Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:46:51 -0500 Subject: [PATCH 26/43] Revert "added Ks characterization plus feedforward that updates with rpm needed." This reverts commit d2abf762a2d65ecdefdd7943f8c3532a022327d8. --- src/main/java/frc/robot/Orchestrator.java | 1 - src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 4 +++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 4f74d5a..3b5dfc6 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -83,7 +83,6 @@ public Command shootOncePID(double setpointRPM) { public Command shootCyclePID(double setpointRPM) { return Commands.parallel( - shooter.runFF(), shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61e9031..07f37b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,7 +80,7 @@ private void configureBindings() { //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); + driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 2ad3428..37fbe9d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -30,6 +30,7 @@ public class ShooterIOSparkMax implements ShooterIO { public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); + feedforward = new SimpleMotorFeedforward(0,0.39,0.52); controller = motor.getClosedLoopController(); var config = new SparkMaxConfig(); config.inverted(false) @@ -82,7 +83,8 @@ public void setVelocity(double setpointRPM) { } @Override - public void goToSetpoint(){ + public void goToSetpoint() { + motor.setVoltage(feedforward.calculateWithVelocities(this.encoder.getVelocity(), this.setpointRPM)); controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } From 97e1fe55481b9c7468d36b153405ac33efe0d06e Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:47:44 -0500 Subject: [PATCH 27/43] Reapply "added Ks characterization plus feedforward that updates with rpm needed." This reverts commit 06cf9b61281535febdf693ae035cc2d33b8bfd23. --- src/main/java/frc/robot/Orchestrator.java | 1 + src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 4 +--- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 3b5dfc6..4f74d5a 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -83,6 +83,7 @@ public Command shootOncePID(double setpointRPM) { public Command shootCyclePID(double setpointRPM) { return Commands.parallel( + shooter.runFF(), shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07f37b0..61e9031 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,7 +80,7 @@ private void configureBindings() { //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - + driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 37fbe9d..2ad3428 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -30,7 +30,6 @@ public class ShooterIOSparkMax implements ShooterIO { public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); - feedforward = new SimpleMotorFeedforward(0,0.39,0.52); controller = motor.getClosedLoopController(); var config = new SparkMaxConfig(); config.inverted(false) @@ -83,8 +82,7 @@ public void setVelocity(double setpointRPM) { } @Override - public void goToSetpoint() { - motor.setVoltage(feedforward.calculateWithVelocities(this.encoder.getVelocity(), this.setpointRPM)); + public void goToSetpoint(){ controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } From f73516737c87c1b04e3e925cda6cdc6b5a971865 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Mon, 27 Oct 2025 20:05:22 -0400 Subject: [PATCH 28/43] added Ks characterization plus feedforward that updates with rpm needed. --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61e9031..b6eafc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,8 +79,9 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); + driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM + driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); + driverController.b().onTrue(shooter.runFF()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 092a2e7..72e5d83 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,7 +9,7 @@ public class ShooterConstants { public static final int SHOOTER_MOTOR_ID = 8; - public static final double ENCODER_VELOCITY_CONVERSION = 1; + public static final double RPM_TO_RADS = (2*Math.PI)/60; public static final double SPINUP_SECONDS = 0.6; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 2ad3428..3f72e31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -45,7 +45,7 @@ public ShooterIOSparkMax() { EncoderConfig enc = new EncoderConfig(); - enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); + enc.velocityConversionFactor(RPM_TO_RADS); config.apply(enc); config.apply(loopConfig); From 5e54ca20330414e09d7a8807c7be5e8aad8726da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?ShayanH=C3=A1sHackedThis?= Date: Mon, 27 Oct 2025 20:51:22 -0400 Subject: [PATCH 29/43] switch to ff --- src/main/java/frc/robot/Orchestrator.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 4f74d5a..20d9d20 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -82,9 +82,9 @@ public Command shootOncePID(double setpointRPM) { } public Command shootCyclePID(double setpointRPM) { - return Commands.parallel( - shooter.runFF(), - shooter.toSetpoint(setpointRPM), + return Commands.sequence( + Commands.sequence(shooter.toSetpoint((setpointRPM)), + shooter.runFF()), Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b6eafc8..99193db 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM + driverController.x().whileTrue(orchestrator.shootCyclePID((2000))).onFalse(shooter.stop()); // TODO: change the RPM driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); driverController.b().onTrue(shooter.runFF()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 3f72e31..664b6f9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -83,7 +83,7 @@ public void setVelocity(double setpointRPM) { @Override public void goToSetpoint(){ - controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); + //controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } public void stop() { From 9b93ec3ab5c3c0e1ae38c89b3cd1e0dc7370f156 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:46:13 -0500 Subject: [PATCH 30/43] Revert "switch to ff" This reverts commit a61686d7bf8c3a57a28f0baf9861a0fa07d3826a. --- src/main/java/frc/robot/Orchestrator.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 20d9d20..4f74d5a 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -82,9 +82,9 @@ public Command shootOncePID(double setpointRPM) { } public Command shootCyclePID(double setpointRPM) { - return Commands.sequence( - Commands.sequence(shooter.toSetpoint((setpointRPM)), - shooter.runFF()), + return Commands.parallel( + shooter.runFF(), + shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 99193db..b6eafc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID((2000))).onFalse(shooter.stop()); // TODO: change the RPM + driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); driverController.b().onTrue(shooter.runFF()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 664b6f9..3f72e31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -83,7 +83,7 @@ public void setVelocity(double setpointRPM) { @Override public void goToSetpoint(){ - //controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); + controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } public void stop() { From 1688feee61a83d41965cb6744029c69b869f3462 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:46:27 -0500 Subject: [PATCH 31/43] Revert "added Ks characterization plus feedforward that updates with rpm needed." This reverts commit 7cec5a1a9a021dac6ee63a9f91171d8207eca32e. --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b6eafc8..61e9031 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,9 +79,8 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(29.44)).onFalse(shooter.stop()); // TODO: change the RPM - driverController.a().whileTrue(shooter.KsFlywheelCharacterization()).onFalse(shooter.stop()); - driverController.b().onTrue(shooter.runFF()); + driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM + driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 72e5d83..092a2e7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,7 +9,7 @@ public class ShooterConstants { public static final int SHOOTER_MOTOR_ID = 8; - public static final double RPM_TO_RADS = (2*Math.PI)/60; + public static final double ENCODER_VELOCITY_CONVERSION = 1; public static final double SPINUP_SECONDS = 0.6; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 3f72e31..2ad3428 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -45,7 +45,7 @@ public ShooterIOSparkMax() { EncoderConfig enc = new EncoderConfig(); - enc.velocityConversionFactor(RPM_TO_RADS); + enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); config.apply(enc); config.apply(loopConfig); From d6f7b1a802cce539802b07e98ce5b37c8357a6d9 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:46:51 -0500 Subject: [PATCH 32/43] Revert "added Ks characterization plus feedforward that updates with rpm needed." This reverts commit d2abf762a2d65ecdefdd7943f8c3532a022327d8. --- src/main/java/frc/robot/Orchestrator.java | 1 - src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 4 +++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 4f74d5a..3b5dfc6 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -83,7 +83,6 @@ public Command shootOncePID(double setpointRPM) { public Command shootCyclePID(double setpointRPM) { return Commands.parallel( - shooter.runFF(), shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61e9031..07f37b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,7 +80,7 @@ private void configureBindings() { //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); + driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 2ad3428..37fbe9d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -30,6 +30,7 @@ public class ShooterIOSparkMax implements ShooterIO { public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); + feedforward = new SimpleMotorFeedforward(0,0.39,0.52); controller = motor.getClosedLoopController(); var config = new SparkMaxConfig(); config.inverted(false) @@ -82,7 +83,8 @@ public void setVelocity(double setpointRPM) { } @Override - public void goToSetpoint(){ + public void goToSetpoint() { + motor.setVoltage(feedforward.calculateWithVelocities(this.encoder.getVelocity(), this.setpointRPM)); controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } From 2bca5d62960da37bb20a370b98707d86ea701ac1 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Mon, 3 Nov 2025 19:47:44 -0500 Subject: [PATCH 33/43] Reapply "added Ks characterization plus feedforward that updates with rpm needed." This reverts commit 06cf9b61281535febdf693ae035cc2d33b8bfd23. --- src/main/java/frc/robot/Orchestrator.java | 1 + src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 4 +--- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 3b5dfc6..4f74d5a 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -83,6 +83,7 @@ public Command shootOncePID(double setpointRPM) { public Command shootCyclePID(double setpointRPM) { return Commands.parallel( + shooter.runFF(), shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07f37b0..61e9031 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -80,7 +80,7 @@ private void configureBindings() { //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - + driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 37fbe9d..2ad3428 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -30,7 +30,6 @@ public class ShooterIOSparkMax implements ShooterIO { public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); - feedforward = new SimpleMotorFeedforward(0,0.39,0.52); controller = motor.getClosedLoopController(); var config = new SparkMaxConfig(); config.inverted(false) @@ -83,8 +82,7 @@ public void setVelocity(double setpointRPM) { } @Override - public void goToSetpoint() { - motor.setVoltage(feedforward.calculateWithVelocities(this.encoder.getVelocity(), this.setpointRPM)); + public void goToSetpoint(){ controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); } From a0b9e9f3696ab5a24ac42c037ab0830e802608ce Mon Sep 17 00:00:00 2001 From: Niti Date: Wed, 5 Nov 2025 20:29:42 -0500 Subject: [PATCH 34/43] externalized pid values --- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 4 ++++ .../java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 092a2e7..37af17c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -14,4 +14,8 @@ public class ShooterConstants { public static final double SPINUP_SECONDS = 0.6; public static final double TOLERANCE = 0.1; // TODO: change tolerance + + public static final double PID_P = 0.000009; + public static final double PID_I = 0.0000003; + public static final double PID_D = 0.0001; } \ 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 2ad3428..3343544 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -23,7 +23,7 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkClosedLoopController controller; - private double setpointRPM; + private double setpointRPM = 3000; private SimpleMotorFeedforward feedforward; @@ -41,7 +41,7 @@ public ShooterIOSparkMax() { loopConfig .feedbackSensor(kPrimaryEncoder) .positionWrappingEnabled(false) - .pid(0.000009,0.0000003,0.0001);// TODO: tune PIDF values + .pid(PID_P, PID_I, PID_D);// TODO: tune PID values EncoderConfig enc = new EncoderConfig(); From 58a9fd0117cb251f4c9747488e051563a05f0843 Mon Sep 17 00:00:00 2001 From: Niti Date: Wed, 5 Nov 2025 20:30:46 -0500 Subject: [PATCH 35/43] back to pid --- src/main/java/frc/robot/Orchestrator.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 4f74d5a..dabf726 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -77,18 +77,15 @@ public Command shootOncePID(double setpointRPM) { Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), - Commands.runOnce(() -> Logger.recordOutput("Shooter/VelocityBeforeShoot", shooter.getVelocity())), indexer.indexIntoShooter())); } public Command shootCyclePID(double setpointRPM) { return Commands.parallel( - shooter.runFF(), shooter.toSetpoint(setpointRPM), Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), - Commands.runOnce(() -> Logger.recordOutput("Shooter/VelocityBeforeShoot", shooter.getVelocity())), indexer.indexIntoShooter()) .repeatedly()); } From 70815776654d5ade3c5c02e6bb6cc0026e2bdbe1 Mon Sep 17 00:00:00 2001 From: Niti Date: Wed, 12 Nov 2025 20:52:24 -0500 Subject: [PATCH 36/43] changed setpoint rpm --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61e9031..649dd6a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,8 +79,8 @@ private void configureBindings() { .onFalse(orchestrator.stopAll()); //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); - driverController.x().whileTrue(orchestrator.shootCyclePID(4500)).onFalse(shooter.stop()); // TODO: change the RPM - driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); + driverController.x().whileTrue(orchestrator.shootCyclePID(2000)).onFalse(shooter.stop()); // TODO: change the RPM +// driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); From 8fd36797d83532b2d06f22767c5fb1d1f9c96dfe Mon Sep 17 00:00:00 2001 From: Niti Date: Wed, 12 Nov 2025 20:52:58 -0500 Subject: [PATCH 37/43] removed duplicate call of goToSetpoint() --- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0e1f046..179517d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -21,7 +21,7 @@ public Shooter(ShooterIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); - io.goToSetpoint(); +// io.goToSetpoint(); } public Command stop() { @@ -48,6 +48,7 @@ public Command toSetpoint(double setpointRPM) { return Commands.run( () -> { io.setVelocity(setpointRPM); + io.goToSetpoint(); }, this ); From 60ab3099cf7f924f68d90964fd09485d40440852 Mon Sep 17 00:00:00 2001 From: Niti Date: Wed, 12 Nov 2025 20:53:38 -0500 Subject: [PATCH 38/43] tuned pid values + started chnage to tunableNumbers --- .../java/frc/lib/utils/TunableNumber.java | 116 ++++++++++++++++++ .../subsystems/shooter/ShooterConstants.java | 9 +- .../subsystems/shooter/ShooterIOSparkMax.java | 2 +- 3 files changed, 123 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/lib/utils/TunableNumber.java diff --git a/src/main/java/frc/lib/utils/TunableNumber.java b/src/main/java/frc/lib/utils/TunableNumber.java new file mode 100644 index 0000000..4788bae --- /dev/null +++ b/src/main/java/frc/lib/utils/TunableNumber.java @@ -0,0 +1,116 @@ + +package frc.lib.utils; + +import frc.robot.Constants; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. + */ +public class TunableNumber implements DoubleSupplier { + private static final String tableKey = "/Tuning"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public TunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public TunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (false) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return false ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged(int id, Consumer action, TunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(TunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, TunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 37af17c..41b8b9a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.MathShared; import edu.wpi.first.math.MathUtil; +import frc.lib.utils.TunableNumber; public class ShooterConstants { @@ -15,7 +16,9 @@ public class ShooterConstants { public static final double TOLERANCE = 0.1; // TODO: change tolerance - public static final double PID_P = 0.000009; - public static final double PID_I = 0.0000003; - public static final double PID_D = 0.0001; + + public static final TunableNumber PID_P = new TunableNumber("Shooter/P", 0.00005); //0.0000009 + public static final TunableNumber PID_I = new TunableNumber("Shooter/I",0.0000001); //0.0000003 + public static final TunableNumber PID_D = new TunableNumber("Shooter/D", 0.0000001); // 0.0001 + } \ 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 3343544..426c81d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -41,7 +41,7 @@ public ShooterIOSparkMax() { loopConfig .feedbackSensor(kPrimaryEncoder) .positionWrappingEnabled(false) - .pid(PID_P, PID_I, PID_D);// TODO: tune PID values + .pid(PID_P.get(), PID_I.get(), PID_D.get());// TODO: tune PID values EncoderConfig enc = new EncoderConfig(); From b428743f60a3847656fe3e192a2d23bf4cd1770c Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Fri, 14 Nov 2025 18:18:24 -0500 Subject: [PATCH 39/43] Fixed usage of TunableNumber so it can actually be tuned now through AdvantageScope. --- src/main/java/frc/lib/utils/TunableNumber.java | 6 ++++-- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/subsystems/shooter/Shooter.java | 3 +-- .../frc/robot/subsystems/shooter/ShooterIOSparkMax.java | 3 --- 4 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/lib/utils/TunableNumber.java b/src/main/java/frc/lib/utils/TunableNumber.java index 4788bae..66ef59b 100644 --- a/src/main/java/frc/lib/utils/TunableNumber.java +++ b/src/main/java/frc/lib/utils/TunableNumber.java @@ -9,6 +9,8 @@ import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; +import static frc.robot.Constants.tuningMode; + /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or * value not in dashboard. @@ -51,7 +53,7 @@ public void initDefault(double defaultValue) { if (!hasDefault) { hasDefault = true; this.defaultValue = defaultValue; - if (false) { + if (tuningMode) { dashboardNumber = new LoggedNetworkNumber(key, defaultValue); } } @@ -66,7 +68,7 @@ public double get() { if (!hasDefault) { return 0.0; } else { - return false ? dashboardNumber.get() : defaultValue; + return tuningMode ? dashboardNumber.get() : defaultValue; } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fc763af..ed4ce79 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,4 +13,5 @@ * constants are needed, to reduce verbosity. */ public final class Constants { + static public final boolean tuningMode = true; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 179517d..0e1f046 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -21,7 +21,7 @@ public Shooter(ShooterIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Shooter", inputs); -// io.goToSetpoint(); + io.goToSetpoint(); } public Command stop() { @@ -48,7 +48,6 @@ public Command toSetpoint(double setpointRPM) { return Commands.run( () -> { io.setVelocity(setpointRPM); - io.goToSetpoint(); }, this ); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index 426c81d..685ad7d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -24,9 +24,6 @@ public class ShooterIOSparkMax implements ShooterIO { private final SparkClosedLoopController controller; private double setpointRPM = 3000; - private SimpleMotorFeedforward feedforward; - - public ShooterIOSparkMax() { motor = new SparkMax(SHOOTER_MOTOR_ID, MotorType.kBrushless); From 2ed9385ede7c89a7c181bfa348b0a97cde518fe7 Mon Sep 17 00:00:00 2001 From: Niti Date: Mon, 17 Nov 2025 20:24:57 -0500 Subject: [PATCH 40/43] tuned tolerance --- .../frc/robot/subsystems/shooter/ShooterConstants.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 41b8b9a..9bd01ae 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -14,11 +14,14 @@ public class ShooterConstants { public static final double SPINUP_SECONDS = 0.6; - public static final double TOLERANCE = 0.1; // TODO: change tolerance + public static final double TOLERANCE = 3; // TODO: change tolerance public static final TunableNumber PID_P = new TunableNumber("Shooter/P", 0.00005); //0.0000009 public static final TunableNumber PID_I = new TunableNumber("Shooter/I",0.0000001); //0.0000003 public static final TunableNumber PID_D = new TunableNumber("Shooter/D", 0.0000001); // 0.0001 -} \ No newline at end of file +} +// 0.00005 +//0.0000001 +//0.0000001 \ No newline at end of file From 20f53d9c5b3dd10d06f6c92361812264ab9c4658 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Tue, 18 Nov 2025 16:11:51 -0500 Subject: [PATCH 41/43] Added a goto distance based setpoint method, which calculates the necessary speed from distance, and then goes to that speed. --- src/main/java/frc/robot/Orchestrator.java | 19 ++++-- src/main/java/frc/robot/RobotContainer.java | 15 ++--- .../frc/robot/subsystems/shooter/Shooter.java | 67 +++++++++---------- .../robot/subsystems/shooter/ShooterIO.java | 4 +- .../subsystems/shooter/ShooterIOSparkMax.java | 6 +- 5 files changed, 60 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index dabf726..5a97251 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -1,7 +1,7 @@ package frc.robot; -import java.util.function.BooleanSupplier; - +import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.indexer.Indexer; @@ -10,13 +10,15 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import java.util.function.BooleanSupplier; + public class Orchestrator { private final Indexer indexer; private final Shooter shooter; @AutoLogOutput private double Distance = 300.0; - @AutoLogOutput private double shootingPower = 0.0; + @AutoLogOutput private final double shootingPower = 0.0; public Orchestrator(Indexer indexer, Shooter shooter) { this.indexer = indexer; @@ -90,6 +92,15 @@ public Command shootCyclePID(double setpointRPM) { .repeatedly()); } + public Command shootDistance(Measure distance) { + return Commands.parallel( + shooter.toSetpoint(distance), + Commands.sequence( + intakeIfNeeded(), + Commands.waitUntil(shooter::atSetpoint), + indexer.indexIntoShooter()).repeatedly()); + } + public Command reverseAll() { return Commands.parallel(shooter.reverse(), indexer.reverse()); @@ -98,4 +109,4 @@ public Command reverseAll() { public Command stopAll() { return Commands.parallel(shooter.stop(), indexer.stop()); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 649dd6a..780d086 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,20 +4,17 @@ package frc.robot; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.drive.*; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveIOSparkMax; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.IndexerIOSparkMax; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIOSparkMax; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIOPhotonVision; /** @@ -80,7 +77,7 @@ private void configureBindings() { //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop()); driverController.x().whileTrue(orchestrator.shootCyclePID(2000)).onFalse(shooter.stop()); // TODO: change the RPM -// driverController.a().whileTrue(shooter.KsFlywheelCharacterization()); + driverController.a().whileTrue(orchestrator.shootDistance(Units.Inches.of(168.5))).onFalse(shooter.stop()); // TODO: Test other distances driverController.leftTrigger().onTrue(indexer.indexUntilSwitch()); driverController.y().onTrue(indexer.indexIntoShooter()); @@ -105,4 +102,4 @@ public Command getAutonomousCommand() { // An example command will be run in autonomous return Commands.none(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0e1f046..e1dd42f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,15 +1,17 @@ package frc.robot.subsystems.shooter; +import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import frc.robot.subsystems.shooter.ShooterConstants; + +import java.util.function.DoubleSupplier; public class Shooter extends SubsystemBase { - private ShooterIO io; + private final ShooterIO io; private ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); public Shooter(ShooterIO io) { @@ -25,16 +27,15 @@ public void periodic() { } public Command stop() { - return Commands.runOnce(() ->{io.stop();}, this); + return Commands.runOnce(() -> { + io.stop(); + }, this); } public Command reverse() { - return Commands.startEnd( - () -> io.setPercent(ShooterConstants.REVERSE_SHOOTER_PERCENT), - () -> io.setPercent(0), - this - ); -} + return Commands.startEnd(() -> io.setPercent(ShooterConstants.REVERSE_SHOOTER_PERCENT), + () -> io.setPercent(0), this); + } public boolean atSetpoint() { return inputs.atSetpoint; @@ -45,36 +46,32 @@ public double getVelocity() { } public Command toSetpoint(double setpointRPM) { - return Commands.run( - () -> { - io.setVelocity(setpointRPM); - }, - this - ); + return Commands.run(() -> { + io.setVelocity(setpointRPM); + }, this); + } + + public Command toSetpoint(Measure distance) { + return Commands.run(() -> io.setDistance(distance.in(Units.Inches))); } public Command toSetpoint(DoubleSupplier setpointRPM) { - return Commands.run( - () -> { - io.setVelocity(setpointRPM.getAsDouble()); - }, - this); + return Commands.run(() -> { + io.setVelocity(setpointRPM.getAsDouble()); + }, this); } public Command fullSpeed() { - return Commands.run( - () -> { - io.setPercent(1); - }, - this - ); + return Commands.run(() -> { + io.setPercent(1); + }, 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); - } // -} +} \ 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 6f84dc5..9703e66 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -30,7 +30,9 @@ default void setVoltage(double voltage) {} default void setVelocity(double RPM) {} - default void goToSetpoint() {} + default void setDistance(double distance) {} + + default void goToSetpoint() {} default void stop() {} } \ 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 685ad7d..4f5b2cb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -14,7 +14,6 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; public class ShooterIOSparkMax implements ShooterIO { @@ -48,7 +47,6 @@ public ShooterIOSparkMax() { motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); encoder = motor.getEncoder(); - } @Override @@ -78,6 +76,10 @@ public void setVelocity(double setpointRPM) { this.setpointRPM = setpointRPM; } + @Override + public void setDistance(double distance) {this.setpointRPM = 113.29128 * Math.pow(distance, + 0.639824);} + @Override public void goToSetpoint(){ controller.setReference(this.setpointRPM, SparkBase.ControlType.kVelocity); From d26090a21bee381ae5e96482c2cd8cb661d724e2 Mon Sep 17 00:00:00 2001 From: EJain-Dev Date: Wed, 19 Nov 2025 20:30:44 -0500 Subject: [PATCH 42/43] Added vision to get distance instead of manually putting it in. --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/Orchestrator.java | 8 ++++++-- src/main/java/frc/robot/RobotContainer.java | 7 ++++++- .../frc/robot/subsystems/vision/VisionIO.java | 17 ++++++++--------- .../subsystems/vision/VisionIOPhotonVision.java | 17 +++++------------ 5 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed4ce79..b8d371e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,4 +14,5 @@ */ public final class Constants { static public final boolean tuningMode = true; -} + static public final String camName = "somename"; // TODO: Change +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 5a97251..ebfe2dd 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -2,11 +2,13 @@ import edu.wpi.first.units.DistanceUnit; import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; +import frc.robot.subsystems.vision.Vision; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -19,10 +21,12 @@ public class Orchestrator { @AutoLogOutput private double Distance = 300.0; @AutoLogOutput private final double shootingPower = 0.0; + private final Vision vision; - public Orchestrator(Indexer indexer, Shooter shooter) { + public Orchestrator(Indexer indexer, Shooter shooter, Vision vision) { this.indexer = indexer; this.shooter = shooter; + this.vision = vision; } public Command spinUpDistance(Double distance) { Distance = distance; @@ -94,7 +98,7 @@ public Command shootCyclePID(double setpointRPM) { public Command shootDistance(Measure distance) { return Commands.parallel( - shooter.toSetpoint(distance), + shooter.toSetpoint(Units.Meters.of(vision.distanceFromTarget())), Commands.sequence( intakeIfNeeded(), Commands.waitUntil(shooter::atSetpoint), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 780d086..ac1d9b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,6 +15,10 @@ import frc.robot.subsystems.indexer.IndexerIOSparkMax; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIOSparkMax; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIOPhotonVision; + +import static frc.robot.Constants.camName; /** @@ -29,7 +33,8 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final Indexer indexer = new Indexer(new IndexerIOSparkMax()); private final Shooter shooter = new Shooter(new ShooterIOSparkMax()); - private final Orchestrator orchestrator = new Orchestrator(indexer, shooter); + private final Vision vision = new Vision(new VisionIOPhotonVision(camName)); + private final Orchestrator orchestrator = new Orchestrator(indexer, shooter, vision); private final CommandXboxController driverController = new CommandXboxController(0); private boolean fastMode = false; private final Trigger fastModeTrigger = new Trigger(() -> fastMode); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index ed64e78..0e4b591 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -1,16 +1,15 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Translation2d; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { @AutoLog - public static class VisionIOInputs { + class VisionIOInputs { public boolean connected = false; public PoseObservation latestTargetObservation = - new PoseObservation(0, new Translation3d(), 0); + new PoseObservation(0, new Translation2d(), 0); public int lastestTagID = 0 ; public double distanceFromTarget = 0; public double yaw = 0; @@ -18,18 +17,18 @@ public static class VisionIOInputs { /** Represents the angle to a simple target, not used for pose estimation. */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + record TargetObservation(Rotation2d tx, Rotation2d ty) {} /** Represents a robot pose sample used for pose estimation. */ - public static record PoseObservation ( + record PoseObservation ( int tagID, - Translation3d translation3d, + Translation2d translation2d, double yaw ){} - public static enum PoseObservationType { + enum PoseObservationType { PHOTONVISION } default void updateInputs(VisionIOInputs inputs) {} -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index ea62a36..e03bcb6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -1,11 +1,6 @@ package frc.robot.subsystems.vision; -import edu.wpi.first.math.geometry.*; - -import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; -import java.util.Set; +import edu.wpi.first.math.geometry.Translation2d; import org.photonvision.PhotonCamera; public class VisionIOPhotonVision implements VisionIO{ protected final PhotonCamera camera; @@ -20,16 +15,14 @@ public void updateInputs (VisionIOInputs inputs) { inputs.connected = camera.isConnected(); var result = camera.getLatestResult(); if (result.hasTargets()) { - inputs.latestTargetObservation = new PoseObservation(result.getBestTarget().fiducialId,result.getBestTarget().bestCameraToTarget.getTranslation(), result.getBestTarget().getYaw()); + inputs.latestTargetObservation = + new PoseObservation(result.getBestTarget().fiducialId,result.getBestTarget().bestCameraToTarget.getTranslation().toTranslation2d(), result.getBestTarget().getYaw()); }else{ - inputs.latestTargetObservation = new PoseObservation(0,new Translation3d(),0); + inputs.latestTargetObservation = new PoseObservation(0, new Translation2d(), 0); } inputs.lastestTagID = inputs.latestTargetObservation.tagID(); - inputs.distanceFromTarget = inputs.latestTargetObservation.translation3d().getNorm(); + inputs.distanceFromTarget = inputs.latestTargetObservation.translation2d().getNorm(); inputs.yaw = inputs.latestTargetObservation.yaw(); - - - } } \ No newline at end of file From ad22f2e90810e7f7b67b074408399648d6e91132 Mon Sep 17 00:00:00 2001 From: Niti Date: Mon, 1 Dec 2025 21:25:02 -0500 Subject: [PATCH 43/43] corrected camera name --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b8d371e..c37107b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,5 +14,5 @@ */ public final class Constants { static public final boolean tuningMode = true; - static public final String camName = "somename"; // TODO: Change + static public final String camName = "Arucam OV9281(Back)"; } \ No newline at end of file