From 0599bca05c7fe66431724ecf777b542e49972d1a Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Mon, 26 Feb 2024 21:25:50 -0500 Subject: [PATCH 01/16] [#332] added a temp path for testing named commands and trying to get cand shots to work --- .../pathplanner/autos/test hee hee.auto | 19 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/command/shoot/PointBlankShot.java | 3 ++- 3 files changed, 22 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/test hee hee.auto diff --git a/src/main/deploy/pathplanner/autos/test hee hee.auto b/src/main/deploy/pathplanner/autos/test hee hee.auto new file mode 100644 index 00000000..3a8351e9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/test hee hee.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Cand-Sub" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ 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 3580d71c..20400388 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -128,7 +128,7 @@ protected void initializeNamedCommands() { leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5)); NamedCommands.registerCommand("Cand-Sub", - new PointBlankShot(flywheel, pivot, indexer, DriverStation.isAutonomousEnabled()) + new PointBlankShot(flywheel, pivot, indexer, true) .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5))); // NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); // NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); diff --git a/src/main/java/frc/robot/command/shoot/PointBlankShot.java b/src/main/java/frc/robot/command/shoot/PointBlankShot.java index fbbb0c23..981a9245 100644 --- a/src/main/java/frc/robot/command/shoot/PointBlankShot.java +++ b/src/main/java/frc/robot/command/shoot/PointBlankShot.java @@ -14,6 +14,7 @@ public class PointBlankShot extends Command { private final Pivot pivot; private final Indexer indexer; private boolean isAutonomous; + private boolean firstRun; private boolean shot = false; private double shotTime = 0; @@ -30,7 +31,7 @@ public PointBlankShot(Flywheel flywheel, Pivot pivot, Indexer indexer, boolean i this.indexer = indexer; this.isAutonomous = isAutonomous; - addRequirements(pivot, flywheel); + addRequirements(pivot, flywheel, indexer); } @Override From 44bf0e8f20e87e49d50c3eb8caf8eddeb0e50f91 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Tue, 27 Feb 2024 21:20:24 -0500 Subject: [PATCH 02/16] [#332] more debugging --- src/main/java/frc/robot/RobotContainer.java | 4 ++- .../java/frc/robot/command/shoot/AmpShot.java | 28 ++++++++++++------- .../frc/robot/command/shoot/PodiumShot.java | 2 +- .../robot/command/shoot/PointBlankShot.java | 11 ++++++-- .../command/tests/IndexerSystemTest.java | 1 + 5 files changed, 32 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 20400388..f3b3a4d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -205,7 +205,7 @@ protected void configureButtonBindings() { .whileTrue(new SmartCollect(() -> 0.50, () -> 0.60, collector, indexer, pivot)); // TODO: find correct button/trigger // cand shots for the robot - new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)); + new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot, indexer, false)); new Trigger(coPilot::getXButton) .whileTrue(new PointBlankShot(flywheel, pivot, indexer, false)); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot)); @@ -242,6 +242,8 @@ protected void configureButtonBindings() { // new Trigger(() -> LightningShuffleboard.getBool("Swerve", "Swap", false)) // .onTrue(new InstantCommand(() -> drivetrain.swap(driver, coPilot))) // .onFalse(new InstantCommand(() -> drivetrain.swap(driver, coPilot))); + + } diff --git a/src/main/java/frc/robot/command/shoot/AmpShot.java b/src/main/java/frc/robot/command/shoot/AmpShot.java index 48177e6a..ca48ae58 100644 --- a/src/main/java/frc/robot/command/shoot/AmpShot.java +++ b/src/main/java/frc/robot/command/shoot/AmpShot.java @@ -3,7 +3,9 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; +import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; +import frc.robot.command.Index; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Pivot; @@ -14,6 +16,7 @@ public class AmpShot extends Command { private Flywheel flywheel; private Indexer indexer; private boolean isAutonomous; + private boolean goShoot = false; private boolean shot = false; private double shotTime = 0; @@ -21,14 +24,14 @@ public class AmpShot extends Command { * Creates a new AmpShot * @param pivot subsystem * @param flywheel subsystem - * indexer subsystem - * isAutonomous boolean if robot is in autonomous + * @param indexer subsystem + * @param isAutonomous boolean if robot is in autonomous */ - public AmpShot(Flywheel flywheel, Pivot pivot) {// , Indexer indexer, boolean isAutonomous) { + public AmpShot(Flywheel flywheel, Pivot pivot, Indexer indexer, boolean isAutonomous) {// , Indexer indexer, boolean isAutonomous) { this.flywheel = flywheel; this.pivot = pivot; - // this.indexer = indexer; - // this.isAutonomous = isAutonomous; + this.indexer = indexer; + this.isAutonomous = isAutonomous; addRequirements(flywheel, pivot); } @@ -43,11 +46,16 @@ public void initialize() { @Override public void execute() { // Checks if autonomous and if the pivot and flywheel are on target then shoots - // if (isAutonomous && pivot.onTarget() && flywheel.allMotorsOnTarget()) { - // shot = true; - // shotTime = Timer.getFPGATimestamp(); - // indexer.indexUp(); - // } + if(isAutonomous && pivot.onTarget() && flywheel.allMotorsOnTarget()) { + goShoot = true; + } + + if(goShoot){ + shot = true; + shotTime = Timer.getFPGATimestamp(); + indexer.indexUp(); + // alongWith(new Index(indiexer, () -> IndexerConstants.INDEXER_DEFAULT_POWER)); + } pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); diff --git a/src/main/java/frc/robot/command/shoot/PodiumShot.java b/src/main/java/frc/robot/command/shoot/PodiumShot.java index a1276599..cdd2336d 100644 --- a/src/main/java/frc/robot/command/shoot/PodiumShot.java +++ b/src/main/java/frc/robot/command/shoot/PodiumShot.java @@ -20,7 +20,7 @@ public PodiumShot(Flywheel flywheel, Pivot pivot) { this.flywheel = flywheel; this.pivot = pivot; - addRequirements(flywheel); + addRequirements(pivot, flywheel); } @Override diff --git a/src/main/java/frc/robot/command/shoot/PointBlankShot.java b/src/main/java/frc/robot/command/shoot/PointBlankShot.java index 981a9245..ee60c8b2 100644 --- a/src/main/java/frc/robot/command/shoot/PointBlankShot.java +++ b/src/main/java/frc/robot/command/shoot/PointBlankShot.java @@ -2,11 +2,14 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Constants.CandConstants; +import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Pivot; +import frc.robot.command.Index; import frc.thunder.shuffleboard.LightningShuffleboard; public class PointBlankShot extends Command { @@ -14,7 +17,7 @@ public class PointBlankShot extends Command { private final Pivot pivot; private final Indexer indexer; private boolean isAutonomous; - private boolean firstRun; + private boolean goShoot = false; private boolean shot = false; private double shotTime = 0; @@ -44,9 +47,13 @@ public void initialize() { public void execute() { // Checks if autonomous and if the pivot and flywheel are on target then shoots if(isAutonomous && pivot.onTarget() && flywheel.allMotorsOnTarget()) { + goShoot = true; + } + + if(goShoot){ shot = true; shotTime = Timer.getFPGATimestamp(); - indexer.indexUp(); + indexer.setPower(0.6); } flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); diff --git a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java index 32aa1dec..69cf390e 100644 --- a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java +++ b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java @@ -4,6 +4,7 @@ package frc.robot.command.tests; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.command.tests.testcmds.IndexerTest; From bde0ad5855fa92a2d288717d9426280868722463 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 1 Mar 2024 19:00:40 -0500 Subject: [PATCH 03/16] [#332] fixed indexer be like --- src/main/java/frc/robot/command/Collect.java | 4 ++-- src/main/java/frc/robot/command/shoot/AmpShot.java | 1 - .../java/frc/robot/command/shoot/PointBlankShot.java | 9 ++++++--- src/main/java/frc/robot/subsystems/Indexer.java | 4 ++++ 4 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/command/Collect.java b/src/main/java/frc/robot/command/Collect.java index 503c07b5..bae3fcf6 100644 --- a/src/main/java/frc/robot/command/Collect.java +++ b/src/main/java/frc/robot/command/Collect.java @@ -25,7 +25,7 @@ public Collect(DoubleSupplier powerSupplier, Collector collector, Indexer indexe this.indexer = indexer; this.powerSupplier = powerSupplier; - addRequirements(collector); + addRequirements(collector, indexer); } @Override @@ -40,7 +40,7 @@ public void execute() { if (powerSupplier.getAsDouble() > 0d) { indexer.setPower(1d); } else { - indexer.stop(); + indexer.setPower(.7); } } diff --git a/src/main/java/frc/robot/command/shoot/AmpShot.java b/src/main/java/frc/robot/command/shoot/AmpShot.java index ca48ae58..8c1ed407 100644 --- a/src/main/java/frc/robot/command/shoot/AmpShot.java +++ b/src/main/java/frc/robot/command/shoot/AmpShot.java @@ -54,7 +54,6 @@ public void execute() { shot = true; shotTime = Timer.getFPGATimestamp(); indexer.indexUp(); - // alongWith(new Index(indiexer, () -> IndexerConstants.INDEXER_DEFAULT_POWER)); } pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); diff --git a/src/main/java/frc/robot/command/shoot/PointBlankShot.java b/src/main/java/frc/robot/command/shoot/PointBlankShot.java index ee60c8b2..b20b3916 100644 --- a/src/main/java/frc/robot/command/shoot/PointBlankShot.java +++ b/src/main/java/frc/robot/command/shoot/PointBlankShot.java @@ -1,5 +1,6 @@ package frc.robot.command.shoot; +import org.opencv.core.Point; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -16,6 +17,7 @@ public class PointBlankShot extends Command { private final Flywheel flywheel; private final Pivot pivot; private final Indexer indexer; + private Index indexCommand; private boolean isAutonomous; private boolean goShoot = false; private boolean shot = false; @@ -32,6 +34,7 @@ public PointBlankShot(Flywheel flywheel, Pivot pivot, Indexer indexer, boolean i this.flywheel = flywheel; this.pivot = pivot; this.indexer = indexer; + this.indexCommand = new Index(indexer, () -> 0.6d); this.isAutonomous = isAutonomous; addRequirements(pivot, flywheel, indexer); @@ -49,11 +52,11 @@ public void execute() { if(isAutonomous && pivot.onTarget() && flywheel.allMotorsOnTarget()) { goShoot = true; } - + if(goShoot){ shot = true; shotTime = Timer.getFPGATimestamp(); - indexer.setPower(0.6); + indexer.indexUp(); } flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); @@ -64,7 +67,7 @@ public void execute() { public void end(boolean interrupted) { flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); - indexer.stop(); + indexCommand.cancel(); } @Override diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 464efeb5..bd54821a 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -23,6 +23,8 @@ public class Indexer extends SubsystemBase { private double timeLastTriggered = 0d; + private double targetPower = 0; // danny was here + private PieceState currentState = PieceState.NONE; public Indexer(Collector collector) { @@ -38,6 +40,7 @@ public Indexer(Collector collector) { private void initLogging() { LightningShuffleboard.setDoubleSupplier("Indexer", "Indexer Power", () -> indexerMotor.get()); + LightningShuffleboard.setDoubleSupplier("Indexer", "Indexer Target Power", () -> targetPower); LightningShuffleboard.setBoolSupplier("Indexer", "Entry Beam Break", () -> getEntryBeamBreakState()); LightningShuffleboard.setBoolSupplier("Indexer", "Exit Beam Break", () -> getExitBeamBreakState()); @@ -69,6 +72,7 @@ public void setPieceState(PieceState state) { * @param power */ public void setPower(double power) { + targetPower = power; indexerMotor.set(power); } From f0c9bd9698bb3c76a27707ded97077870475c92e Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 1 Mar 2024 19:03:10 -0500 Subject: [PATCH 04/16] [#332] bug fix :) --- src/main/java/frc/robot/command/shoot/PointBlankShot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/command/shoot/PointBlankShot.java b/src/main/java/frc/robot/command/shoot/PointBlankShot.java index f08c2e08..0b82dbe6 100644 --- a/src/main/java/frc/robot/command/shoot/PointBlankShot.java +++ b/src/main/java/frc/robot/command/shoot/PointBlankShot.java @@ -20,7 +20,7 @@ public PointBlankShot(Flywheel flywheel, Pivot pivot) { this.flywheel = flywheel; this.pivot = pivot; - addRequirements(pivot, flywheel, indexer); + addRequirements(pivot, flywheel); } @Override From 524a027cda7936747aaf27c38fc9c4867bf5fdc2 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 1 Mar 2024 19:21:03 -0500 Subject: [PATCH 05/16] [#332] formatting changes --- .../command/shoot/AutonCand/AmpShotAuton.java | 17 +++++++++------ .../robot/command/shoot/AutonCand/CandC1.java | 16 ++++++++++---- .../robot/command/shoot/AutonCand/CandC2.java | 19 +++++++++++++---- .../robot/command/shoot/AutonCand/CandC3.java | 19 +++++++++++++---- .../command/shoot/AutonCand/CandLine.java | 21 ++++++++++++++----- .../shoot/AutonCand/PointBlankShotAuton.java | 17 ++++++++++----- 6 files changed, 81 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java index fc93c19e..d777e397 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java @@ -13,8 +13,11 @@ public class AmpShotAuton extends Command { private Pivot pivot; private Flywheel flywheel; private Indexer indexer; + private boolean shot = false; private double shotTime = 0; + + private boolean startIndexing = false; /** * Creates a new AmpShot @@ -33,29 +36,31 @@ public AmpShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; - flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); - flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); } @Override public void execute() { - // Checks if autonomous and if the pivot and flywheel are on target then shoots + // Checks if the pivot and flywheel are on target then shoots if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + startIndexing = true; + } + + if(startIndexing) { shot = true; shotTime = Timer.getFPGATimestamp(); indexer.indexUp(); } pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); - flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); - flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); + flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); indexer.stop(); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java index 997c5603..0defb38a 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java @@ -18,6 +18,8 @@ public class CandC1 extends Command { private boolean shot = false; private double shotTime = 0; + private boolean startIndexing = false; + /** * Creates a new CandC1. * @param flywheel subsystem @@ -35,25 +37,31 @@ public CandC1(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void execute() { + // Checks if the pivot and flywheel are on target then shoots if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { - indexer.setPower(IndexerConstants.INDEXER_DEFAULT_POWER); + startIndexing = true; + } + + if(startIndexing) { shot = true; shotTime = Timer.getFPGATimestamp(); + indexer.indexUp(); } - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); indexer.stop(); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java index 5a7e0a22..b52e635c 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java @@ -14,9 +14,12 @@ public class CandC2 extends Command { private final Flywheel flywheel; private final Pivot pivot; private final Indexer indexer; + private boolean shot = false; private double shotTime = 0; + private boolean startIndexing = false; + /** * Creates a new CandC2. * @param flywheel subsystem @@ -34,23 +37,31 @@ public CandC2(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void execute() { - if(pivot.onTarget() && flywheel.allMotorsOnTarget()) { - indexer.setPower(IndexerConstants.INDEXER_DEFAULT_POWER); + // Checks if the pivot and flywheel are on target then shoots + if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + startIndexing = true; + } + + if(startIndexing) { shot = true; shotTime = Timer.getFPGATimestamp(); + indexer.indexUp(); } + + pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); indexer.stop(); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java index 737d23fe..154d9e40 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java @@ -14,9 +14,12 @@ public class CandC3 extends Command { private final Flywheel flywheel; private final Pivot pivot; private final Indexer indexer; + private boolean shot = false; private double shotTime = 0; + private boolean startIndexing = false; + /** * Creates a new CandC3. * @param flywheel subsystem @@ -34,23 +37,31 @@ public CandC3(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void execute() { - if(pivot.onTarget() && flywheel.allMotorsOnTarget()) { - indexer.setPower(IndexerConstants.INDEXER_DEFAULT_POWER); + // Checks if the pivot and flywheel are on target then shoots + if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + startIndexing = true; + } + + if(startIndexing) { shot = true; shotTime = Timer.getFPGATimestamp(); + indexer.indexUp(); } + + pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); indexer.stop(); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java index a3ea8d06..0b5b65bd 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java @@ -14,9 +14,12 @@ public class CandLine extends Command { private final Flywheel flywheel; private final Pivot pivot; private final Indexer indexer; + private boolean shot = false; private double shotTime = 0; + private boolean startIndexing = false; + /** * Creates a new CandLine. * @param flywheel subsystem @@ -28,29 +31,37 @@ public CandLine(Flywheel flywheel, Pivot pivot, Indexer indexer) { this.pivot = pivot; this.indexer = indexer; - addRequirements(pivot, flywheel); + addRequirements(pivot, flywheel, indexer); } @Override public void initialize() { shot = false; - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void execute() { - if(pivot.onTarget() && flywheel.allMotorsOnTarget()) { - indexer.setPower(IndexerConstants.INDEXER_DEFAULT_POWER); + // Checks if the pivot and flywheel are on target then shoots + if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + startIndexing = true; + } + + if(startIndexing) { shot = true; shotTime = Timer.getFPGATimestamp(); + indexer.indexUp(); } + + pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); indexer.stop(); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index 59c385eb..04cfea61 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -12,8 +12,11 @@ public class PointBlankShotAuton extends Command { private final Flywheel flywheel; private final Pivot pivot; private final Indexer indexer; + private boolean shot = false; private double shotTime = 0; + + private boolean startIndexing = false; /** * Creates a new PointBlankShot. @@ -32,27 +35,31 @@ public PointBlankShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; - flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); } @Override public void execute() { - // Checks if autonomous and if the pivot and flywheel are on target then shoots - if(pivot.onTarget() && flywheel.allMotorsOnTarget()) { + // Checks if the pivot and flywheel are on target then shoots + if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + startIndexing = true; + } + + if(startIndexing) { shot = true; shotTime = Timer.getFPGATimestamp(); indexer.indexUp(); } - flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + flywheel.getBias()); + flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); indexer.stop(); } From c5f13d69c305206a57bf31022c114a65da2c0aab Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Fri, 1 Mar 2024 20:32:39 -0500 Subject: [PATCH 06/16] [#332] more formatting --- src/main/java/frc/robot/RobotContainer.java | 8 +++++--- src/main/java/frc/robot/command/Collect.java | 13 ++----------- .../command/shoot/AutonCand/AmpShotAuton.java | 5 +++-- .../frc/robot/command/shoot/AutonCand/CandC1.java | 3 ++- .../frc/robot/command/shoot/AutonCand/CandC2.java | 3 ++- .../frc/robot/command/shoot/AutonCand/CandC3.java | 3 ++- .../robot/command/shoot/AutonCand/CandLine.java | 3 ++- .../shoot/AutonCand/PointBlankShotAuton.java | 6 ++++-- src/main/java/frc/robot/subsystems/Flywheel.java | 14 ++++++++++++++ 9 files changed, 36 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 818ad0c5..72e418d6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -133,7 +133,7 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("Cand-Sub", new PointBlankShotAuton(flywheel, pivot, indexer) - .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5))); + .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(1))); NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); NamedCommands.registerCommand("Cand-C3", new CandC3(flywheel, pivot, indexer)); @@ -203,7 +203,8 @@ protected void configureButtonBindings() { // cand shots for the robot new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)); - new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); + // new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); + new Trigger(coPilot::getXButton).whileTrue(new PointBlankShotAuton(flywheel, pivot, indexer)); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot)); new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(flywheel, pivot)); @@ -253,7 +254,8 @@ protected void configureDefaultCommands() { /* copilot */ collector.setDefaultCommand( - new Collect(() -> MathUtil.applyDeadband((coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), ControllerConstants.DEADBAND), collector, indexer)); + new Collect(() -> MathUtil.applyDeadband((coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), ControllerConstants.DEADBAND), collector)); + // climber.setDefaultCommand(new ManualClimb(() -> coPilot.getLeftY(),() -> // coPilot.getRightY(), climber)); } diff --git a/src/main/java/frc/robot/command/Collect.java b/src/main/java/frc/robot/command/Collect.java index bae3fcf6..8e70762d 100644 --- a/src/main/java/frc/robot/command/Collect.java +++ b/src/main/java/frc/robot/command/Collect.java @@ -4,28 +4,24 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Collector; -import frc.robot.subsystems.Indexer; import frc.thunder.shuffleboard.LightningShuffleboard; public class Collect extends Command { // Declares collector private Collector collector; - private Indexer indexer; private DoubleSupplier powerSupplier; /** * Creates a new Collect. * @param powerSupplier DoubleSupplier for power of motor (-1 to 1) * @param collector subsystem - * @param indexer subsystem */ - public Collect(DoubleSupplier powerSupplier, Collector collector, Indexer indexer) { + public Collect(DoubleSupplier powerSupplier, Collector collector) { this.collector = collector; - this.indexer = indexer; this.powerSupplier = powerSupplier; - addRequirements(collector, indexer); + addRequirements(collector); } @Override @@ -37,11 +33,6 @@ public void initialize() { @Override public void execute() { collector.setPower(powerSupplier.getAsDouble()); - if (powerSupplier.getAsDouble() > 0d) { - indexer.setPower(1d); - } else { - indexer.setPower(.7); - } } @Override diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java index d777e397..a8e8f2a1 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java @@ -16,7 +16,7 @@ public class AmpShotAuton extends Command { private boolean shot = false; private double shotTime = 0; - + private boolean startIndexing = false; /** @@ -43,7 +43,8 @@ public void initialize() { @Override public void execute() { // Checks if the pivot and flywheel are on target then shoots - if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + // also checks whether or not the flywheel's target RPM is greater than 0 + if (pivot.onTarget() && flywheel.allMotorsOnTarget() && (flywheel.getTopMotorRPM() != 0 && flywheel.getBottomMotorRPM() != 0)) { startIndexing = true; } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java index 0defb38a..3cc8e4e1 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java @@ -44,7 +44,8 @@ public void initialize() { @Override public void execute() { // Checks if the pivot and flywheel are on target then shoots - if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + // also checks whether or not the flywheel's target RPM is greater than 0 + if (pivot.onTarget() && flywheel.allMotorsOnTarget() && (flywheel.getTopMotorRPM() != 0 && flywheel.getBottomMotorRPM() != 0)) { startIndexing = true; } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java index b52e635c..2f0e80ea 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java @@ -44,7 +44,8 @@ public void initialize() { @Override public void execute() { // Checks if the pivot and flywheel are on target then shoots - if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + // also checks whether or not the flywheel's target RPM is greater than 0 + if (pivot.onTarget() && flywheel.allMotorsOnTarget() && (flywheel.getTopMotorRPM() != 0 && flywheel.getBottomMotorRPM() != 0)) { startIndexing = true; } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java index 154d9e40..f016886e 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java @@ -44,7 +44,8 @@ public void initialize() { @Override public void execute() { // Checks if the pivot and flywheel are on target then shoots - if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + // also checks whether or not the flywheel's target RPM is greater than 0 + if (pivot.onTarget() && flywheel.allMotorsOnTarget() && (flywheel.getTopMotorRPM() != 0 && flywheel.getBottomMotorRPM() != 0)) { startIndexing = true; } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java index 0b5b65bd..84751abf 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java @@ -44,7 +44,8 @@ public void initialize() { @Override public void execute() { // Checks if the pivot and flywheel are on target then shoots - if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + // also checks whether or not the flywheel's target RPM is greater than 0 + if (pivot.onTarget() && flywheel.allMotorsOnTarget() && (flywheel.getTopMotorRPM() != 0 && flywheel.getBottomMotorRPM() != 0)) { startIndexing = true; } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index 04cfea61..c54fc349 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -15,7 +15,7 @@ public class PointBlankShotAuton extends Command { private boolean shot = false; private double shotTime = 0; - + private boolean startIndexing = false; /** @@ -35,6 +35,7 @@ public PointBlankShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; + startIndexing = false; pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); } @@ -42,7 +43,8 @@ public void initialize() { @Override public void execute() { // Checks if the pivot and flywheel are on target then shoots - if (pivot.onTarget() && flywheel.allMotorsOnTarget()) { + // also checks whether or not the flywheel's target RPM is greater than 0 + if (pivot.onTarget() && flywheel.allMotorsOnTarget() && (flywheel.getTopMotorRPM() != 0 && flywheel.getBottomMotorRPM() != 0)) { startIndexing = true; } diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index e7911d39..db1bce65 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -131,6 +131,20 @@ public boolean allMotorsOnTarget() { return (topMotorRPMOnTarget() && bottomMotorRPMOnTarget()); } + /** + * @return the top motor's target RPM + */ + public double topMotorTargetRPM() { + return topTargetRPS * 60; + } + + /** + * @return the bottom motor's target RPM + */ + public double bottomMotorTargetRPM() { + return bottomTargetRPS * 60; + } + /** * Sets the voltage to a small amount so the flywheel coasts to a stop * @param coast Whether or not to coast the flywheel From d15573603aa7ca619eed3678a3229dd1d54e5e05 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 2 Mar 2024 08:36:35 -0500 Subject: [PATCH 07/16] [#332] continuing to make sure all of the commands are formatted the same --- src/main/java/frc/robot/RobotContainer.java | 17 ++++++++--------- .../java/frc/robot/command/shoot/AmpShot.java | 10 +++++----- .../command/shoot/AutonCand/AmpShotAuton.java | 13 ++++++++----- .../robot/command/shoot/AutonCand/CandC1.java | 9 +++++---- .../robot/command/shoot/AutonCand/CandC2.java | 9 +++++---- .../robot/command/shoot/AutonCand/CandC3.java | 9 +++++---- .../robot/command/shoot/AutonCand/CandLine.java | 11 ++++++----- .../shoot/AutonCand/PointBlankShotAuton.java | 9 +++++---- .../frc/robot/command/shoot/PodiumShot.java | 12 ++++++------ .../frc/robot/command/shoot/PointBlankShot.java | 13 +++++++------ 10 files changed, 60 insertions(+), 52 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 72e418d6..9ec52374 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -132,13 +132,13 @@ protected void initializeNamedCommands() { leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5)); NamedCommands.registerCommand("Cand-Sub", - new PointBlankShotAuton(flywheel, pivot, indexer) + new PointBlankShotAuton(pivot, flywheel, indexer) .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(1))); - NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); - NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); - NamedCommands.registerCommand("Cand-C3", new CandC3(flywheel, pivot, indexer)); - NamedCommands.registerCommand("Cand-Line", new CandLine(flywheel, pivot, indexer)); - NamedCommands.registerCommand("AMP", new AmpShotAuton(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Cand-C1", new CandC1(pivot, flywheel, indexer)); + NamedCommands.registerCommand("Cand-C2", new CandC2(pivot, flywheel, indexer)); + NamedCommands.registerCommand("Cand-C3", new CandC3(pivot, flywheel, indexer)); + NamedCommands.registerCommand("Cand-Line", new CandLine(pivot, flywheel, indexer)); + NamedCommands.registerCommand("AMP", new AmpShotAuton(pivot, flywheel, indexer)); NamedCommands.registerCommand("Stow", new Stow(flywheel, pivot)); NamedCommands.registerCommand("Smart-Shoot", new SmartShoot(flywheel, pivot, drivetrain, indexer, leds) @@ -202,9 +202,8 @@ protected void configureButtonBindings() { pivot)); // TODO: find correct button/trigger // cand shots for the robot - new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)); - // new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); - new Trigger(coPilot::getXButton).whileTrue(new PointBlankShotAuton(flywheel, pivot, indexer)); + new Trigger(coPilot::getAButton).whileTrue(new AmpShot(pivot, flywheel)); + new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot)); new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(flywheel, pivot)); diff --git a/src/main/java/frc/robot/command/shoot/AmpShot.java b/src/main/java/frc/robot/command/shoot/AmpShot.java index 42db62bd..9ecdad0c 100644 --- a/src/main/java/frc/robot/command/shoot/AmpShot.java +++ b/src/main/java/frc/robot/command/shoot/AmpShot.java @@ -18,18 +18,18 @@ public class AmpShot extends Command { * @param pivot subsystem * @param flywheel subsystem */ - public AmpShot(Flywheel flywheel, Pivot pivot) { - this.flywheel = flywheel; + public AmpShot(Pivot pivot, Flywheel flywheel) { this.pivot = pivot; + this.flywheel = flywheel; - addRequirements(flywheel, pivot); + addRequirements(pivot, flywheel); } @Override public void initialize() { + pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); - pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); } @Override @@ -41,8 +41,8 @@ public void execute() { @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); } @Override diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java index a8e8f2a1..1915a927 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java @@ -25,19 +25,21 @@ public class AmpShotAuton extends Command { * @param flywheel subsystem * @param indexer subsystem */ - public AmpShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { - this.flywheel = flywheel; + public AmpShotAuton(Pivot pivot, Flywheel flywheel, Indexer indexer) { this.pivot = pivot; + this.flywheel = flywheel; this.indexer = indexer; - addRequirements(flywheel, pivot, indexer); + addRequirements(pivot, flywheel, indexer); } @Override public void initialize() { shot = false; + startIndexing = false; pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); + flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); + flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); } @Override @@ -55,7 +57,8 @@ public void execute() { } pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); + flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); + flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); } @Override diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java index 3cc8e4e1..ea4da1ef 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java @@ -11,8 +11,8 @@ public class CandC1 extends Command { - private final Flywheel flywheel; private final Pivot pivot; + private final Flywheel flywheel; private final Indexer indexer; private boolean shot = false; @@ -22,13 +22,13 @@ public class CandC1 extends Command { /** * Creates a new CandC1. - * @param flywheel subsystem * @param pivot subsystem + * @param flywheel subsystem * @param indexer subsystem */ - public CandC1(Flywheel flywheel, Pivot pivot, Indexer indexer) { - this.flywheel = flywheel; + public CandC1(Pivot pivot, Flywheel flywheel, Indexer indexer) { this.pivot = pivot; + this.flywheel = flywheel; this.indexer = indexer; addRequirements(pivot, flywheel, indexer); @@ -37,6 +37,7 @@ public CandC1(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; + startIndexing = false; pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java index 2f0e80ea..541d470f 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java @@ -11,8 +11,8 @@ public class CandC2 extends Command { - private final Flywheel flywheel; private final Pivot pivot; + private final Flywheel flywheel; private final Indexer indexer; private boolean shot = false; @@ -22,13 +22,13 @@ public class CandC2 extends Command { /** * Creates a new CandC2. - * @param flywheel subsystem * @param pivot subsystem + * @param flywheel subsystem * @param indexer subsystem */ - public CandC2(Flywheel flywheel, Pivot pivot, Indexer indexer) { - this.flywheel = flywheel; + public CandC2(Pivot pivot, Flywheel flywheel, Indexer indexer) { this.pivot = pivot; + this.flywheel = flywheel; this.indexer = indexer; addRequirements(pivot, flywheel, indexer); @@ -37,6 +37,7 @@ public CandC2(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; + startIndexing = false; pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java index f016886e..c4c5e914 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java @@ -11,8 +11,8 @@ public class CandC3 extends Command { - private final Flywheel flywheel; private final Pivot pivot; + private final Flywheel flywheel; private final Indexer indexer; private boolean shot = false; @@ -22,13 +22,13 @@ public class CandC3 extends Command { /** * Creates a new CandC3. - * @param flywheel subsystem * @param pivot subsystem + * @param flywheel subsystem * @param indexer subsystem */ - public CandC3(Flywheel flywheel, Pivot pivot, Indexer indexer) { - this.flywheel = flywheel; + public CandC3(Pivot pivot, Flywheel flywheel, Indexer indexer) { this.pivot = pivot; + this.flywheel = flywheel; this.indexer = indexer; addRequirements(pivot, flywheel, indexer); @@ -37,6 +37,7 @@ public CandC3(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; + startIndexing = false; pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java index 84751abf..18b88eb7 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java @@ -11,8 +11,8 @@ public class CandLine extends Command { - private final Flywheel flywheel; private final Pivot pivot; + private final Flywheel flywheel; private final Indexer indexer; private boolean shot = false; @@ -22,13 +22,13 @@ public class CandLine extends Command { /** * Creates a new CandLine. - * @param flywheel subsystem - * @param pivot subsystem + * @param pivot subsystem + * @param flywheel subsystem * @param indexer subsystem */ - public CandLine(Flywheel flywheel, Pivot pivot, Indexer indexer) { - this.flywheel = flywheel; + public CandLine(Pivot pivot, Flywheel flywheel, Indexer indexer) { this.pivot = pivot; + this.flywheel = flywheel; this.indexer = indexer; addRequirements(pivot, flywheel, indexer); @@ -37,6 +37,7 @@ public CandLine(Flywheel flywheel, Pivot pivot, Indexer indexer) { @Override public void initialize() { shot = false; + startIndexing = false; pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index c54fc349..ccaa83eb 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -9,8 +9,9 @@ import frc.robot.subsystems.Pivot; public class PointBlankShotAuton extends Command { - private final Flywheel flywheel; + private final Pivot pivot; + private final Flywheel flywheel; private final Indexer indexer; private boolean shot = false; @@ -20,13 +21,13 @@ public class PointBlankShotAuton extends Command { /** * Creates a new PointBlankShot. - * @param flywheel subsystem * @param pivot subsystem + * @param flywheel subsystem * @param indexer subsystem */ - public PointBlankShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { - this.flywheel = flywheel; + public PointBlankShotAuton(Pivot pivot, Flywheel flywheel, Indexer indexer) { this.pivot = pivot; + this.flywheel = flywheel; this.indexer = indexer; addRequirements(pivot, flywheel, indexer); diff --git a/src/main/java/frc/robot/command/shoot/PodiumShot.java b/src/main/java/frc/robot/command/shoot/PodiumShot.java index cdd2336d..f85d8d84 100644 --- a/src/main/java/frc/robot/command/shoot/PodiumShot.java +++ b/src/main/java/frc/robot/command/shoot/PodiumShot.java @@ -8,17 +8,17 @@ public class PodiumShot extends Command { - private final Flywheel flywheel; private final Pivot pivot; + private final Flywheel flywheel; /** * Creates a new PodiumShot. - * @param flywheel * @param pivot + * @param flywheel */ - public PodiumShot(Flywheel flywheel, Pivot pivot) { - this.flywheel = flywheel; + public PodiumShot(Pivot pivot, Flywheel flywheel) { this.pivot = pivot; + this.flywheel = flywheel; addRequirements(pivot, flywheel); } @@ -28,13 +28,13 @@ public void initialize() {} @Override public void execute() { - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); } } diff --git a/src/main/java/frc/robot/command/shoot/PointBlankShot.java b/src/main/java/frc/robot/command/shoot/PointBlankShot.java index 0b82dbe6..c9a511d1 100644 --- a/src/main/java/frc/robot/command/shoot/PointBlankShot.java +++ b/src/main/java/frc/robot/command/shoot/PointBlankShot.java @@ -8,37 +8,38 @@ import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; public class PointBlankShot extends Command { - private final Flywheel flywheel; + private final Pivot pivot; + private final Flywheel flywheel; /** * Creates a new PointBlankShot. - * @param flywheel subsystem * @param pivot subsystem + * @param flywheel subsystem */ public PointBlankShot(Flywheel flywheel, Pivot pivot) { - this.flywheel = flywheel; this.pivot = pivot; + this.flywheel = flywheel; addRequirements(pivot, flywheel); } @Override public void initialize() { - flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); } @Override public void execute() { - flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + flywheel.getBias()); + flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); } @Override From 651c7d7c9e12562aa5f200b4e81df8569b8af56f Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 2 Mar 2024 09:58:57 -0500 Subject: [PATCH 08/16] [#332] added the correct constants for the cand shots --- .../java/frc/robot/command/shoot/AutonCand/CandC1.java | 8 ++++---- .../java/frc/robot/command/shoot/AutonCand/CandC2.java | 8 ++++---- .../java/frc/robot/command/shoot/AutonCand/CandC3.java | 8 ++++---- .../java/frc/robot/command/shoot/AutonCand/CandLine.java | 8 ++++---- src/main/java/frc/robot/subsystems/Flywheel.java | 2 +- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java index ea4da1ef..024fb605 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java @@ -38,8 +38,8 @@ public CandC1(Pivot pivot, Flywheel flywheel, Indexer indexer) { public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C1_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.C1_RPM + flywheel.getBias()); } @Override @@ -56,8 +56,8 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C1_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.C1_RPM + flywheel.getBias()); } @Override diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java index 541d470f..62e7ddcd 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java @@ -38,8 +38,8 @@ public CandC2(Pivot pivot, Flywheel flywheel, Indexer indexer) { public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C2_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.C2_RPM + flywheel.getBias()); } @Override @@ -56,8 +56,8 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C2_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.C2_RPM + flywheel.getBias()); } @Override diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java index c4c5e914..84de9a50 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java @@ -38,8 +38,8 @@ public CandC3(Pivot pivot, Flywheel flywheel, Indexer indexer) { public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C3_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.C3_RPM + flywheel.getBias()); } @Override @@ -56,8 +56,8 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C3_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.C3_RPM + flywheel.getBias()); } @Override diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java index 18b88eb7..992ebf71 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java @@ -38,8 +38,8 @@ public CandLine(Pivot pivot, Flywheel flywheel, Indexer indexer) { public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.LINE_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.LINE_RPM + flywheel.getBias()); } @Override @@ -56,8 +56,8 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); - flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.LINE_ANGLE + pivot.getBias()); + flywheel.setAllMotorsRPM(CandConstants.LINE_RPM + flywheel.getBias()); } @Override diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index db1bce65..95155708 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -64,7 +64,7 @@ public void periodic() { topMotor.set(0d); } else { topMotor.setControl(topRPMPID.withVelocity((topTargetRPS + bias))); - bottomMotor.setControl(bottomRPMPID.withVelocity((topTargetRPS + bias))); + bottomMotor.setControl(bottomRPMPID.withVelocity((bottomTargetRPS + bias))); } } From 415ab152cf4c5f802e15be95490fe26055690353 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 2 Mar 2024 10:09:37 -0500 Subject: [PATCH 09/16] [#332] formatting fixes --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../java/frc/robot/command/shoot/AmpShot.java | 8 +++----- .../command/shoot/AutonCand/AmpShotAuton.java | 8 ++++---- .../robot/command/shoot/AutonCand/CandC1.java | 3 +-- .../robot/command/shoot/AutonCand/CandC2.java | 3 +-- .../robot/command/shoot/AutonCand/CandC3.java | 3 +-- .../command/shoot/AutonCand/CandLine.java | 3 +-- .../shoot/AutonCand/PointBlankShotAuton.java | 2 +- .../frc/robot/command/shoot/PodiumShot.java | 6 +++--- .../robot/command/shoot/PointBlankShot.java | 7 +++---- .../frc/robot/command/shoot/SmartShoot.java | 1 - .../robot/command/shoot/SourceCollect.java | 19 +++++++++---------- .../java/frc/robot/command/shoot/Stow.java | 11 ++++++++--- 13 files changed, 37 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ec52374..f3d16f81 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -203,9 +203,9 @@ protected void configureButtonBindings() { // cand shots for the robot new Trigger(coPilot::getAButton).whileTrue(new AmpShot(pivot, flywheel)); - new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); + new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(pivot, flywheel)); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot)); - new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(flywheel, pivot)); + new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(pivot, flywheel)); // new Trigger(coPilot::getBButton).whileTrue(new Climb(climber, // TODO need new button start? Back? // drivetrain).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); diff --git a/src/main/java/frc/robot/command/shoot/AmpShot.java b/src/main/java/frc/robot/command/shoot/AmpShot.java index 9ecdad0c..81911759 100644 --- a/src/main/java/frc/robot/command/shoot/AmpShot.java +++ b/src/main/java/frc/robot/command/shoot/AmpShot.java @@ -2,16 +2,14 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; -import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.command.Index; -import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Flywheel; public class AmpShot extends Command { - private Pivot pivot; - private Flywheel flywheel; + private final Pivot pivot; + private final Flywheel flywheel; /** * Creates a new AmpShot diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java index 1915a927..f771922b 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java @@ -4,15 +4,15 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; public class AmpShotAuton extends Command { - private Pivot pivot; - private Flywheel flywheel; - private Indexer indexer; + private final Pivot pivot; + private final Flywheel flywheel; + private final Indexer indexer; private boolean shot = false; private double shotTime = 0; diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java index 024fb605..1f941cfd 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java @@ -3,11 +3,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; -import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; public class CandC1 extends Command { diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java index 62e7ddcd..f88ebd24 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java @@ -3,11 +3,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; -import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; public class CandC2 extends Command { diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java index 84de9a50..3759ac65 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java @@ -3,11 +3,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; -import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; public class CandC3 extends Command { diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java index 992ebf71..db72f257 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java @@ -3,11 +3,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; -import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; public class CandLine extends Command { diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index ccaa83eb..dc396feb 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -4,9 +4,9 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; public class PointBlankShotAuton extends Command { diff --git a/src/main/java/frc/robot/command/shoot/PodiumShot.java b/src/main/java/frc/robot/command/shoot/PodiumShot.java index f85d8d84..374f11ec 100644 --- a/src/main/java/frc/robot/command/shoot/PodiumShot.java +++ b/src/main/java/frc/robot/command/shoot/PodiumShot.java @@ -3,8 +3,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Flywheel; public class PodiumShot extends Command { @@ -13,8 +13,8 @@ public class PodiumShot extends Command { /** * Creates a new PodiumShot. - * @param pivot - * @param flywheel + * @param pivot subsystem + * @param flywheel subsystem */ public PodiumShot(Pivot pivot, Flywheel flywheel) { this.pivot = pivot; diff --git a/src/main/java/frc/robot/command/shoot/PointBlankShot.java b/src/main/java/frc/robot/command/shoot/PointBlankShot.java index c9a511d1..76cb7771 100644 --- a/src/main/java/frc/robot/command/shoot/PointBlankShot.java +++ b/src/main/java/frc/robot/command/shoot/PointBlankShot.java @@ -1,12 +1,11 @@ package frc.robot.command.shoot; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Constants.CandConstants; -import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Flywheel; + public class PointBlankShot extends Command { private final Pivot pivot; @@ -17,7 +16,7 @@ public class PointBlankShot extends Command { * @param pivot subsystem * @param flywheel subsystem */ - public PointBlankShot(Flywheel flywheel, Pivot pivot) { + public PointBlankShot(Pivot pivot, Flywheel flywheel) { this.pivot = pivot; this.flywheel = flywheel; diff --git a/src/main/java/frc/robot/command/shoot/SmartShoot.java b/src/main/java/frc/robot/command/shoot/SmartShoot.java index 9bfab5b4..e245d386 100644 --- a/src/main/java/frc/robot/command/shoot/SmartShoot.java +++ b/src/main/java/frc/robot/command/shoot/SmartShoot.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.Constants.CandConstants; -import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.LEDsConstants.LED_STATES; import frc.robot.Constants.ShooterConstants.ShootingState; import frc.robot.Constants.PivotConstants; diff --git a/src/main/java/frc/robot/command/shoot/SourceCollect.java b/src/main/java/frc/robot/command/shoot/SourceCollect.java index a5543150..82775e1b 100644 --- a/src/main/java/frc/robot/command/shoot/SourceCollect.java +++ b/src/main/java/frc/robot/command/shoot/SourceCollect.java @@ -3,43 +3,42 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Flywheel; public class SourceCollect extends Command { - private final Flywheel flywheel; private final Pivot pivot; + private final Flywheel flywheel; /** * Creates a new SourceCollect. - * - * @param flywheel subsystem * @param pivot subsystem + * @param flywheel subsystem */ - public SourceCollect(Flywheel flywheel, Pivot pivot) { - this.flywheel = flywheel; + public SourceCollect(Pivot pivot, Flywheel flywheel) { this.pivot = pivot; + this.flywheel = flywheel; - addRequirements(flywheel, pivot); + addRequirements(pivot, flywheel); } @Override public void initialize() { - flywheel.setAllMotorsRPM(CandConstants.SOURCE_RPM); pivot.setTargetAngle(CandConstants.SOURCE_ANGLE); + flywheel.setAllMotorsRPM(CandConstants.SOURCE_RPM); } @Override public void execute() { - flywheel.setAllMotorsRPM(CandConstants.SOURCE_RPM); pivot.setTargetAngle(CandConstants.SOURCE_ANGLE); + flywheel.setAllMotorsRPM(CandConstants.SOURCE_RPM); } @Override public void end(boolean interrupted) { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); } @Override diff --git a/src/main/java/frc/robot/command/shoot/Stow.java b/src/main/java/frc/robot/command/shoot/Stow.java index efabffe4..dc5fd936 100644 --- a/src/main/java/frc/robot/command/shoot/Stow.java +++ b/src/main/java/frc/robot/command/shoot/Stow.java @@ -2,25 +2,30 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Flywheel; public class Stow extends Command { private Pivot pivot; private Flywheel flywheel; + /** + * Creates a new Stow. + * @param pivot subsystem + * @param flywheel subsystem + */ public Stow(Flywheel flywheel, Pivot pivot) { - this.flywheel = flywheel; this.pivot = pivot; + this.flywheel = flywheel; addRequirements(pivot, flywheel); } @Override public void initialize() { - flywheel.coast(true); pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + flywheel.coast(true); } @Override From f71ad4956d598232712c0d24a8e940871e6a930b Mon Sep 17 00:00:00 2001 From: EvanLeroy <128870446+EvanLeroy@users.noreply.github.com> Date: Sat, 2 Mar 2024 14:32:52 -0500 Subject: [PATCH 10/16] [#366] added flywheel stop --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/command/SmartCollect.java | 4 +++- src/main/java/frc/robot/subsystems/Flywheel.java | 6 +++++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f8a30f62..3eb26ca8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -202,7 +202,8 @@ protected void configureButtonBindings() { /* copilot */ new Trigger(coPilot::getBButton) - .whileTrue(new SmartCollect(() -> 0.50, () -> 0.60, collector, indexer, pivot)); // TODO: find correct button/trigger + .whileTrue(new InstantCommand(() -> flywheel.stop(true), flywheel) + .andThen(new SmartCollect(() -> 0.50, () -> 0.60, collector, indexer, pivot))); // TODO: find correct button/trigger // cand shots for the robot new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)); diff --git a/src/main/java/frc/robot/command/SmartCollect.java b/src/main/java/frc/robot/command/SmartCollect.java index 0fc79ed2..436f3aff 100644 --- a/src/main/java/frc/robot/command/SmartCollect.java +++ b/src/main/java/frc/robot/command/SmartCollect.java @@ -42,7 +42,9 @@ public SmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, } @Override - public void initialize() {} + public void initialize() { + + } @Override public void execute() { diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index d39ba0ae..d54c600a 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -135,7 +135,11 @@ public boolean allMotorsOnTarget() { public void coast(boolean coast) { this.coast = coast; } - + public void stop(boolean stop) { + if (stop) { + setAllMotorsRPM(0); + } + } /** * @return The bias to add to the target RPM of the flywheel */ From 63f26cdc8d7141c4693462a814822dc9a314a8da Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 2 Mar 2024 14:35:49 -0500 Subject: [PATCH 11/16] [#332] adding test hee hee --- .../pathplanner/autos/test hee hee.auto | 31 +++++++++++ .../deploy/pathplanner/paths/Move Back.path | 52 +++++++++++++++++++ .../pathplanner/paths/Move Forward.path | 52 +++++++++++++++++++ src/main/java/frc/robot/Constants.java | 2 +- .../shoot/AutonCand/PointBlankShotAuton.java | 4 +- 5 files changed, 139 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Move Back.path create mode 100644 src/main/deploy/pathplanner/paths/Move Forward.path diff --git a/src/main/deploy/pathplanner/autos/test hee hee.auto b/src/main/deploy/pathplanner/autos/test hee hee.auto index 3a8351e9..db81e583 100644 --- a/src/main/deploy/pathplanner/autos/test hee hee.auto +++ b/src/main/deploy/pathplanner/autos/test hee hee.auto @@ -10,6 +10,37 @@ "data": { "name": "Cand-Sub" } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Move Forward" + } + }, + { + "type": "named", + "data": { + "name": "Collect" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Move Back" + } + }, + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Move Back.path b/src/main/deploy/pathplanner/paths/Move Back.path new file mode 100644 index 00000000..9798aca8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Move Back.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.65, + "y": 5.539225686154654 + }, + "prevControl": null, + "nextControl": { + "x": 1.6233924503512331, + "y": 5.531608026126181 + }, + "isLocked": false, + "linkedName": "C2" + }, + { + "anchor": { + "x": 1.334102025955381, + "y": 5.567399971117918 + }, + "prevControl": { + "x": 2.483157024493337, + "y": 5.559782311089444 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "SubwooferMiddle" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Move Forward.path b/src/main/deploy/pathplanner/paths/Move Forward.path new file mode 100644 index 00000000..3e519a34 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Move Forward.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.334102025955381, + "y": 5.567399971117918 + }, + "prevControl": null, + "nextControl": { + "x": 2.334102025955381, + "y": 5.567399971117918 + }, + "isLocked": false, + "linkedName": "SubwooferMiddle" + }, + { + "anchor": { + "x": 2.65, + "y": 5.539225686154654 + }, + "prevControl": { + "x": 1.65, + "y": 5.539225686154654 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "C2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cd6cf820..cd1dcc1e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -526,7 +526,7 @@ public class CandConstants { // TODO get real public static final double SOURCE_ANGLE = 45d; // TODO test // TODO find time to shoot - public static final double TIME_TO_SHOOT = 2d; // Time in seconds it takes from indexer start to flywheel exit + public static final double TIME_TO_SHOOT = 1d; // Time in seconds it takes from indexer start to flywheel exit } public class ClimbConstants { // TODO: find real values diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index dc396feb..4d123e9a 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -15,6 +15,7 @@ public class PointBlankShotAuton extends Command { private final Indexer indexer; private boolean shot = false; + private double startTime = 0; private double shotTime = 0; private boolean startIndexing = false; @@ -37,6 +38,7 @@ public PointBlankShotAuton(Pivot pivot, Flywheel flywheel, Indexer indexer) { public void initialize() { shot = false; startIndexing = false; + startTime = Timer.getFPGATimestamp(); pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); } @@ -68,6 +70,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return shot && Timer.getFPGATimestamp() - shotTime >= CandConstants.TIME_TO_SHOOT; + return shot && shotTime - startTime >= CandConstants.TIME_TO_SHOOT; } } \ No newline at end of file From 581ec57639deeee4773146e6918f5d3fce34e102 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 2 Mar 2024 14:48:06 -0500 Subject: [PATCH 12/16] [#364] Tune --- src/main/java/frc/robot/Constants.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 74bb713b..415b3ad2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -493,6 +493,7 @@ public class ShooterConstants { put(1.08d, 60d); put(2.01d, 50d); put(2.94d, 39d); + put(3.99d, 31d); } }; @@ -501,9 +502,9 @@ public class ShooterConstants { { // As distance get smaller RPM gets smaller put(1.08d, 2000d); - put(2.01d, 2250d); + put(2.01d, 2000d); put(2.94d, 2500d); - + put(3.99d, 3250d); } }; From ecd8a0ea66ff007ca7ad4865d27c8b8163155cab Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 2 Mar 2024 15:00:15 -0500 Subject: [PATCH 13/16] [#332] made the flywheel be used before the pivot --- src/main/java/frc/robot/RobotContainer.java | 18 +++++++------- .../java/frc/robot/command/shoot/AmpShot.java | 18 +++++++------- .../command/shoot/AutonCand/AmpShotAuton.java | 22 +++++++++-------- .../robot/command/shoot/AutonCand/CandC1.java | 22 +++++++++-------- .../robot/command/shoot/AutonCand/CandC2.java | 22 +++++++++-------- .../robot/command/shoot/AutonCand/CandC3.java | 22 +++++++++-------- .../command/shoot/AutonCand/CandLine.java | 24 ++++++++++--------- .../shoot/AutonCand/PointBlankShotAuton.java | 16 ++++++------- .../frc/robot/command/shoot/PodiumShot.java | 16 ++++++------- .../robot/command/shoot/PointBlankShot.java | 16 ++++++------- .../robot/command/shoot/SourceCollect.java | 10 ++++---- 11 files changed, 108 insertions(+), 98 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b41173f9..6642adbc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -136,13 +136,13 @@ protected void initializeNamedCommands() { leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5)); NamedCommands.registerCommand("Cand-Sub", - new PointBlankShotAuton(pivot, flywheel, indexer) + new PointBlankShotAuton(flywheel, pivot, indexer) .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(1))); - NamedCommands.registerCommand("Cand-C1", new CandC1(pivot, flywheel, indexer)); - NamedCommands.registerCommand("Cand-C2", new CandC2(pivot, flywheel, indexer)); - NamedCommands.registerCommand("Cand-C3", new CandC3(pivot, flywheel, indexer)); - NamedCommands.registerCommand("Cand-Line", new CandLine(pivot, flywheel, indexer)); - NamedCommands.registerCommand("AMP", new AmpShotAuton(pivot, flywheel, indexer)); + NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Cand-C3", new CandC3(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Cand-Line", new CandLine(flywheel, pivot, indexer)); + NamedCommands.registerCommand("AMP", new AmpShotAuton(flywheel, pivot, indexer)); NamedCommands.registerCommand("Stow", new Stow(flywheel, pivot)); NamedCommands.registerCommand("Smart-Shoot", new SmartShoot(flywheel, pivot, drivetrain, indexer, leds) @@ -205,10 +205,10 @@ protected void configureButtonBindings() { .whileTrue(new SmartCollect(() -> 0.50, () -> 0.60, collector, indexer, pivot)); // TODO: find correct button/trigger // cand shots for the robot - new Trigger(coPilot::getAButton).whileTrue(new AmpShot(pivot, flywheel)); - new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(pivot, flywheel)); + new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot)); + new Trigger(coPilot::getXButton).whileTrue(new PointBlankShot(flywheel, pivot)); // new Trigger(coPilot::getYButton).whileTrue(new PodiumShot(flywheel, pivot)); - new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(pivot, flywheel)); + new Trigger(coPilot::getYButton).whileTrue(new SourceCollect(flywheel, pivot)); // new Trigger(coPilot::getBButton).whileTrue(new Climb(climber, // TODO need new button start? Back? // drivetrain).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); diff --git a/src/main/java/frc/robot/command/shoot/AmpShot.java b/src/main/java/frc/robot/command/shoot/AmpShot.java index 81911759..a089a602 100644 --- a/src/main/java/frc/robot/command/shoot/AmpShot.java +++ b/src/main/java/frc/robot/command/shoot/AmpShot.java @@ -3,44 +3,44 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; public class AmpShot extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; /** * Creates a new AmpShot - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem */ - public AmpShot(Pivot pivot, Flywheel flywheel) { - this.pivot = pivot; + public AmpShot(Flywheel flywheel, Pivot pivot) { this.flywheel = flywheel; + this.pivot = pivot; - addRequirements(pivot, flywheel); + addRequirements(flywheel, pivot); } @Override public void initialize() { - pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); } @Override public void execute() { - pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); } @Override diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java index f771922b..fe6e6304 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/AmpShotAuton.java @@ -4,42 +4,44 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Indexer; public class AmpShotAuton extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; private final Indexer indexer; private boolean shot = false; + private double startTime = 0; private double shotTime = 0; private boolean startIndexing = false; /** * Creates a new AmpShot - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem * @param indexer subsystem */ - public AmpShotAuton(Pivot pivot, Flywheel flywheel, Indexer indexer) { - this.pivot = pivot; + public AmpShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { this.flywheel = flywheel; + this.pivot = pivot; this.indexer = indexer; - addRequirements(pivot, flywheel, indexer); + addRequirements(flywheel, pivot, indexer); } @Override public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); + startTime = Timer.getFPGATimestamp(); flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); } @Override @@ -56,20 +58,20 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); flywheel.setTopMoterRPM(CandConstants.AMP_TOP_RPM + flywheel.getBias()); flywheel.setBottomMoterRPM(CandConstants.AMP_BOTTOM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.AMP_ANGLE + pivot.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); indexer.stop(); } @Override public boolean isFinished() { - return shot && Timer.getFPGATimestamp() - shotTime >= CandConstants.TIME_TO_SHOOT; + return shot && shotTime - startTime >= CandConstants.TIME_TO_SHOOT; } } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java index 1f941cfd..4db83136 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC1.java @@ -4,41 +4,43 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Indexer; public class CandC1 extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; private final Indexer indexer; private boolean shot = false; + private double startTime = 0; private double shotTime = 0; private boolean startIndexing = false; /** * Creates a new CandC1. - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem * @param indexer subsystem */ - public CandC1(Pivot pivot, Flywheel flywheel, Indexer indexer) { - this.pivot = pivot; + public CandC1(Flywheel flywheel, Pivot pivot, Indexer indexer) { this.flywheel = flywheel; + this.pivot = pivot; this.indexer = indexer; - addRequirements(pivot, flywheel, indexer); + addRequirements(flywheel, pivot, indexer); } @Override public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.C1_ANGLE + pivot.getBias()); + startTime = Timer.getFPGATimestamp(); flywheel.setAllMotorsRPM(CandConstants.C1_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C1_ANGLE + pivot.getBias()); } @Override @@ -55,19 +57,19 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.C1_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.C1_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C1_ANGLE + pivot.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); indexer.stop(); } @Override public boolean isFinished() { - return shot && Timer.getFPGATimestamp() - shotTime >= CandConstants.TIME_TO_SHOOT; + return shot && shotTime - startTime >= CandConstants.TIME_TO_SHOOT; } } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java index f88ebd24..d78eeaf1 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC2.java @@ -4,41 +4,43 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Indexer; public class CandC2 extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; private final Indexer indexer; private boolean shot = false; private double shotTime = 0; + private double startTime = 0; private boolean startIndexing = false; /** * Creates a new CandC2. - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem * @param indexer subsystem */ - public CandC2(Pivot pivot, Flywheel flywheel, Indexer indexer) { - this.pivot = pivot; + public CandC2(Flywheel flywheel, Pivot pivot, Indexer indexer) { this.flywheel = flywheel; + this.pivot = pivot; this.indexer = indexer; - addRequirements(pivot, flywheel, indexer); + addRequirements(flywheel, pivot, indexer); } @Override public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.C2_ANGLE + pivot.getBias()); + startTime = Timer.getFPGATimestamp(); flywheel.setAllMotorsRPM(CandConstants.C2_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C2_ANGLE + pivot.getBias()); } @Override @@ -55,19 +57,19 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.C2_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.C2_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C2_ANGLE + pivot.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); indexer.stop(); } @Override public boolean isFinished() { - return shot && Timer.getFPGATimestamp() - shotTime >= CandConstants.TIME_TO_SHOOT; + return shot && shotTime - startTime >= CandConstants.TIME_TO_SHOOT; } } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java index 3759ac65..c675e2c3 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandC3.java @@ -4,41 +4,43 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Indexer; public class CandC3 extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; private final Indexer indexer; private boolean shot = false; private double shotTime = 0; + private double startTime = 0; private boolean startIndexing = false; /** * Creates a new CandC3. - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem * @param indexer subsystem */ - public CandC3(Pivot pivot, Flywheel flywheel, Indexer indexer) { - this.pivot = pivot; + public CandC3(Flywheel flywheel, Pivot pivot, Indexer indexer) { this.flywheel = flywheel; + this.pivot = pivot; this.indexer = indexer; - addRequirements(pivot, flywheel, indexer); + addRequirements(flywheel, pivot, indexer); } @Override public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.C3_ANGLE + pivot.getBias()); + startTime = Timer.getFPGATimestamp(); flywheel.setAllMotorsRPM(CandConstants.C3_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C3_ANGLE + pivot.getBias()); } @Override @@ -55,19 +57,19 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.C3_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.C3_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.C3_ANGLE + pivot.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); indexer.stop(); } @Override public boolean isFinished() { - return shot && Timer.getFPGATimestamp() - shotTime >= CandConstants.TIME_TO_SHOOT; + return shot && shotTime - startTime >= CandConstants.TIME_TO_SHOOT; } } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java index db72f257..3afdeccd 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/CandLine.java @@ -4,41 +4,43 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Indexer; public class CandLine extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; private final Indexer indexer; private boolean shot = false; private double shotTime = 0; + private double startTime = 0; private boolean startIndexing = false; /** * Creates a new CandLine. - * @param pivot subsystem - * @param flywheel subsystem + * @param flywheel subsystem + * @param pivot subsystem * @param indexer subsystem */ - public CandLine(Pivot pivot, Flywheel flywheel, Indexer indexer) { - this.pivot = pivot; + public CandLine(Flywheel flywheel, Pivot pivot, Indexer indexer) { this.flywheel = flywheel; + this.pivot = pivot; this.indexer = indexer; - addRequirements(pivot, flywheel, indexer); + addRequirements(flywheel, pivot, indexer); } @Override public void initialize() { shot = false; startIndexing = false; - pivot.setTargetAngle(CandConstants.LINE_ANGLE + pivot.getBias()); + startTime = Timer.getFPGATimestamp(); flywheel.setAllMotorsRPM(CandConstants.LINE_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.LINE_ANGLE + pivot.getBias()); } @Override @@ -55,19 +57,19 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.LINE_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.LINE_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.LINE_ANGLE + pivot.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); indexer.stop(); } @Override public boolean isFinished() { - return shot && Timer.getFPGATimestamp() - shotTime >= CandConstants.TIME_TO_SHOOT; + return shot && startTime - shotTime >= CandConstants.TIME_TO_SHOOT; } } diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index 4d123e9a..4bb9fd4e 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -4,14 +4,14 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Indexer; public class PointBlankShotAuton extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; private final Indexer indexer; private boolean shot = false; @@ -22,16 +22,16 @@ public class PointBlankShotAuton extends Command { /** * Creates a new PointBlankShot. - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem * @param indexer subsystem */ - public PointBlankShotAuton(Pivot pivot, Flywheel flywheel, Indexer indexer) { - this.pivot = pivot; + public PointBlankShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { this.flywheel = flywheel; + this.pivot = pivot; this.indexer = indexer; - addRequirements(pivot, flywheel, indexer); + addRequirements(flywheel, pivot, indexer); } @Override @@ -57,14 +57,14 @@ public void execute() { indexer.indexUp(); } - pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + flywheel.getBias()); flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); + pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + flywheel.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); indexer.stop(); } diff --git a/src/main/java/frc/robot/command/shoot/PodiumShot.java b/src/main/java/frc/robot/command/shoot/PodiumShot.java index 374f11ec..0225cf05 100644 --- a/src/main/java/frc/robot/command/shoot/PodiumShot.java +++ b/src/main/java/frc/robot/command/shoot/PodiumShot.java @@ -3,24 +3,24 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; public class PodiumShot extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; /** * Creates a new PodiumShot. - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem */ - public PodiumShot(Pivot pivot, Flywheel flywheel) { - this.pivot = pivot; + public PodiumShot(Flywheel flywheel, Pivot pivot) { this.flywheel = flywheel; + this.pivot = pivot; - addRequirements(pivot, flywheel); + addRequirements(flywheel, pivot); } @Override @@ -28,13 +28,13 @@ public void initialize() {} @Override public void execute() { - pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.PODIUM_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.PODIUM_ANGLE + pivot.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); } } diff --git a/src/main/java/frc/robot/command/shoot/PointBlankShot.java b/src/main/java/frc/robot/command/shoot/PointBlankShot.java index 76cb7771..b7b25f1f 100644 --- a/src/main/java/frc/robot/command/shoot/PointBlankShot.java +++ b/src/main/java/frc/robot/command/shoot/PointBlankShot.java @@ -3,42 +3,42 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; public class PointBlankShot extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; /** * Creates a new PointBlankShot. * @param pivot subsystem * @param flywheel subsystem */ - public PointBlankShot(Pivot pivot, Flywheel flywheel) { - this.pivot = pivot; + public PointBlankShot(Flywheel flywheel, Pivot pivot) { this.flywheel = flywheel; + this.pivot = pivot; - addRequirements(pivot, flywheel); + addRequirements(flywheel, pivot); } @Override public void initialize() { - pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + pivot.getBias()); flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + flywheel.getBias()); + pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + pivot.getBias()); } @Override public void execute() { - pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + flywheel.getBias()); flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); + pivot.setTargetAngle(CandConstants.POINT_BLANK_ANGLE + flywheel.getBias()); } @Override public void end(boolean interrupted) { - pivot.setTargetAngle(PivotConstants.STOW_ANGLE); flywheel.coast(true); + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); } @Override diff --git a/src/main/java/frc/robot/command/shoot/SourceCollect.java b/src/main/java/frc/robot/command/shoot/SourceCollect.java index 9511dcfc..8f1350da 100644 --- a/src/main/java/frc/robot/command/shoot/SourceCollect.java +++ b/src/main/java/frc/robot/command/shoot/SourceCollect.java @@ -3,22 +3,22 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.PivotConstants; -import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Pivot; public class SourceCollect extends Command { - private final Pivot pivot; private final Flywheel flywheel; + private final Pivot pivot; /** * Creates a new SourceCollect. - * @param pivot subsystem * @param flywheel subsystem + * @param pivot subsystem */ - public SourceCollect(Pivot pivot, Flywheel flywheel) { - this.pivot = pivot; + public SourceCollect(Flywheel flywheel, Pivot pivot) { this.flywheel = flywheel; + this.pivot = pivot; addRequirements(flywheel); } From 50a134fda3752cf6b1d0461c7b45c6eee035efba Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 2 Mar 2024 15:09:33 -0500 Subject: [PATCH 14/16] [#366] Clean --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/Flywheel.java | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3eb26ca8..416327c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -202,7 +202,7 @@ protected void configureButtonBindings() { /* copilot */ new Trigger(coPilot::getBButton) - .whileTrue(new InstantCommand(() -> flywheel.stop(true), flywheel) + .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) .andThen(new SmartCollect(() -> 0.50, () -> 0.60, collector, indexer, pivot))); // TODO: find correct button/trigger // cand shots for the robot diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index d54c600a..20ce5b0b 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -135,11 +135,11 @@ public boolean allMotorsOnTarget() { public void coast(boolean coast) { this.coast = coast; } - public void stop(boolean stop) { - if (stop) { - setAllMotorsRPM(0); - } + + public void stop() { + setAllMotorsRPM(0); } + /** * @return The bias to add to the target RPM of the flywheel */ From 8f440871080f41d7358080f0a1fc830298fab9b6 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 2 Mar 2024 15:19:02 -0500 Subject: [PATCH 15/16] [#332] removed vandalism --- src/main/java/frc/robot/subsystems/Indexer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 0e300fdf..8ddeb939 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -22,7 +22,7 @@ public class Indexer extends SubsystemBase { private double timeLastTriggered = 0d; - private double targetPower = 0; // danny was here + private double targetPower = 0; private PieceState currentState = PieceState.NONE; From f372c66ce70d3e7c8dea8d3b4ff59cd3398eb0d3 Mon Sep 17 00:00:00 2001 From: Meh243 <120067923+Meh243@users.noreply.github.com> Date: Sat, 2 Mar 2024 15:35:22 -0500 Subject: [PATCH 16/16] [#371] created the collect and go command --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 2 + .../java/frc/robot/command/CollectAndGo.java | 42 +++++++++++++++++++ 3 files changed, 45 insertions(+) create mode 100644 src/main/java/frc/robot/command/CollectAndGo.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 415b3ad2..55f95632 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -397,6 +397,7 @@ public class CollectorConstants { // TODO: get real public static final double MOTOR_KA = 0; public static final double COLLECTOR_SYSTEST_POWER = 0.25; + public static final double COLLECTOR_GRABANDGO_POWER = 0.75; } public class FlywheelConstants { // TODO: get real diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f1a44c9..9b8143c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -60,6 +60,7 @@ import frc.robot.command.Climb; import frc.robot.command.CollisionDetection; import frc.robot.command.Collect; +import frc.robot.command.CollectAndGo; import frc.robot.subsystems.Limelights; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Climber; @@ -153,6 +154,7 @@ protected void initializeNamedCommands() { .alongWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); NamedCommands.registerCommand("PathFind", new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); + NamedCommands.registerCommand("Collect-And-Go", new CollectAndGo(collector)); // make sure named commands are initialized before autobuilder! autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/command/CollectAndGo.java b/src/main/java/frc/robot/command/CollectAndGo.java new file mode 100644 index 00000000..eb1fed1c --- /dev/null +++ b/src/main/java/frc/robot/command/CollectAndGo.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Collector; +import frc.robot.Constants.CollectorConstants; + +public class CollectAndGo extends Command { + + private final Collector collector; + + /** Creates a new CollectAndGo. + * @param collector subsystem + */ + public CollectAndGo(Collector collector) { + this.collector = collector; + addRequirements(collector); + } + + @Override + public void initialize() { + collector.setPower(CollectorConstants.COLLECTOR_GRABANDGO_POWER); + } + + @Override + public void execute() { + collector.setPower(CollectorConstants.COLLECTOR_GRABANDGO_POWER); + } + + @Override + public void end(boolean interrupted) { + collector.stop(); + } + + @Override + public boolean isFinished() { + return collector.getEntryBeamBreakState(); + } +}