From c0541c7cea5595060bc235e7317e814340ea87ba Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 13 Apr 2024 08:56:14 -0400 Subject: [PATCH 1/6] [#536] Working note pass --- src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/command/shoot/NotePass.java | 64 +++++++++---------- 2 files changed, 36 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a791d4a1..8573a413 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -249,8 +249,9 @@ protected void configureButtonBindings() { new Trigger(driver::getLeftBumper).whileTrue( new ComboPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver, limelights, 0d)); - // new Trigger(driver::getYButton) - // .whileTrue(new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); + new Trigger(driver::getYButton) + .whileTrue(new NotePass(drivetrain, flywheel, pivot, driver, indexer) + .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED)); @@ -268,7 +269,7 @@ protected void configureButtonBindings() { new Trigger(coPilot::getXButton) .whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); // new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); - new Trigger(coPilot::getYButton).whileTrue(new NotePass(drivetrain, flywheel, pivot, driver, indexer)); + // new Trigger(coPilot::getYButton).whileTrue(new NotePass(drivetrain, flywheel, pivot, driver, indexer)); // new Trigger(coPilot::getYButton).whileTrue(new Tune(flywheel, pivot)); // .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); new Trigger(coPilot::getAButton).whileTrue(new AmpShot(flywheel, pivot) diff --git a/src/main/java/frc/robot/command/shoot/NotePass.java b/src/main/java/frc/robot/command/shoot/NotePass.java index 61b2b1e7..70e2ae98 100644 --- a/src/main/java/frc/robot/command/shoot/NotePass.java +++ b/src/main/java/frc/robot/command/shoot/NotePass.java @@ -29,7 +29,7 @@ public class NotePass extends Command { private final Swerve drivetrain; - private final Flywheel flywheel; + private final Flywheel flywheel; private final Pivot pivot; private Translation2d targetPose; private double currentHeading; @@ -37,7 +37,7 @@ public class NotePass extends Command { private double feedForwardOutput; private double pidOutput; private PIDController pidController = VisionConstants.COMBO_CONTROLLER; // TAG_AIM_CONTROLLER - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25, 0.5); + private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25, 0.5); private XboxControllerFilter driver; private Indexer indexer; @@ -45,10 +45,10 @@ public class NotePass extends Command { * Creates a new NotePass. * * @param drivetrain subsystem - * @param pivot subsystem + * @param pivot subsystem * @param flywheel subsystem - * @param driver the driver's controller, used for drive input - * @param indexer subsystem + * @param driver the driver's controller, used for drive input + * @param indexer subsystem */ public NotePass(Swerve drivetrain, Flywheel flywheel, Pivot pivot, XboxControllerFilter driver, Indexer indexer) { this.drivetrain = drivetrain; @@ -62,7 +62,7 @@ public NotePass(Swerve drivetrain, Flywheel flywheel, Pivot pivot, XboxControlle @Override public void initialize() { - if(isBlueAlliance()){ + if (isBlueAlliance()) { targetPose = DrivetrainConstants.BLUE_CORNER_POSE; } else { targetPose = DrivetrainConstants.RED_CORNER_POSE; @@ -72,28 +72,28 @@ public void initialize() { @Override public void execute() { Pose2d pose = drivetrain.getPose(); - var deltaX = targetPose.getX() - pose.getX(); - var deltaY = targetPose.getY() - pose.getY(); - - currentHeading = (pose.getRotation().getDegrees() + 360) % 360; - - // Calculate vector to target, add 180 to make it point backwards - targetHeading = Math.toDegrees(Math.atan2(deltaY, deltaX)) + 180; - targetHeading = (targetHeading + 360) % 360; // Modulo 360 to keep it in the range of 0-360 - - pidOutput = pidController.calculate(currentHeading, targetHeading); - feedForwardOutput = feedforward.calculate(pidOutput); - - if(inTolerance()) { - feedForwardOutput = 0; - } - - drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), feedForwardOutput); - + var deltaX = targetPose.getX() - pose.getX(); + var deltaY = targetPose.getY() - pose.getY(); + + currentHeading = (pose.getRotation().getDegrees() + 360) % 360; + + // Calculate vector to target, add 180 to make it point backwards + targetHeading = Math.toDegrees(Math.atan2(deltaY, deltaX)) + 180; + targetHeading = (targetHeading + 360) % 360; // Modulo 360 to keep it in the range of 0-360 + + pidOutput = pidController.calculate(currentHeading, targetHeading); + feedForwardOutput = feedforward.calculate(pidOutput); + + if (inTolerance()) { + feedForwardOutput = 0; + } + + drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), feedForwardOutput); + flywheel.setAllMotorsRPM(ShooterConstants.NOTEPASS_SPEED_MAP.get(drivetrain.distanceToCorner()) + flywheel.getBias()); pivot.setTargetAngle(ShooterConstants.NOTEPASS_ANGLE_MAP.get(drivetrain.distanceToCorner()) + pivot.getBias()); - if (flywheel.allMotorsOnTarget() && pivot.onTarget() /*&& inTolerance() */) { + if (flywheel.allMotorsOnTarget() && pivot.onTarget() && inTolerance()) { indexer.indexUp(); new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); @@ -120,13 +120,13 @@ public boolean isFinished() { } private boolean isBlueAlliance() { - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() && alliance.get() == Alliance.Blue; - } + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == Alliance.Blue; + } private boolean inTolerance() { - double difference = Math.abs(currentHeading - targetHeading); - difference = difference > 180 ? 360 - difference : difference; - return difference <= VisionConstants.POINTATPOINT_ALIGNMENT_TOLERANCE; // TODO not has Target but if the correct filter is set - } + double difference = Math.abs(currentHeading - targetHeading); + difference = difference > 180 ? 360 - difference : difference; + return difference <= VisionConstants.POINTATPOINT_ALIGNMENT_TOLERANCE; // TODO not has Target but if the correct filter is set + } } From ce683dbd780ad67518bdbab593cbe7c0d360069e Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 13 Apr 2024 12:35:27 -0400 Subject: [PATCH 2/6] [#536] 3-SB-[F4-C3]-EC --- .pathplanner/settings.json | 3 +- .../{2-SB-[C3]-EC.auto => 2-SB-[C3]-EF.auto} | 2 +- ...3-SF-[C2-F3]-EC.auto => 2-SB-[F4]-EC.auto} | 0 .../pathplanner/autos/2-SB-[F4]-EF.auto | 2 +- .../pathplanner/autos/2-SF-[C2]-EC.auto | 35 +- .../pathplanner/autos/2-ST-[C1]-EC.auto | 34 +- .../pathplanner/autos/3-B-[F5-F4]-EC.auto | 338 ------------------ ...F3]-EC.auto => 3-SB-[C1-C2-C3-F1]-EF.auto} | 0 ...C1-F1]-EF.auto => 3-SB-[C1-C2-C3]-EF.auto} | 0 ...C3-F3]-EC.auto => 3-SB-[C2-C3-F3]-EF.auto} | 0 ...F-[C2-C3]-EC.auto => 3-SB-[C2-C3]-EC.auto} | 0 ...F1-F2]-EF.auto => 4-SB-[C2-C3-F3]-EF.auto} | 145 +++----- .../deploy/pathplanner/autos/Point-Blank.auto | 2 +- .../deploy/pathplanner/autos/Smart-Shoot.auto | 2 +- src/main/deploy/pathplanner/autos/nuh-uh.auto | 2 +- .../deploy/pathplanner/autos/womp-womp.auto | 2 +- src/main/deploy/pathplanner/paths/BB-SB.path | 8 +- src/main/deploy/pathplanner/paths/BF-SB.path | 8 +- src/main/deploy/pathplanner/paths/BS-C3.path | 58 +++ .../paths/{F3-SFS.path => C3-SB.path} | 22 +- src/main/deploy/pathplanner/paths/F4-BS.path | 2 +- .../pathplanner/paths/Linked Point.path | 24 +- src/main/deploy/pathplanner/paths/SB-BF.path | 8 +- src/main/deploy/pathplanner/paths/SB-C2.path | 8 +- src/main/deploy/pathplanner/paths/SB-C3.path | 8 +- src/main/deploy/pathplanner/paths/SB-F3.path | 8 +- src/main/deploy/pathplanner/paths/SB-F4.path | 14 +- src/main/deploy/pathplanner/paths/SB-F5.path | 8 +- src/main/deploy/pathplanner/paths/SF-C3.path | 2 +- src/main/deploy/pathplanner/paths/ST-C1.path | 14 +- src/main/deploy/pathplanner/paths/ST-F1.path | 8 +- src/main/deploy/pathplanner/paths/ST-TL.path | 8 +- src/main/deploy/pathplanner/paths/TF-T.path | 8 +- src/main/java/frc/robot/RobotContainer.java | 26 +- .../java/frc/robot/subsystems/Collector.java | 9 + src/main/java/frc/robot/subsystems/LEDs.java | 6 +- 36 files changed, 276 insertions(+), 548 deletions(-) rename src/main/deploy/pathplanner/autos/{2-SB-[C3]-EC.auto => 2-SB-[C3]-EF.auto} (99%) rename src/main/deploy/pathplanner/autos/{3-SF-[C2-F3]-EC.auto => 2-SB-[F4]-EC.auto} (100%) delete mode 100644 src/main/deploy/pathplanner/autos/3-B-[F5-F4]-EC.auto rename src/main/deploy/pathplanner/autos/{4-SB-[C3-C2-F3]-EC.auto => 3-SB-[C1-C2-C3-F1]-EF.auto} (100%) rename src/main/deploy/pathplanner/autos/{4-SB-[C3-C2-C1-F1]-EF.auto => 3-SB-[C1-C2-C3]-EF.auto} (100%) rename src/main/deploy/pathplanner/autos/{3-SF-[C2-C3-F3]-EC.auto => 3-SB-[C2-C3-F3]-EF.auto} (100%) rename src/main/deploy/pathplanner/autos/{3-SF-[C2-C3]-EC.auto => 3-SB-[C2-C3]-EC.auto} (100%) rename src/main/deploy/pathplanner/autos/{3-ST-[F1-F2]-EF.auto => 4-SB-[C2-C3-F3]-EF.auto} (56%) create mode 100644 src/main/deploy/pathplanner/paths/BS-C3.path rename src/main/deploy/pathplanner/paths/{F3-SFS.path => C3-SB.path} (68%) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index cf941033..633c3b0d 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,7 +10,8 @@ "Top side paths" ], "autoFolders": [ - "Point Blank" + "Hide", + "Good" ], "defaultMaxVel": 5.0, "defaultMaxAccel": 4.0, diff --git a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto rename to src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto index 8ff90a92..66ca1fc2 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto @@ -121,6 +121,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto index 7efbcaff..0d51980d 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto @@ -103,6 +103,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto index 8af55fba..f1dd5869 100644 --- a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto @@ -18,7 +18,32 @@ { "type": "wait", "data": { - "waitTime": 4.0 + "waitTime": 6.0 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Enable-Vision" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Stop-Flywheel" } } ] @@ -39,6 +64,12 @@ "data": { "name": "Smart-Collect" } + }, + { + "type": "named", + "data": { + "name": "Stop-Flywheel" + } } ] } @@ -65,6 +96,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto index aaef91c9..68e4f7e8 100644 --- a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto @@ -24,20 +24,26 @@ ] } }, + { + "type": "named", + "data": { + "name": "Enable-Vision" + } + }, { "type": "deadline", "data": { "commands": [ { - "type": "path", + "type": "wait", "data": { - "pathName": "ST-C1" + "waitTime": 1.0 } }, { "type": "named", "data": { - "name": "Smart-Collect" + "name": "Stop-Flywheel" } } ] @@ -48,26 +54,38 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Point-At-Speaker" + "pathName": "ST-C1" } }, { "type": "named", "data": { - "name": "preAim" + "name": "Smart-Collect" } }, { "type": "named", "data": { - "name": "Smart-Collect" + "name": "Stop-Flywheel" } } ] } }, + { + "type": "named", + "data": { + "name": "Point-At-Speaker" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "deadline", "data": { @@ -90,6 +108,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-B-[F5-F4]-EC.auto b/src/main/deploy/pathplanner/autos/3-B-[F5-F4]-EC.auto deleted file mode 100644 index b9991797..00000000 --- a/src/main/deploy/pathplanner/autos/3-B-[F5-F4]-EC.auto +++ /dev/null @@ -1,338 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "BS-F5" - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F5-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BS-F4" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F4-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - } - ] - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4-SB-[C3-C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/4-SB-[C3-C2-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/4-SB-[C3-C2-C1-F1]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/4-SB-[C3-C2-C1-F1]-EF.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[C2-C3-F3]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SF-[C2-C3-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C2-C3-F3]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[C2-C3]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C2-C3]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/3-ST-[F1-F2]-EF.auto b/src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto similarity index 56% rename from src/main/deploy/pathplanner/autos/3-ST-[F1-F2]-EF.auto rename to src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto index 7a6f0633..51f07127 100644 --- a/src/main/deploy/pathplanner/autos/3-ST-[F1-F2]-EF.auto +++ b/src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto @@ -18,29 +18,16 @@ { "type": "wait", "data": { - "waitTime": 6.0 + "waitTime": 3.0 } } ] } }, { - "type": "deadline", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ST-F1" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Flywheel" - } - } - ] + "pathName": "SB-F4" } }, { @@ -50,64 +37,64 @@ } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "F1-TS" + "commands": [ + { + "type": "path", + "data": { + "pathName": "F4-BS" + } + } + ] } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "race", + "type": "deadline", "data": { "commands": [ { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] + "name": "Point-At-Speaker-left" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 2.0 + "name": "preAim" } } ] } }, { - "type": "deadline", + "type": "wait", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop-Drive" } } ] @@ -120,7 +107,7 @@ { "type": "path", "data": { - "pathName": "TS-F2" + "pathName": "BS-C3" } }, { @@ -141,62 +128,36 @@ { "type": "path", "data": { - "pathName": "F2-TS" + "pathName": "C3-SB" } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "race", + "type": "deadline", "data": { "commands": [ { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] + "name": "Point-At-Speaker" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 2.0 + "name": "preAim" } } ] } }, { - "type": "deadline", + "type": "wait", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] + "waitTime": 2.0 } } ] @@ -207,15 +168,15 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "TS-F2" + "name": "Smart-Shoot" } }, { "type": "named", "data": { - "name": "Stop-Flywheel" + "name": "Stop-Drive" } } ] @@ -224,6 +185,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Point-Blank.auto b/src/main/deploy/pathplanner/autos/Point-Blank.auto index 3a8351e9..26809fee 100644 --- a/src/main/deploy/pathplanner/autos/Point-Blank.auto +++ b/src/main/deploy/pathplanner/autos/Point-Blank.auto @@ -14,6 +14,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Smart-Shoot.auto b/src/main/deploy/pathplanner/autos/Smart-Shoot.auto index b94aaf61..f0bdd08b 100644 --- a/src/main/deploy/pathplanner/autos/Smart-Shoot.auto +++ b/src/main/deploy/pathplanner/autos/Smart-Shoot.auto @@ -33,6 +33,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/nuh-uh.auto b/src/main/deploy/pathplanner/autos/nuh-uh.auto index 8bb33787..109510cc 100644 --- a/src/main/deploy/pathplanner/autos/nuh-uh.auto +++ b/src/main/deploy/pathplanner/autos/nuh-uh.auto @@ -39,6 +39,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/womp-womp.auto b/src/main/deploy/pathplanner/autos/womp-womp.auto index cb9b6edd..fb67bf21 100644 --- a/src/main/deploy/pathplanner/autos/womp-womp.auto +++ b/src/main/deploy/pathplanner/autos/womp-womp.auto @@ -51,6 +51,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BB-SB.path b/src/main/deploy/pathplanner/paths/BB-SB.path index 708b60f0..06a4cac8 100644 --- a/src/main/deploy/pathplanner/paths/BB-SB.path +++ b/src/main/deploy/pathplanner/paths/BB-SB.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": { - "x": 1.4696783237842028, - "y": 2.8971303760630094 + "x": 1.1141331061329973, + "y": 2.9196157097460684 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BF-SB.path b/src/main/deploy/pathplanner/paths/BF-SB.path index 1d3f05de..d7e09cb5 100644 --- a/src/main/deploy/pathplanner/paths/BF-SB.path +++ b/src/main/deploy/pathplanner/paths/BF-SB.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": { - "x": 1.6275494087571425, - "y": 2.354448521468532 + "x": 1.2720041911059372, + "y": 2.376933855151591 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BS-C3.path b/src/main/deploy/pathplanner/paths/BS-C3.path new file mode 100644 index 00000000..6065fa75 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BS-C3.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9112722961537205, + "y": 3.6537810022838304 + }, + "prevControl": null, + "nextControl": { + "x": 1.8175856037874687, + "y": 3.876286896653679 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.75, + "y": 4.098792791023527 + }, + "prevControl": { + "x": 1.735609747966998, + "y": 3.9816844255657124 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": true + }, + "reversed": false, + "folder": "Return Paths", + "previewStartingState": { + "rotation": 1.0416266760099266, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F3-SFS.path b/src/main/deploy/pathplanner/paths/C3-SB.path similarity index 68% rename from src/main/deploy/pathplanner/paths/F3-SFS.path rename to src/main/deploy/pathplanner/paths/C3-SB.path index 7d72ce09..20bb67c2 100644 --- a/src/main/deploy/pathplanner/paths/F3-SFS.path +++ b/src/main/deploy/pathplanner/paths/C3-SB.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.279501243687172, - "y": 2.4313569347859425 + "x": 2.5292958694025494, + "y": 4.123137615644601 }, "prevControl": null, "nextControl": { - "x": 6.74488051678806, - "y": 3.457518987110047 + "x": 2.2977299021645106, + "y": 4.11050362756931 }, "isLocked": false, - "linkedName": "F4-R" + "linkedName": "C3" }, { "anchor": { - "x": 4.3135235820021185, - "y": 4.973650307660976 + "x": 2.1103565174320065, + "y": 4.133925300660873 }, "prevControl": { - "x": 5.774186683508502, - "y": 3.9382435521627808 + "x": 2.3679949214391995, + "y": 4.157346973752436 }, "nextControl": null, "isLocked": false, - "linkedName": "SFS" + "linkedName": null } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -5.194428907734747, + "rotation": -34.777831366363976, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/F4-BS.path b/src/main/deploy/pathplanner/paths/F4-BS.path index 7dff70f7..e70e96d2 100644 --- a/src/main/deploy/pathplanner/paths/F4-BS.path +++ b/src/main/deploy/pathplanner/paths/F4-BS.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "BS" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Linked Point.path b/src/main/deploy/pathplanner/paths/Linked Point.path index c0bb5451..2d779509 100644 --- a/src/main/deploy/pathplanner/paths/Linked Point.path +++ b/src/main/deploy/pathplanner/paths/Linked Point.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": { - "x": 0.455637834356241, - "y": 6.906859967572057 + "x": 1.0476544030047628, + "y": 7.084464938166614 }, "nextControl": { - "x": 0.9028367310599924, - "y": 6.614460688943504 + "x": 1.4948532997085144, + "y": 6.7920656595380615 }, "isLocked": false, "linkedName": "SubwooferTop" @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": { - "x": 0.9664642404366074, - "y": 3.6864858009336725 + "x": 0.6109190227854017, + "y": 3.7089711346167316 }, "nextControl": { - "x": 1.935734339259029, - "y": 4.405642948910615 + "x": 1.5801891216078232, + "y": 4.428128282593675 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-BF.path b/src/main/deploy/pathplanner/paths/SB-BF.path index 72214ebd..7e506e15 100644 --- a/src/main/deploy/pathplanner/paths/SB-BF.path +++ b/src/main/deploy/pathplanner/paths/SB-BF.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 1.0408986583738933, - "y": 2.9860391252314047 + "x": 0.6853534407226877, + "y": 3.0085244589144637 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-C2.path b/src/main/deploy/pathplanner/paths/SB-C2.path index 01bd4df3..63ebe89f 100644 --- a/src/main/deploy/pathplanner/paths/SB-C2.path +++ b/src/main/deploy/pathplanner/paths/SB-C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 2.900385031351464, - "y": 3.7259535721709405 + "x": 2.544839813700259, + "y": 3.7484389058539995 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-C3.path b/src/main/deploy/pathplanner/paths/SB-C3.path index 2077d729..8fdce66f 100644 --- a/src/main/deploy/pathplanner/paths/SB-C3.path +++ b/src/main/deploy/pathplanner/paths/SB-C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 1.854549532589, - "y": 3.9474882553368715 + "x": 1.4990043149377943, + "y": 3.9699735890199306 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-F3.path b/src/main/deploy/pathplanner/paths/SB-F3.path index 00b46eca..87f19370 100644 --- a/src/main/deploy/pathplanner/paths/SB-F3.path +++ b/src/main/deploy/pathplanner/paths/SB-F3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 1.573608059768328, - "y": 2.9463781897786516 + "x": 1.2180628421171225, + "y": 2.9688635234617107 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-F4.path b/src/main/deploy/pathplanner/paths/SB-F4.path index 3f47249c..3b35cc08 100644 --- a/src/main/deploy/pathplanner/paths/SB-F4.path +++ b/src/main/deploy/pathplanner/paths/SB-F4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 4.1436198255133565, - "y": 1.8512344381172867 + "x": 3.866981999299233, + "y": 1.7332038087756627 }, "isLocked": false, "linkedName": "SubwooferBottom" @@ -20,8 +20,8 @@ "y": 2.318745636064738 }, "prevControl": { - "x": 5.658739990803804, - "y": 0.9251560871167382 + "x": 5.31912573097614, + "y": 0.9485777602083012 }, "nextControl": null, "isLocked": false, @@ -38,7 +38,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.0, + "velocity": 1.0, "rotation": 1.301952672578864, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/SB-F5.path b/src/main/deploy/pathplanner/paths/SB-F5.path index e7a9e8a6..e9dd9392 100644 --- a/src/main/deploy/pathplanner/paths/SB-F5.path +++ b/src/main/deploy/pathplanner/paths/SB-F5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 3.1963933156757256, - "y": 1.0421451276309737 + "x": 2.8408480980245203, + "y": 1.0646304613140327 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SF-C3.path b/src/main/deploy/pathplanner/paths/SF-C3.path index 7b7b0332..d7a96cd6 100644 --- a/src/main/deploy/pathplanner/paths/SF-C3.path +++ b/src/main/deploy/pathplanner/paths/SF-C3.path @@ -20,7 +20,7 @@ "y": 4.123137615644601 }, "prevControl": { - "x": 1.872921944042086, + "x": 1.8729219440420857, "y": 4.28954227277824 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/ST-C1.path b/src/main/deploy/pathplanner/paths/ST-C1.path index 80949365..1b28b510 100644 --- a/src/main/deploy/pathplanner/paths/ST-C1.path +++ b/src/main/deploy/pathplanner/paths/ST-C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": null, "nextControl": { - "x": 1.5770914424711733, - "y": 6.915037974220092 + "x": 1.738340399757724, + "y": 7.084464938160646 }, "isLocked": false, "linkedName": "SubwooferTop" @@ -20,8 +20,8 @@ "y": 7.007485005961003 }, "prevControl": { - "x": 2.0485713043498155, - "y": 6.887303864697819 + "x": 2.1527519978116896, + "y": 7.005529395674177 }, "nextControl": null, "isLocked": false, @@ -40,7 +40,7 @@ "goalEndState": { "velocity": 0, "rotation": 0, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": "Top side paths", diff --git a/src/main/deploy/pathplanner/paths/ST-F1.path b/src/main/deploy/pathplanner/paths/ST-F1.path index bd2c1e91..bac4341c 100644 --- a/src/main/deploy/pathplanner/paths/ST-F1.path +++ b/src/main/deploy/pathplanner/paths/ST-F1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": null, "nextControl": { - "x": 1.2252593735956716, - "y": 7.489009593403803 + "x": 1.8172759422441933, + "y": 7.66661456399836 }, "isLocked": false, "linkedName": "SubwooferTop" diff --git a/src/main/deploy/pathplanner/paths/ST-TL.path b/src/main/deploy/pathplanner/paths/ST-TL.path index d06e0488..3dd8f5d8 100644 --- a/src/main/deploy/pathplanner/paths/ST-TL.path +++ b/src/main/deploy/pathplanner/paths/ST-TL.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": null, "nextControl": { - "x": 1.7238989114161292, - "y": 6.487803446366367 + "x": 2.315915480064651, + "y": 6.665408416960925 }, "isLocked": false, "linkedName": "SubwooferTop" diff --git a/src/main/deploy/pathplanner/paths/TF-T.path b/src/main/deploy/pathplanner/paths/TF-T.path index 12441cd9..7f2bc8d3 100644 --- a/src/main/deploy/pathplanner/paths/TF-T.path +++ b/src/main/deploy/pathplanner/paths/TF-T.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": { - "x": 0.9884527461362631, - "y": 6.90685996756609 + "x": 1.5804693147847848, + "y": 7.0844649381606475 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8573a413..9d6d02c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -296,27 +296,17 @@ protected void configureButtonBindings() { .alongWith(new InstantCommand(() -> flywheel.resetBias()))); /* Other */ - new Trigger( - () -> ((limelights.getStopMe().hasTarget() || limelights.getChamps().hasTarget()) - && DriverStation.isEnabled())) + new Trigger(() -> ((limelights.getStopMe().hasTarget()) && DriverStation.isEnabled())) .whileTrue(leds.enableState(LED_STATES.HAS_VISION)); - new Trigger(() -> indexer.getEntryBeamBreakState() || indexer.getExitBeamBreakState() - || collector.getEntryBeamBreakState()) + new Trigger(() -> indexer.hasNote() && DriverStation.isEnabled()) .whileTrue(leds.enableState(LED_STATES.HAS_PIECE)) - .whileTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); - new Trigger(() -> drivetrain.isInField() && triggerInit) - .whileFalse(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() - && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) - .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> DriverStation.isDisabled() - && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) + .onTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); + + new Trigger(() -> DriverStation.isDisabled() && triggerInit + && !(limelights.getStopMe().hasTarget() || drivetrain.isInField() || drivetrain.isStable())) .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() - && (limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) + new Trigger(() -> drivetrain.isStable() && DriverStation.isDisabled() && drivetrain.isInField() + && limelights.getStopMe().hasTarget() && triggerInit) .whileTrue(leds.enableState(LED_STATES.GOOD_POSE)); triggerInit = true; diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index da096228..ba536cb7 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -10,11 +10,13 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.RobotMap.CAN; import frc.robot.Constants.RobotMap.DIO; import frc.robot.Constants; import frc.robot.Constants.CollectorConstants; import frc.thunder.hardware.ThunderBird; +import frc.thunder.shuffleboard.LightningShuffleboard; public class Collector extends SubsystemBase { @@ -27,6 +29,7 @@ public class Collector extends SubsystemBase { 0, false, false, false); private boolean hasPiece; + private double targetPower; private DoubleLogEntry collectorPowerLog; private BooleanLogEntry hasPieceLog; @@ -80,6 +83,7 @@ public void setPower(double power) { // Convert from -1,1 to RPS power = power * 100; motor.setControl(velocityVoltage.withVelocity(power)); + targetPower = power; } /** @@ -104,6 +108,11 @@ public void periodic() { public void updateLogging() { collectorPowerLog.append(motor.get()); hasPieceLog.append(hasPiece()); + + if(!DriverStation.isFMSAttached()) { + LightningShuffleboard.setDouble("Collector", "Collector power", getPower()); + LightningShuffleboard.setDouble("Collector", "Collector target power", targetPower); + } } /** diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java index 8c6b24f3..29e131b8 100644 --- a/src/main/java/frc/robot/subsystems/LEDs.java +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -145,6 +145,7 @@ public void periodic() { setStrandSingleHSV(1, 7, LEDsConstants.RED_HUE, 255, 255); } } + leds.setData(ledBuffer); } @@ -156,9 +157,7 @@ public Command enableState(LED_STATES state) { return new StartEndCommand( () -> { ledStates.put(state, true); - if (state.getPriority() > this.state.getPriority()) { - this.state = state; - } + reloadStates(); }, () -> { ledStates.put(state, false); @@ -168,7 +167,6 @@ public Command enableState(LED_STATES state) { public void setState(LED_STATES state, boolean value) { ledStates.put(state, value); - System.out.println("Setting " + state + " to " + value); } public void reloadStates() { From ce1f8caf3c50ac485b78f05ddddacb7ab8207d9e Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 13 Apr 2024 14:34:04 -0400 Subject: [PATCH 3/6] [#536] I hate path planner --- .pathplanner/settings.json | 3 +- .../pathplanner/autos/2-SB-[C3]-EF.auto | 2 +- .../pathplanner/autos/2-SF-[C2]-EC.auto | 2 +- .../pathplanner/autos/2-ST-[C1]-EC.auto | 2 +- .../autos/3-SB-[C1-C2-C3-F1]-EF.auto | 264 -------------- .../pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto | 342 ------------------ ...2-SB-[F4]-EC.auto => 3-SB-[C2-F3]-EC.auto} | 0 ...C2-C3-F3]-EF.auto => 3-SB-[F4-C3]-EC.auto} | 2 +- ...C2-C3-F3]-EF.auto => 3-SB-[F4-F3]-EC.auto} | 78 ++-- .../{2-SB-[F4]-EF.auto => 3-SB-[F4]-EC.auto} | 2 +- ...3-SB-[C2-C3]-EC.auto => 3-SB-[F4]-EF.auto} | 31 +- src/main/deploy/pathplanner/paths/BB-C3.path | 8 +- src/main/deploy/pathplanner/paths/BF-C3.path | 8 +- src/main/deploy/pathplanner/paths/BS-F3.path | 30 +- src/main/deploy/pathplanner/paths/C2-C3.path | 30 +- src/main/deploy/pathplanner/paths/C3-C2.path | 8 +- src/main/deploy/pathplanner/paths/C3-F1.path | 8 +- src/main/deploy/pathplanner/paths/C3-F2.path | 8 +- src/main/deploy/pathplanner/paths/C3-F3.path | 8 +- src/main/deploy/pathplanner/paths/C3-SB.path | 8 +- src/main/deploy/pathplanner/paths/F3-TS.path | 10 +- .../pathplanner/paths/Linked Point.path | 12 +- src/main/deploy/pathplanner/paths/SB-C3.path | 26 +- src/main/deploy/pathplanner/paths/SF-C3.path | 8 +- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/command/shoot/SmartShoot.java | 15 +- .../java/frc/robot/subsystems/Flywheel.java | 4 +- 28 files changed, 178 insertions(+), 749 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto delete mode 100644 src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto rename src/main/deploy/pathplanner/autos/{2-SB-[F4]-EC.auto => 3-SB-[C2-F3]-EC.auto} (100%) rename src/main/deploy/pathplanner/autos/{4-SB-[C2-C3-F3]-EF.auto => 3-SB-[F4-C3]-EC.auto} (99%) rename src/main/deploy/pathplanner/autos/{3-SB-[C2-C3-F3]-EF.auto => 3-SB-[F4-F3]-EC.auto} (79%) rename src/main/deploy/pathplanner/autos/{2-SB-[F4]-EF.auto => 3-SB-[F4]-EC.auto} (99%) rename src/main/deploy/pathplanner/autos/{3-SB-[C2-C3]-EC.auto => 3-SB-[F4]-EF.auto} (80%) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 633c3b0d..4f5f74f1 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,8 +10,7 @@ "Top side paths" ], "autoFolders": [ - "Hide", - "Good" + "Hide" ], "defaultMaxVel": 5.0, "defaultMaxAccel": 4.0, diff --git a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto index 66ca1fc2..8ff90a92 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto @@ -121,6 +121,6 @@ ] } }, - "folder": "Good", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto index f1dd5869..92df6a07 100644 --- a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto @@ -96,6 +96,6 @@ ] } }, - "folder": "Good", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto index 68e4f7e8..5f925bfe 100644 --- a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto @@ -108,6 +108,6 @@ ] } }, - "folder": "Good", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto deleted file mode 100644 index e3ecf6af..00000000 --- a/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto +++ /dev/null @@ -1,264 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Cand-Sub" - } - }, - { - "type": "wait", - "data": { - "waitTime": 4.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SB-C3" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Cand-3.5" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "C3-C2" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Cand-3.5" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "C2-F3" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Flywheel" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F3-TS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Cand-3.5" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto deleted file mode 100644 index 2cb13842..00000000 --- a/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto +++ /dev/null @@ -1,342 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Cand-Sub" - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SB-C3" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.1 - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "C3-C2" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.1 - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "C2-C1" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.1 - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "C1-F1" - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[C2-F3]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C2-F3]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto rename to src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto index 51f07127..4f26199f 100644 --- a/src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto @@ -185,6 +185,6 @@ ] } }, - "folder": "Good", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SB-[C2-C3-F3]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EC.auto similarity index 79% rename from src/main/deploy/pathplanner/autos/3-SB-[C2-C3-F3]-EF.auto rename to src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EC.auto index e19b8448..a61fac0e 100644 --- a/src/main/deploy/pathplanner/autos/3-SB-[C2-C3-F3]-EF.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EC.auto @@ -18,35 +18,22 @@ { "type": "wait", "data": { - "waitTime": 4.0 + "waitTime": 3.0 } } ] } }, { - "type": "deadline", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SF-C2" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] + "pathName": "SB-F4" } }, { "type": "named", "data": { - "name": "Cand-C2" + "name": "Chase-Pieces" } }, { @@ -56,13 +43,7 @@ { "type": "path", "data": { - "pathName": "C2-C3" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" + "pathName": "F4-BS" } } ] @@ -79,13 +60,13 @@ { "type": "named", "data": { - "name": "Point-At-Speaker" + "name": "Point-At-Speaker-left" } }, { "type": "named", "data": { - "name": "Smart-Collect" + "name": "preAim" } } ] @@ -94,37 +75,31 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 2.5 } } ] } }, - { - "type": "named", - "data": { - "name": "Cand-C3" - } - }, { "type": "deadline", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "C3-F3" - } - }, { "type": "named", "data": { - "name": "Stop-Flywheel" + "name": "Smart-Shoot" } } ] } }, + { + "type": "path", + "data": { + "pathName": "BS-F3" + } + }, { "type": "named", "data": { @@ -144,7 +119,7 @@ { "type": "named", "data": { - "name": "Smart-Collect" + "name": "preAim" } } ] @@ -167,7 +142,7 @@ { "type": "named", "data": { - "name": "Smart-Collect" + "name": "preAim" } } ] @@ -176,16 +151,29 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.5 } } ] } }, { - "type": "named", + "type": "deadline", "data": { - "name": "Cand-3.5" + "commands": [ + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop-Drive" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4]-EC.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto rename to src/main/deploy/pathplanner/autos/3-SB-[F4]-EC.auto index 0d51980d..7efbcaff 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4]-EC.auto @@ -103,6 +103,6 @@ ] } }, - "folder": "Good", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SB-[C2-C3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4]-EF.auto similarity index 80% rename from src/main/deploy/pathplanner/autos/3-SB-[C2-C3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[F4]-EF.auto index deb669f7..70284020 100644 --- a/src/main/deploy/pathplanner/autos/3-SB-[C2-C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4]-EF.auto @@ -5,6 +5,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Enable-Vision" + } + }, { "type": "race", "data": { @@ -43,10 +49,16 @@ ] } }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, { "type": "named", "data": { - "name": "Cand-C2" + "name": "Smart-Shoot" } }, { @@ -107,9 +119,22 @@ } }, { - "type": "named", + "type": "deadline", "data": { - "name": "Cand-C3" + "commands": [ + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop-Drive" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/paths/BB-C3.path b/src/main/deploy/pathplanner/paths/BB-C3.path index 7137fb10..e59dbbee 100644 --- a/src/main/deploy/pathplanner/paths/BB-C3.path +++ b/src/main/deploy/pathplanner/paths/BB-C3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": { - "x": 0.7828469918894099, - "y": 4.07380290159056 + "x": 0.62154604392606, + "y": 3.4170729034972855 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BF-C3.path b/src/main/deploy/pathplanner/paths/BF-C3.path index 15baf048..94420de2 100644 --- a/src/main/deploy/pathplanner/paths/BF-C3.path +++ b/src/main/deploy/pathplanner/paths/BF-C3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": { - "x": 0.9308511340515406, - "y": 4.123137615644601 + "x": 0.7695501860881908, + "y": 3.4664076175513263 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BS-F3.path b/src/main/deploy/pathplanner/paths/BS-F3.path index 959671da..5904a59b 100644 --- a/src/main/deploy/pathplanner/paths/BS-F3.path +++ b/src/main/deploy/pathplanner/paths/BS-F3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.7804749123011758, - "y": 4.141627021992783 + "x": 3.5273677394715697, + "y": 2.705203242075528 }, "prevControl": null, "nextControl": { - "x": 2.3132898240848454, - "y": 5.000051046533143 + "x": 4.487656336225653, + "y": 3.314166742456167 }, "isLocked": false, - "linkedName": "BottomLine" + "linkedName": null }, { "anchor": { - "x": 5.02403235575702, - "y": 4.193450694593698 + "x": 4.956089798056913, + "y": 4.075371117931965 }, "prevControl": { - "x": 4.185342216838281, - "y": 3.9467771243234804 + "x": 4.511078009317217, + "y": 3.9348410793825868 }, "nextControl": { - "x": 6.0208435353281695, - "y": 4.486630453291094 + "x": 5.576190448104412, + "y": 4.271192375841702 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 4.084914323674802 }, "prevControl": { - "x": 5.563418065297667, - "y": 4.459858150485532 + "x": 6.595606914466325, + "y": 4.098792791023527 }, "nextControl": null, "isLocked": false, @@ -54,13 +54,13 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 3.5, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 1.5, + "velocity": 1.0, "rotation": 0, "rotateFast": true }, diff --git a/src/main/deploy/pathplanner/paths/C2-C3.path b/src/main/deploy/pathplanner/paths/C2-C3.path index fd65532b..4792dee4 100644 --- a/src/main/deploy/pathplanner/paths/C2-C3.path +++ b/src/main/deploy/pathplanner/paths/C2-C3.path @@ -8,24 +8,40 @@ }, "prevControl": null, "nextControl": { - "x": 1.7434960996048117, - "y": 5.4821089822359825 + "x": 1.6419230556007463, + "y": 5.515804013063089 }, "isLocked": false, "linkedName": "C2" }, { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.61, + "y": 4.12 }, "prevControl": { - "x": 1.845187834519813, - "y": 4.132382318818692 + "x": 1.6536338921465275, + "y": 4.063660281386183 + }, + "nextControl": { + "x": 2.7942534209337393, + "y": 4.130854405863817 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3, + "y": 4.12 + }, + "prevControl": { + "x": 2.4031274310765443, + "y": 4.122214464115091 }, "nextControl": null, "isLocked": false, - "linkedName": "C3" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/C3-C2.path b/src/main/deploy/pathplanner/paths/C3-C2.path index 3f353069..a17fc995 100644 --- a/src/main/deploy/pathplanner/paths/C3-C2.path +++ b/src/main/deploy/pathplanner/paths/C3-C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": null, "nextControl": { - "x": 1.513990998898458, - "y": 4.123137615644601 + "x": 1.352690050935108, + "y": 3.4664076175513263 }, "isLocked": false, "linkedName": "C3" diff --git a/src/main/deploy/pathplanner/paths/C3-F1.path b/src/main/deploy/pathplanner/paths/C3-F1.path index 9bf53c22..77a4b122 100644 --- a/src/main/deploy/pathplanner/paths/C3-F1.path +++ b/src/main/deploy/pathplanner/paths/C3-F1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": null, "nextControl": { - "x": 2.479961155348506, - "y": 6.816813002995374 + "x": 2.318660207385156, + "y": 6.1600830049021 }, "isLocked": false, "linkedName": "C3" diff --git a/src/main/deploy/pathplanner/paths/C3-F2.path b/src/main/deploy/pathplanner/paths/C3-F2.path index 41724462..5d4a9859 100644 --- a/src/main/deploy/pathplanner/paths/C3-F2.path +++ b/src/main/deploy/pathplanner/paths/C3-F2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": null, "nextControl": { - "x": 2.9730416217589193, - "y": 7.294070804357825 + "x": 2.8117406737955695, + "y": 6.63734080626455 }, "isLocked": false, "linkedName": "C3" diff --git a/src/main/deploy/pathplanner/paths/C3-F3.path b/src/main/deploy/pathplanner/paths/C3-F3.path index eab3ced5..05cb8d0a 100644 --- a/src/main/deploy/pathplanner/paths/C3-F3.path +++ b/src/main/deploy/pathplanner/paths/C3-F3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": null, "nextControl": { - "x": 2.236602840156842, - "y": 2.6395508016873483 + "x": 3.0120909314571827, + "y": 2.623227386255057 }, "isLocked": false, "linkedName": "C3" diff --git a/src/main/deploy/pathplanner/paths/C3-SB.path b/src/main/deploy/pathplanner/paths/C3-SB.path index 20bb67c2..b7894305 100644 --- a/src/main/deploy/pathplanner/paths/C3-SB.path +++ b/src/main/deploy/pathplanner/paths/C3-SB.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": null, "nextControl": { - "x": 2.2977299021645106, - "y": 4.11050362756931 + "x": 2.1364289542011607, + "y": 3.4537736294760353 }, "isLocked": false, "linkedName": "C3" diff --git a/src/main/deploy/pathplanner/paths/F3-TS.path b/src/main/deploy/pathplanner/paths/F3-TS.path index 49e88a96..630e02d7 100644 --- a/src/main/deploy/pathplanner/paths/F3-TS.path +++ b/src/main/deploy/pathplanner/paths/F3-TS.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 3.010020434455283, - "y": 5.54 + "x": 4.089487893669082, + "y": 5.5040931765173085 }, "prevControl": { - "x": 3.373056367374508, - "y": 4.9427473361651435 + "x": 4.452523826588307, + "y": 4.906840512682452 }, "nextControl": null, "isLocked": false, - "linkedName": "C2" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Linked Point.path b/src/main/deploy/pathplanner/paths/Linked Point.path index 2d779509..7085de7b 100644 --- a/src/main/deploy/pathplanner/paths/Linked Point.path +++ b/src/main/deploy/pathplanner/paths/Linked Point.path @@ -96,16 +96,16 @@ }, { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": { - "x": 2.5136517268124985, - "y": 4.217968219382087 + "x": 2.3523507788491487, + "y": 3.561238221288812 }, "nextControl": { - "x": 2.5449400119926002, - "y": 4.028307011907109 + "x": 2.3836390640292504, + "y": 3.3715770138138343 }, "isLocked": false, "linkedName": "C3" diff --git a/src/main/deploy/pathplanner/paths/SB-C3.path b/src/main/deploy/pathplanner/paths/SB-C3.path index 8fdce66f..06eea819 100644 --- a/src/main/deploy/pathplanner/paths/SB-C3.path +++ b/src/main/deploy/pathplanner/paths/SB-C3.path @@ -12,23 +12,23 @@ "y": 3.9699735890199306 }, "isLocked": false, - "linkedName": "SubwooferBottom" + "linkedName": null }, { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.6460991383521244, + "y": 4.09478126648561 }, "prevControl": { - "x": 1.8950725343919426, - "y": 4.123137615644601 + "x": 2.0118758033415176, + "y": 4.09478126648561 }, "nextControl": { - "x": 2.7144209061955142, - "y": 4.123137615644601 + "x": 2.8312241751450893, + "y": 4.09478126648561 }, "isLocked": false, - "linkedName": "C3" + "linkedName": null }, { "anchor": { @@ -44,7 +44,13 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -55,7 +61,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": -33.32764937243033, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SF-C3.path b/src/main/deploy/pathplanner/paths/SF-C3.path index d7a96cd6..9ddb280e 100644 --- a/src/main/deploy/pathplanner/paths/SF-C3.path +++ b/src/main/deploy/pathplanner/paths/SF-C3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5292958694025494, - "y": 4.123137615644601 + "x": 2.3679949214391995, + "y": 3.4664076175513263 }, "prevControl": { - "x": 1.8729219440420857, - "y": 4.28954227277824 + "x": 1.7116209960787359, + "y": 3.6328122746849654 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 375b2a34..e0eaac8b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -36,7 +36,7 @@ public class Constants { public static final String HOOT_PATH = "U/logs"; // "/home/lvuser/logs"; public class DrivetrainConstants { - public static final double MaxSpeed = Units.feetToMeters(16.5); // 16.5 ft/s to meters per second top speed (5.0292m/s) + public static final double MaxSpeed = Units.feetToMeters(18); // 16.5 ft/s to meters per second top speed (5.0292m/s) private static final double WHEELBASE = TunerConstants.kFrontLeftXPosInches * 2; // 2 * x distance from center of robot to wheel public static final double MaxAngularRate = 2 * Math.PI * ( // convert to radians per second TunerConstants.kSpeedAt12VoltsMps / Math.PI * Math.sqrt(2 * Math.pow(WHEELBASE, 2))); // free speed / circumference of circle with radius of wheelbase @@ -182,7 +182,7 @@ public static class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final double kSlipCurrentA = 50d; + private static final double kSlipCurrentA = 60d; // Theoretical free speed (m/s) at 12v applied output; // This needs to be tuned to your individual robot @@ -547,6 +547,7 @@ public class ShooterConstants { put(1.64d, 45d); put(2d, 41d); put(2.5d, 36d); + put(2.9d, 33.5d); put(3d, 32.5d); put(3.51d, 29.5d); put(4.02d, 28.25d); @@ -564,6 +565,7 @@ public class ShooterConstants { put(1.64d, 2500d); put(2d, 2500d); put(2.5d, 3200d); + put(2.9d, 3550d); put(3d, 3800d); put(3.51d, 4000d); put(4.02d, 4300d); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9d6d02c3..04770d91 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -300,7 +300,7 @@ protected void configureButtonBindings() { .whileTrue(leds.enableState(LED_STATES.HAS_VISION)); new Trigger(() -> indexer.hasNote() && DriverStation.isEnabled()) .whileTrue(leds.enableState(LED_STATES.HAS_PIECE)) - .onTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); + .whileTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); new Trigger(() -> DriverStation.isDisabled() && triggerInit && !(limelights.getStopMe().hasTarget() || drivetrain.isInField() || drivetrain.isStable())) diff --git a/src/main/java/frc/robot/command/shoot/SmartShoot.java b/src/main/java/frc/robot/command/shoot/SmartShoot.java index 240ece8c..0e1f2dad 100644 --- a/src/main/java/frc/robot/command/shoot/SmartShoot.java +++ b/src/main/java/frc/robot/command/shoot/SmartShoot.java @@ -41,15 +41,14 @@ public class SmartShoot extends Command { /** * SmartShoot to control flywheel, pivot, drivetrain, and indexer * - * @param flywheel subsystem to set target RPM - * @param pivot subsystem to set target angle + * @param flywheel subsystem to set target RPM + * @param pivot subsystem to set target angle * @param drivetrain subsystem to get distance from speaker - * @param indexer subsystem to set power - * @param collector subsystem to set power - * @param leds subsystem to provide driver feedback + * @param indexer subsystem to set power + * @param collector subsystem to set power + * @param leds subsystem to provide driver feedback */ - public SmartShoot(Flywheel flywheel, Pivot pivot, Swerve drivetrain, Indexer indexer, Collector collector, - LEDs leds) { + public SmartShoot(Flywheel flywheel, Pivot pivot, Swerve drivetrain, Indexer indexer, Collector collector, LEDs leds) { this.flywheel = flywheel; this.pivot = pivot; this.drivetrain = drivetrain; @@ -177,4 +176,4 @@ public double calculateTargetRPM(double distance) { } return ShooterConstants.STEALTH_SPEED_MAP.get(distance); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Flywheel.java b/src/main/java/frc/robot/subsystems/Flywheel.java index 95c524eb..695596cb 100644 --- a/src/main/java/frc/robot/subsystems/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/Flywheel.java @@ -99,8 +99,8 @@ private void initLogging() { @Override public void periodic() { if (coast) { - bottomMotor.set(0d); - topMotor.set(0d); + applyPowerTop(-8.33d); + applyPowerBottom(-8.33d); } else { applyPowerTop(topTargetRPS + bias); applyPowerBottom(bottomTargetRPS + bias); From 3bb2d08ba4286cc9e906c779c39ddf7f4a2b7073 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 13 Apr 2024 14:39:57 -0400 Subject: [PATCH 4/6] [#536] Names fixed --- .../pathplanner/autos/{2-SB-[C3]-EF.auto => 2-SB-[C3]-EC.auto} | 0 .../pathplanner/autos/{3-SB-[F4]-EC.auto => 2-SB-[F4]-EF.auto} | 0 .../pathplanner/autos/{3-SB-[F4]-EF.auto => 3-SF-[C2-C3]-EC.auto} | 0 .../autos/{3-SB-[C2-F3]-EC.auto => 3-SF-[C2-F3]-EC.auto} | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename src/main/deploy/pathplanner/autos/{2-SB-[C3]-EF.auto => 2-SB-[C3]-EC.auto} (100%) rename src/main/deploy/pathplanner/autos/{3-SB-[F4]-EC.auto => 2-SB-[F4]-EF.auto} (100%) rename src/main/deploy/pathplanner/autos/{3-SB-[F4]-EF.auto => 3-SF-[C2-C3]-EC.auto} (100%) rename src/main/deploy/pathplanner/autos/{3-SB-[C2-F3]-EC.auto => 3-SF-[C2-F3]-EC.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto rename to src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SB-[F4]-EC.auto rename to src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4]-EF.auto b/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SB-[F4]-EF.auto rename to src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/3-SB-[C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SB-[C2-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto From a03918bfb61e26c4ca84c46202d8d1a30fed8883 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Sat, 13 Apr 2024 15:40:20 -0400 Subject: [PATCH 5/6] [#536] Improved drive --- .pathplanner/settings.json | 3 +- .../pathplanner/autos/2-SB-[C3]-EC.auto | 2 +- .../pathplanner/autos/2-SF-[C2]-EC.auto | 2 +- .../pathplanner/autos/2-ST-[C1]-EC.auto | 2 +- .../pathplanner/autos/3-SB-[F4-C3]-EC.auto | 2 +- .../pathplanner/autos/3-SB-[F5-F4]-EF.auto | 14 +++--- .../pathplanner/autos/3-SF-[C2-C3]-EC.auto | 2 +- .../pathplanner/autos/3-SF-[C2-F3]-EC.auto | 44 ------------------- src/main/deploy/pathplanner/paths/F5-BS.path | 12 ++--- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../java/frc/robot/subsystems/Swerve.java | 18 ++++++-- 12 files changed, 37 insertions(+), 70 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 4f5f74f1..8ac4643b 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,7 +10,8 @@ "Top side paths" ], "autoFolders": [ - "Hide" + "Hide", + "New Folder" ], "defaultMaxVel": 5.0, "defaultMaxAccel": 4.0, diff --git a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto index 8ff90a92..5aba3b7c 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto @@ -121,6 +121,6 @@ ] } }, - "folder": null, + "folder": "New Folder", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto index 92df6a07..a4b11c40 100644 --- a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto @@ -96,6 +96,6 @@ ] } }, - "folder": null, + "folder": "New Folder", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto index 5f925bfe..9a23b42f 100644 --- a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto @@ -108,6 +108,6 @@ ] } }, - "folder": null, + "folder": "New Folder", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto index 4f26199f..c2f827ce 100644 --- a/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto @@ -185,6 +185,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto index b6762627..321bf45b 100644 --- a/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto @@ -18,18 +18,12 @@ { "type": "wait", "data": { - "waitTime": 6.0 + "waitTime": 4.0 } } ] } }, - { - "type": "named", - "data": { - "name": "Bias-Down" - } - }, { "type": "path", "data": { @@ -269,6 +263,12 @@ "data": { "pathName": "BS-F4" } + }, + { + "type": "deadline", + "data": { + "commands": [] + } } ] } diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto index 70284020..cefa8866 100644 --- a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto @@ -140,6 +140,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto index dcd671e4..81356a2f 100644 --- a/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto @@ -24,12 +24,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "Bias-Down" - } - }, { "type": "deadline", "data": { @@ -49,44 +43,6 @@ ] } }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, { "type": "wait", "data": { diff --git a/src/main/deploy/pathplanner/paths/F5-BS.path b/src/main/deploy/pathplanner/paths/F5-BS.path index 1a79789b..82c9b602 100644 --- a/src/main/deploy/pathplanner/paths/F5-BS.path +++ b/src/main/deploy/pathplanner/paths/F5-BS.path @@ -20,8 +20,8 @@ "y": 4.141627021992783 }, "prevControl": { - "x": 2.3428906525172715, - "y": 3.4016063111821313 + "x": 2.034348684081985, + "y": 2.5358043023778354 }, "nextControl": null, "isLocked": false, @@ -32,18 +32,18 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0.0, "rotation": -42.0, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": "Return Paths", "previewStartingState": null, - "useDefaultConstraints": false + "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 e0eaac8b..0cdf1316 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -43,7 +43,7 @@ public class DrivetrainConstants { public static final double ROT_MULT = 0.04; // TODO Tune for Driver - public static final double SLOW_ROT_MULT = 0.3; + public static final double SLOW_ROT_MULT = 0.7; public static final double SLOW_SPEED_MULT = 0.4; public static final double SYS_TEST_SPEED_DRIVE = 0.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 04770d91..8cfa13f9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -369,8 +369,8 @@ protected void configureDefaultCommands() { /* driver */ drivetrain.registerTelemetry(logger::telemeterize); - drivetrain.setDefaultCommand(drivetrain.applyPercentRequestField(() -> -driver.getLeftY(), - () -> -driver.getLeftX(), () -> -driver.getRightX())); + drivetrain.setDefaultCommand(drivetrain.applyPercentRequestField(() -> -(driver.getLeftY() * drivetrain.getSpeedMult()), + () -> -(driver.getLeftX() * drivetrain.getSpeedMult()), () -> -(driver.getRightX() * drivetrain.getRotMult()))); // .alongWith(new CollisionDetection(drivetrain, CollisionType.TELEOP))); /* copilot */ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 57e8f616..9788b88a 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -53,7 +53,9 @@ public class Swerve extends SwerveDrivetrain implements Subsystem { private boolean disableVision = false; private boolean robotCentricControl = false; private double maxSpeed = DrivetrainConstants.MaxSpeed; + private double speedMult = 1; private double maxAngularRate = DrivetrainConstants.MaxAngularRate * DrivetrainConstants.ROT_MULT; + private double angularMult = 1; private LinearFilter xFilter = LinearFilter.singlePoleIIR(2, 0.01); private LinearFilter yFilter = LinearFilter.singlePoleIIR(2, 0.01); private LinearFilter rotFilter = LinearFilter.singlePoleIIR(2, 0.01); @@ -327,14 +329,22 @@ public boolean inSlowMode() { */ public void setSlowMode(boolean slow) { if (slow) { - maxSpeed = DrivetrainConstants.MaxSpeed * DrivetrainConstants.SLOW_SPEED_MULT; - maxAngularRate = DrivetrainConstants.MaxAngularRate * DrivetrainConstants.ROT_MULT * DrivetrainConstants.SLOW_ROT_MULT; + speedMult = DrivetrainConstants.SLOW_SPEED_MULT; + angularMult = DrivetrainConstants.SLOW_ROT_MULT; } else { - maxSpeed = DrivetrainConstants.MaxSpeed; - maxAngularRate = DrivetrainConstants.MaxAngularRate * DrivetrainConstants.ROT_MULT; + speedMult = 1; + angularMult = 1; } } + public double getSpeedMult() { + return speedMult; + } + + public double getRotMult() { + return angularMult; + } + public boolean isInField() { return field.isPoseInRegion(getPose()); } From badfe9ecf2436d1d5394e4716e5944048b6c93f6 Mon Sep 17 00:00:00 2001 From: Fritz Geib Date: Mon, 15 Apr 2024 15:20:36 -0400 Subject: [PATCH 6/6] [#543] talking is in fact hard --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8cfa13f9..7630b51d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -256,7 +256,7 @@ protected void configureButtonBindings() { new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED)); /* COPILOT */ - new Trigger(coPilot::getBButton) + new Trigger(() -> (coPilot.getBButton() && !driver.getRightBumper())) .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) .deadlineWith(leds.enableState(LED_STATES.COLLECTING))); @@ -266,7 +266,7 @@ protected void configureButtonBindings() { // .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); // cand shots for the robot - new Trigger(coPilot::getXButton) + new Trigger(() -> coPilot.getXButton() && !driver.getAButton()) .whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); // new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); // new Trigger(coPilot::getYButton).whileTrue(new NotePass(drivetrain, flywheel, pivot, driver, indexer));