From 4cea3c53d1fc75fd02f055928649799e6fae3a9f Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 13 Aug 2024 11:50:05 -0400 Subject: [PATCH 01/37] fix swerveDrive.stop(), fix naming and place SwerveDriveToShoot in correct directory --- .../stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java | 2 +- .../com/stuypulse/robot/commands/auton/UntilNoteShot.java | 5 ++--- .../commands/{shooter => swerve}/SwerveDriveToShoot.java | 3 +-- src/main/java/com/stuypulse/robot/constants/Settings.java | 2 +- .../com/stuypulse/robot/subsystems/swerve/SwerveDrive.java | 5 ++++- 5 files changed, 9 insertions(+), 8 deletions(-) rename src/main/java/com/stuypulse/robot/commands/{shooter => swerve}/SwerveDriveToShoot.java (99%) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index dc8315c..7e3e9b5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -5,7 +5,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java index 2fa0eb2..f44a66e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java @@ -1,11 +1,10 @@ package com.stuypulse.robot.commands.auton; import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.commands.swerve.SwerveDriveStop; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -13,7 +12,7 @@ public class UntilNoteShot extends SequentialCommandGroup { public UntilNoteShot(double timeout) { addCommands( - new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), + new SwerveDriveStop(), new WaitUntilCommand(Shooter.getInstance()::hasNote) .withTimeout(timeout), new IntakeStop() diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java similarity index 99% rename from src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java rename to src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java index d500dba..4eba69d 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.shooter; +package com.stuypulse.robot.commands.swerve; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings.Alignment; @@ -16,7 +16,6 @@ import com.stuypulse.stuylib.streams.numbers.filters.Derivative; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9e36938..a40a474 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -411,7 +411,7 @@ public interface Buzz { public interface Auton { double MAX_SHOT_DISTANCE = 3.1; - SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/Shoot Wait Delay", 0.45); + SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Shoot Wait Delay", 0.45); double SHOOTER_STARTUP_DELAY = 0.5; double DEFAULT_INTAKE_TIMEOUT = 0.75; diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 4a08e0a..ea031fd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -120,7 +120,10 @@ public Field2d getField() { } public void stop() { - stop(); + setControl(new SwerveRequest.FieldCentric() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(0)); } public Command followPathCommand(String pathName) { From 5bdde91126b812833288f787e72c6786f9feb67f Mon Sep 17 00:00:00 2001 From: jopy-man Date: Wed, 14 Aug 2024 16:35:14 -0400 Subject: [PATCH 02/37] changed points locations on pathplanner, fixed imports, added last path that i forgot :skull: --- .../pathplanner/paths/Point Locations.path | 28 +++++++++---------- .../commands/auton/ADEF/FivePieceADEF.java | 3 +- .../auton/BF_Series/FivePieceBFAC.java | 5 ++-- .../auton/BF_Series/FivePieceBFCA.java | 5 ++-- .../auton/BF_Series/FivePieceBFED.java | 5 ++-- .../auton/BF_Series/FivePieceBFGH.java | 5 ++-- .../commands/auton/CBA/FourPieceCBA.java | 2 +- .../commands/auton/HGF/FourPieceHGF.java | 2 +- 8 files changed, 30 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Point Locations.path b/src/main/deploy/pathplanner/paths/Point Locations.path index 9ef4882..f226216 100644 --- a/src/main/deploy/pathplanner/paths/Point Locations.path +++ b/src/main/deploy/pathplanner/paths/Point Locations.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.8606755147290057, - "y": 6.160145784763296 + "x": 2.2258235550697294, + "y": 6.68477672334809 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 5.55 }, "prevControl": { - "x": 0.23853011040910355, - "y": 5.685869940164359 + "x": 0.11367740203340196, + "y": 5.552889340951021 }, "nextControl": { - "x": 0.26146988959089645, - "y": 5.4141300598356406 + "x": 0.40237890185601805, + "y": 5.5467703476329575 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 5.55 }, "prevControl": { - "x": 2.89563193569979, - "y": 5.358936627062242 + "x": 2.904530075373245, + "y": 5.358940399662314 }, "nextControl": { - "x": 2.90436806430021, - "y": 5.741063372937758 + "x": 2.8948686180482484, + "y": 5.7664201926246115 }, "isLocked": false, "linkedName": null @@ -68,12 +68,12 @@ "y": 7.44 }, "prevControl": { - "x": 7.180548926469876, - "y": 6.917676610673163 + "x": 7.263808318646551, + "y": 6.7699486192294955 }, "nextControl": { - "x": 8.69419010200492, - "y": 7.63677199205437 + "x": 8.331646965563277, + "y": 7.474054717454673 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 7e3e9b5..83cb724 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -27,7 +27,8 @@ public FivePieceADEF(PathPlannerPath... paths) { new FollowPathAndIntake(paths[3]), new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - new FollowPathAndIntake(paths[5]) + new FollowPathAndIntake(paths[5]), + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java index 9eac536..bfb3ab0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java @@ -5,7 +5,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -28,7 +28,8 @@ public FivePieceBFAC(PathPlannerPath... paths) { new FollowPathAndIntake(paths[3]), new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - new FollowPathAndIntake(paths[5]) + new FollowPathAndIntake(paths[5]), + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java index 46740f5..b294672 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java @@ -5,7 +5,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -28,7 +28,8 @@ public FivePieceBFCA(PathPlannerPath... paths) { new FollowPathAndIntake(paths[3]), new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - new FollowPathAndIntake(paths[5]) + new FollowPathAndIntake(paths[5]), + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java index 08c6acc..8acbfdf 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java @@ -5,7 +5,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -28,7 +28,8 @@ public FivePieceBFED(PathPlannerPath... paths) { new FollowPathAndIntake(paths[3]), new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - new FollowPathAndIntake(paths[5]) + new FollowPathAndIntake(paths[5]), + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java index 6474c31..1f96687 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java @@ -5,7 +5,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -28,7 +28,8 @@ public FivePieceBFGH(PathPlannerPath... paths) { new FollowPathAndIntake(paths[3]), new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - new FollowPathAndIntake(paths[5]) + new FollowPathAndIntake(paths[5]), + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java index a25756a..152e657 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java @@ -4,7 +4,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.UntilNoteShot; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.constants.Settings.Auton; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index eacb6a3..71434d9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -4,7 +4,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.UntilNoteShot; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.constants.Settings.Auton; From 4818827e01d62a4687d6138b9e14b39b96155206 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 16 Aug 2024 11:23:13 -0400 Subject: [PATCH 03/37] changing pathplanner paths --- .../pathplanner/autos/{CBA.auto => BCA.auto} | 6 +- src/main/deploy/pathplanner/paths/A to C.path | 18 +++--- src/main/deploy/pathplanner/paths/A to D.path | 16 ++--- .../deploy/pathplanner/paths/Amp to A.path | 14 ++--- src/main/deploy/pathplanner/paths/B to A.path | 16 ++--- src/main/deploy/pathplanner/paths/B to F.path | 27 ++++---- src/main/deploy/pathplanner/paths/C to A.path | 18 +++--- src/main/deploy/pathplanner/paths/C to B.path | 16 ++--- .../deploy/pathplanner/paths/Center to B.path | 4 +- .../deploy/pathplanner/paths/Center to C.path | 16 +++-- .../pathplanner/paths/D Shoot to E.path | 12 ++-- .../deploy/pathplanner/paths/D to Shoot.path | 8 +-- .../pathplanner/paths/E Shoot to D.path | 10 +-- .../pathplanner/paths/E Shoot to F.path | 18 +++--- src/main/deploy/pathplanner/paths/E to G.path | 58 +++++++++++++++++ .../deploy/pathplanner/paths/E to Shoot.path | 8 +-- .../pathplanner/paths/F Shoot ALT to E.path | 16 +++-- .../pathplanner/paths/F Shoot to A.path | 8 +-- .../pathplanner/paths/F Shoot to C.path | 8 +-- .../pathplanner/paths/F Shoot to E.path | 16 ++--- .../pathplanner/paths/F Shoot to G.path | 22 ++++--- .../pathplanner/paths/F to A Shoot.path | 14 ++--- .../commands/auton/ADEF/FivePieceADEF.java | 1 + .../auton/ADEF/ReroutableFivePieceADEG.java | 62 +++++++++++++++++++ 24 files changed, 278 insertions(+), 134 deletions(-) rename src/main/deploy/pathplanner/autos/{CBA.auto => BCA.auto} (88%) create mode 100644 src/main/deploy/pathplanner/paths/E to G.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEG.java diff --git a/src/main/deploy/pathplanner/autos/CBA.auto b/src/main/deploy/pathplanner/autos/BCA.auto similarity index 88% rename from src/main/deploy/pathplanner/autos/CBA.auto rename to src/main/deploy/pathplanner/autos/BCA.auto index 699f42d..b9a7a01 100644 --- a/src/main/deploy/pathplanner/autos/CBA.auto +++ b/src/main/deploy/pathplanner/autos/BCA.auto @@ -14,19 +14,19 @@ { "type": "path", "data": { - "pathName": "Center to C" + "pathName": "Center to B" } }, { "type": "path", "data": { - "pathName": "C to B" + "pathName": "B to A" } }, { "type": "path", "data": { - "pathName": "B to A" + "pathName": "A to C" } } ] diff --git a/src/main/deploy/pathplanner/paths/A to C.path b/src/main/deploy/pathplanner/paths/A to C.path index 8ab12e3..406941f 100644 --- a/src/main/deploy/pathplanner/paths/A to C.path +++ b/src/main/deploy/pathplanner/paths/A to C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.49, - "y": 6.82 + "x": 2.8863114562166623, + "y": 7.01052576026283 }, "prevControl": null, "nextControl": { - "x": 1.8063404583877622, - "y": 5.684733839028691 + "x": 2.202651914604424, + "y": 5.87525959929152 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5041716843106308, - "y": 4.352132867212163 + "x": 2.8863114562166623, + "y": 4.113686110556745 }, "prevControl": { - "x": 1.7061923065438394, - "y": 5.606093593139567 + "x": 2.088332078449871, + "y": 5.367646836484149 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -35.0, + "rotationDegrees": -88.86571603713863, "rotateFast": true } ], diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path index e37f2b3..5bbf990 100644 --- a/src/main/deploy/pathplanner/paths/A to D.path +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.5387368492370914, - "y": 6.712803832043677 + "x": 2.8794368823856695, + "y": 6.997401573858207 }, "prevControl": null, "nextControl": { - "x": 3.679387291810298, - "y": 6.898225029708988 + "x": 4.020087324958876, + "y": 7.182822771523519 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.80980431566518, - "y": 7.431413130125368 + "x": 8.280832807945378, + "y": 7.446364126359206 }, "prevControl": { - "x": 5.426682819511473, - "y": 7.094030199035101 + "x": 5.883481424700945, + "y": 7.187437870039431 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Amp to A.path b/src/main/deploy/pathplanner/paths/Amp to A.path index 769f691..b2ffbb0 100644 --- a/src/main/deploy/pathplanner/paths/Amp to A.path +++ b/src/main/deploy/pathplanner/paths/Amp to A.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.26274434319702, - "y": 6.615980477868694 + "x": 1.8552696033963498, + "y": 7.114461624756583 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5146518038571792, - "y": 6.712803832043677 + "x": 2.8923206990759223, + "y": 7.002978151231599 }, "prevControl": { - "x": 1.7508529994738564, - "y": 6.559928693704114 + "x": 2.004875713620475, + "y": 6.9965362428864735 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 34.85599579668576, + "rotation": 0.9005777857670384, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index 3920a74..7e36a7c 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.403841365532434, - "y": 5.547443382833194 + "x": 2.8930898821619073, + "y": 5.547731826490439 }, "prevControl": null, "nextControl": { - "x": 2.1124651977726576, - "y": 6.184182756200334 + "x": 2.394178503014744, + "y": 6.1130813946895755 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.505085089225238, - "y": 6.749916915942463 + "x": 2.8930898821619073, + "y": 7.002641633631481 }, "prevControl": { - "x": 1.9565133270891542, - "y": 6.234996913818232 + "x": 1.734892450439663, + "y": 6.382103179012906 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path index d0cd21c..2fc835d 100644 --- a/src/main/deploy/pathplanner/paths/B to F.path +++ b/src/main/deploy/pathplanner/paths/B to F.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.54, - "y": 5.53 + "x": 2.8982818679923077, + "y": 5.535376823171801 }, "prevControl": null, "nextControl": { - "x": 3.5538181530232116, - "y": 5.137757241660317 + "x": 4.242237014979983, + "y": 4.7729721631318265 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.282130804402978, + "y": 4.107388424040241 }, "prevControl": { - "x": 5.998955035484476, - "y": 3.6317929071870045 + "x": 6.3516254804087, + "y": 3.9144676912865735 }, "nextControl": null, "isLocked": false, @@ -30,8 +30,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6, - "rotationDegrees": -24.38902882105597, + "waypointRelativePos": 0.7, + "rotationDegrees": -1.764677017473934, + "rotateFast": true + }, + { + "waypointRelativePos": 0.25, + "rotationDegrees": -28.00487762270733, "rotateFast": true } ], @@ -45,7 +50,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -19.61556919738271, + "rotation": 0.6307416329486492, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C to A.path b/src/main/deploy/pathplanner/paths/C to A.path index 3ae2554..01dba07 100644 --- a/src/main/deploy/pathplanner/paths/C to A.path +++ b/src/main/deploy/pathplanner/paths/C to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.52, - "y": 4.38 + "x": 2.887272935074144, + "y": 4.1086864204978415 }, "prevControl": null, "nextControl": { - "x": 1.9086797539294484, - "y": 5.314909721151288 + "x": 2.2759526890035926, + "y": 5.04359614164913 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.47, - "y": 6.77 + "x": 2.887272935074144, + "y": 6.9843735353393335 }, "prevControl": { - "x": 1.9818002210409187, - "y": 5.977080210298814 + "x": 2.3990731561150627, + "y": 6.191453745638148 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 1.0388554318784187, + "rotationDegrees": 91.71738197268806, "rotateFast": true } ], diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 073ac45..48a6cf4 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.51, - "y": 4.29 + "x": 2.883907759072959, + "y": 4.097244822093811 }, "prevControl": null, "nextControl": { - "x": 2.0727080470157966, - "y": 4.981565001262443 + "x": 2.4466158060887557, + "y": 4.788809823356254 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.425282344054272, - "y": 5.545520425118232 + "x": 2.883907759072959, + "y": 5.542924432203031 }, "prevControl": { - "x": 1.6559550362404298, - "y": 5.38476116014732 + "x": 1.2805456163367686, + "y": 5.77300632279836 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center to B.path b/src/main/deploy/pathplanner/paths/Center to B.path index ea98b40..f849102 100644 --- a/src/main/deploy/pathplanner/paths/Center to B.path +++ b/src/main/deploy/pathplanner/paths/Center to B.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.537823444322484, + "x": 2.900108677821523, "y": 5.5301848373414 }, "prevControl": { - "x": 2.0945336170806272, + "x": 2.456818850579666, "y": 5.562923192438648 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Center to C.path b/src/main/deploy/pathplanner/paths/Center to C.path index 190fa1d..6a6c8eb 100644 --- a/src/main/deploy/pathplanner/paths/Center to C.path +++ b/src/main/deploy/pathplanner/paths/Center to C.path @@ -16,19 +16,25 @@ }, { "anchor": { - "x": 2.509507891969653, - "y": 4.287761857703775 + "x": 2.8846769421589444, + "y": 4.107628793754611 }, "prevControl": { - "x": 2.1220319124045988, - "y": 4.703745685393156 + "x": 2.4972009625938902, + "y": 4.523612621443992 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -49.766999639914516, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/D Shoot to E.path b/src/main/deploy/pathplanner/paths/D Shoot to E.path index a269951..be0d69a 100644 --- a/src/main/deploy/pathplanner/paths/D Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/D Shoot to E.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.834129730759462, - "y": 5.769064259482685 + "x": 8.276698448858209, + "y": 5.777236829771279 }, "prevControl": { - "x": 6.47604084456681, - "y": 6.294320159324842 + "x": 6.877506341508219, + "y": 6.152405879960569 }, "nextControl": null, "isLocked": false, @@ -30,8 +30,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5, - "rotationDegrees": 0, + "waypointRelativePos": 0.45, + "rotationDegrees": -49.26302603704863, "rotateFast": true } ], diff --git a/src/main/deploy/pathplanner/paths/D to Shoot.path b/src/main/deploy/pathplanner/paths/D to Shoot.path index 63adf4e..059ffbe 100644 --- a/src/main/deploy/pathplanner/paths/D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/D to Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.7855750484566455, - "y": 7.436124376527027 + "x": 8.27660230097246, + "y": 7.439537626471086 }, "prevControl": null, "nextControl": { - "x": 6.609157592385126, - "y": 7.043167967474329 + "x": 7.10018484490094, + "y": 7.046581217418388 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/E Shoot to D.path b/src/main/deploy/pathplanner/paths/E Shoot to D.path index 71074da..a587139 100644 --- a/src/main/deploy/pathplanner/paths/E Shoot to D.path +++ b/src/main/deploy/pathplanner/paths/E Shoot to D.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.79, - "y": 7.44 + "x": 8.28015977274514, + "y": 7.438287703956361 }, "prevControl": { - "x": 6.876162419906684, - "y": 7.238618253300495 + "x": 7.3621851694026805, + "y": 7.235994284344516 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 5.550205795548943, + "rotation": -0.9439960906702931, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/E Shoot to F.path b/src/main/deploy/pathplanner/paths/E Shoot to F.path index 95aa5b2..1cdaec7 100644 --- a/src/main/deploy/pathplanner/paths/E Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/E Shoot to F.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.704247851390048, - "y": 5.800312322350837 + "x": 7.290846102339526, + "y": 6.4834430505914575 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.8917703882654795, - "y": 4.342229634980103 + "x": 8.278092593201556, + "y": 4.0919086144347885 }, "prevControl": { - "x": 6.716218263165693, - "y": 5.79459152314882 + "x": 6.759869403295355, + "y": 4.027393383097778 }, "nextControl": null, "isLocked": false, @@ -30,8 +30,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2, - "rotationDegrees": -39.907792341252474, + "waypointRelativePos": 0.7, + "rotationDegrees": -21.216895027434855, "rotateFast": true } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -31.96161156944142, + "rotation": -0.1285171141429038, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/E to G.path b/src/main/deploy/pathplanner/paths/E to G.path new file mode 100644 index 0000000..f95def7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E to G.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": null, + "nextControl": { + "x": 7.8090351325791945, + "y": 5.11184538645119 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": { + "x": 8.025271727626789, + "y": 3.749766362999988 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -74.9003182107392, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -39.63046020059535, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index 9902b87..0d1865b 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.83, - "y": 5.77 + "x": 8.292370554235157, + "y": 5.779496305086361 }, "prevControl": null, "nextControl": { - "x": 6.464262728562661, - "y": 6.586753953827849 + "x": 6.926633282797818, + "y": 6.59625025891421 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path index b0d2f33..1857ea3 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path +++ b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path @@ -16,19 +16,25 @@ }, { "anchor": { - "x": 7.83, - "y": 5.77 + "x": 8.270208466570208, + "y": 5.774448541084582 }, "prevControl": { - "x": 6.77751254033014, - "y": 6.22081510067038 + "x": 7.096098559756644, + "y": 5.919054961249804 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": -20.76066485737566, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/F Shoot to A.path b/src/main/deploy/pathplanner/paths/F Shoot to A.path index 227629f..dc2df2e 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to A.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to A.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.487778469790571, - "y": 6.823421974596926 + "x": 2.900637491193137, + "y": 6.988027154997763 }, "prevControl": { - "x": 2.094966282566494, - "y": 6.342153732484548 + "x": 1.2426152754091224, + "y": 6.554159820559224 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F Shoot to C.path b/src/main/deploy/pathplanner/paths/F Shoot to C.path index aeef997..edbee97 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5184015714013572, - "y": 4.376602504135068 + "x": 2.905012219994678, + "y": 4.103734804381811 }, "prevControl": { - "x": 1.7198933802629512, - "y": 5.131796072743931 + "x": 1.200165983851313, + "y": 4.875658105110853 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F Shoot to E.path b/src/main/deploy/pathplanner/paths/F Shoot to E.path index 1ae9e0d..9f16753 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to E.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.607152401060755, - "y": 3.1411502462141825 + "x": 5.590230373169081, + "y": 2.934143848198411 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.83, - "y": 5.77 + "x": 8.279150219944786, + "y": 5.767670115139337 }, "prevControl": { - "x": 7.661789273883607, - "y": 5.857831093630937 + "x": 7.115280062963401, + "y": 6.115533165776154 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.4, - "rotationDegrees": 17.078402120790262, + "rotationDegrees": 27.738650602589555, "rotateFast": false } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 7.5783021431903075, + "rotation": 2.0861407973812502, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/F Shoot to G.path b/src/main/deploy/pathplanner/paths/F Shoot to G.path index 6dd5ba4..6c6cc30 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to G.path @@ -8,27 +8,33 @@ }, "prevControl": null, "nextControl": { - "x": 5.415973831144092, - "y": 4.249032236330842 + "x": 6.0115504085174845, + "y": 4.516209234241385 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.93, - "y": 2.73 + "x": 8.278429110801675, + "y": 2.4413378597962554 }, "prevControl": { - "x": 6.730603199234672, - "y": 3.645327872322412 + "x": 7.286807891138104, + "y": 2.3061058584914784 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -40.973340418312205, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -38.92997350795309, + "rotation": 0.3103702110425773, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/F to A Shoot.path b/src/main/deploy/pathplanner/paths/F to A Shoot.path index 2d63eaa..dd8ff12 100644 --- a/src/main/deploy/pathplanner/paths/F to A Shoot.path +++ b/src/main/deploy/pathplanner/paths/F to A Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.278765628401793, + "y": 4.102244512152715 }, "prevControl": null, "nextControl": { - "x": 4.9685862178644005, - "y": 4.268436132668396 + "x": 5.2842878007184595, + "y": 3.8711530687570304 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 6.434696072517145 }, "prevControl": { - "x": 4.892244796580366, - "y": 4.086860850433009 + "x": 5.215157470865537, + "y": 4.363382169844697 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -28.880848497857865, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 83cb724..4ee2392 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -12,6 +12,7 @@ public class FivePieceADEF extends SequentialCommandGroup { public FivePieceADEF(PathPlannerPath... paths) { + addCommands( new ShooterScoreSpeaker(), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEG.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEG.java new file mode 100644 index 0000000..864ecd7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEG.java @@ -0,0 +1,62 @@ +package com.stuypulse.robot.commands.auton.ADEF; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.DoNothingCommand; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class ReroutableFivePieceADEG extends SequentialCommandGroup { + + public ReroutableFivePieceADEG(PathPlannerPath... paths) { + + PathPlannerPath E_TO_G = paths[7]; + + addCommands( + // Preload + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + // Intake + Shoot A + new FollowPathAndIntake(paths[0]), + new ShooterScoreSpeaker(), + + // Intake + Shoot D + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + // Intake + Shoot E + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new ConditionalCommand( + new SequentialCommandGroup( + new FollowPathAndIntake(paths[5]), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()), + new DoNothingCommand(), + Intake.getInstance()::hasNote)), + + new SequentialCommandGroup( + new FollowPathAndIntake(E_TO_G), + + new ConditionalCommand( + new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot()), + new DoNothingCommand(), + Intake.getInstance()::hasNote) + ), + Intake.getInstance()::hasNote) + ); + + } + +} From 1fece2a277fe6a2875544b92ac00b487bafdc8f5 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Mon, 19 Aug 2024 17:13:28 -0400 Subject: [PATCH 04/37] first redirection added, changed pathplanner robot locations to center of note, might change how redirection is done. experimenting with these... --- networktables.json | 1 + simgui-ds.json | 92 +++++++++++++++++++ simgui.json | 1 + src/main/deploy/pathplanner/paths/A to C.path | 16 ++-- src/main/deploy/pathplanner/paths/A to D.path | 12 +-- .../deploy/pathplanner/paths/Amp to A.path | 8 +- src/main/deploy/pathplanner/paths/B to A.path | 16 +++- src/main/deploy/pathplanner/paths/B to F.path | 12 +-- src/main/deploy/pathplanner/paths/C to A.path | 18 ++-- src/main/deploy/pathplanner/paths/C to B.path | 4 +- .../deploy/pathplanner/paths/Center to B.path | 8 +- .../deploy/pathplanner/paths/Center to C.path | 8 +- .../paths/{D to E.path => D Down.path} | 20 ++-- .../pathplanner/paths/D Shoot to E.path | 4 +- .../deploy/pathplanner/paths/D to Shoot.path | 8 +- .../pathplanner/paths/E Shoot to D.path | 16 +++- .../pathplanner/paths/E Shoot to F.path | 8 +- src/main/deploy/pathplanner/paths/E to F.path | 52 ----------- src/main/deploy/pathplanner/paths/E to G.path | 58 ------------ .../deploy/pathplanner/paths/E to Shoot.path | 4 +- .../pathplanner/paths/F Shoot ALT to E.path | 8 +- .../pathplanner/paths/F Shoot to A.path | 8 +- .../pathplanner/paths/F Shoot to C.path | 4 +- .../pathplanner/paths/F Shoot to E.path | 8 +- .../pathplanner/paths/F Shoot to G.path | 4 +- .../pathplanner/paths/F to A Shoot.path | 4 +- .../pathplanner/paths/F to C Shoot.path | 10 +- src/main/deploy/pathplanner/paths/F to E.path | 52 ----------- src/main/deploy/pathplanner/paths/F to G.path | 52 ----------- .../pathplanner/paths/F to Shoot ALT.path | 10 +- .../deploy/pathplanner/paths/F to Shoot.path | 18 ++-- .../pathplanner/paths/FA Shoot to A.path | 8 +- .../pathplanner/paths/FC Shoot to C.path | 10 +- .../pathplanner/paths/G Shoot to F.path | 16 ++-- .../pathplanner/paths/G Shoot to H.path | 12 +-- src/main/deploy/pathplanner/paths/G to F.path | 58 ------------ src/main/deploy/pathplanner/paths/G to H.path | 58 ------------ .../deploy/pathplanner/paths/G to Shoot.path | 18 ++-- .../pathplanner/paths/H Shoot to G.path | 12 +-- .../paths/{E to D.path => H Up.path} | 20 ++-- src/main/deploy/pathplanner/paths/H to G.path | 58 ------------ .../deploy/pathplanner/paths/H to Shoot.path | 22 +++-- .../pathplanner/paths/Point Locations.path | 34 +++---- .../deploy/pathplanner/paths/Source to H.path | 37 ++------ 44 files changed, 311 insertions(+), 596 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json rename src/main/deploy/pathplanner/paths/{D to E.path => D Down.path} (75%) delete mode 100644 src/main/deploy/pathplanner/paths/E to F.path delete mode 100644 src/main/deploy/pathplanner/paths/E to G.path delete mode 100644 src/main/deploy/pathplanner/paths/F to E.path delete mode 100644 src/main/deploy/pathplanner/paths/F to G.path delete mode 100644 src/main/deploy/pathplanner/paths/G to F.path delete mode 100644 src/main/deploy/pathplanner/paths/G to H.path rename src/main/deploy/pathplanner/paths/{E to D.path => H Up.path} (75%) delete mode 100644 src/main/deploy/pathplanner/paths/H to G.path diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/simgui.json @@ -0,0 +1 @@ +{} diff --git a/src/main/deploy/pathplanner/paths/A to C.path b/src/main/deploy/pathplanner/paths/A to C.path index 406941f..681e7ff 100644 --- a/src/main/deploy/pathplanner/paths/A to C.path +++ b/src/main/deploy/pathplanner/paths/A to C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.8863114562166623, - "y": 7.01052576026283 + "x": 2.9, + "y": 7.01 }, "prevControl": null, "nextControl": { - "x": 2.202651914604424, - "y": 5.87525959929152 + "x": 2.2163404583877617, + "y": 5.87473383902869 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.8863114562166623, - "y": 4.113686110556745 + "x": 2.9, + "y": 4.1 }, "prevControl": { - "x": 2.088332078449871, - "y": 5.367646836484149 + "x": 2.1020206222332085, + "y": 5.353960725927404 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path index 5bbf990..d9c2175 100644 --- a/src/main/deploy/pathplanner/paths/A to D.path +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.8794368823856695, + "x": 2.9, "y": 6.997401573858207 }, "prevControl": null, "nextControl": { - "x": 4.020087324958876, + "x": 4.040650442573206, "y": 7.182822771523519 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.280832807945378, - "y": 7.446364126359206 + "x": 8.29, + "y": 7.44 }, "prevControl": { - "x": 5.883481424700945, - "y": 7.187437870039431 + "x": 5.892648616755566, + "y": 7.181073743680225 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Amp to A.path b/src/main/deploy/pathplanner/paths/Amp to A.path index b2ffbb0..dc47433 100644 --- a/src/main/deploy/pathplanner/paths/Amp to A.path +++ b/src/main/deploy/pathplanner/paths/Amp to A.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.435487934219916, + "x": 1.44, "y": 6.595791855088175 }, "prevControl": null, "nextControl": { - "x": 1.8552696033963498, + "x": 1.8597816691764337, "y": 7.114461624756583 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.8923206990759223, + "x": 2.9, "y": 7.002978151231599 }, "prevControl": { - "x": 2.004875713620475, + "x": 2.0125550145445525, "y": 6.9965362428864735 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index 7e36a7c..bcbb7d6 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.8930898821619073, + "x": 2.9, "y": 5.547731826490439 }, "prevControl": null, "nextControl": { - "x": 2.394178503014744, + "x": 2.401088620852837, "y": 6.1130813946895755 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.8930898821619073, + "x": 2.9, "y": 7.002641633631481 }, "prevControl": { - "x": 1.734892450439663, + "x": 1.7418025682777556, "y": 6.382103179012906 }, "nextControl": null, @@ -28,7 +28,13 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 40.15509788025759, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path index 2fc835d..fc559e8 100644 --- a/src/main/deploy/pathplanner/paths/B to F.path +++ b/src/main/deploy/pathplanner/paths/B to F.path @@ -4,24 +4,24 @@ { "anchor": { "x": 2.8982818679923077, - "y": 5.535376823171801 + "y": 5.55 }, "prevControl": null, "nextControl": { "x": 4.242237014979983, - "y": 4.7729721631318265 + "y": 4.787595339960025 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.282130804402978, - "y": 4.107388424040241 + "x": 8.29, + "y": 4.1 }, "prevControl": { - "x": 6.3516254804087, - "y": 3.9144676912865735 + "x": 6.3594946760057205, + "y": 3.9070792672463317 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C to A.path b/src/main/deploy/pathplanner/paths/C to A.path index 01dba07..a6f408a 100644 --- a/src/main/deploy/pathplanner/paths/C to A.path +++ b/src/main/deploy/pathplanner/paths/C to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.887272935074144, - "y": 4.1086864204978415 + "x": 2.9, + "y": 4.1 }, "prevControl": null, "nextControl": { - "x": 2.2759526890035926, - "y": 5.04359614164913 + "x": 2.2886797539294483, + "y": 5.034909721151288 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.887272935074144, - "y": 6.9843735353393335 + "x": 2.9, + "y": 7.0 }, "prevControl": { - "x": 2.3990731561150627, - "y": 6.191453745638148 + "x": 2.4118002210409184, + "y": 6.207080210298814 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ { "waypointRelativePos": 0.5, "rotationDegrees": 91.71738197268806, - "rotateFast": true + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 48a6cf4..c8c4f4b 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.883907759072959, + "x": 2.9, "y": 4.097244822093811 }, "prevControl": null, "nextControl": { - "x": 2.4466158060887557, + "x": 2.4627080470157967, "y": 4.788809823356254 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center to B.path b/src/main/deploy/pathplanner/paths/Center to B.path index f849102..7e25762 100644 --- a/src/main/deploy/pathplanner/paths/Center to B.path +++ b/src/main/deploy/pathplanner/paths/Center to B.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.900108677821523, - "y": 5.5301848373414 + "x": 2.9, + "y": 5.55 }, "prevControl": { - "x": 2.456818850579666, - "y": 5.562923192438648 + "x": 2.456710172758143, + "y": 5.582738355097248 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Center to C.path b/src/main/deploy/pathplanner/paths/Center to C.path index 6a6c8eb..0ed52ec 100644 --- a/src/main/deploy/pathplanner/paths/Center to C.path +++ b/src/main/deploy/pathplanner/paths/Center to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.8846769421589444, - "y": 4.107628793754611 + "x": 2.9, + "y": 4.1 }, "prevControl": { - "x": 2.4972009625938902, - "y": 4.523612621443992 + "x": 2.5125240204349457, + "y": 4.5159838276893804 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/D to E.path b/src/main/deploy/pathplanner/paths/D Down.path similarity index 75% rename from src/main/deploy/pathplanner/paths/D to E.path rename to src/main/deploy/pathplanner/paths/D Down.path index 9e2d9a5..aa79312 100644 --- a/src/main/deploy/pathplanner/paths/D to E.path +++ b/src/main/deploy/pathplanner/paths/D Down.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.79, + "x": 8.29, "y": 7.44 }, "prevControl": null, "nextControl": { - "x": 7.583520266556904, - "y": 6.532141954722897 + "x": 8.29, + "y": 4.0982082285165475 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.83, - "y": 5.77 + "x": 8.29, + "y": 0.77 }, "prevControl": { - "x": 7.066677306717709, - "y": 6.05322933581135 + "x": 8.29, + "y": 4.089534458398493 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -90.0, "rotateFast": false }, "reversed": false, - "folder": "Reroutes", + "folder": null, "previewStartingState": { - "rotation": 0, + "rotation": -90.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/D Shoot to E.path b/src/main/deploy/pathplanner/paths/D Shoot to E.path index be0d69a..a6b0dc6 100644 --- a/src/main/deploy/pathplanner/paths/D Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/D Shoot to E.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.276698448858209, + "x": 8.29, "y": 5.777236829771279 }, "prevControl": { - "x": 6.877506341508219, + "x": 6.89080789265001, "y": 6.152405879960569 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/D to Shoot.path b/src/main/deploy/pathplanner/paths/D to Shoot.path index 059ffbe..3b653a7 100644 --- a/src/main/deploy/pathplanner/paths/D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/D to Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.27660230097246, - "y": 7.439537626471086 + "x": 8.29, + "y": 7.44 }, "prevControl": null, "nextControl": { - "x": 7.10018484490094, - "y": 7.046581217418388 + "x": 7.113582543928479, + "y": 7.047043590947302 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/E Shoot to D.path b/src/main/deploy/pathplanner/paths/E Shoot to D.path index a587139..c4506a0 100644 --- a/src/main/deploy/pathplanner/paths/E Shoot to D.path +++ b/src/main/deploy/pathplanner/paths/E Shoot to D.path @@ -16,19 +16,25 @@ }, { "anchor": { - "x": 8.28015977274514, - "y": 7.438287703956361 + "x": 8.29, + "y": 7.45 }, "prevControl": { - "x": 7.3621851694026805, - "y": 7.235994284344516 + "x": 7.3720253966575395, + "y": 7.247706580388155 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 35.0358419299735, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/E Shoot to F.path b/src/main/deploy/pathplanner/paths/E Shoot to F.path index 1cdaec7..a5f9537 100644 --- a/src/main/deploy/pathplanner/paths/E Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/E Shoot to F.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.278092593201556, - "y": 4.0919086144347885 + "x": 8.29, + "y": 4.1 }, "prevControl": { - "x": 6.759869403295355, - "y": 4.027393383097778 + "x": 6.771776810093797, + "y": 4.0354847686629896 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/E to F.path b/src/main/deploy/pathplanner/paths/E to F.path deleted file mode 100644 index 0611a22..0000000 --- a/src/main/deploy/pathplanner/paths/E to F.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.83, - "y": 5.77 - }, - "prevControl": null, - "nextControl": { - "x": 8.037386361231057, - "y": 5.1801103853323776 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.89, - "y": 4.34 - }, - "prevControl": { - "x": 7.447759451880511, - "y": 4.608703500331107 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -28.367522393402137, - "rotateFast": false - }, - "reversed": false, - "folder": "Reroutes", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to G.path b/src/main/deploy/pathplanner/paths/E to G.path deleted file mode 100644 index f95def7..0000000 --- a/src/main/deploy/pathplanner/paths/E to G.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.83, - "y": 5.77 - }, - "prevControl": null, - "nextControl": { - "x": 7.8090351325791945, - "y": 5.11184538645119 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.93, - "y": 2.73 - }, - "prevControl": { - "x": 8.025271727626789, - "y": 3.749766362999988 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -74.9003182107392, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -39.63046020059535, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index 0d1865b..677f283 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.292370554235157, + "x": 8.29, "y": 5.779496305086361 }, "prevControl": null, "nextControl": { - "x": 6.926633282797818, + "x": 6.92426272856266, "y": 6.59625025891421 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path index 1857ea3..ff2701e 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path +++ b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.270208466570208, - "y": 5.774448541084582 + "x": 8.29, + "y": 5.78 }, "prevControl": { - "x": 7.096098559756644, - "y": 5.919054961249804 + "x": 7.115890093186435, + "y": 5.924606420165222 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F Shoot to A.path b/src/main/deploy/pathplanner/paths/F Shoot to A.path index dc2df2e..aa8c1e5 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to A.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to A.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.900637491193137, - "y": 6.988027154997763 + "x": 2.9, + "y": 7.0 }, "prevControl": { - "x": 1.2426152754091224, - "y": 6.554159820559224 + "x": 1.2419777842159854, + "y": 6.566132665561462 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F Shoot to C.path b/src/main/deploy/pathplanner/paths/F Shoot to C.path index edbee97..a094e84 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to C.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.905012219994678, + "x": 2.9, "y": 4.103734804381811 }, "prevControl": { - "x": 1.200165983851313, + "x": 1.1951537638566347, "y": 4.875658105110853 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/F Shoot to E.path b/src/main/deploy/pathplanner/paths/F Shoot to E.path index 9f16753..ff5fe7d 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to E.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.279150219944786, - "y": 5.767670115139337 + "x": 8.29, + "y": 5.78 }, "prevControl": { - "x": 7.115280062963401, - "y": 6.115533165776154 + "x": 7.126129843018615, + "y": 6.127863050636817 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F Shoot to G.path b/src/main/deploy/pathplanner/paths/F Shoot to G.path index 6c6cc30..f34c410 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/F Shoot to G.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.278429110801675, + "x": 8.29, "y": 2.4413378597962554 }, "prevControl": { - "x": 7.286807891138104, + "x": 7.298378780336429, "y": 2.3061058584914784 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/F to A Shoot.path b/src/main/deploy/pathplanner/paths/F to A Shoot.path index dd8ff12..b72b5a3 100644 --- a/src/main/deploy/pathplanner/paths/F to A Shoot.path +++ b/src/main/deploy/pathplanner/paths/F to A Shoot.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.278765628401793, + "x": 8.29, "y": 4.102244512152715 }, "prevControl": null, "nextControl": { - "x": 5.2842878007184595, + "x": 5.295522172316666, "y": 3.8711530687570304 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F to C Shoot.path b/src/main/deploy/pathplanner/paths/F to C Shoot.path index 03d2921..1c12f0b 100644 --- a/src/main/deploy/pathplanner/paths/F to C Shoot.path +++ b/src/main/deploy/pathplanner/paths/F to C Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.29, + "y": 4.1 }, "prevControl": null, "nextControl": { - "x": 5.6385927597004, - "y": 3.2728247757462774 + "x": 6.0385927597004, + "y": 3.032824775746277 }, "isLocked": false, "linkedName": null @@ -56,7 +56,7 @@ "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -27.412130290314096, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/F to E.path b/src/main/deploy/pathplanner/paths/F to E.path deleted file mode 100644 index 586f73a..0000000 --- a/src/main/deploy/pathplanner/paths/F to E.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.89, - "y": 4.34 - }, - "prevControl": null, - "nextControl": { - "x": 7.538234612369523, - "y": 5.378126956030698 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.83, - "y": 5.77 - }, - "prevControl": { - "x": 7.287673222109838, - "y": 5.7519018618766395 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "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": "Reroutes", - "previewStartingState": { - "rotation": -29.424188910993948, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to G.path b/src/main/deploy/pathplanner/paths/F to G.path deleted file mode 100644 index 0dba821..0000000 --- a/src/main/deploy/pathplanner/paths/F to G.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.89, - "y": 4.34 - }, - "prevControl": null, - "nextControl": { - "x": 7.929652655250251, - "y": 3.767938313406389 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.93, - "y": 2.73 - }, - "prevControl": { - "x": 7.565636759807746, - "y": 2.9737087031837754 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -36.8764980084921, - "rotateFast": false - }, - "reversed": false, - "folder": "Reroutes", - "previewStartingState": { - "rotation": -28.532662399377827, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to Shoot ALT.path b/src/main/deploy/pathplanner/paths/F to Shoot ALT.path index e119541..9d0249a 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot ALT.path +++ b/src/main/deploy/pathplanner/paths/F to Shoot ALT.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.29, + "y": 4.1 }, "prevControl": null, "nextControl": { - "x": 7.037832940993264, - "y": 5.5022538765315625 + "x": 7.437832940993263, + "y": 5.262253876531562 }, "isLocked": false, "linkedName": null @@ -51,7 +51,7 @@ "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -27.35687342191869, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/F to Shoot.path b/src/main/deploy/pathplanner/paths/F to Shoot.path index 164c2fe..d147ace 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/F to Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.29, + "y": 4.102965621295826 }, "prevControl": null, "nextControl": { - "x": 5.625660869067274, - "y": 4.243149238716632 + "x": 6.0256608690672735, + "y": 4.006114860012458 }, "isLocked": false, "linkedName": null @@ -28,7 +28,13 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -19.01631267638155, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -45,7 +51,7 @@ "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -27.729825561933676, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/FA Shoot to A.path b/src/main/deploy/pathplanner/paths/FA Shoot to A.path index a52fbdf..9142313 100644 --- a/src/main/deploy/pathplanner/paths/FA Shoot to A.path +++ b/src/main/deploy/pathplanner/paths/FA Shoot to A.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.49, - "y": 6.82 + "x": 2.9, + "y": 7.0 }, "prevControl": { - "x": 2.395476499468936, - "y": 6.670691058088565 + "x": 2.8054764994689356, + "y": 6.8506910580885645 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/FC Shoot to C.path b/src/main/deploy/pathplanner/paths/FC Shoot to C.path index accb863..ef5a379 100644 --- a/src/main/deploy/pathplanner/paths/FC Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/FC Shoot to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.52, - "y": 4.35 + "x": 2.9, + "y": 4.1 }, "prevControl": { - "x": 2.368075591798505, - "y": 4.424436077294773 + "x": 2.748075591798505, + "y": 4.1744360772947715 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -28.277746130553208, + "rotation": -34.89230355927469, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/G Shoot to F.path b/src/main/deploy/pathplanner/paths/G Shoot to F.path index 65685f5..6836437 100644 --- a/src/main/deploy/pathplanner/paths/G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/G Shoot to F.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.29, + "y": 4.1 }, "prevControl": { - "x": 6.6666540280625215, - "y": 4.085370558203914 + "x": 7.066654028062521, + "y": 3.845370558203914 }, "nextControl": null, "isLocked": false, @@ -30,9 +30,9 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.4, - "rotationDegrees": -12.184647650336021, - "rotateFast": true + "waypointRelativePos": 0.7, + "rotationDegrees": 7.29052801209588, + "rotateFast": false } ], "constraintZones": [], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -27.573918256423003, + "rotation": 0.45368694477510463, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/G Shoot to H.path b/src/main/deploy/pathplanner/paths/G Shoot to H.path index 79b34d8..d78d8a2 100644 --- a/src/main/deploy/pathplanner/paths/G Shoot to H.path +++ b/src/main/deploy/pathplanner/paths/G Shoot to H.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.018108710138552, - "y": 1.1315632861920477 + "x": 8.29, + "y": 0.77 }, "prevControl": { - "x": 7.387955466945161, - "y": 2.4966709680443175 + "x": 7.6598467568066075, + "y": 2.1351076818522676 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -49.02018512209823, + "rotationDegrees": -58.70191149373465, "rotateFast": false } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -49.24596024004399, + "rotation": -56.9138613699413, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/G to F.path b/src/main/deploy/pathplanner/paths/G to F.path deleted file mode 100644 index 58f3cbd..0000000 --- a/src/main/deploy/pathplanner/paths/G to F.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.93, - "y": 2.73 - }, - "prevControl": null, - "nextControl": { - "x": 7.497323686983685, - "y": 3.4047396749927423 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.89, - "y": 4.34 - }, - "prevControl": { - "x": 7.064994718717117, - "y": 4.620818133935375 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.75, - "rotationDegrees": 0, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -26.210890099444256, - "rotateFast": false - }, - "reversed": false, - "folder": "Reroutes", - "previewStartingState": { - "rotation": -35.86786412528748, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G to H.path b/src/main/deploy/pathplanner/paths/G to H.path deleted file mode 100644 index b7848fb..0000000 --- a/src/main/deploy/pathplanner/paths/G to H.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 7.93, - "y": 2.73 - }, - "prevControl": null, - "nextControl": { - "x": 8.028733051513724, - "y": 2.080446770640565 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.03, - "y": 1.13 - }, - "prevControl": { - "x": 7.876867465974517, - "y": 1.4229875278946982 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -62.33749298525765, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -51.63406355487065, - "rotateFast": false - }, - "reversed": false, - "folder": "Reroutes", - "previewStartingState": { - "rotation": -37.64093244998374, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G to Shoot.path b/src/main/deploy/pathplanner/paths/G to Shoot.path index 2519878..287edd7 100644 --- a/src/main/deploy/pathplanner/paths/G to Shoot.path +++ b/src/main/deploy/pathplanner/paths/G to Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.93, - "y": 2.73 + "x": 8.29, + "y": 2.44 }, "prevControl": null, "nextControl": { - "x": 6.794920886590286, - "y": 3.474898154111784 + "x": 7.154920886590285, + "y": 3.184898154111784 }, "isLocked": false, "linkedName": null @@ -28,7 +28,13 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -37.74307863582837, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -45,7 +51,7 @@ "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -15.0, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/H Shoot to G.path b/src/main/deploy/pathplanner/paths/H Shoot to G.path index 3ddeed4..50e421e 100644 --- a/src/main/deploy/pathplanner/paths/H Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/H Shoot to G.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.929219989764386, - "y": 2.728964260011853 + "x": 8.287322790233379, + "y": 2.44 }, "prevControl": { - "x": 6.772224406613993, - "y": 3.2354713221331206 + "x": 7.729761200779842, + "y": 2.1500660505264464 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -25.0, + "rotationDegrees": -47.64200112069574, "rotateFast": true } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -40.317362875171, + "rotation": -0.4937166178222, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/E to D.path b/src/main/deploy/pathplanner/paths/H Up.path similarity index 75% rename from src/main/deploy/pathplanner/paths/E to D.path rename to src/main/deploy/pathplanner/paths/H Up.path index 1173976..f8ab84d 100644 --- a/src/main/deploy/pathplanner/paths/E to D.path +++ b/src/main/deploy/pathplanner/paths/H Up.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.83, - "y": 5.77 + "x": 8.29, + "y": 0.77 }, "prevControl": null, "nextControl": { - "x": 7.677216381218478, - "y": 6.4247447663422115 + "x": 8.294726177430418, + "y": 4.093543128491389 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.79, + "x": 8.29, "y": 7.44 }, "prevControl": { - "x": 7.058408588543368, - "y": 7.234165742513032 + "x": 8.285399832512846, + "y": 4.093495054548515 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 90.0, "rotateFast": false }, "reversed": false, - "folder": "Reroutes", + "folder": null, "previewStartingState": { - "rotation": 0, + "rotation": 90.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/H to G.path b/src/main/deploy/pathplanner/paths/H to G.path deleted file mode 100644 index 8949975..0000000 --- a/src/main/deploy/pathplanner/paths/H to G.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.03, - "y": 1.13 - }, - "prevControl": null, - "nextControl": { - "x": 7.191669558190307, - "y": 1.9568005895684404 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.93, - "y": 2.73 - }, - "prevControl": { - "x": 7.173353385955284, - "y": 3.0555786278983272 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.35, - "rotationDegrees": 0, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -40.39965604665211, - "rotateFast": false - }, - "reversed": false, - "folder": "Reroutes", - "previewStartingState": { - "rotation": -49.521729413623916, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H to Shoot.path b/src/main/deploy/pathplanner/paths/H to Shoot.path index 5c284ec..50f03a2 100644 --- a/src/main/deploy/pathplanner/paths/H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/H to Shoot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.03, - "y": 1.13 + "x": 8.29, + "y": 0.77 }, "prevControl": null, "nextControl": { - "x": 7.760720819990748, - "y": 2.1380393542037086 + "x": 8.020720819990748, + "y": 1.7780393542037092 }, "isLocked": false, "linkedName": null @@ -20,15 +20,21 @@ "y": 4.919501541012011 }, "prevControl": { - "x": 6.113755611067771, - "y": 4.501017868293177 + "x": 5.988426841995054, + "y": 4.331701441490681 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": -41.19150714584497, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -45,7 +51,7 @@ "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -57.0617176721166, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Point Locations.path b/src/main/deploy/pathplanner/paths/Point Locations.path index f226216..aed960c 100644 --- a/src/main/deploy/pathplanner/paths/Point Locations.path +++ b/src/main/deploy/pathplanner/paths/Point Locations.path @@ -64,15 +64,15 @@ }, { "anchor": { - "x": 8.28, + "x": 8.3, "y": 7.44 }, "prevControl": { - "x": 7.263808318646551, + "x": 7.283808318646552, "y": 6.7699486192294955 }, "nextControl": { - "x": 8.331646965563277, + "x": 8.351646965563278, "y": 7.474054717454673 }, "isLocked": false, @@ -80,31 +80,31 @@ }, { "anchor": { - "x": 8.28, - "y": 5.77 + "x": 8.29, + "y": 5.78 }, "prevControl": { - "x": 8.28838041697661, - "y": 6.311456818590719 + "x": 8.29838041697661, + "y": 6.32145681859072 }, "nextControl": { - "x": 8.269224286337115, - "y": 5.073783611933209 + "x": 8.279224286337115, + "y": 5.083783611933209 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.28, + "x": 8.29, "y": 4.1 }, "prevControl": { - "x": 8.286182106596115, + "x": 8.296182106596115, "y": 4.544857124277373 }, "nextControl": { - "x": 8.271362241199185, + "x": 8.281362241199185, "y": 3.4784370294187017 }, "isLocked": false, @@ -112,15 +112,15 @@ }, { "anchor": { - "x": 8.28, + "x": 8.29, "y": 2.44 }, "prevControl": { - "x": 8.274967786914742, + "x": 8.284967786914741, "y": 3.4775236245040944 }, "nextControl": { - "x": 8.282900374354867, + "x": 8.292900374354867, "y": 1.842011230029793 }, "isLocked": false, @@ -128,11 +128,11 @@ }, { "anchor": { - "x": 8.28, + "x": 8.29, "y": 0.77 }, "prevControl": { - "x": 8.266410625083155, + "x": 8.276410625083155, "y": 1.8410385351276657 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Source to H.path b/src/main/deploy/pathplanner/paths/Source to H.path index 57f2fda..4e0b4ae 100644 --- a/src/main/deploy/pathplanner/paths/Source to H.path +++ b/src/main/deploy/pathplanner/paths/Source to H.path @@ -16,28 +16,12 @@ }, { "anchor": { - "x": 4.19535290219204, - "y": 2.5364461465297556 + "x": 8.29, + "y": 0.77 }, "prevControl": { - "x": 3.256787052187917, - "y": 3.2931577463451562 - }, - "nextControl": { - "x": 5.156266890844794, - "y": 1.76171652938544 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.03, - "y": 1.13 - }, - "prevControl": { - "x": 6.683160416439879, - "y": 1.5490605600333218 + "x": 6.943160416439879, + "y": 1.1890605600333215 }, "nextControl": null, "isLocked": false, @@ -46,14 +30,9 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7, - "rotationDegrees": 2.3059547563747755, - "rotateFast": false - }, - { - "waypointRelativePos": 1.35, - "rotationDegrees": -23.99917974539861, - "rotateFast": false + "waypointRelativePos": 0.8, + "rotationDegrees": -36.85881982674874, + "rotateFast": true } ], "constraintZones": [], @@ -66,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -35.9402672501694, + "rotation": 1.8521041029649359, "rotateFast": false }, "reversed": false, From 3fd8e5f885634eb0fba2217d50997040d6ffd3ff Mon Sep 17 00:00:00 2001 From: jopy-man Date: Wed, 21 Aug 2024 10:02:25 -0400 Subject: [PATCH 05/37] name change --- .../auton/{CBA/FourPieceCBA.java => BCA/FourPieceBCA.java} | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) rename src/main/java/com/stuypulse/robot/commands/auton/{CBA/FourPieceCBA.java => BCA/FourPieceBCA.java} (90%) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java similarity index 90% rename from src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java rename to src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index 152e657..9c18c49 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.CBA; +package com.stuypulse.robot.commands.auton.BCA; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; @@ -12,9 +12,9 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -public class FourPieceCBA extends SequentialCommandGroup { +public class FourPieceBCA extends SequentialCommandGroup { - public FourPieceCBA(PathPlannerPath... path) { + public FourPieceBCA(PathPlannerPath... path) { addCommands( new ParallelCommandGroup( new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) From d396417336f32db1aec97d97e3e35c9a1e7ce0cb Mon Sep 17 00:00:00 2001 From: jopy-man Date: Thu, 29 Aug 2024 18:55:01 -0400 Subject: [PATCH 06/37] trying out simulation stuff --- simgui-ds.json | 13 +- simgui.json | 64 ++++- .../deploy/pathplanner/autos/BAC BLUE.auto | 37 +++ .../autos/{BCA.auto => BAC RED.auto} | 0 src/main/deploy/pathplanner/paths/A to C.path | 10 +- src/main/java/com/stuypulse/robot/Robot.java | 19 ++ .../com/stuypulse/robot/RobotContainer.java | 19 +- ...ADEG.java => ReroutableFivePieceADEF.java} | 10 +- .../FourPieceBAC.java} | 6 +- .../robot/commands/auton/UntilNoteShot.java | 19 +- .../com/stuypulse/robot/util/PathUtil.java | 220 ++++++++++++++++++ 11 files changed, 397 insertions(+), 20 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/BAC BLUE.auto rename src/main/deploy/pathplanner/autos/{BCA.auto => BAC RED.auto} (100%) rename src/main/java/com/stuypulse/robot/commands/auton/ADEF/{ReroutableFivePieceADEG.java => ReroutableFivePieceADEF.java} (88%) rename src/main/java/com/stuypulse/robot/commands/auton/{BCA/FourPieceBCA.java => BAC/FourPieceBAC.java} (90%) create mode 100644 src/main/java/com/stuypulse/robot/util/PathUtil.java diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4fb780a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,14 @@ { + "Joysticks": { + "window": { + "visible": false + } + }, + "System Joysticks": { + "window": { + "visible": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -88,5 +98,6 @@ "buttonCount": 0, "povCount": 0 } - ] + ], + "zeroDisconnectedJoysticks": false } diff --git a/simgui.json b/simgui.json index 0967ef4..f40a134 100644 --- a/simgui.json +++ b/simgui.json @@ -1 +1,63 @@ -{} +{ + "HALProvider": { + "Other Devices": { + "window": { + "visible": false + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/Pose": "Field2d", + "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/Module 0": "Mechanism2d", + "/SmartDashboard/Module 1": "Mechanism2d", + "/SmartDashboard/Module 2": "Mechanism2d", + "/SmartDashboard/Module 3": "Mechanism2d", + "/SmartDashboard/Scheduler": "Scheduler" + }, + "windows": { + "/Pose": { + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "robotPose": { + "width": 0.8679999709129333 + }, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } + }, + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "retained": { + "SmartDashboard": { + "Autonomous": { + "open": true + }, + "open": true + } + }, + "transitory": { + "Pose": { + "open": true + }, + "SmartDashboard": { + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/deploy/pathplanner/autos/BAC BLUE.auto b/src/main/deploy/pathplanner/autos/BAC BLUE.auto new file mode 100644 index 0000000..3508333 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BAC BLUE.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4053936459807443, + "y": 5.5301848373414 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "B to A" + } + }, + { + "type": "path", + "data": { + "pathName": "A to C" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BCA.auto b/src/main/deploy/pathplanner/autos/BAC RED.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/BCA.auto rename to src/main/deploy/pathplanner/autos/BAC RED.auto diff --git a/src/main/deploy/pathplanner/paths/A to C.path b/src/main/deploy/pathplanner/paths/A to C.path index 681e7ff..d8270cf 100644 --- a/src/main/deploy/pathplanner/paths/A to C.path +++ b/src/main/deploy/pathplanner/paths/A to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9, - "y": 4.1 + "x": 2.4750869488718146, + "y": 4.106330797297011 }, "prevControl": { - "x": 2.1020206222332085, - "y": 5.353960725927404 + "x": 1.6771075711050232, + "y": 5.360291523224416 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -35.01025016685515, + "rotation": 0.6983315875415402, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e7130ca..a1046e5 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -1,5 +1,7 @@ package com.stuypulse.robot; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinding; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; @@ -8,8 +10,10 @@ import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.shooter.Shooter; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -25,6 +29,7 @@ public class Robot extends TimedRobot { } private RobotContainer robot; + private CommandScheduler scheduler; private Command auto; /*************************/ @@ -33,7 +38,18 @@ public class Robot extends TimedRobot { @Override public void robotInit() { + Pathfinding.setPathfinder(new LocalADStar()); + + DataLogManager.start(); + + scheduler = CommandScheduler.getInstance(); + robot = new RobotContainer(); + + SmartDashboard.putString("Robot State", "DISABLED"); + SmartDashboard.putString("Robot", ROBOT.name()); + + SmartDashboard.putData(CommandScheduler.getInstance()); } @Override @@ -80,6 +96,9 @@ public void autonomousInit() { if (auto != null) { auto.schedule(); } + + SmartDashboard.putString("Robot State", "AUTON"); + } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 58132af..09832dc 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -12,6 +12,8 @@ import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.auton.Mobility; +import com.stuypulse.robot.commands.auton.BAC.FourPieceBAC; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeStop; @@ -46,6 +48,7 @@ import com.stuypulse.robot.subsystems.swerve.Telemetry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.subsystems.vision.NoteVision; +import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.robot.util.SLColor; import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; @@ -280,9 +283,19 @@ private void configureOperatorBindings() { /**************/ public void configureAutons() { - autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + autonChooser.addOption("Do Nothing", new DoNothingAuton()); + autonChooser.addOption("Mobility", new Mobility()); + + AutonConfig BAC = new AutonConfig("4 BAC", FourPieceBAC::new, + "Center to B", "B to A", "A to C"); + AutonConfig BAC_RED = new AutonConfig("4 BAC RED", FourPieceBAC::new, + "Center to B", "B to A", "A to C"); + + BAC.registerDefaultBlue(autonChooser); + BAC_RED.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); + } public Command getAutonomousCommand() { @@ -293,7 +306,9 @@ public static String getAutonomousCommandNameStatic() { if (autonChooser.getSelected() == null) { return "Do Nothing"; } - + return autonChooser.getSelected().getName(); + } + } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEG.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java similarity index 88% rename from src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEG.java rename to src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java index 864ecd7..37966ec 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEG.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java @@ -12,11 +12,11 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -public class ReroutableFivePieceADEG extends SequentialCommandGroup { +public class ReroutableFivePieceADEF extends SequentialCommandGroup { - public ReroutableFivePieceADEG(PathPlannerPath... paths) { + public ReroutableFivePieceADEF(PathPlannerPath... paths) { - PathPlannerPath E_TO_G = paths[7]; + PathPlannerPath D_DOWN = paths[7]; addCommands( // Preload @@ -47,7 +47,7 @@ public ReroutableFivePieceADEG(PathPlannerPath... paths) { Intake.getInstance()::hasNote)), new SequentialCommandGroup( - new FollowPathAndIntake(E_TO_G), + new FollowPathAndIntake(D_DOWN), new ConditionalCommand( new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot()), @@ -55,7 +55,7 @@ public ReroutableFivePieceADEG(PathPlannerPath... paths) { Intake.getInstance()::hasNote) ), Intake.getInstance()::hasNote) - ); + ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBAC.java similarity index 90% rename from src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java rename to src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBAC.java index 9c18c49..288627a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBAC.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.BCA; +package com.stuypulse.robot.commands.auton.BAC; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; @@ -12,9 +12,9 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; -public class FourPieceBCA extends SequentialCommandGroup { +public class FourPieceBAC extends SequentialCommandGroup { - public FourPieceBCA(PathPlannerPath... path) { + public FourPieceBAC(PathPlannerPath... path) { addCommands( new ParallelCommandGroup( new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java index f44a66e..cbffb05 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java @@ -4,17 +4,21 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveStop; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class UntilNoteShot extends SequentialCommandGroup { - public UntilNoteShot(double timeout) { + public UntilNoteShot(double delay) { addCommands( - new SwerveDriveStop(), + new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), new WaitUntilCommand(Shooter.getInstance()::hasNote) - .withTimeout(timeout), + .withTimeout(delay), new IntakeStop() ); } @@ -22,4 +26,13 @@ public UntilNoteShot(double timeout) { public UntilNoteShot() { this(Settings.Auton.SHOOT_WAIT_DELAY.get()); } + +public static Command UntilNoteShot(double timeout) { + return new SequentialCommandGroup( + new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), + new WaitCommand(timeout), + new IntakeStop() + ); + } + } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/PathUtil.java b/src/main/java/com/stuypulse/robot/util/PathUtil.java new file mode 100644 index 0000000..1f5eb70 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/PathUtil.java @@ -0,0 +1,220 @@ +package com.stuypulse.robot.util; + +import java.io.IOException; +import java.nio.file.DirectoryStream; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.Collections; +import java.util.HashMap; +import java.util.List; +import java.util.function.Function; +import java.util.stream.Collectors; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; +import com.pathplanner.lib.path.RotationTarget; +import com.stuypulse.robot.constants.Field; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; + +public class PathUtil { + public static class AutonConfig { + + private final String name; + private final Function auton; + private final String[] paths; + + public AutonConfig(String name, Function auton, String... paths) { + this.name = name; + this.auton = auton; + this.paths = paths; + + for (String path : paths) { + try { + PathPlannerPath.fromPathFile(path); + } catch (RuntimeException e) { + DriverStation.reportError("Path \"" + path + "\" not found. Did you mean \"" + PathUtil.findClosestMatch(PathUtil.getPathFileNames(), path) + "\"?", false); + + throw e; + } + } + } + + public AutonConfig registerBlue(SendableChooser chooser) { + chooser.addOption("Blue " + name, auton.apply(loadPaths(paths))); + return this; + } + + public AutonConfig registerRed(SendableChooser chooser) { + chooser.addOption("Red " + name, auton.apply(loadPathsRed(paths))); + return this; + } + + public AutonConfig registerDefaultBlue(SendableChooser chooser) { + chooser.setDefaultOption("Blue " + name, auton.apply(loadPaths(paths))); + return this; + } + + public AutonConfig registerDefaultRed(SendableChooser chooser) { + chooser.setDefaultOption("Red " + name, auton.apply(loadPathsRed(paths))); + return this; + } + + } + + /*** PATH LOADING ***/ + + public static PathPlannerPath[] loadPathsRed(String... names) { + PathPlannerPath[] output = new PathPlannerPath[names.length]; + for (int i = 0; i < names.length; i++) { + output[i] = loadRed(names[i]); + } + return output; + } + + public static PathPlannerPath[] loadPaths(String... names) { + PathPlannerPath[] output = new PathPlannerPath[names.length]; + for (int i = 0; i < names.length; i++) { + output[i] = load(names[i]); + } + return output; + } + + public static PathPlannerPath load(String name) { + return PathPlannerPath.fromPathFile(name); + } + + public static PathPlannerPath loadRed(String name) { + return flipPath(PathPlannerPath.fromPathFile(name)); + } + + + /*** PATH MIRRORING ***/ + + public static Translation2d flipFieldTranslation(Translation2d pose) { + return new Translation2d(pose.getX(), Field.WIDTH - pose.getY()); + } + + public static Rotation2d flipFieldRotation(Rotation2d rotation) { + return rotation.times(-1); + } + + public static Pose2d flipFieldPose(Pose2d pose) { + return new Pose2d( + flipFieldTranslation(pose.getTranslation()), + flipFieldRotation(pose.getRotation())); + } + + public static PathPoint flipPathPoint(PathPoint point) { + return new PathPoint( + flipFieldTranslation(point.position), + point.rotationTarget == null ? null : new RotationTarget( + point.rotationTarget.getPosition(), + flipFieldRotation(point.rotationTarget.getTarget())), + point.constraints + ); + } + + public static PathPlannerPath flipPath(PathPlannerPath path) { + List newPathPoints = path.getAllPathPoints() + .stream().map(PathUtil::flipPathPoint) + .collect(Collectors.toList()); + + GoalEndState newEndState = + new GoalEndState( + path.getGoalEndState().getVelocity(), + flipFieldRotation(path.getGoalEndState().getRotation()), + path.getGoalEndState().shouldRotateFast()); + + return PathPlannerPath.fromPathPoints( + newPathPoints, + path.getGlobalConstraints(), + newEndState + ); + } + + /*** PATH FILENAME CORRECTION ***/ + + public static List getPathFileNames() { + // ../../../../../deploy/pathplanner/paths + + Path path = Paths.get("").toAbsolutePath().resolve("src/main/deploy/pathplanner/paths"); + ArrayList fileList = new ArrayList(); + try (DirectoryStream stream = Files.newDirectoryStream(path, "*.path")) { + for (Path file : stream) { + fileList.add(file.getFileName().toString().replaceFirst(".path","")); + } + } catch (IOException error) { + DriverStation.reportError(error.getMessage(), false); + } + Collections.sort(fileList); + return fileList; + } + + public static String findClosestMatch(List paths, String input) { + double closestValue = 10.0; + String matching = ""; + + for (String fileName : paths){ + HashMap fileChars = countChars(fileName.toCharArray()); + HashMap inputChars = countChars(input.toCharArray()); + + double proximity = compareNameProximity(fileChars, inputChars); + closestValue = Math.min(proximity, closestValue); + + if (proximity == closestValue) { + matching = fileName; + } + } + + return matching; + } + + public static HashMap countChars(char[] chars) { + HashMap letterMap = new HashMap<>(); + for (char i = 'a'; i <= 'z'; i++) letterMap.put(i, 0); + for (char i = 'a'; i <= 'z'; i++) letterMap.put(i, 0); + letterMap.put('(', 0); + letterMap.put(' ', 0); + letterMap.put(')', 0); + for (char letter : chars) { + if (letterMap.containsKey(letter)) { + letterMap.put(letter, letterMap.get(letter)); + } else { + letterMap.put(letter, 1); + } + } + return letterMap; + } + + public static double compareNameProximity(HashMap list1, HashMap list2) { + double proximity = 0.0; + int list1sum = 0, list2sum = 0; + for (char key : list1.keySet()) { + if (!list2.containsKey(key)) { + proximity += 0.1; + continue; + } + proximity += 0.05 * Math.abs(list1.get(key) - list2.get(key)); + } + for (char key : list2.keySet()) { + if (!list1.containsKey(key)) { + proximity += 0.1; + continue; + } + proximity += 0.05 * Math.abs(list1.get(key) - list2.get(key)); + } + for (int count : list1.values()) list1sum += count; + for (int count : list2.values()) list2sum += count; + proximity += 0.4 * Math.abs(list2sum - list1sum); + return proximity; + } +} \ No newline at end of file From 08da4797aa5a3635c0a6a42567d1bed8f9112476 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 30 Aug 2024 18:07:46 -0400 Subject: [PATCH 07/37] auton stuff bca --- .../autos/{BAC BLUE.auto => BCA Blue.auto} | 4 +- .../autos/{BAC RED.auto => BCA Red.auto} | 4 +- src/main/deploy/pathplanner/autos/BFAC.auto | 2 +- src/main/deploy/pathplanner/paths/A to C.path | 58 ------------------- .../paths/{C to B.path => B to C.path} | 24 +++----- src/main/deploy/pathplanner/paths/C to A.path | 20 +++---- .../com/stuypulse/robot/RobotContainer.java | 14 ++--- .../commands/auton/BAC/FourPieceBAC.java | 46 --------------- .../commands/auton/BAC/FourPieceBCA.java | 46 +++++++++++++++ .../auton/FollowPathAlignAndShoot.java | 2 +- .../commands/auton/HGF/FourPieceHGF.java | 10 ++-- .../{UntilNoteShot.java => ShootRoutine.java} | 6 +- .../commands/swerve/SwerveDriveToShoot.java | 2 - .../stuypulse/robot/constants/Settings.java | 18 +++--- 14 files changed, 95 insertions(+), 161 deletions(-) rename src/main/deploy/pathplanner/autos/{BAC BLUE.auto => BCA Blue.auto} (89%) rename src/main/deploy/pathplanner/autos/{BAC RED.auto => BCA Red.auto} (88%) delete mode 100644 src/main/deploy/pathplanner/paths/A to C.path rename src/main/deploy/pathplanner/paths/{C to B.path => B to C.path} (67%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBAC.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java rename src/main/java/com/stuypulse/robot/commands/auton/{UntilNoteShot.java => ShootRoutine.java} (90%) diff --git a/src/main/deploy/pathplanner/autos/BAC BLUE.auto b/src/main/deploy/pathplanner/autos/BCA Blue.auto similarity index 89% rename from src/main/deploy/pathplanner/autos/BAC BLUE.auto rename to src/main/deploy/pathplanner/autos/BCA Blue.auto index 3508333..c03eafe 100644 --- a/src/main/deploy/pathplanner/autos/BAC BLUE.auto +++ b/src/main/deploy/pathplanner/autos/BCA Blue.auto @@ -20,13 +20,13 @@ { "type": "path", "data": { - "pathName": "B to A" + "pathName": "B to C" } }, { "type": "path", "data": { - "pathName": "A to C" + "pathName": "C to A" } } ] diff --git a/src/main/deploy/pathplanner/autos/BAC RED.auto b/src/main/deploy/pathplanner/autos/BCA Red.auto similarity index 88% rename from src/main/deploy/pathplanner/autos/BAC RED.auto rename to src/main/deploy/pathplanner/autos/BCA Red.auto index b9a7a01..6581ec2 100644 --- a/src/main/deploy/pathplanner/autos/BAC RED.auto +++ b/src/main/deploy/pathplanner/autos/BCA Red.auto @@ -20,13 +20,13 @@ { "type": "path", "data": { - "pathName": "B to A" + "pathName": "B to C Red" } }, { "type": "path", "data": { - "pathName": "A to C" + "pathName": "C to A Red" } } ] diff --git a/src/main/deploy/pathplanner/autos/BFAC.auto b/src/main/deploy/pathplanner/autos/BFAC.auto index 2293e32..e85fb93 100644 --- a/src/main/deploy/pathplanner/autos/BFAC.auto +++ b/src/main/deploy/pathplanner/autos/BFAC.auto @@ -38,7 +38,7 @@ { "type": "path", "data": { - "pathName": "A to C" + "pathName": "C to A" } } ] diff --git a/src/main/deploy/pathplanner/paths/A to C.path b/src/main/deploy/pathplanner/paths/A to C.path deleted file mode 100644 index d8270cf..0000000 --- a/src/main/deploy/pathplanner/paths/A to C.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.9, - "y": 7.01 - }, - "prevControl": null, - "nextControl": { - "x": 2.2163404583877617, - "y": 5.87473383902869 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.4750869488718146, - "y": 4.106330797297011 - }, - "prevControl": { - "x": 1.6771075711050232, - "y": 5.360291523224416 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -88.86571603713863, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.6983315875415402, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": { - "rotation": 40.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/B to C.path similarity index 67% rename from src/main/deploy/pathplanner/paths/C to B.path rename to src/main/deploy/pathplanner/paths/B to C.path index c8c4f4b..50204c9 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/B to C.path @@ -4,37 +4,31 @@ { "anchor": { "x": 2.9, - "y": 4.097244822093811 + "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 2.4627080470157967, - "y": 4.788809823356254 + "x": 1.979636893611585, + "y": 4.568946349574247 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.883907759072959, - "y": 5.542924432203031 + "x": 2.5, + "y": 4.1 }, "prevControl": { - "x": 1.2805456163367686, - "y": 5.77300632279836 + "x": 2.0252109914562095, + "y": 4.045661481389927 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0, - "rotateFast": true - } - ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -51,7 +45,7 @@ "reversed": false, "folder": "ABCDE", "previewStartingState": { - "rotation": -32.0, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/C to A.path b/src/main/deploy/pathplanner/paths/C to A.path index a6f408a..fc7aa75 100644 --- a/src/main/deploy/pathplanner/paths/C to A.path +++ b/src/main/deploy/pathplanner/paths/C to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.9, + "x": 2.5, "y": 4.1 }, "prevControl": null, "nextControl": { - "x": 2.2886797539294483, - "y": 5.034909721151288 + "x": 2.1540599070781474, + "y": 4.355657228204337 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9, - "y": 7.0 + "x": 2.8953493574769893, + "y": 6.995863207686237 }, "prevControl": { - "x": 2.4118002210409184, - "y": 6.207080210298814 + "x": 1.8478662161937423, + "y": 7.153016926941592 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 91.71738197268806, + "rotationDegrees": 63.01873139991995, "rotateFast": false } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 38.18757752913914, + "rotation": 0.6983315875415402, "rotateFast": false }, "reversed": false, "folder": "ABCDE", "previewStartingState": { - "rotation": -35.0, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a0615e4..1c1534a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,7 +13,7 @@ import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; -import com.stuypulse.robot.commands.auton.BAC.FourPieceBAC; +import com.stuypulse.robot.commands.auton.BAC.FourPieceBCA; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeAcquireForever; import com.stuypulse.robot.commands.intake.IntakeDeacquire; @@ -282,13 +282,13 @@ public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Mobility", new Mobility()); - AutonConfig BAC = new AutonConfig("4 BAC", FourPieceBAC::new, - "Center to B", "B to A", "A to C"); - AutonConfig BAC_RED = new AutonConfig("4 BAC RED", FourPieceBAC::new, - "Center to B", "B to A", "A to C"); + AutonConfig BCA = new AutonConfig("4 BCA", FourPieceBCA::new, + "Center to B", "B to C", "C to A"); + AutonConfig BCA_RED = new AutonConfig("4 BCA RED", FourPieceBCA::new, + "Center to B", "B to C", "C to A"); - BAC.registerDefaultBlue(autonChooser); - BAC_RED.registerRed(autonChooser); + BCA.registerDefaultBlue(autonChooser); + BCA_RED.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBAC.java b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBAC.java deleted file mode 100644 index 288627a..0000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBAC.java +++ /dev/null @@ -1,46 +0,0 @@ -package com.stuypulse.robot.commands.auton.BAC; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.auton.UntilNoteShot; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.constants.Settings.Auton; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class FourPieceBAC extends SequentialCommandGroup { - - public FourPieceBAC(PathPlannerPath... path) { - addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterScoreSpeaker()), - - SwerveDriveToPose.speakerRelative(-15) - .withTolerance(0.03, 0.03, 3) - ), - - new UntilNoteShot(0.7), - - new FollowPathAndIntake(path[0]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new UntilNoteShot(), - - new FollowPathAndIntake(path[1]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new UntilNoteShot(), - - new FollowPathAndIntake(path[2]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new UntilNoteShot(0.7) - ); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java new file mode 100644 index 0000000..ef321ee --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java @@ -0,0 +1,46 @@ +package com.stuypulse.robot.commands.auton.BAC; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FourPieceBCA extends SequentialCommandGroup { + + public FourPieceBCA(PathPlannerPath... path) { + addCommands( + // Preload Shot + new ArmToSubwooferShot(), + new ShooterSetRPM(Settings.Shooter.SPEAKER), + new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), + new ShooterFeederShoot(), + + // Drive to B + Shoot B + new FollowPathAndIntake(path[0]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new ShootRoutine(), + + // Drive to C + Shoot C + new FollowPathAndIntake(path[1]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new ShootRoutine(), + + // Drive to A + Shoot A + new FollowPathAndIntake(path[2]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new ShootRoutine(0.7) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java index 9fd382d..af26f18 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java @@ -36,7 +36,7 @@ public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand, boole ); if (noteShot) - addCommands(new UntilNoteShot(0.75)); + addCommands(new ShootRoutine(0.75)); else addCommands(new ShooterScoreSpeaker()); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index 71434d9..2aae170 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -2,7 +2,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.auton.UntilNoteShot; +import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; @@ -24,22 +24,22 @@ public FourPieceHGF(PathPlannerPath... paths) { .withTolerance(0.03, 0.03, 3) ), - new UntilNoteShot(0.7), + new ShootRoutine(0.7), new FollowPathAndIntake(paths[0]), new SwerveDriveToShoot() .withTolerance(0.03, 3), - new UntilNoteShot(), + new ShootRoutine(), new FollowPathAndIntake(paths[1]), new SwerveDriveToShoot() .withTolerance(0.03, 3), - new UntilNoteShot(), + new ShootRoutine(), new FollowPathAndIntake(paths[2]), new SwerveDriveToShoot() .withTolerance(0.03, 3), - new UntilNoteShot(0.7) + new ShootRoutine(0.7) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java similarity index 90% rename from src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java rename to src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java index cbffb05..58dcc12 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -12,9 +12,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class UntilNoteShot extends SequentialCommandGroup { +public class ShootRoutine extends SequentialCommandGroup { - public UntilNoteShot(double delay) { + public ShootRoutine(double delay) { addCommands( new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), new WaitUntilCommand(Shooter.getInstance()::hasNote) @@ -23,7 +23,7 @@ public UntilNoteShot(double delay) { ); } - public UntilNoteShot() { + public ShootRoutine() { this(Settings.Auton.SHOOT_WAIT_DELAY.get()); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java index 4eba69d..639b0ad 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -148,6 +148,4 @@ public void end(boolean interrupted) { SmartDashboard.putBoolean("AutonAlignment", false); } - // NEED TO CHANGE SO THAT ALLOWED SHOOT DISTANCE IS ALL THE WAY FROM ALLIANCE WING LINE AND NOT AROUND PDOIUM - } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 2393e5d..df11a84 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -238,13 +238,13 @@ public interface Turn { } public interface Turn { - SmartNumber kP = new SmartNumber("Swerve/Turn/PID/kP", 7.0); + SmartNumber kP = new SmartNumber("Swerve/Turn/PID/kP", 9.0); SmartNumber kI = new SmartNumber("Swerve/Turn/PID/kI", 0.0); - SmartNumber kD = new SmartNumber("Swerve/Turn/PID/kD", 0.0); + SmartNumber kD = new SmartNumber("Swerve/Turn/PID/kD", 0.2); - SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", 0.0); - SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 1.5); - SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", 0.0); + SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", 0.30718); + SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 1.42659); + SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", 0.0036513); boolean INVERTED = true; @@ -252,13 +252,13 @@ public interface Turn { } public interface Drive { - SmartNumber kP = new SmartNumber("Swerve/Drive/PID/kP", 3.0); + SmartNumber kP = new SmartNumber("Swerve/Drive/PID/kP", 9.0); SmartNumber kI = new SmartNumber("Swerve/Drive/PID/kI", 0); SmartNumber kD = new SmartNumber("Swerve/Drive/PID/kD", 0); - SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", 0.0); - SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", 0.0); - SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", 0.0); + SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", 0.31007); + SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", 1.62153); + SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", 0.0048373); double L2 = ((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)); // 6.74607175 double L3 = ((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 6.12244898 From 9814c181f22d5fe5bd586a4d37196e8c20710a4d Mon Sep 17 00:00:00 2001 From: Ian Shi <69531533+IanShiii@users.noreply.github.com> Date: Mon, 2 Sep 2024 18:57:12 -0400 Subject: [PATCH 08/37] 8/28 (#37) (#38) * fix camera rotations * get rid of outlier data * move automatically scheduled commands to triggers inside robot container * potential fix for having to double click to shoot * feeder runs backwards on deacquire, dont retry intake * tune amp angle, fix intake led colors * switch ferry buttons * added arm climb commands * clean up climb commands and default led instruction * use old low ferry data as lob ferry data, fix arm angles for ferrying * sysid swerve --------- Co-authored-by: Tmanxyz --- src/main/java/com/stuypulse/robot/Robot.java | 27 ----- .../com/stuypulse/robot/RobotContainer.java | 71 ++++++++---- .../robot/commands/leds/LEDDefaultMode.java | 4 + .../shooter/ShooterAcquireFromIntake.java | 25 ----- .../ShooterAcquireFromIntakeWithRetry.java | 64 +++++++++++ .../stuypulse/robot/constants/Cameras.java | 4 +- .../robot/constants/LEDInstructions.java | 6 +- .../stuypulse/robot/constants/Settings.java | 14 +-- .../util/ShooterLobFerryInterpolation.java | 106 ++++++++---------- .../util/ShooterLowFerryInterpolation.java | 82 +------------- 10 files changed, 182 insertions(+), 221 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 490a5d6..e175561 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -57,33 +57,6 @@ public void robotInit() { @Override public void robotPeriodic() { - if (Arm.getInstance().getState() == Arm.State.FEED - && Arm.getInstance().atTarget() - && !Shooter.getInstance().hasNote() - && Intake.getInstance().hasNote() - && Intake.getInstance().getState() != Intake.State.DEACQUIRING - ) { - CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake() - .andThen(new BuzzController(robot.driver))); - } - - if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED - && Arm.getInstance().atIntakeShouldShootAngle() - ) { - CommandScheduler.getInstance().schedule(new IntakeShoot() - .until( - () -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED - || !Arm.getInstance().atIntakeShouldShootAngle() - )); - } - - if (Arm.getInstance().getState() == Arm.State.AMP - && !Shooter.getInstance().hasNote() - && Shooter.getInstance().getFeederState() != Shooter.FeederState.DEACQUIRING - ) { - CommandScheduler.getInstance().schedule(new ShooterManualIntake().until(() -> Arm.getInstance().getState() != Arm.State.AMP)); - } - CommandScheduler.getInstance().run(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1c1534a..241f2a7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -24,6 +24,7 @@ import com.stuypulse.robot.commands.leds.LEDReset; import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; +import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntakeWithRetry; import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; @@ -71,6 +72,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class RobotContainer { @@ -124,10 +126,18 @@ private void configureDefaultCommands() { private void configureButtonBindings() { configureOperatorBindings(); configureDriverBindings(); + configureAutomaticCommandScheduling(); } private void configureDriverBindings() { - driver.getDPadUp().onTrue(new SwerveDriveSeedFieldRelative()); + driver.getDPadRight().onTrue(new SwerveDriveSeedFieldRelative()); + + driver.getDPadUp() + .onTrue(new ArmToPreClimb()) + .onTrue(new ShooterStop()) + .onTrue(new IntakeStop()); + + driver.getDPadDown().onTrue(new ArmToClimbing()); // intake field relative driver.getRightTriggerButton() @@ -150,23 +160,22 @@ private void configureDriverBindings() { // deacquire driver.getDPadLeft() .whileTrue(new IntakeDeacquire()) + .whileTrue(new ShooterFeederDeacquire()) .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)); // speaker align and score // score amp driver.getRightBumper() .whileTrue(new ConditionalCommand( - new ArmWaitUntilAtTarget() - .withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) - .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE))), + new SwerveDriveDrive(driver) + .alongWith(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) + .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE)))), new SwerveDriveDriveAlignedSpeaker(driver) .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), () -> Arm.getInstance().getState() == Arm.State.AMP)) @@ -182,9 +191,7 @@ private void configureDriverBindings() { .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) ) @@ -201,9 +208,7 @@ private void configureDriverBindings() { .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) ) @@ -220,9 +225,7 @@ private void configureDriverBindings() { .whileTrue(new ArmToSubwooferShot().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL)) .onFalse(new ConditionalCommand( @@ -237,9 +240,7 @@ private void configureDriverBindings() { .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) ) @@ -274,6 +275,38 @@ private void configureOperatorBindings() { } + private void configureAutomaticCommandScheduling() { + // automatic handoff + new Trigger(() -> arm.getState() == Arm.State.FEED + && arm.atTarget() + && !shooter.hasNote() + && intake.hasNote() + && intake.getState() != Intake.State.DEACQUIRING) + // .onTrue(new ShooterAcquireFromIntakeWithRetry().andThen(new BuzzController(driver))); + .onTrue(new ShooterAcquireFromIntake().andThen(new BuzzController(driver))); + + // feeder automatically pushes note further into shooter when its sticking too far out + new Trigger(() -> arm.getState() == Arm.State.AMP + && !shooter.hasNote() + && shooter.getFeederState() != Shooter.FeederState.DEACQUIRING) + .onTrue(new ShooterManualIntake().until(() -> arm.getState() != Arm.State.AMP)); + + // run the intake when the arm is moving up from a low angle (to prevent intake from gripping it) + new Trigger(() -> arm.getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED + && arm.atIntakeShouldShootAngle()) + .onTrue(new IntakeShoot() + .until( + () -> arm.getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED + || !arm.atIntakeShouldShootAngle() + ) + ); + + // run the intake when shooting in case the intake is holding onto the note also + new Trigger(() -> shooter.getFeederState() == Shooter.FeederState.SHOOTING + && arm.atIntakeShouldShootAngle()) + .onTrue(new IntakeShoot().until(() -> shooter.getFeederState() != Shooter.FeederState.SHOOTING)); + } + /**************/ /*** AUTONS ***/ /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index 2fdd384..dc7bfc6 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -39,6 +39,10 @@ public LEDDefaultMode() { private LEDInstruction getInstruction() { + if (Arm.getInstance().getState() == Arm.State.CLIMBING) { + return LEDInstructions.CLIMBING; + } + if (Arm.getInstance().getState() == Arm.State.AMP) { return LEDInstructions.ARM_AT_AMP; } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java index 95c297e..a35138c 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java @@ -12,44 +12,19 @@ public class ShooterAcquireFromIntake extends Command { private final Shooter shooter; private final Intake intake; - private final StopWatch stopWatch; - private boolean isFeeding; - public ShooterAcquireFromIntake() { shooter = Shooter.getInstance(); intake = Intake.getInstance(); - stopWatch = new StopWatch(); - isFeeding = true; - addRequirements(shooter, intake); } @Override public void initialize() { - stopWatch.reset(); intake.setState(Intake.State.FEEDING); shooter.setFeederState(Shooter.FeederState.INTAKING); } - @Override - public void execute() { - if (isFeeding) { - if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) { - intake.setState(Intake.State.DEACQUIRING); - isFeeding = false; - stopWatch.reset(); - } - } - else { - if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) { - intake.setState(Intake.State.FEEDING); - isFeeding = true; - stopWatch.reset(); - } - } - } - @Override public boolean isFinished() { return shooter.hasNote(); diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java new file mode 100644 index 0000000..3d6ceef --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java @@ -0,0 +1,64 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ShooterAcquireFromIntakeWithRetry extends Command { + + private final Shooter shooter; + private final Intake intake; + + private final StopWatch stopWatch; + private boolean isFeeding; + + public ShooterAcquireFromIntakeWithRetry() { + shooter = Shooter.getInstance(); + intake = Intake.getInstance(); + + stopWatch = new StopWatch(); + isFeeding = true; + + addRequirements(shooter, intake); + } + + @Override + public void initialize() { + stopWatch.reset(); + intake.setState(Intake.State.FEEDING); + shooter.setFeederState(Shooter.FeederState.INTAKING); + } + + @Override + public void execute() { + if (isFeeding) { + if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) { + intake.setState(Intake.State.DEACQUIRING); + isFeeding = false; + stopWatch.reset(); + } + } + else { + if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) { + intake.setState(Intake.State.FEEDING); + isFeeding = true; + stopWatch.reset(); + } + } + } + + @Override + public boolean isFinished() { + return shooter.hasNote(); + } + + @Override + public void end(boolean interrupted) { + shooter.setFeederState(Shooter.FeederState.STOP); + intake.setState(Intake.State.STOP); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 8f69290..3bc76a2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -28,7 +28,7 @@ public interface Limelight { "tower-cam", new Pose3d( new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(180)) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-15), Units.degreesToRadians(180)) ), "11", 3000 @@ -38,7 +38,7 @@ public interface Limelight { "plate-cam", new Pose3d( new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0)) + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-10), Units.degreesToRadians(0)) ), "96", 3001 diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index 3644885..9718f24 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -55,8 +55,8 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction FIELD_RELATIVE_INTAKING = new LEDRainbow(); - LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE); + LEDInstruction FIELD_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE); + LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDRainbow(); LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.GOLD); LEDInstruction SPEAKER_ALIGN = GREEN; @@ -71,6 +71,8 @@ public interface LEDInstructions { LEDInstruction ARM_AT_AMP = YELLOW; LEDInstruction AMP_SCORE = GREEN; + LEDInstruction CLIMBING = new LEDRainbow(); + LEDInstruction ATTENTION = new LED694(0.01, SLColor.BLUE); LEDInstruction CONTAINS_NOTE = RED; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index df11a84..58432d5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -66,11 +66,11 @@ public interface Arm { SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5); - SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 43); - SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50); - SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); - SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); - SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7); + SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 49); + SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", MIN_ANGLE.get()); + SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", -50); + SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 90); + SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get()); SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 0); SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 4); @@ -108,7 +108,7 @@ public interface Intake { double INTAKE_FEED_SPEED = 0.4; double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 25; - double ARM_SPEED_THRESHOLD_TO_FEED = 2.5; // degrees per second + double ARM_SPEED_THRESHOLD_TO_FEED = 1.75; // degrees per second double INTAKE_SHOOT_SPEED = 0.9; double INTAKE_SHOOT_TIME = 0.75; @@ -127,7 +127,7 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; - boolean ALWAYS_KEEP_AT_SPEED = false; + boolean ALWAYS_KEEP_AT_SPEED = true; double TARGET_RPM_THRESHOLD = 200; double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0; diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java index 5d3c1e9..6563926 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java @@ -13,67 +13,57 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - {4000, 562}, - {4000, 538}, - {4000, 578}, - {4000, 498}, - {4000, 527}, - {4000, 499}, - {4000, 481}, - {4000, 611}, - {4000, 528}, - {4000, 365}, - {4000, 487}, - {4000, 548}, - {3800, 456}, - {3800, 457}, - {3800, 515}, - {3800, 563}, - {3800, 560}, - {3800, 495}, - {3600, 519}, - {3600, 505}, - {3600, 525}, - {3600, 513}, - {3600, 521}, - {3600, 513}, - {3600, 554}, - {3600, 495}, - {3400, 428}, - {3400, 415}, - {3400, 430}, - {3400, 428}, - {3400, 437}, - {3200, 439}, - {3200, 425}, - {3200, 455}, - {3200, 446}, + {4000, 551}, + {4000, 573}, + {4000, 546}, + {4000, 566}, + {4000, 579}, + {4000, 458}, + {4000, 533}, + {3800, 561}, + {3800, 499}, + {3800, 576}, + {3800, 517}, + {3800, 497}, + {3800, 522}, + {3600, 456}, + {3600, 516}, + {3600, 465}, + {3600, 422}, + {3600, 542}, + {3400, 435}, + {3400, 467}, + {3400, 496}, + {3400, 461}, + {3400, 470}, + {3400, 448}, + {3400, 516}, + {3200, 433}, + // {3200, 456}, {3200, 447}, - {3200, 447}, - {3000, 413}, - {3000, 428}, - {3000, 418}, - {3000, 413}, - {3000, 369}, + {3200, 406}, + {3200, 424}, + {3200, 454}, + // {3000, 414}, + {3000, 388}, + {3000, 389}, + {3000, 392}, + // {3000, 405}, {3000, 400}, - {3000, 342}, - {3000, 426}, - {2600, 285}, + // {3000, 430}, {2600, 316}, - {2600, 302}, - {2600, 304}, - {2600, 338}, - {2600, 318}, - {2200, 258}, - {2200, 307}, - {2200, 267}, - {2200, 267}, - {2200, 306}, - {2200, 272}, - {1500, 194}, - {1500, 198}, - {1500, 132}, - {1500, 133}, + // {2600, 354}, + // {2600, 339}, + // {2200, 288}, + {2200, 241}, + // {2200, 268}, + {2200, 218}, + {2200, 210}, + {1500, 145}, + {1500, 145}, + {1500, 136}, + // {1500, 184}, + // {1500, 178}, }; static { diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java index 6fdd7b7..7276aeb 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java @@ -13,88 +13,8 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - {4000, 551}, - {4000, 573}, - {4000,546 }, - {4000, 566}, - {4000, 579}, - {4000, 458}, - {4000, 533}, - {3800, 561}, - {3800, 499}, - {3800, 576}, - {3800, 517}, - {3800, 497}, - {3800, 522}, - {3600, 456}, - {3600, 516}, - {3600, 465}, - {3600, 422}, - {3600, 542}, - {3400, 435}, - {3400, 467}, - {3400, 496}, - {3400, 461}, - {3400, 470}, - {3400, 448}, - {3400, 516}, - {3200, 433}, - {3200, 456}, - {3200, 447}, - {3200, 406}, - {3200, 424}, - {3200, 454}, - {3000, 414}, - {3000, 388}, - {3000, 389}, - {3000, 392}, - {3000, 405}, - {3000, 400}, - {3000, 430}, - {2600, 316}, - {2600, 354}, - {2600, 339}, - {2200, 288}, - {2200, 241}, - {2200, 268}, - {2200, 218}, - {2200, 210}, - {1500, 145}, - {1500, 145}, - {1500, 136}, - {1500, 184}, - {1500, 178}, + }; - // private static final double[][] RPMAndDistance = { - // {1000, 55.5}, - // {1000, 42.5}, - // {1000, 57}, - // {1200, 73}, - // {2900, 295}, - // {2900, 265.5}, - // {2900, 279}, - // {2900, 289}, - // {3000, 306}, - // {3000, 317}, - // {3000, 311.5}, - // {3000, 324}, - // {3000, 303}, - // {3000, 300}, - // {3000, 303.5}, - // {3700, 437}, - // {3700, 447.5}, - // {3700, 419}, - // {3700, 416}, - // {3700, 400}, - // {3700, 381.5}, - // {3800, 410.5}, - // {3800, 441.25}, - // {3900, 457.5}, - // {3900, 427}, - // {4000, 476}, - // {4000, 435}, - // {4000, 439}, - // }; static { interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap(); From f5ee841a8a4df5c5ba0a955fd4d761258db95d82 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Fri, 30 Aug 2024 18:52:59 -0400 Subject: [PATCH 09/37] hgf auton chooser --- .../deploy/pathplanner/autos/HGF Red.auto | 55 +++++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 11 +++- 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/autos/HGF Red.auto diff --git a/src/main/deploy/pathplanner/autos/HGF Red.auto b/src/main/deploy/pathplanner/autos/HGF Red.auto new file mode 100644 index 0000000..dc4a147 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HGF Red.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7437038962619593, + "y": 4.320740582515391 + }, + "rotation": -59.682052822906385 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source to H" + } + }, + { + "type": "path", + "data": { + "pathName": "H to Shoot Red" + } + }, + { + "type": "path", + "data": { + "pathName": "H Shoot to G Red" + } + }, + { + "type": "path", + "data": { + "pathName": "G to Shoot Red" + } + }, + { + "type": "path", + "data": { + "pathName": "G Shoot to F Red" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot Red" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 241f2a7..2d827b8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.BAC.FourPieceBCA; +import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeAcquireForever; import com.stuypulse.robot.commands.intake.IntakeDeacquire; @@ -318,11 +319,19 @@ public void configureAutons() { AutonConfig BCA = new AutonConfig("4 BCA", FourPieceBCA::new, "Center to B", "B to C", "C to A"); AutonConfig BCA_RED = new AutonConfig("4 BCA RED", FourPieceBCA::new, - "Center to B", "B to C", "C to A"); + "Center to B", "B to C Red", "C to A Red"); + AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new, + "Source to H", "H to Shoot", "H Shoot to G", "G to Shoot", "G Shoot to F", "F to Shoot"); + AutonConfig HGF_RED = new AutonConfig("4 HGF RED", FourPieceHGF::new, + "Source to H", "H to Shoot Red", "H Shoot to G Red", "G to Shoot Red", "G Shoot to F Red", "F to Shoot Red"); + BCA.registerDefaultBlue(autonChooser); BCA_RED.registerRed(autonChooser); + HGF.registerDefaultBlue(autonChooser); + HGF_RED.registerRed(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } From db9445dae9e4f131693c859d692e7fdc53cef401 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Mon, 2 Sep 2024 13:20:45 -0400 Subject: [PATCH 10/37] added test autons, made tweaks to adef, hgf, and bca auton code. --- .pathplanner/settings.json | 5 +- simgui.json | 11 +++++ .../deploy/pathplanner/autos/BCA Red.auto | 4 +- .../deploy/pathplanner/autos/HGF Red.auto | 10 ++-- .../deploy/pathplanner/autos/Square Test.auto | 43 ++++++++++++++++ .../pathplanner/autos/Sraight Line.auto | 25 ++++++++++ src/main/deploy/pathplanner/paths/D Down.path | 2 +- src/main/deploy/pathplanner/paths/H Up.path | 2 +- .../pathplanner/paths/Square Bottom.path | 49 +++++++++++++++++++ .../deploy/pathplanner/paths/Square Left.path | 49 +++++++++++++++++++ .../pathplanner/paths/Square Right.path | 49 +++++++++++++++++++ .../deploy/pathplanner/paths/Square Top.path | 49 +++++++++++++++++++ .../pathplanner/paths/Straight Line.path | 49 +++++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 19 +++++-- .../commands/auton/ADEF/FivePieceADEF.java | 20 ++++++-- .../commands/auton/BAC/FourPieceBCA.java | 6 +-- .../commands/auton/HGF/FourPieceHGF.java | 40 +++++++-------- .../commands/auton/tests/SquareTest.java | 18 +++++++ .../commands/auton/tests/StraightLine.java | 15 ++++++ 19 files changed, 424 insertions(+), 41 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Square Test.auto create mode 100644 src/main/deploy/pathplanner/autos/Sraight Line.auto create mode 100644 src/main/deploy/pathplanner/paths/Square Bottom.path create mode 100644 src/main/deploy/pathplanner/paths/Square Left.path create mode 100644 src/main/deploy/pathplanner/paths/Square Right.path create mode 100644 src/main/deploy/pathplanner/paths/Square Top.path create mode 100644 src/main/deploy/pathplanner/paths/Straight Line.path create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 75a4120..56c73fa 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -5,9 +5,12 @@ "pathFolders": [ "ABCDE", "HGF", + "Tests", "Reroutes" ], - "autoFolders": [], + "autoFolders": [ + "Tests" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/simgui.json b/simgui.json index f40a134..e61d9cd 100644 --- a/simgui.json +++ b/simgui.json @@ -1,5 +1,10 @@ { "HALProvider": { + "Addressable LEDs": { + "0": { + "serpentine": true + } + }, "Other Devices": { "window": { "visible": false @@ -53,6 +58,12 @@ "open": true }, "SmartDashboard": { + "Alignment": { + "open": true + }, + "Autonomous": { + "open": true + }, "open": true } } diff --git a/src/main/deploy/pathplanner/autos/BCA Red.auto b/src/main/deploy/pathplanner/autos/BCA Red.auto index 6581ec2..23c1178 100644 --- a/src/main/deploy/pathplanner/autos/BCA Red.auto +++ b/src/main/deploy/pathplanner/autos/BCA Red.auto @@ -20,13 +20,13 @@ { "type": "path", "data": { - "pathName": "B to C Red" + "pathName": "B to C" } }, { "type": "path", "data": { - "pathName": "C to A Red" + "pathName": "C to A" } } ] diff --git a/src/main/deploy/pathplanner/autos/HGF Red.auto b/src/main/deploy/pathplanner/autos/HGF Red.auto index dc4a147..dcb5cb0 100644 --- a/src/main/deploy/pathplanner/autos/HGF Red.auto +++ b/src/main/deploy/pathplanner/autos/HGF Red.auto @@ -20,31 +20,31 @@ { "type": "path", "data": { - "pathName": "H to Shoot Red" + "pathName": "H to Shoot" } }, { "type": "path", "data": { - "pathName": "H Shoot to G Red" + "pathName": "H Shoot to G" } }, { "type": "path", "data": { - "pathName": "G to Shoot Red" + "pathName": "G to Shoot" } }, { "type": "path", "data": { - "pathName": "G Shoot to F Red" + "pathName": "G Shoot to F" } }, { "type": "path", "data": { - "pathName": "F to Shoot Red" + "pathName": "F to Shoot" } } ] diff --git a/src/main/deploy/pathplanner/autos/Square Test.auto b/src/main/deploy/pathplanner/autos/Square Test.auto new file mode 100644 index 0000000..3201137 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Square Test.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Square Top" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Right" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Bottom" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Left" + } + } + ] + } + }, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Sraight Line.auto b/src/main/deploy/pathplanner/autos/Sraight Line.auto new file mode 100644 index 0000000..a8ab7a5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Sraight Line.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.5, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight Line" + } + } + ] + } + }, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D Down.path b/src/main/deploy/pathplanner/paths/D Down.path index aa79312..6b7cd02 100644 --- a/src/main/deploy/pathplanner/paths/D Down.path +++ b/src/main/deploy/pathplanner/paths/D Down.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "Reroutes", "previewStartingState": { "rotation": -90.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/H Up.path b/src/main/deploy/pathplanner/paths/H Up.path index f8ab84d..5f01f56 100644 --- a/src/main/deploy/pathplanner/paths/H Up.path +++ b/src/main/deploy/pathplanner/paths/H Up.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "Reroutes", "previewStartingState": { "rotation": 90.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Square Bottom.path b/src/main/deploy/pathplanner/paths/Square Bottom.path new file mode 100644 index 0000000..4f00d91 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Bottom.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.381966011250105, + "y": 5.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 5.5 + }, + "prevControl": { + "x": 3.5206906325745555, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Left.path b/src/main/deploy/pathplanner/paths/Square Left.path new file mode 100644 index 0000000..9b4fb71 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Left.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 6.618033988749895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 2.0, + "y": 5.479309367425445 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Right.path b/src/main/deploy/pathplanner/paths/Square Right.path new file mode 100644 index 0000000..52be2d5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Right.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.5, + "y": 5.88 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 5.5 + }, + "prevControl": { + "x": 3.5, + "y": 7.020690632574555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Top.path b/src/main/deploy/pathplanner/paths/Square Top.path new file mode 100644 index 0000000..7364cca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Top.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.118033988749895, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 7.0 + }, + "prevControl": { + "x": 2.9019564509574227, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path new file mode 100644 index 0000000..3c285ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5, + "y": 5.552154629234854 + }, + "prevControl": null, + "nextControl": { + "x": 2.9, + "y": 5.552154629234854 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.15, + "y": 5.55 + }, + "prevControl": { + "x": 2.9000000000000004, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 2d827b8..48bc17a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,8 +13,11 @@ import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; +import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; import com.stuypulse.robot.commands.auton.BAC.FourPieceBCA; import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF; +import com.stuypulse.robot.commands.auton.tests.SquareTest; +import com.stuypulse.robot.commands.auton.tests.StraightLine; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeAcquireForever; import com.stuypulse.robot.commands.intake.IntakeDeacquire; @@ -315,23 +318,31 @@ private void configureAutomaticCommandScheduling() { public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Mobility", new Mobility()); + autonChooser.addOption("Square Test", new SquareTest()); + autonChooser.addOption("Straight Line Test", new StraightLine()); AutonConfig BCA = new AutonConfig("4 BCA", FourPieceBCA::new, "Center to B", "B to C", "C to A"); AutonConfig BCA_RED = new AutonConfig("4 BCA RED", FourPieceBCA::new, - "Center to B", "B to C Red", "C to A Red"); + "Center to B", "B to C", "C to A"); AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new, "Source to H", "H to Shoot", "H Shoot to G", "G to Shoot", "G Shoot to F", "F to Shoot"); AutonConfig HGF_RED = new AutonConfig("4 HGF RED", FourPieceHGF::new, - "Source to H", "H to Shoot Red", "H Shoot to G Red", "G to Shoot Red", "G Shoot to F Red", "F to Shoot Red"); - + "Source to H", "H to Shoot", "H Shoot to G", "G to Shoot", "G Shoot to F", "F to Shoot"); + AutonConfig ADEF = new AutonConfig("5 ADEF", FivePieceADEF::new, + "Amp to A", "A to D", "D to Shoot", "D Shoot to E", "E to Shoot", "E Shoot to F", "F to Shoot"); + AutonConfig ADEF_RED = new AutonConfig("5 ADEF RED", FivePieceADEF::new, + "Amp to A", "A to D", "D to Shoot", "D Shoot to E", "E to Shoot", "E Shoot to F", "F to Shoot"); BCA.registerDefaultBlue(autonChooser); BCA_RED.registerRed(autonChooser); - HGF.registerDefaultBlue(autonChooser); + HGF.registerBlue(autonChooser); HGF_RED.registerRed(autonChooser); + ADEF.registerBlue(autonChooser); + ADEF_RED.registerRed(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 4ee2392..8973b22 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -1,11 +1,17 @@ package com.stuypulse.robot.commands.auton.ADEF; import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -14,20 +20,26 @@ public class FivePieceADEF extends SequentialCommandGroup { public FivePieceADEF(PathPlannerPath... paths) { addCommands( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), + // Preload Shot + new ArmToSubwooferShot(), + new ShooterSetRPM(Settings.Shooter.SPEAKER), + new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), + new ShooterFeederShoot(), + // Drive, Intake, Shoot A new FollowPathAndIntake(paths[0]), new SwerveDriveToShoot(), + new ShootRoutine(), + // Drive, Intake, Shoot D new FollowPathAndIntake(paths[1]), new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + // Drive, Intake, Shoot E new FollowPathAndIntake(paths[3]), new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + // Drive, Intake, Shoot F new FollowPathAndIntake(paths[5]), new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java index ef321ee..ec765ff 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java @@ -23,19 +23,19 @@ public FourPieceBCA(PathPlannerPath... path) { new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), new ShooterFeederShoot(), - // Drive to B + Shoot B + // Drive, Intake, Shoot B new FollowPathAndIntake(path[0]), new SwerveDriveToShoot() .withTolerance(0.03, 3), new ShootRoutine(), - // Drive to C + Shoot C + // DDrive, Intake, Shoot C new FollowPathAndIntake(path[1]), new SwerveDriveToShoot() .withTolerance(0.03, 3), new ShootRoutine(), - // Drive to A + Shoot A + // Drive, Intake, Shoot A new FollowPathAndIntake(path[2]), new SwerveDriveToShoot() .withTolerance(0.03, 3), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index 2aae170..ea99130 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -1,11 +1,18 @@ package com.stuypulse.robot.commands.auton.HGF; import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Auton; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -16,30 +23,23 @@ public class FourPieceHGF extends SequentialCommandGroup { public FourPieceHGF(PathPlannerPath... paths) { addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterScoreSpeaker()), - - SwerveDriveToPose.speakerRelative(-15) - .withTolerance(0.03, 0.03, 3) - ), - - new ShootRoutine(0.7), + // Preload Shot + new ArmToSubwooferShot(), + new ShooterSetRPM(Settings.Shooter.SPEAKER), + new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), + new ShooterFeederShoot(), + // Drive, Intake, Shoot H new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(), - - new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(), + new FollowPathAlignAndShoot(paths[1], new SwerveDriveToShoot()), + // Drive, Intake, Shoot G new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(0.7) + new FollowPathAlignAndShoot(paths[3], new SwerveDriveToShoot()), + + // Drive, Intake, Shoot F + new FollowPathAndIntake(paths[4]), + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java new file mode 100644 index 0000000..4983519 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class SquareTest extends SequentialCommandGroup { + + public SquareTest() { + addCommands( + SwerveDrive.getInstance().followPathCommand("Square Top"), + SwerveDrive.getInstance().followPathCommand("Square Right"), + SwerveDrive.getInstance().followPathCommand("Square Bottom"), + SwerveDrive.getInstance().followPathCommand("Square Left") + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java new file mode 100644 index 0000000..84685ba --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java @@ -0,0 +1,15 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class StraightLine extends SequentialCommandGroup{ + + public StraightLine() { + addCommands( + SwerveDrive.getInstance().followPathCommand("Straight Line") + ); + } + +} From 67868d07efce6d780ff9b101aa98ca6d3865a345 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Tue, 3 Sep 2024 14:43:03 -0400 Subject: [PATCH 11/37] n/a --- src/main/java/com/stuypulse/robot/Robot.java | 4 ++++ src/main/java/com/stuypulse/robot/RobotContainer.java | 2 +- .../commands/auton/{BAC => BCA}/FourPieceBCA.java | 10 +++++----- 3 files changed, 10 insertions(+), 6 deletions(-) rename src/main/java/com/stuypulse/robot/commands/auton/{BAC => BCA}/FourPieceBCA.java (87%) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e175561..e12c4bf 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -2,8 +2,10 @@ import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; +import com.revrobotics.CANSparkBase.IdleMode; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.intake.IntakeShoot; +import com.stuypulse.robot.commands.leds.LEDReset; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; import com.stuypulse.robot.commands.shooter.ShooterManualIntake; import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; @@ -90,6 +92,8 @@ public void autonomousInit() { auto.schedule(); } + scheduler.schedule(new LEDReset()); + SmartDashboard.putString("Robot State", "AUTON"); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 48bc17a..74c0d36 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,7 +14,7 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; -import com.stuypulse.robot.commands.auton.BAC.FourPieceBCA; +import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF; import com.stuypulse.robot.commands.auton.tests.SquareTest; import com.stuypulse.robot.commands.auton.tests.StraightLine; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java similarity index 87% rename from src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java rename to src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index ec765ff..d2543d2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.commands.auton.BAC; +package com.stuypulse.robot.commands.auton.BCA; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; @@ -26,19 +26,19 @@ public FourPieceBCA(PathPlannerPath... path) { // Drive, Intake, Shoot B new FollowPathAndIntake(path[0]), new SwerveDriveToShoot() - .withTolerance(0.03, 3), + .withTolerance(0.03, 2), new ShootRoutine(), - // DDrive, Intake, Shoot C + // Drive, Intake, Shoot C new FollowPathAndIntake(path[1]), new SwerveDriveToShoot() - .withTolerance(0.03, 3), + .withTolerance(0.03, 2), new ShootRoutine(), // Drive, Intake, Shoot A new FollowPathAndIntake(path[2]), new SwerveDriveToShoot() - .withTolerance(0.03, 3), + .withTolerance(0.03, 2), new ShootRoutine(0.7) ); } From 7e1014f444d71fa23109c60f489a106ed89a75f4 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 3 Sep 2024 15:47:51 -0400 Subject: [PATCH 12/37] fix ShootRoutine, add FollowPathPointSpeaker --- .../deploy/pathplanner/autos/BCA Red.auto | 37 --- .../autos/{BCA Blue.auto => BCA.auto} | 0 .../commands/auton/ADEF/FivePieceADEF.java | 19 +- .../auton/ADEF/ReroutableFivePieceADEF.java | 92 +++--- .../commands/auton/BAC/FourPieceBCA.java | 17 +- .../auton/BF_Series/FivePieceBFAC.java | 19 +- .../auton/BF_Series/FivePieceBFCA.java | 12 +- .../auton/BF_Series/FivePieceBFED.java | 12 +- .../auton/BF_Series/FivePieceBFGH.java | 12 +- .../auton/FollowPathAlignAndShoot.java | 44 --- .../commands/auton/HGF/FourPieceHGF.java | 82 ++--- .../robot/commands/auton/ShootRoutine.java | 47 +-- .../swerve/SwerveDriveAlignToSpeaker.java | 97 ++++++ .../robot/subsystems/swerve/SwerveDrive.java | 25 ++ .../util/FollowPathPointSpeakerCommand.java | 291 ++++++++++++++++++ 15 files changed, 573 insertions(+), 233 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/BCA Red.auto rename src/main/deploy/pathplanner/autos/{BCA Blue.auto => BCA.auto} (100%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java create mode 100644 src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java diff --git a/src/main/deploy/pathplanner/autos/BCA Red.auto b/src/main/deploy/pathplanner/autos/BCA Red.auto deleted file mode 100644 index 6581ec2..0000000 --- a/src/main/deploy/pathplanner/autos/BCA Red.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4365936349060198, - "y": 5.5179740558513855 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center to B" - } - }, - { - "type": "path", - "data": { - "pathName": "B to C Red" - } - }, - { - "type": "path", - "data": { - "pathName": "C to A Red" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BCA Blue.auto b/src/main/deploy/pathplanner/autos/BCA.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/BCA Blue.auto rename to src/main/deploy/pathplanner/autos/BCA.auto diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 4ee2392..52debb2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -1,11 +1,12 @@ package com.stuypulse.robot.commands.auton.ADEF; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -14,22 +15,22 @@ public class FivePieceADEF extends SequentialCommandGroup { public FivePieceADEF(PathPlannerPath... paths) { addCommands( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), + ShootRoutine.fromSubwoofer(), new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[5]), - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), + ShootRoutine.fromAnywhere() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java index 37966ec..d61a03f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java @@ -1,62 +1,62 @@ -package com.stuypulse.robot.commands.auton.ADEF; +// package com.stuypulse.robot.commands.auton.ADEF; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.DoNothingCommand; -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.subsystems.intake.Intake; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.stuypulse.robot.commands.DoNothingCommand; +// import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +// import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +// import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +// import com.stuypulse.robot.subsystems.intake.Intake; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.ConditionalCommand; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -public class ReroutableFivePieceADEF extends SequentialCommandGroup { +// public class ReroutableFivePieceADEF extends SequentialCommandGroup { - public ReroutableFivePieceADEF(PathPlannerPath... paths) { +// public ReroutableFivePieceADEF(PathPlannerPath... paths) { - PathPlannerPath D_DOWN = paths[7]; +// PathPlannerPath D_DOWN = paths[7]; - addCommands( - // Preload - new ShooterScoreSpeaker(), +// addCommands( +// // Preload +// new ShooterScoreSpeaker(), - new ShooterWaitForTarget() - .withTimeout(1.0), +// new ShooterWaitForTarget() +// .withTimeout(1.0), - // Intake + Shoot A - new FollowPathAndIntake(paths[0]), - new ShooterScoreSpeaker(), +// // Intake + Shoot A +// new FollowPathAndIntake(paths[0]), +// new ShooterScoreSpeaker(), - // Intake + Shoot D - new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), +// // Intake + Shoot D +// new FollowPathAndIntake(paths[1]), +// new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), - // Intake + Shoot E - new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), +// // Intake + Shoot E +// new FollowPathAndIntake(paths[3]), +// new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - new ConditionalCommand( - new SequentialCommandGroup( - new FollowPathAndIntake(paths[5]), +// new ConditionalCommand( +// new SequentialCommandGroup( +// new FollowPathAndIntake(paths[5]), - new ConditionalCommand( - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()), - new DoNothingCommand(), - Intake.getInstance()::hasNote)), +// new ConditionalCommand( +// new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()), +// new DoNothingCommand(), +// Intake.getInstance()::hasNote)), - new SequentialCommandGroup( - new FollowPathAndIntake(D_DOWN), +// new SequentialCommandGroup( +// new FollowPathAndIntake(D_DOWN), - new ConditionalCommand( - new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot()), - new DoNothingCommand(), - Intake.getInstance()::hasNote) - ), - Intake.getInstance()::hasNote) - ); +// new ConditionalCommand( +// new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot()), +// new DoNothingCommand(), +// Intake.getInstance()::hasNote) +// ), +// Intake.getInstance()::hasNote) +// ); - } +// } -} +// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java index ef321ee..7156e46 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java @@ -18,28 +18,19 @@ public class FourPieceBCA extends SequentialCommandGroup { public FourPieceBCA(PathPlannerPath... path) { addCommands( // Preload Shot - new ArmToSubwooferShot(), - new ShooterSetRPM(Settings.Shooter.SPEAKER), - new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), - new ShooterFeederShoot(), + ShootRoutine.fromSubwoofer(), // Drive to B + Shoot B new FollowPathAndIntake(path[0]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(), + ShootRoutine.fromAnywhere().withTimeout(2.5), // Drive to C + Shoot C new FollowPathAndIntake(path[1]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(), + ShootRoutine.fromAnywhere().withTimeout(2.5), // Drive to A + Shoot A new FollowPathAndIntake(path[2]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(0.7) + ShootRoutine.fromAnywhere().withTimeout(2.5) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java index bfb3ab0..95a36b8 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java @@ -1,11 +1,12 @@ package com.stuypulse.robot.commands.auton.BF_Series; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -14,22 +15,22 @@ public class FivePieceBFAC extends SequentialCommandGroup { public FivePieceBFAC(PathPlannerPath... paths) { addCommands( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), + ShootRoutine.fromSubwoofer(), new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[5]), - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), + ShootRoutine.fromAnywhere() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java index b294672..721a94d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java @@ -1,11 +1,12 @@ package com.stuypulse.robot.commands.auton.BF_Series; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -23,13 +24,16 @@ public FivePieceBFCA(PathPlannerPath... paths) { new SwerveDriveToShoot(), new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[5]), - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), + ShootRoutine.fromAnywhere() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java index 8acbfdf..72ba47d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java @@ -1,11 +1,12 @@ package com.stuypulse.robot.commands.auton.BF_Series; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -23,13 +24,16 @@ public FivePieceBFED(PathPlannerPath... paths) { new SwerveDriveToShoot(), new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[5]), - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), + ShootRoutine.fromAnywhere() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java index 1f96687..5e806c9 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java @@ -1,11 +1,12 @@ package com.stuypulse.robot.commands.auton.BF_Series; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -23,13 +24,16 @@ public FivePieceBFGH(PathPlannerPath... paths) { new SwerveDriveToShoot(), new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[5]), - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), + ShootRoutine.fromAnywhere() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java deleted file mode 100644 index af26f18..0000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java +++ /dev/null @@ -1,44 +0,0 @@ -package com.stuypulse.robot.commands.auton; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.constants.Settings.Auton; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class FollowPathAlignAndShoot extends SequentialCommandGroup { - - public double getPathTime(PathPlannerPath path) { - return path.getTrajectory(new ChassisSpeeds(), path.getStartingDifferentialPose().getRotation()) - .getTotalTimeSeconds(); - } - - public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand) { - this(path, alignCommand, false); - } - - public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand, boolean noteShot) { - addCommands( - new ParallelCommandGroup( - SwerveDrive.getInstance().followPathCommand(path), - new WaitCommand(getPathTime(path) - Auton.SHOOTER_START_PRE) - .andThen(new ShooterScoreSpeaker()) - ), - alignCommand, - new ShooterWaitForTarget() - .withTimeout(0.5) - ); - - if (noteShot) - addCommands(new ShootRoutine(0.75)); - else - addCommands(new ShooterScoreSpeaker()); - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index 2aae170..9d1ec0a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -1,46 +1,46 @@ -package com.stuypulse.robot.commands.auton.HGF; +// package com.stuypulse.robot.commands.auton.HGF; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.constants.Settings.Auton; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +// import com.stuypulse.robot.commands.auton.ShootRoutine; +// import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +// import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +// import com.stuypulse.robot.constants.Settings.Auton; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; +// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.WaitCommand; -public class FourPieceHGF extends SequentialCommandGroup { +// public class FourPieceHGF extends SequentialCommandGroup { - public FourPieceHGF(PathPlannerPath... paths) { - addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterScoreSpeaker()), +// public FourPieceHGF(PathPlannerPath... paths) { +// addCommands( +// new ParallelCommandGroup( +// new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) +// .andThen(new ShooterScoreSpeaker()), - SwerveDriveToPose.speakerRelative(-15) - .withTolerance(0.03, 0.03, 3) - ), - - new ShootRoutine(0.7), - - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(), - - new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(), - - new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new ShootRoutine(0.7) - ); - } - -} \ No newline at end of file +// SwerveDriveToPose.speakerRelative(-15) +// .withTolerance(0.03, 0.03, 3) +// ), + +// new ShootRoutine(0.7), + +// new FollowPathAndIntake(paths[0]), +// new SwerveDriveToShoot() +// .withTolerance(0.03, 3), +// new ShootRoutine(), + +// new FollowPathAndIntake(paths[1]), +// new SwerveDriveToShoot() +// .withTolerance(0.03, 3), +// new ShootRoutine(), + +// new FollowPathAndIntake(paths[2]), +// new SwerveDriveToShoot() +// .withTolerance(0.03, 3), +// new ShootRoutine(0.7) +// ); +// } + +// } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java index 58dcc12..84f918e 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -1,38 +1,41 @@ package com.stuypulse.robot.commands.auton; -import com.stuypulse.robot.commands.intake.IntakeStop; -import com.stuypulse.robot.commands.swerve.SwerveDriveStop; +import com.stuypulse.robot.commands.arm.ArmToSpeaker; +import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.leds.LEDSet; +import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.swerve.SwerveDriveAlignToSpeaker; +import com.stuypulse.robot.constants.LEDInstructions; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class ShootRoutine extends SequentialCommandGroup { +public abstract class ShootRoutine { - public ShootRoutine(double delay) { - addCommands( - new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), - new WaitUntilCommand(Shooter.getInstance()::hasNote) - .withTimeout(delay), - new IntakeStop() + public static Command fromSubwoofer() { + return new SequentialCommandGroup( + new ArmToSubwooferShot(), + new ShooterSetRPM(Settings.Shooter.SPEAKER), + new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), + new ShooterFeederShoot() ); } - public ShootRoutine() { - this(Settings.Auton.SHOOT_WAIT_DELAY.get()); - } - -public static Command UntilNoteShot(double timeout) { - return new SequentialCommandGroup( - new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), - new WaitCommand(timeout), - new IntakeStop() - ); + public static Command fromAnywhere() { + return new SwerveDriveAlignToSpeaker() + .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) + .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) + .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) + .andThen(new WaitUntilCommand(() -> SwerveDrive.getInstance().isAlignedToSpeaker())) + .andThen(new ShooterFeederShoot()) + ) + .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java new file mode 100644 index 0000000..09a45dc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java @@ -0,0 +1,97 @@ +package com.stuypulse.robot.commands.swerve; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.util.AngleVelocity; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveAlignToSpeaker extends Command { + + private final SwerveDrive swerve; + private final SwerveRequest.FieldCentric drive; + + private final AngleController controller; + private final IStream angleVelocity; + + public SwerveDriveAlignToSpeaker() { + swerve = SwerveDrive.getInstance(); + + drive = new SwerveRequest.FieldCentric() + .withDeadband(Settings.Swerve.MAX_LINEAR_VELOCITY * Settings.Driver.Drive.DEADBAND.get()) + .withRotationalDeadband(Settings.Swerve.MAX_ANGULAR_VELOCITY * Settings.Driver.Turn.DEADBAND.get()) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + + controller = new AnglePIDController(Settings.Swerve.Assist.kP, Settings.Swerve.Assist.kI, Settings.Swerve.Assist.kD) + .setOutputFilter(x -> -x); + + AngleVelocity derivative = new AngleVelocity(); + + angleVelocity = IStream.create(() -> derivative.get(Angle.fromRotation2d(getTargetAngle()))) + .filtered( + new LowPassFilter(Assist.ANGLE_DERIV_RC), + // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST + // so that angular velocity doesn't oscillate + x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST), + // new RateLimit(Settings.Swerve.MAX_ANGULAR_ACCEL), + x -> SLMath.clamp(x, -Settings.Swerve.MAX_ANGULAR_VELOCITY, Settings.Swerve.MAX_ANGULAR_VELOCITY), + x -> -x + ); + + addRequirements(swerve); + } + + private Rotation2d getTargetAngle() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return currentPose.minus(speakerPose).getAngle(); + } + + private double getDistanceToTarget() { + Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return currentPose.getDistance(speakerPose); + } + + protected double getAngleError() { + return controller.getError().getRotation2d().getDegrees(); + } + + @Override + public boolean isFinished() { + return controller.isDoneDegrees(Alignment.ANGLE_TOLERANCE.get()); + } + + @Override + public void execute() { + swerve.setControl( + drive.withVelocityX(0) + .withVelocityY(0) + .withRotationalRate( + SLMath.clamp(angleVelocity.get() + + controller.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(swerve.getPose().getRotation())), + -Settings.Swerve.MAX_ANGULAR_VELOCITY, + Settings.Swerve.MAX_ANGULAR_VELOCITY + ) + ) + ); + SmartDashboard.putNumber("Alignment/Distance to Target", getDistanceToTarget()); + SmartDashboard.putNumber("Alignment/Target Angle", getTargetAngle().getDegrees()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index af30011..905c4f3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.ReplanningConfig; @@ -17,12 +18,14 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Swerve.Motion; import com.stuypulse.robot.subsystems.vision.AprilTagVision; +import com.stuypulse.robot.util.FollowPathPointSpeakerCommand; import com.stuypulse.robot.util.vision.VisionData; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.DriverStation; @@ -130,6 +133,24 @@ public Command followPathCommand(String pathName) { return followPathCommand(PathPlannerPath.fromPathFile(pathName)); } + public Command followPathWithSpeakerAlignCommand(PathPlannerPath path) { + return new FollowPathPointSpeakerCommand( + path, + () -> getPose(), + this::getChassisSpeeds, + this::setChassisSpeeds, + new PPHolonomicDriveController( + Motion.XY, + Motion.THETA, + 0.02, + 4.9, + Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH)), + new ReplanningConfig(false, false), + () -> false, + this + ); + } + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); setControl(drive.withVelocityX(chassisSpeeds.vxMetersPerSecond) @@ -138,6 +159,10 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { ); } + public ChassisSpeeds getChassisSpeeds() { + return m_kinematics.toChassisSpeeds(m_moduleStates); + } + public Command followPathCommand(PathPlannerPath path) { return new FollowPathHolonomic( path, diff --git a/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java b/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java new file mode 100644 index 0000000..f0cdc24 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java @@ -0,0 +1,291 @@ +package com.stuypulse.robot.util; + +import com.pathplanner.lib.controllers.PathFollowingController; +import com.pathplanner.lib.path.EventMarker; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPlannerTrajectory; +import com.pathplanner.lib.util.PPLibTelemetry; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.util.ReplanningConfig; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Swerve.Assist; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.util.AngleVelocity; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.*; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.Supplier; + +/** Base command for following a path */ +public class FollowPathPointSpeakerCommand extends Command { + private final Timer timer = new Timer(); + private final PathPlannerPath originalPath; + private final Supplier poseSupplier; + private final Supplier speedsSupplier; + private final Consumer output; + private final PathFollowingController controller; + private final ReplanningConfig replanningConfig; + private final BooleanSupplier shouldFlipPath; + + // For event markers + private final Map currentEventCommands = new HashMap<>(); + private final List> untriggeredEvents = new ArrayList<>(); + + private PathPlannerPath path; + private PathPlannerTrajectory generatedTrajectory; + + // Heading Controller + + private final AngleController headingController; + private final IStream angleVelocity; + + /** + * Construct a base path following command + * + * @param path The path to follow + * @param poseSupplier Function that supplies the current field-relative pose of the robot + * @param speedsSupplier Function that supplies the current robot-relative chassis speeds + * @param outputRobotRelative Function that will apply the robot-relative output speeds of this + * command + * @param controller Path following controller that will be used to follow the path + * @param replanningConfig Path replanning configuration + * @param shouldFlipPath Should the path be flipped to the other side of the field? This will + * maintain a global blue alliance origin. + * @param requirements Subsystems required by this command, usually just the drive subsystem + */ + public FollowPathPointSpeakerCommand( + PathPlannerPath path, + Supplier poseSupplier, + Supplier speedsSupplier, + Consumer outputRobotRelative, + PathFollowingController controller, + ReplanningConfig replanningConfig, + BooleanSupplier shouldFlipPath, + Subsystem... requirements) { + this.originalPath = path; + this.poseSupplier = poseSupplier; + this.speedsSupplier = speedsSupplier; + this.output = outputRobotRelative; + this.controller = controller; + this.replanningConfig = replanningConfig; + this.shouldFlipPath = shouldFlipPath; + + + headingController = new AnglePIDController(Assist.kP, Assist.kI, Assist.kD) + .setOutputFilter(x -> -x); + + AngleVelocity derivative = new AngleVelocity(); + + angleVelocity = IStream.create(() -> derivative.get(Angle.fromRotation2d(getTargetAngle()))) + .filtered(new LowPassFilter(Assist.ANGLE_DERIV_RC)) + // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST + // so that angular velocity doesn't oscillate + .filtered(x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST)) + .filtered(x -> -x); + + Set driveRequirements = Set.of(requirements); + m_requirements.addAll(driveRequirements); + + for (EventMarker marker : this.originalPath.getEventMarkers()) { + var reqs = marker.getCommand().getRequirements(); + + if (!Collections.disjoint(driveRequirements, reqs)) { + throw new IllegalArgumentException( + "Events that are triggered during path following cannot require the drive subsystem"); + } + + m_requirements.addAll(reqs); + } + } + + public Rotation2d getTargetAngle() { + Translation2d robotPose = poseSupplier.get().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return speakerPose.minus(robotPose).getAngle() + .plus(Rotation2d.fromDegrees(180)); + } + + public double getDistanceToTarget() { + Translation2d robotPose = poseSupplier.get().getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + return speakerPose.getDistance(robotPose); + } + + @Override + public void initialize() { + if (shouldFlipPath.getAsBoolean() && !originalPath.preventFlipping) { + path = originalPath.flipPath(); + } else { + path = originalPath; + } + + Pose2d currentPose = poseSupplier.get(); + ChassisSpeeds currentSpeeds = speedsSupplier.get(); + + controller.reset(currentPose, currentSpeeds); + + ChassisSpeeds fieldSpeeds = + ChassisSpeeds.fromRobotRelativeSpeeds(currentSpeeds, currentPose.getRotation()); + Rotation2d currentHeading = + new Rotation2d(fieldSpeeds.vxMetersPerSecond, fieldSpeeds.vyMetersPerSecond); + Rotation2d targetHeading = + path.getPoint(1).position.minus(path.getPoint(0).position).getAngle(); + Rotation2d headingError = currentHeading.minus(targetHeading); + boolean onHeading = + Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond) < 0.25 + || Math.abs(headingError.getDegrees()) < 30; + + if (!path.isChoreoPath() + && replanningConfig.enableInitialReplanning + && (currentPose.getTranslation().getDistance(path.getPoint(0).position) > 0.25 + || !onHeading)) { + replanPath(currentPose, currentSpeeds); + } else { + generatedTrajectory = path.getTrajectory(currentSpeeds, currentPose.getRotation()); + PathPlannerLogging.logActivePath(path); + PPLibTelemetry.setCurrentPath(path); + } + + // Initialize marker stuff + currentEventCommands.clear(); + untriggeredEvents.clear(); + untriggeredEvents.addAll(generatedTrajectory.getEventCommands()); + + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + double currentTime = timer.get(); + PathPlannerTrajectory.State targetState = generatedTrajectory.sample(currentTime); + if (!controller.isHolonomic() && path.isReversed()) { + targetState = targetState.reverse(); + } + + Pose2d currentPose = poseSupplier.get(); + ChassisSpeeds currentSpeeds = speedsSupplier.get(); + + if (!path.isChoreoPath() && replanningConfig.enableDynamicReplanning) { + double previousError = Math.abs(controller.getPositionalError()); + double currentError = currentPose.getTranslation().getDistance(targetState.positionMeters); + + if (currentError >= replanningConfig.dynamicReplanningTotalErrorThreshold + || currentError - previousError + >= replanningConfig.dynamicReplanningErrorSpikeThreshold) { + replanPath(currentPose, currentSpeeds); + timer.reset(); + targetState = generatedTrajectory.sample(0); + } + } + + ChassisSpeeds targetSpeeds = controller.calculateRobotRelativeSpeeds(currentPose, targetState); + + targetSpeeds.omegaRadiansPerSecond += angleVelocity.get() + + headingController.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(currentPose.getRotation())); + + double currentVel = + Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); + + PPLibTelemetry.setCurrentPose(currentPose); + PathPlannerLogging.logCurrentPose(currentPose); + + if (controller.isHolonomic()) { + PPLibTelemetry.setTargetPose(targetState.getTargetHolonomicPose()); + PathPlannerLogging.logTargetPose(targetState.getTargetHolonomicPose()); + } else { + PPLibTelemetry.setTargetPose(targetState.getDifferentialPose()); + PathPlannerLogging.logTargetPose(targetState.getDifferentialPose()); + } + + PPLibTelemetry.setVelocities( + currentVel, + targetState.velocityMps, + currentSpeeds.omegaRadiansPerSecond, + targetSpeeds.omegaRadiansPerSecond); + PPLibTelemetry.setPathInaccuracy(controller.getPositionalError()); + + output.accept(targetSpeeds); + + if (!untriggeredEvents.isEmpty() && timer.hasElapsed(untriggeredEvents.get(0).getFirst())) { + // Time to trigger this event command + Pair event = untriggeredEvents.remove(0); + + for (var runningCommand : currentEventCommands.entrySet()) { + if (!runningCommand.getValue()) { + continue; + } + + if (!Collections.disjoint( + runningCommand.getKey().getRequirements(), event.getSecond().getRequirements())) { + runningCommand.getKey().end(true); + runningCommand.setValue(false); + } + } + + event.getSecond().initialize(); + currentEventCommands.put(event.getSecond(), true); + } + + // Run event marker commands + for (Map.Entry runningCommand : currentEventCommands.entrySet()) { + if (!runningCommand.getValue()) { + continue; + } + + runningCommand.getKey().execute(); + + if (runningCommand.getKey().isFinished()) { + runningCommand.getKey().end(false); + runningCommand.setValue(false); + } + } + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(generatedTrajectory.getTotalTimeSeconds()); + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + + // Only output 0 speeds when ending a path that is supposed to stop, this allows interrupting + // the command to smoothly transition into some auto-alignment routine + if (!interrupted && path.getGoalEndState().getVelocity() < 0.1) { + output.accept(new ChassisSpeeds()); + } + + PathPlannerLogging.logActivePath(null); + + // End markers + for (Map.Entry runningCommand : currentEventCommands.entrySet()) { + if (runningCommand.getValue()) { + runningCommand.getKey().end(true); + } + } + } + + private void replanPath(Pose2d currentPose, ChassisSpeeds currentSpeeds) { + PathPlannerPath replanned = path.replan(currentPose, currentSpeeds); + generatedTrajectory = replanned.getTrajectory(currentSpeeds, currentPose.getRotation()); + PathPlannerLogging.logActivePath(replanned); + PPLibTelemetry.setCurrentPath(replanned); + } +} From 24cc80f737d2b9a537ea498984fa074b93b98bf1 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 3 Sep 2024 16:00:11 -0400 Subject: [PATCH 13/37] fix subsystem requirement conflicts with automatic command scheduling stuff --- src/main/java/com/stuypulse/robot/Robot.java | 1 + src/main/java/com/stuypulse/robot/RobotContainer.java | 3 +-- .../stuypulse/robot/commands/auton/BAC/FourPieceBCA.java | 7 +++++++ 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e175561..d893ab6 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -109,6 +109,7 @@ public void teleopInit() { if (auto != null) { auto.cancel(); } + robot.configureAutomaticCommandScheduling(); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 241f2a7..04aea2a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -126,7 +126,6 @@ private void configureDefaultCommands() { private void configureButtonBindings() { configureOperatorBindings(); configureDriverBindings(); - configureAutomaticCommandScheduling(); } private void configureDriverBindings() { @@ -275,7 +274,7 @@ private void configureOperatorBindings() { } - private void configureAutomaticCommandScheduling() { + protected void configureAutomaticCommandScheduling() { // automatic handoff new Trigger(() -> arm.getState() == Arm.State.FEED && arm.atTarget() diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java index 7156e46..1cb5d75 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BAC/FourPieceBCA.java @@ -1,15 +1,18 @@ package com.stuypulse.robot.commands.auton.BAC; import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmSetState; import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.arm.Arm; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -19,17 +22,21 @@ public FourPieceBCA(PathPlannerPath... path) { addCommands( // Preload Shot ShootRoutine.fromSubwoofer(), + new ArmSetState(Arm.State.FEED), // Drive to B + Shoot B new FollowPathAndIntake(path[0]), + new ShooterAcquireFromIntake(), ShootRoutine.fromAnywhere().withTimeout(2.5), // Drive to C + Shoot C new FollowPathAndIntake(path[1]), + new ShooterAcquireFromIntake(), ShootRoutine.fromAnywhere().withTimeout(2.5), // Drive to A + Shoot A new FollowPathAndIntake(path[2]), + new ShooterAcquireFromIntake(), ShootRoutine.fromAnywhere().withTimeout(2.5) ); } From 64dc8b77c43120db6ba51982ed37d2a3db72e855 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 3 Sep 2024 16:11:14 -0400 Subject: [PATCH 14/37] sussy --- .../deploy/pathplanner/autos/HGF Red.auto | 55 +++++++++++++++++++ .../deploy/pathplanner/autos/Square Test.auto | 43 +++++++++++++++ .../pathplanner/autos/Sraight Line.auto | 25 +++++++++ src/main/deploy/pathplanner/paths/D Down.path | 2 +- src/main/deploy/pathplanner/paths/H Up.path | 2 +- .../pathplanner/paths/Square Bottom.path | 49 +++++++++++++++++ .../deploy/pathplanner/paths/Square Left.path | 49 +++++++++++++++++ .../pathplanner/paths/Square Right.path | 49 +++++++++++++++++ .../deploy/pathplanner/paths/Square Top.path | 49 +++++++++++++++++ .../pathplanner/paths/Straight Line.path | 49 +++++++++++++++++ src/main/java/com/stuypulse/robot/Robot.java | 4 ++ .../com/stuypulse/robot/RobotContainer.java | 15 +++++ .../commands/auton/ADEF/FivePieceADEF.java | 4 ++ 13 files changed, 393 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/HGF Red.auto create mode 100644 src/main/deploy/pathplanner/autos/Square Test.auto create mode 100644 src/main/deploy/pathplanner/autos/Sraight Line.auto create mode 100644 src/main/deploy/pathplanner/paths/Square Bottom.path create mode 100644 src/main/deploy/pathplanner/paths/Square Left.path create mode 100644 src/main/deploy/pathplanner/paths/Square Right.path create mode 100644 src/main/deploy/pathplanner/paths/Square Top.path create mode 100644 src/main/deploy/pathplanner/paths/Straight Line.path diff --git a/src/main/deploy/pathplanner/autos/HGF Red.auto b/src/main/deploy/pathplanner/autos/HGF Red.auto new file mode 100644 index 0000000..dcb5cb0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HGF Red.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7437038962619593, + "y": 4.320740582515391 + }, + "rotation": -59.682052822906385 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source to H" + } + }, + { + "type": "path", + "data": { + "pathName": "H to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "H Shoot to G" + } + }, + { + "type": "path", + "data": { + "pathName": "G to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "G Shoot to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Square Test.auto b/src/main/deploy/pathplanner/autos/Square Test.auto new file mode 100644 index 0000000..3201137 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Square Test.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Square Top" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Right" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Bottom" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Left" + } + } + ] + } + }, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Sraight Line.auto b/src/main/deploy/pathplanner/autos/Sraight Line.auto new file mode 100644 index 0000000..a8ab7a5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Sraight Line.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.5, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight Line" + } + } + ] + } + }, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D Down.path b/src/main/deploy/pathplanner/paths/D Down.path index aa79312..6b7cd02 100644 --- a/src/main/deploy/pathplanner/paths/D Down.path +++ b/src/main/deploy/pathplanner/paths/D Down.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "Reroutes", "previewStartingState": { "rotation": -90.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/H Up.path b/src/main/deploy/pathplanner/paths/H Up.path index f8ab84d..5f01f56 100644 --- a/src/main/deploy/pathplanner/paths/H Up.path +++ b/src/main/deploy/pathplanner/paths/H Up.path @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "Reroutes", "previewStartingState": { "rotation": 90.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Square Bottom.path b/src/main/deploy/pathplanner/paths/Square Bottom.path new file mode 100644 index 0000000..4f00d91 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Bottom.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.381966011250105, + "y": 5.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 5.5 + }, + "prevControl": { + "x": 3.5206906325745555, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Left.path b/src/main/deploy/pathplanner/paths/Square Left.path new file mode 100644 index 0000000..9b4fb71 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Left.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 6.618033988749895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 2.0, + "y": 5.479309367425445 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Right.path b/src/main/deploy/pathplanner/paths/Square Right.path new file mode 100644 index 0000000..52be2d5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Right.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.5, + "y": 5.88 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 5.5 + }, + "prevControl": { + "x": 3.5, + "y": 7.020690632574555 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Top.path b/src/main/deploy/pathplanner/paths/Square Top.path new file mode 100644 index 0000000..7364cca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Square Top.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.118033988749895, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 7.0 + }, + "prevControl": { + "x": 2.9019564509574227, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path new file mode 100644 index 0000000..3c285ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5, + "y": 5.552154629234854 + }, + "prevControl": null, + "nextControl": { + "x": 2.9, + "y": 5.552154629234854 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.15, + "y": 5.55 + }, + "prevControl": { + "x": 2.9000000000000004, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": "Tests", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d893ab6..060365a 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -2,8 +2,10 @@ import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; +import com.revrobotics.CANSparkBase.IdleMode; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.intake.IntakeShoot; +import com.stuypulse.robot.commands.leds.LEDReset; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; import com.stuypulse.robot.commands.shooter.ShooterManualIntake; import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; @@ -90,6 +92,8 @@ public void autonomousInit() { auto.schedule(); } + scheduler.schedule(new LEDReset()); + SmartDashboard.putString("Robot State", "AUTON"); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 04aea2a..2a32975 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,6 +13,7 @@ import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; +import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; import com.stuypulse.robot.commands.auton.BAC.FourPieceBCA; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeAcquireForever; @@ -318,10 +319,24 @@ public void configureAutons() { "Center to B", "B to C", "C to A"); AutonConfig BCA_RED = new AutonConfig("4 BCA RED", FourPieceBCA::new, "Center to B", "B to C", "C to A"); + // AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new, + // "Source to H", "H to Shoot", "H Shoot to G", "G to Shoot", "G Shoot to F", "F to Shoot"); + // AutonConfig HGF_RED = new AutonConfig("4 HGF RED", FourPieceHGF::new, + // "Source to H", "H to Shoot", "H Shoot to G", "G to Shoot", "G Shoot to F", "F to Shoot"); + AutonConfig ADEF = new AutonConfig("5 ADEF", FivePieceADEF::new, + "Amp to A", "A to D", "D to Shoot", "D Shoot to E", "E to Shoot", "E Shoot to F", "F to Shoot"); + AutonConfig ADEF_RED = new AutonConfig("5 ADEF RED", FivePieceADEF::new, + "Amp to A", "A to D", "D to Shoot", "D Shoot to E", "E to Shoot", "E Shoot to F", "F to Shoot"); BCA.registerDefaultBlue(autonChooser); BCA_RED.registerRed(autonChooser); + // HGF.registerBlue(autonChooser); + // HGF_RED.registerRed(autonChooser); + + ADEF.registerBlue(autonChooser); + ADEF_RED.registerRed(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 52debb2..7ea4cfa 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -4,6 +4,7 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -20,14 +21,17 @@ public FivePieceADEF(PathPlannerPath... paths) { new FollowPathAndIntake(paths[0]), ShootRoutine.fromAnywhere(), + // Drive, Intake, Shoot D new FollowPathAndIntake(paths[1]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), ShootRoutine.fromAnywhere(), + // Drive, Intake, Shoot E new FollowPathAndIntake(paths[3]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), ShootRoutine.fromAnywhere(), + // Drive, Intake, Shoot F new FollowPathAndIntake(paths[5]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), ShootRoutine.fromAnywhere() From 4dd8479841cde101eaaef4bca8fd828111e1fc02 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Tue, 3 Sep 2024 16:17:24 -0400 Subject: [PATCH 15/37] oopsies! --- src/main/java/com/stuypulse/robot/RobotContainer.java | 4 +--- src/main/java/com/stuypulse/robot/commands/auton/branch.java | 5 ----- 2 files changed, 1 insertion(+), 8 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/branch.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index f743300..120f974 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -14,7 +14,7 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; -import com.stuypulse.robot.commands.auton.BAC.FourPieceBCA; +import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeAcquireForever; import com.stuypulse.robot.commands.intake.IntakeDeacquire; @@ -314,8 +314,6 @@ protected void configureAutomaticCommandScheduling() { public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Mobility", new Mobility()); - autonChooser.addOption("Square Test", new SquareTest()); - autonChooser.addOption("Straight Line Test", new StraightLine()); AutonConfig BCA = new AutonConfig("4 BCA", FourPieceBCA::new, "Center to B", "B to C", "C to A"); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/branch.java b/src/main/java/com/stuypulse/robot/commands/auton/branch.java deleted file mode 100644 index d1427c6..0000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/branch.java +++ /dev/null @@ -1,5 +0,0 @@ -package com.stuypulse.robot.commands.auton; - -public class branch { - -} From 5f7ad3d5099250ed3d4e22571a736f7516966c12 Mon Sep 17 00:00:00 2001 From: Ian Shi <69531533+IanShiii@users.noreply.github.com> Date: Wed, 4 Sep 2024 13:47:41 -0400 Subject: [PATCH 16/37] Vision fixes (#39) * reject vision data with pose ambiguity higher than a certain threshold * fix comparison operator :sob: * update ferry alignment commands * new method to calculate arm angle for speaker --- .../com/stuypulse/robot/RobotContainer.java | 14 +-- ...java => SwerveDriveDriveAlignedFerry.java} | 4 +- .../SwerveDriveDriveAlignedLobFerry.java | 24 ---- ...> SwerveDriveDriveAlignedManualFerry.java} | 4 +- ...SwerveDriveDriveAlignedManualLobFerry.java | 23 ---- .../stuypulse/robot/constants/Settings.java | 4 + .../robot/subsystems/arm/ArmImpl.java | 104 +++++++++++++----- .../robot/subsystems/vision/PhotonVision.java | 19 ++-- 8 files changed, 101 insertions(+), 95 deletions(-) rename src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/{SwerveDriveDriveAlignedLowFerry.java => SwerveDriveDriveAlignedFerry.java} (81%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java rename src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/{SwerveDriveDriveAlignedManualLowFerry.java => SwerveDriveDriveAlignedManualFerry.java} (77%) delete mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 120f974..6f7e9a4 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -37,10 +37,8 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedAmp; -import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLobFerry; -import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedLowFerry; -import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualLobFerry; -import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualLowFerry; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedFerry; +import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedManualFerry; import com.stuypulse.robot.commands.swerve.driveAligned.SwerveDriveDriveAlignedSpeaker; import com.stuypulse.robot.commands.swerve.noteAlignment.SwerveDriveDriveToNote; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; @@ -186,7 +184,7 @@ private void configureDriverBindings() { // lob ferry align and shoot driver.getLeftStickButton() - .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedFerry(driver) .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) @@ -203,7 +201,7 @@ private void configureDriverBindings() { // low ferry align and shoot driver.getRightStickButton() - .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedFerry(driver) .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) @@ -235,7 +233,7 @@ private void configureDriverBindings() { // manual lob ferry driver.getTopButton() - .whileTrue(new SwerveDriveDriveAlignedManualLobFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) @@ -251,7 +249,7 @@ private void configureDriverBindings() { // manual low ferry driver.getLeftButton() - .whileTrue(new SwerveDriveDriveAlignedManualLowFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedFerry.java similarity index 81% rename from src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedFerry.java index 70d1445..2acf4d2 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedFerry.java @@ -6,9 +6,9 @@ import edu.wpi.first.math.geometry.Rotation2d; -public class SwerveDriveDriveAlignedLowFerry extends SwerveDriveDriveAligned { +public class SwerveDriveDriveAlignedFerry extends SwerveDriveDriveAligned { - public SwerveDriveDriveAlignedLowFerry(Gamepad driver) { + public SwerveDriveDriveAlignedFerry(Gamepad driver) { super(driver); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java deleted file mode 100644 index 6b92860..0000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedLobFerry.java +++ /dev/null @@ -1,24 +0,0 @@ -package com.stuypulse.robot.commands.swerve.driveAligned; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.input.Gamepad; -import edu.wpi.first.math.geometry.Rotation2d; - - -public class SwerveDriveDriveAlignedLobFerry extends SwerveDriveDriveAligned { - - public SwerveDriveDriveAlignedLobFerry(Gamepad driver) { - super(driver); - } - - @Override - protected Rotation2d getTargetAngle() { - return SwerveDrive.getInstance().getPose().getTranslation().minus(Field.getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); - } - - @Override - protected double getDistanceToTarget() { - return SwerveDrive.getInstance().getPose().getTranslation().getDistance(Field.getAmpCornerPose()); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLowFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualFerry.java similarity index 77% rename from src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLowFerry.java rename to src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualFerry.java index a4a71e8..44ffb4e 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLowFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualFerry.java @@ -5,9 +5,9 @@ import edu.wpi.first.math.geometry.Rotation2d; -public class SwerveDriveDriveAlignedManualLowFerry extends SwerveDriveDriveAligned { +public class SwerveDriveDriveAlignedManualFerry extends SwerveDriveDriveAligned { - public SwerveDriveDriveAlignedManualLowFerry(Gamepad driver) { + public SwerveDriveDriveAlignedManualFerry(Gamepad driver) { super(driver); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java deleted file mode 100644 index 614d976..0000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedManualLobFerry.java +++ /dev/null @@ -1,23 +0,0 @@ -package com.stuypulse.robot.commands.swerve.driveAligned; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.stuylib.input.Gamepad; -import edu.wpi.first.math.geometry.Rotation2d; - - -public class SwerveDriveDriveAlignedManualLobFerry extends SwerveDriveDriveAligned { - - public SwerveDriveDriveAlignedManualLobFerry(Gamepad driver) { - super(driver); - } - - @Override - protected Rotation2d getTargetAngle() { - return Field.getManualFerryPosition().minus(Field.getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); - } - - @Override - protected double getDistanceToTarget() { - return Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose()); - } -} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 58432d5..deb7f9f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -137,6 +137,9 @@ public interface Shooter { new SmartNumber("Shooter/Speaker RPM differential", 500) ); + // TODO: Find velocity + double SPEAKER_SHOT_VELOCITY = 10.0; // m/s + // Different falling debounce is used to detect note shooting; SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.0); //0.01 SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.0); //0.005 @@ -410,6 +413,7 @@ public interface Rotation { public interface Vision { SmartBoolean IS_ACTIVE = new SmartBoolean("Vision/Is Active", true); + double POSE_AMBIGUITY_RATIO_THRESHOLD = 0.25; } public interface Buzz { diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 005c34a..6a46d5a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -12,6 +12,7 @@ import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; +import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; @@ -22,7 +23,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DigitalInput; @@ -110,36 +114,43 @@ private double getTargetDegrees() { } } + private double getNoteHeightAtSpeakerGivenArmAngle(double armAngle) { + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + + Rotation2d robotAngleToSpeaker = Field.getAllianceSpeakerPose().minus(robotPose).getRotation(); + Translation2d noteStartingPose = SwerveDrive.getInstance().getPose() + .minus(new Pose2d(Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * -robotAngleToSpeaker.getCos(), Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * -robotAngleToSpeaker.getSin(), robotAngleToSpeaker)) + .getTranslation(); + Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + + double horizontalDistance = noteStartingPose.getDistance(speakerPose); + + double shooterAngle = armAngle + 180 - Settings.ANGLE_BETWEEN_ARM_AND_SHOOTER; + + return -16 * Math.pow(horizontalDistance / (Settings.Shooter.SPEAKER_SHOT_VELOCITY * Math.cos(Angle.fromDegrees(shooterAngle).toRadians())), 2) + + Math.tan(Angle.fromDegrees(shooterAngle).toRadians()) * horizontalDistance + + Settings.HEIGHT_TO_ARM_PIVOT - (Math.sin(Angle.fromDegrees(armAngle).toRadians()) * Settings.Arm.LENGTH); + } + private double getSpeakerAngle() { try { - Pose3d speakerPose = new Pose3d( - Field.getAllianceSpeakerPose().getX(), - Field.getAllianceSpeakerPose().getY(), - Field.SPEAKER_MAX_HEIGHT, - new Rotation3d() - ); - - Pose2d robotPose = SwerveDrive.getInstance().getPose(); - - Pose3d armPivotPose = new Pose3d( - robotPose.getX() + Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * robotPose.getRotation().getCos(), - robotPose.getY() + Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * robotPose.getRotation().getSin(), - Settings.HEIGHT_TO_ARM_PIVOT, - new Rotation3d() - ); - - Translation3d pivotToSpeaker = speakerPose.minus(armPivotPose).getTranslation(); - - double angleFromPivotToSpeaker = Units.radiansToDegrees( - Math.atan( - pivotToSpeaker.getZ() - / pivotToSpeaker.toTranslation2d().getNorm() - ) - ); - - double angleBetweenPivotToSpeakerAndArm = Units.radiansToDegrees(Math.acos(Settings.Arm.LENGTH / pivotToSpeaker.getNorm())); - - return -(angleBetweenPivotToSpeakerAndArm - angleFromPivotToSpeaker); + double angle = Settings.Arm.MIN_ANGLE.get(); + double angleIncrement = 0.4; + double heightToleranceAtSpeakerOpening = (Field.SPEAKER_MAX_HEIGHT - Field.SPEAKER_MIN_HEIGHT) / 4; + double lastError = Double.MAX_VALUE; + while (angle < Settings.Arm.SUBWOOFER_SHOT_ANGLE.get()) { + double error = Math.abs(getNoteHeightAtSpeakerGivenArmAngle(angle) - (Field.SPEAKER_MAX_HEIGHT +Field.SPEAKER_MIN_HEIGHT)/2); + if (error < heightToleranceAtSpeakerOpening) { + return angle; + } + + if (error > lastError) { + return angle - angleIncrement * 0.75; + } + + angle += angleIncrement; + } + return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); } catch (Exception exception) { exception.printStackTrace(); @@ -147,6 +158,43 @@ private double getSpeakerAngle() { } } + // private double getSpeakerAngle() { + // try { + // Pose3d speakerPose = new Pose3d( + // Field.getAllianceSpeakerPose().getX(), + // Field.getAllianceSpeakerPose().getY(), + // Field.SPEAKER_MAX_HEIGHT, + // new Rotation3d() + // ); + + // Pose2d robotPose = SwerveDrive.getInstance().getPose(); + + // Pose3d armPivotPose = new Pose3d( + // robotPose.getX() + Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * robotPose.getRotation().getCos(), + // robotPose.getY() + Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * robotPose.getRotation().getSin(), + // Settings.HEIGHT_TO_ARM_PIVOT, + // new Rotation3d() + // ); + + // Translation3d pivotToSpeaker = speakerPose.minus(armPivotPose).getTranslation(); + + // double angleFromPivotToSpeaker = Units.radiansToDegrees( + // Math.atan( + // pivotToSpeaker.getZ() + // / pivotToSpeaker.toTranslation2d().getNorm() + // ) + // ); + + // double angleBetweenPivotToSpeakerAndArm = Units.radiansToDegrees(Math.acos(Settings.Arm.LENGTH / pivotToSpeaker.getNorm())); + + // return -(angleBetweenPivotToSpeakerAndArm - angleFromPivotToSpeaker); + // } + // catch (Exception exception) { + // exception.printStackTrace(); + // return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); + // } + // } + private double getDegrees() { return 360 * armEncoder.getPosition(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java index 1e645aa..cb2dc88 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.subsystems.vision; import com.stuypulse.robot.constants.Cameras; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.util.vision.VisionData; @@ -103,14 +104,16 @@ public void periodic() { final int index = i; if (enabled[index]) { PhotonPipelineResult latestResult = cameras[index].getLatestResult(); - Optional estimatedRobotPose = poseEstimators[index].update(latestResult); - estimatedRobotPose.ifPresent( - (EstimatedRobotPose robotPose) -> { - VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea()); - outputs.add(data); - updateTelemetry("Vision/" + cameras[index].getName(), data); - } - ); + if (latestResult.getBestTarget().getPoseAmbiguity() < Settings.Vision.POSE_AMBIGUITY_RATIO_THRESHOLD) { + Optional estimatedRobotPose = poseEstimators[index].update(latestResult); + estimatedRobotPose.ifPresent( + (EstimatedRobotPose robotPose) -> { + VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea()); + outputs.add(data); + updateTelemetry("Vision/" + cameras[index].getName(), data); + } + ); + } } } From 28823a1f8c63e4b94bdbf404a3fa78ad6d5dda8f Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 15:22:58 -0400 Subject: [PATCH 17/37] automatic state machine for flywheel, move some triggers to subsystem periodics --- .../com/stuypulse/robot/RobotContainer.java | 68 +++---------- .../commands/arm/ArmToLobFerryManual.java | 10 ++ .../commands/arm/ArmToLowFerryManual.java | 10 ++ .../commands/shooter/ShooterScoreSpeaker.java | 16 ---- .../robot/commands/shooter/ShooterSetRPM.java | 30 ------ .../robot/commands/shooter/ShooterStop.java | 21 ---- .../commands/shooter/ShooterToFerry.java | 11 --- .../shooter/ShooterToLobFerryManual.java | 19 ---- .../shooter/ShooterToLowFerryManual.java | 19 ---- .../stuypulse/robot/constants/Settings.java | 4 +- .../stuypulse/robot/subsystems/arm/Arm.java | 2 + .../robot/subsystems/arm/ArmImpl.java | 5 +- .../robot/subsystems/intake/Intake.java | 16 ++++ .../robot/subsystems/shooter/Shooter.java | 81 +++++++++++----- .../robot/subsystems/shooter/ShooterImpl.java | 96 +++++++++++++------ 15 files changed, 179 insertions(+), 229 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerryManual.java create mode 100644 src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerryManual.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterToFerry.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLobFerryManual.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLowFerryManual.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 91dec58..65c0ec8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -27,9 +27,6 @@ import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; import com.stuypulse.robot.commands.shooter.ShooterManualIntake; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.ShooterSetRPM; -import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; @@ -131,7 +128,7 @@ private void configureDriverBindings() { driver.getDPadUp() .onTrue(new ArmToPreClimb()) - .onTrue(new ShooterStop()) + .onTrue(new ShooterFeederStop()) .onTrue(new IntakeStop()); driver.getDPadDown().onTrue(new ArmToClimbing()); @@ -168,7 +165,7 @@ private void configureDriverBindings() { .alongWith(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE)))), new SwerveDriveDriveAlignedSpeaker(driver) - .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) + .alongWith(new ArmToSpeaker() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker())) @@ -176,15 +173,12 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), () -> Arm.getInstance().getState() == Arm.State.AMP)) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()); // lob ferry align and shoot driver.getLeftStickButton() .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver) - .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .alongWith(new ArmToLobFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry())) @@ -192,16 +186,13 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()); // low ferry align and shoot driver.getRightStickButton() .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver) - .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry())) @@ -209,31 +200,25 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()); // arm to amp driver.getLeftBumper().onTrue(new ArmToAmp()); // manual speaker at subwoofer driver.getRightMenuButton() - .whileTrue(new ArmToSubwooferShot().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) + .whileTrue(new ArmToSubwooferShot() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new ShooterFeederShoot()) ) .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL)) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()); // manual lob ferry driver.getTopButton() .whileTrue(new SwerveDriveDriveAlignedManualLobFerry(driver) - .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .alongWith(new ArmToLobFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry())) @@ -241,15 +226,12 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()); // manual low ferry driver.getLeftButton() .whileTrue(new SwerveDriveDriveAlignedManualLowFerry(driver) - .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry())) @@ -259,10 +241,7 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()); // human player attention button driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION)); @@ -281,27 +260,6 @@ private void configureAutomaticCommandScheduling() { && intake.getState() != Intake.State.DEACQUIRING) // .onTrue(new ShooterAcquireFromIntakeWithRetry().andThen(new BuzzController(driver))); .onTrue(new ShooterAcquireFromIntake().andThen(new BuzzController(driver))); - - // feeder automatically pushes note further into shooter when its sticking too far out - new Trigger(() -> arm.getState() == Arm.State.AMP - && !shooter.hasNote() - && shooter.getFeederState() != Shooter.FeederState.DEACQUIRING) - .onTrue(new ShooterManualIntake().until(() -> arm.getState() != Arm.State.AMP)); - - // run the intake when the arm is moving up from a low angle (to prevent intake from gripping it) - new Trigger(() -> arm.getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED - && arm.atIntakeShouldShootAngle()) - .onTrue(new IntakeShoot() - .until( - () -> arm.getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED - || !arm.atIntakeShouldShootAngle() - ) - ); - - // run the intake when shooting in case the intake is holding onto the note also - new Trigger(() -> shooter.getFeederState() == Shooter.FeederState.SHOOTING - && arm.atIntakeShouldShootAngle()) - .onTrue(new IntakeShoot().until(() -> shooter.getFeederState() != Shooter.FeederState.SHOOTING)); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerryManual.java new file mode 100644 index 0000000..666dda2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerryManual.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; + +public class ArmToLobFerryManual extends ArmSetState{ + + public ArmToLobFerryManual(){ + super(Arm.State.LOB_FERRY_MANUAL); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerryManual.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerryManual.java new file mode 100644 index 0000000..e2b98a1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLowFerryManual.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.arm; + +import com.stuypulse.robot.subsystems.arm.Arm; + +public class ArmToLowFerryManual extends ArmSetState{ + + public ArmToLowFerryManual(){ + super(Arm.State.LOW_FERRY_MANUAL); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java deleted file mode 100644 index 76b2d34..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterScoreSpeaker.java +++ /dev/null @@ -1,16 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class ShooterScoreSpeaker extends SequentialCommandGroup { - - public ShooterScoreSpeaker() { - addCommands( - new ShooterSetRPM(Settings.Shooter.SPEAKER), - new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET), - new ShooterFeederShoot() - ); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java deleted file mode 100644 index 31928d8..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import java.util.function.Supplier; - -import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.robot.util.ShooterSpeeds; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ShooterSetRPM extends InstantCommand { - - private final Shooter shooter; - private final Supplier speeds; - - public ShooterSetRPM(Supplier speeds) { - shooter = Shooter.getInstance(); - this.speeds = speeds; - addRequirements(shooter); - } - - public ShooterSetRPM(ShooterSpeeds speeds) { - this(() -> speeds); - } - - @Override - public void initialize() { - shooter.setTargetSpeeds(speeds.get()); - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java deleted file mode 100644 index 5c1e236..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java +++ /dev/null @@ -1,21 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.subsystems.shooter.Shooter; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ShooterStop extends InstantCommand { - - private final Shooter shooter; - - public ShooterStop() { - shooter = Shooter.getInstance(); - addRequirements(shooter); - } - - @Override - public void initialize() { - shooter.stop(); - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToFerry.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToFerry.java deleted file mode 100644 index c9224a1..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToFerry.java +++ /dev/null @@ -1,11 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.subsystems.shooter.Shooter; - -public class ShooterToFerry extends ShooterSetRPM { - - public ShooterToFerry() { - super(Shooter.getInstance()::getFerrySpeeds); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLobFerryManual.java deleted file mode 100644 index 104daa3..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLobFerryManual.java +++ /dev/null @@ -1,19 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.util.ShooterLobFerryInterpolation; -import com.stuypulse.robot.util.ShooterSpeeds; - -import edu.wpi.first.math.util.Units; - -public class ShooterToLobFerryManual extends ShooterSetRPM { - - public ShooterToLobFerryManual() { - super(new ShooterSpeeds(ShooterLobFerryInterpolation.getRPM(getFerryDistance()))); - } - - private static double getFerryDistance() { - return Units.metersToInches(Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose())); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLowFerryManual.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLowFerryManual.java deleted file mode 100644 index db0bd36..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterToLowFerryManual.java +++ /dev/null @@ -1,19 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.util.ShooterLowFerryInterpolation; -import com.stuypulse.robot.util.ShooterSpeeds; - -import edu.wpi.first.math.util.Units; - -public class ShooterToLowFerryManual extends ShooterSetRPM { - - public ShooterToLowFerryManual() { - super(new ShooterSpeeds(ShooterLowFerryInterpolation.getRPM(getFerryDistance()))); - } - - private static double getFerryDistance() { - return Units.metersToInches(Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose())); - } - -} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f1b4dae..f9c68e5 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -127,10 +127,8 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; - boolean ALWAYS_KEEP_AT_SPEED = true; - double TARGET_RPM_THRESHOLD = 200; - double MAX_WAIT_TO_REACH_TARGET = ALWAYS_KEEP_AT_SPEED ? 1.5 : 2.0; + double MAX_WAIT_TO_REACH_TARGET = 2.0; ShooterSpeeds SPEAKER = new ShooterSpeeds( new SmartNumber("Shooter/Speaker RPM", 5500), diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index a3547b7..8be89ff 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -20,7 +20,9 @@ public enum State { SUBWOOFER_SHOT, SPEAKER, LOW_FERRY, + LOW_FERRY_MANUAL, LOB_FERRY, + LOB_FERRY_MANUAL, FEED, STOW, PRE_CLIMB, diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 005c34a..f94f46a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -95,8 +95,12 @@ private double getTargetDegrees() { return getSpeakerAngle(); case LOW_FERRY: return Settings.Arm.LOW_FERRY_ANGLE.get(); + case LOW_FERRY_MANUAL: + return Settings.Arm.LOW_FERRY_ANGLE.get(); case LOB_FERRY: return Settings.Arm.LOB_FERRY_ANGLE.get(); + case LOB_FERRY_MANUAL: + return Settings.Arm.LOB_FERRY_ANGLE.get(); case FEED: return Settings.Arm.FEED_ANGLE.get(); case STOW: @@ -209,7 +213,6 @@ else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTrigger SmartDashboard.putNumber("Arm/Right Duty Cycle", rightMotor.get()); SmartDashboard.putNumber("Arm/Arm Angle", getDegrees()); - SmartDashboard.putNumber("Arm/Shooter Angle", getDegrees() + 96); // shooter is offset 96 degrees counterclockwise from arm (thanks kevin)] SmartDashboard.putBoolean("Arm/At Target", atTarget()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 43b7ddd..ee86f49 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -1,5 +1,9 @@ package com.stuypulse.robot.subsystems.intake; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.shooter.Shooter; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -42,6 +46,18 @@ public State getState() { @Override public void periodic() { + // run the intake when the arm is moving up from a low angle (to prevent intake from gripping it) + // run the intake when shooting in case the intake is holding onto the note also + boolean shouldShoot = (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && Arm.getInstance().atIntakeShouldShootAngle()) + || (Arm.getInstance().atIntakeShouldShootAngle() && Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED); + + if (state == State.STOP && shouldShoot) { + setState(State.SHOOTING); + } + if (state == State.SHOOTING && !shouldShoot) { + setState(State.STOP); + } + SmartDashboard.putString("Intake/State", state.name()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 6f5fad3..023e608 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -2,6 +2,7 @@ import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.util.ShooterSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,34 +26,21 @@ public enum FeederState { STOP } + public enum FlywheelState { + STOP, + SPEAKER, + LOW_FERRY, + LOB_FERRY, + LOW_FERRY_MANUAL, + LOB_FERRY_MANUAL, + } + private FeederState feederState; - - private final SmartNumber leftTargetRPM; - private final SmartNumber rightTargetRPM; + private FlywheelState flywheelState; public Shooter() { feederState = FeederState.STOP; - leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getLeftRPM() : 0); - rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getRightRPM() : 0); - } - - public void setTargetSpeeds(ShooterSpeeds speeds) { - this.leftTargetRPM.set(speeds.getLeftRPM()); - this.rightTargetRPM.set(speeds.getRightRPM()); - } - - public double getLeftTargetRPM() { - return leftTargetRPM.get(); - } - - public double getRightTargetRPM() { - return rightTargetRPM.get(); - } - - public void stop() { - setFeederState(FeederState.STOP); - leftTargetRPM.set(0); - rightTargetRPM.set(0); + flywheelState = FlywheelState.SPEAKER; } public void setFeederState(FeederState feederState) { @@ -63,9 +51,50 @@ public FeederState getFeederState() { return feederState; } - public abstract boolean atTargetSpeeds(); + public FlywheelState getFlywheelState() { + return flywheelState; + } - public abstract ShooterSpeeds getFerrySpeeds(); + public abstract boolean atTargetSpeeds(); public abstract boolean hasNote(); + + @Override + public void periodic() { + // feeder automatically pushes note further into shooter when its sticking too far out + if (Arm.getInstance().getState() == Arm.State.AMP && !hasNote() && feederState != FeederState.DEACQUIRING) { + feederState = FeederState.INTAKING; + } + + if (feederState == FeederState.INTAKING && hasNote()) { + feederState = FeederState.STOP; + } + + // automatically determine flywheel speeds based on arm state + switch (Arm.getInstance().getState()) { + case LOW_FERRY: + flywheelState = FlywheelState.LOW_FERRY; + break; + case LOW_FERRY_MANUAL: + flywheelState = FlywheelState.LOW_FERRY_MANUAL; + break; + case LOB_FERRY: + flywheelState = FlywheelState.LOB_FERRY; + break; + case LOB_FERRY_MANUAL: + flywheelState = FlywheelState.LOB_FERRY_MANUAL; + break; + case SPEAKER: + flywheelState = FlywheelState.SPEAKER; + break; + case CLIMBING: + flywheelState = FlywheelState.STOP; + break; + case PRE_CLIMB: + flywheelState = FlywheelState.STOP; + break; + default: + break; + } + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 65bf49e..0fd72cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -17,6 +17,7 @@ import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterLowFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; +import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; @@ -41,7 +42,8 @@ public class ShooterImpl extends Shooter { private final BStream hasNote; - private boolean isShooting; + private final SmartNumber leftTargetRPM; + private final SmartNumber rightTargetRPM; protected ShooterImpl() { leftMotor = new CANSparkMax(Ports.Shooter.LEFT_MOTOR, MotorType.kBrushless); @@ -81,7 +83,8 @@ protected ShooterImpl() { Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor); Motors.Shooter.FEEDER_MOTOR.configure(feederMotor); - isShooting = false; + leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.SPEAKER.getLeftRPM()); + rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.SPEAKER.getRightRPM()); } private double getLeftShooterRPM() { @@ -94,8 +97,13 @@ private double getRightShooterRPM() { @Override public boolean atTargetSpeeds() { - return Math.abs(getLeftShooterRPM() - getLeftTargetRPM()) < Settings.Shooter.TARGET_RPM_THRESHOLD - && Math.abs(getRightShooterRPM() - getRightTargetRPM()) < Settings.Shooter.TARGET_RPM_THRESHOLD; + return Math.abs(getLeftShooterRPM() - leftTargetRPM.get()) < Settings.Shooter.TARGET_RPM_THRESHOLD + && Math.abs(getRightShooterRPM() - rightTargetRPM.get()) < Settings.Shooter.TARGET_RPM_THRESHOLD; + } + + private void setTargetSpeeds(ShooterSpeeds speeds) { + this.leftTargetRPM.set(speeds.getLeftRPM()); + this.rightTargetRPM.set(speeds.getRightRPM()); } private void setLeftShooterRPM(double rpm) { @@ -126,48 +134,80 @@ private void setFeederBasedOnState() { } } + private void setFlywheelsBasedOnState() { + double manualFerryDistance = Units.metersToInches(Field.getManualFerryPosition().getDistance(Field.getAmpCornerPose())); + switch (getFlywheelState()) { + case SPEAKER: + setTargetSpeeds(Settings.Shooter.SPEAKER); + break; + case LOW_FERRY: + setTargetSpeeds(getLowFerrySpeeds()); + break; + case LOW_FERRY_MANUAL: + setTargetSpeeds(new ShooterSpeeds(ShooterLowFerryInterpolation.getRPM(manualFerryDistance))); + break; + case LOB_FERRY: + setTargetSpeeds(getLobFerrySpeeds()); + break; + case LOB_FERRY_MANUAL: + setTargetSpeeds(new ShooterSpeeds(ShooterLobFerryInterpolation.getRPM(manualFerryDistance))); + break; + case STOP: + setTargetSpeeds(new ShooterSpeeds()); + break; + default: + setTargetSpeeds(new ShooterSpeeds()); + break; + } + + if (leftTargetRPM.get() == 0) { + leftMotor.set(0); + } + else { + setLeftShooterRPM(leftTargetRPM.get()); + } + + if (rightTargetRPM.get() == 0) { + rightMotor.set(0); + } + else { + setRightShooterRPM(rightTargetRPM.get()); + } + } + @Override public boolean hasNote() { return hasNote.get(); } - @Override - public ShooterSpeeds getFerrySpeeds() { + private ShooterSpeeds getLowFerrySpeeds() { Translation2d ferryZone = Robot.isBlue() ? new Translation2d(0, Field.WIDTH - 1.5) : new Translation2d(0, 1.5); double distanceToFerryInInches = Units.metersToInches(SwerveDrive.getInstance().getPose().getTranslation().getDistance(ferryZone)); - if (Arm.getInstance().getState() == Arm.State.LOB_FERRY) { - double targetRPM = ShooterLobFerryInterpolation.getRPM(distanceToFerryInInches); - return new ShooterSpeeds(targetRPM, 500); - } - else { - double targetRPM = ShooterLowFerryInterpolation.getRPM(distanceToFerryInInches); - return new ShooterSpeeds(targetRPM, 500); - } + double targetRPM = ShooterLobFerryInterpolation.getRPM(distanceToFerryInInches); + return new ShooterSpeeds(targetRPM, 500); + } + + private ShooterSpeeds getLobFerrySpeeds() { + Translation2d ferryZone = Robot.isBlue() + ? new Translation2d(0, Field.WIDTH - 1.5) + : new Translation2d(0, 1.5); + + double distanceToFerryInInches = Units.metersToInches(SwerveDrive.getInstance().getPose().getTranslation().getDistance(ferryZone)); + + double targetRPM = ShooterLowFerryInterpolation.getRPM(distanceToFerryInInches); + return new ShooterSpeeds(targetRPM, 500); } @Override public void periodic () { super.periodic(); - if (getLeftTargetRPM() == 0) { - leftMotor.set(0); - } - else { - setLeftShooterRPM(getLeftTargetRPM()); - } - - if (getRightTargetRPM() == 0) { - rightMotor.set(0); - } - else { - setRightShooterRPM(getRightTargetRPM()); - } - setFeederBasedOnState(); + setFlywheelsBasedOnState(); SmartDashboard.putNumber("Shooter/Feeder Speed", feederMotor.get()); From 8420e92b5ae4e63bf2b1f6f5037125e8aaad8464 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 15:34:21 -0400 Subject: [PATCH 18/37] resolve errors --- .../com/stuypulse/robot/RobotContainer.java | 8 ++--- .../commands/auton/ADEF/FivePieceADEF.java | 8 ----- .../commands/auton/BCA/FourPieceBCA.java | 7 ---- .../auton/BF_Series/FivePieceBFAC.java | 3 -- .../auton/BF_Series/FivePieceBFCA.java | 8 ++--- .../auton/BF_Series/FivePieceBFED.java | 8 ++--- .../auton/BF_Series/FivePieceBFGH.java | 8 ++--- .../commands/auton/FollowPathWithShoot.java | 23 ------------ .../auton/FollowPathWithShootAndIntake.java | 35 ------------------- .../robot/commands/auton/ShootRoutine.java | 4 +-- 10 files changed, 11 insertions(+), 101 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 05c3bc7..6a8f1b8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -178,7 +178,7 @@ private void configureDriverBindings() { // lob ferry align and shoot driver.getLeftStickButton() - .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedFerry(driver) .alongWith(new ArmToLobFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) @@ -192,7 +192,7 @@ private void configureDriverBindings() { // low ferry align and shoot driver.getRightStickButton() - .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedFerry(driver) .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) @@ -218,7 +218,7 @@ private void configureDriverBindings() { // manual lob ferry driver.getTopButton() - .whileTrue(new SwerveDriveDriveAlignedManualLobFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) .alongWith(new ArmToLobFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) @@ -231,7 +231,7 @@ private void configureDriverBindings() { // manual low ferry driver.getLeftButton() - .whileTrue(new SwerveDriveDriveAlignedManualLowFerry(driver) + .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 5e69f21..1fc3b99 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -3,11 +3,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.ShooterSetRPM; -import com.stuypulse.robot.commands.shooter.ShooterSetRPM; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -22,19 +17,16 @@ public FivePieceADEF(PathPlannerPath... paths) { new FollowPathAndIntake(paths[0]), ShootRoutine.fromAnywhere(), - // Drive, Intake, Shoot D // Drive, Intake, Shoot D new FollowPathAndIntake(paths[1]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), ShootRoutine.fromAnywhere(), - // Drive, Intake, Shoot E // Drive, Intake, Shoot E new FollowPathAndIntake(paths[3]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), ShootRoutine.fromAnywhere(), - // Drive, Intake, Shoot F // Drive, Intake, Shoot F new FollowPathAndIntake(paths[5]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index 14e0bc0..d4745cd 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -2,16 +2,9 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.arm.ArmSetState; -import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; -import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; -import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; -import com.stuypulse.robot.commands.shooter.ShooterSetRPM; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java index 95a36b8..12fe070 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java @@ -3,9 +3,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java index 721a94d..c2e0d4f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java @@ -3,7 +3,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -15,13 +14,10 @@ public class FivePieceBFCA extends SequentialCommandGroup { public FivePieceBFCA(PathPlannerPath... paths) { addCommands( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), + ShootRoutine.fromSubwoofer(), new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[1]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java index 72ba47d..31284a4 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java @@ -3,7 +3,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -15,13 +14,10 @@ public class FivePieceBFED extends SequentialCommandGroup { public FivePieceBFED(PathPlannerPath... paths) { addCommands( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), + ShootRoutine.fromSubwoofer(), new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[1]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java index 5e806c9..e2ef338 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java @@ -3,7 +3,6 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -15,13 +14,10 @@ public class FivePieceBFGH extends SequentialCommandGroup { public FivePieceBFGH(PathPlannerPath... paths) { addCommands( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), + ShootRoutine.fromSubwoofer(), new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), + ShootRoutine.fromAnywhere(), new FollowPathAndIntake(paths[1]), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java deleted file mode 100644 index 84370df..0000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java +++ /dev/null @@ -1,23 +0,0 @@ - -package com.stuypulse.robot.commands.auton; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class FollowPathWithShoot extends ParallelCommandGroup { - - public FollowPathWithShoot(PathPlannerPath path, double shootTime) { - addCommands( - SwerveDrive.getInstance().followPathCommand(path), - new WaitCommand(shootTime) - .andThen(new ShooterWaitForTarget()) - .andThen(new ShooterScoreSpeaker()) - ); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java deleted file mode 100644 index 77c6afd..0000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java +++ /dev/null @@ -1,35 +0,0 @@ -package com.stuypulse.robot.commands.auton; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.intake.IntakeAcquire; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.constants.Settings.Auton; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.subsystems.intake.Intake; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class FollowPathWithShootAndIntake extends SequentialCommandGroup { - - public FollowPathWithShootAndIntake(PathPlannerPath path, double shootTime) { - this(path, shootTime, Auton.DEFAULT_INTAKE_TIMEOUT); - } - - public FollowPathWithShootAndIntake(PathPlannerPath path, double shootTime, double intakeTimeout) { - addCommands( - new ParallelCommandGroup( - SwerveDrive.getInstance().followPathCommand(path) - .until(() -> Intake.getInstance().hasNote()), - new SequentialCommandGroup( - new WaitCommand(shootTime), - new ShooterWaitForTarget(), - new ShooterScoreSpeaker(), - new IntakeAcquire() - ) - ) - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java index 84f918e..16f948f 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -5,7 +5,6 @@ import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.leds.LEDSet; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; -import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveAlignToSpeaker; import com.stuypulse.robot.constants.LEDInstructions; @@ -21,7 +20,6 @@ public abstract class ShootRoutine { public static Command fromSubwoofer() { return new SequentialCommandGroup( new ArmToSubwooferShot(), - new ShooterSetRPM(Settings.Shooter.SPEAKER), new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), new ShooterFeederShoot() ); @@ -29,7 +27,7 @@ public static Command fromSubwoofer() { public static Command fromAnywhere() { return new SwerveDriveAlignToSpeaker() - .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) + .alongWith(new ArmToSpeaker() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> SwerveDrive.getInstance().isAlignedToSpeaker())) From 0a207a4ce14832510c2ef2eeeb69fb399d3fae3e Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 15:48:06 -0400 Subject: [PATCH 19/37] move handoff automation to subsystem periodics --- src/main/java/com/stuypulse/robot/Robot.java | 1 - .../com/stuypulse/robot/RobotContainer.java | 11 ----------- .../robot/subsystems/intake/Intake.java | 18 ++++++++++++++++++ .../robot/subsystems/shooter/Shooter.java | 16 +++++++++++++++- 4 files changed, 33 insertions(+), 13 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 060365a..e12c4bf 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -113,7 +113,6 @@ public void teleopInit() { if (auto != null) { auto.cancel(); } - robot.configureAutomaticCommandScheduling(); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 6a8f1b8..ea3e0c8 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -252,17 +252,6 @@ private void configureOperatorBindings() { } - protected void configureAutomaticCommandScheduling() { - // automatic handoff - new Trigger(() -> arm.getState() == Arm.State.FEED - && arm.atTarget() - && !shooter.hasNote() - && intake.hasNote() - && intake.getState() != Intake.State.DEACQUIRING) - // .onTrue(new ShooterAcquireFromIntakeWithRetry().andThen(new BuzzController(driver))); - .onTrue(new ShooterAcquireFromIntake().andThen(new BuzzController(driver))); - } - /**************/ /*** AUTONS ***/ /**************/ diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index ee86f49..558e53e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -46,6 +46,24 @@ public State getState() { @Override public void periodic() { + if (state == State.ACQUIRING && hasNote()) { + state = State.STOP; + } + + // automatic handoff + boolean shouldHandoff = Arm.getInstance().getState() == Arm.State.FEED + && Arm.getInstance().atValidFeedAngle() + && !Shooter.getInstance().hasNote() + && hasNote() + && getState() != Intake.State.DEACQUIRING; + + if (shouldHandoff) { + setState(State.FEEDING); + } + if (state == State.FEEDING && !shouldHandoff) { + setState(State.STOP); + } + // run the intake when the arm is moving up from a low angle (to prevent intake from gripping it) // run the intake when shooting in case the intake is holding onto the note also boolean shouldShoot = (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && Arm.getInstance().atIntakeShouldShootAngle()) diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 023e608..32d3782 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,7 @@ import com.stuypulse.stuylib.network.SmartNumber; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.util.ShooterSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -65,11 +66,24 @@ public void periodic() { if (Arm.getInstance().getState() == Arm.State.AMP && !hasNote() && feederState != FeederState.DEACQUIRING) { feederState = FeederState.INTAKING; } - if (feederState == FeederState.INTAKING && hasNote()) { feederState = FeederState.STOP; } + // automatic handoff + boolean shouldHandoff = Arm.getInstance().getState() == Arm.State.FEED + && Arm.getInstance().atValidFeedAngle() + && hasNote() + && Intake.getInstance().hasNote() + && Intake.getInstance().getState() != Intake.State.DEACQUIRING; + + if (shouldHandoff) { + setFeederState(FeederState.INTAKING); + } + if (feederState == FeederState.INTAKING && !shouldHandoff) { + setFeederState(FeederState.STOP); + } + // automatically determine flywheel speeds based on arm state switch (Arm.getInstance().getState()) { case LOW_FERRY: From aed476020a40878b9d2e795dc57295db76080305 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 16:18:49 -0400 Subject: [PATCH 20/37] remove unecessary commands that are automated with intake logic, improve handoff logic --- src/main/java/com/stuypulse/robot/Robot.java | 9 ------ .../com/stuypulse/robot/RobotContainer.java | 9 +----- .../robot/commands/intake/IntakeAcquire.java | 14 ++------- .../commands/intake/IntakeAcquireForever.java | 20 ------------ .../commands/intake/IntakeDeacquire.java | 4 +-- .../robot/commands/intake/IntakeFeed.java | 31 ------------------- .../robot/commands/intake/IntakeShoot.java | 19 ------------ .../intake/IntakeWaitUntilHasNote.java | 12 +++++++ .../robot/subsystems/intake/Intake.java | 4 ++- .../robot/subsystems/shooter/Shooter.java | 7 +++-- 10 files changed, 24 insertions(+), 105 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeFeed.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java create mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeWaitUntilHasNote.java diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index e12c4bf..cf181ad 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -2,18 +2,9 @@ import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; -import com.revrobotics.CANSparkBase.IdleMode; -import com.stuypulse.robot.commands.BuzzController; -import com.stuypulse.robot.commands.intake.IntakeShoot; import com.stuypulse.robot.commands.leds.LEDReset; -import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; -import com.stuypulse.robot.commands.shooter.ShooterManualIntake; import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; -import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.RobotType; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.shooter.Shooter; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ea3e0c8..70bfbea 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -16,10 +16,7 @@ import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; import com.stuypulse.robot.commands.intake.IntakeAcquire; -import com.stuypulse.robot.commands.intake.IntakeAcquireForever; import com.stuypulse.robot.commands.intake.IntakeDeacquire; -import com.stuypulse.robot.commands.intake.IntakeFeed; -import com.stuypulse.robot.commands.intake.IntakeShoot; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.leds.LEDDefaultMode; import com.stuypulse.robot.commands.leds.LEDReset; @@ -50,7 +47,6 @@ import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.subsystems.vision.NoteVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; -import com.stuypulse.robot.util.SLColor; import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.robot.subsystems.arm.Arm; @@ -111,7 +107,6 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - intake.setDefaultCommand(new IntakeStop()); leds.setDefaultCommand(new LEDDefaultMode()); } @@ -236,9 +231,7 @@ private void configureDriverBindings() { .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)) ) diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java index 7fb25d0..de45e84 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java @@ -2,9 +2,9 @@ import com.stuypulse.robot.subsystems.intake.Intake; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class IntakeAcquire extends Command { +public class IntakeAcquire extends InstantCommand { private final Intake intake; @@ -17,14 +17,4 @@ public IntakeAcquire() { public void initialize() { intake.setState(Intake.State.ACQUIRING); } - - @Override - public void end(boolean interrupted) { - intake.setState(Intake.State.STOP); - } - - @Override - public boolean isFinished() { - return intake.hasNote(); - } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java deleted file mode 100644 index ce35769..0000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java +++ /dev/null @@ -1,20 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class IntakeAcquireForever extends InstantCommand { - - private final Intake intake; - - public IntakeAcquireForever() { - intake = Intake.getInstance(); - addRequirements(intake); - } - - @Override - public void initialize() { - intake.setState(Intake.State.ACQUIRING); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java index 4c8ecdb..335cfbc 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java @@ -2,9 +2,9 @@ import com.stuypulse.robot.subsystems.intake.Intake; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class IntakeDeacquire extends Command { +public class IntakeDeacquire extends InstantCommand { private final Intake intake; diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeFeed.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeFeed.java deleted file mode 100644 index 1d4f289..0000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeFeed.java +++ /dev/null @@ -1,31 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.shooter.Shooter; - -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeFeed extends Command { - - private final Intake intake; - - public IntakeFeed() { - intake = Intake.getInstance(); - addRequirements(intake); - } - - @Override - public void initialize() { - intake.setState(Intake.State.FEEDING); - } - - @Override - public void end(boolean interrupted) { - intake.setState(Intake.State.STOP); - } - - @Override - public boolean isFinished() { - return Shooter.getInstance().hasNote(); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java deleted file mode 100644 index a0afb12..0000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java +++ /dev/null @@ -1,19 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.intake.Intake; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class IntakeShoot extends SequentialCommandGroup { - - public IntakeShoot() { - addCommands( - new InstantCommand(() -> Intake.getInstance().setState(Intake.State.SHOOTING), Intake.getInstance()), - new WaitCommand(Settings.Intake.INTAKE_SHOOT_TIME), - new IntakeStop() - ); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeWaitUntilHasNote.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeWaitUntilHasNote.java new file mode 100644 index 0000000..73a4b53 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeWaitUntilHasNote.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class IntakeWaitUntilHasNote extends WaitUntilCommand { + + public IntakeWaitUntilHasNote() { + super(() -> Intake.getInstance().hasNote()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 558e53e..124ef19 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -3,6 +3,7 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.shooter.Shooter.FeederState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -55,7 +56,8 @@ public void periodic() { && Arm.getInstance().atValidFeedAngle() && !Shooter.getInstance().hasNote() && hasNote() - && getState() != Intake.State.DEACQUIRING; + && getState() != Intake.State.DEACQUIRING + && Shooter.getInstance().getFeederState() != Shooter.FeederState.DEACQUIRING; if (shouldHandoff) { setState(State.FEEDING); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 32d3782..e1f39f7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -73,14 +73,15 @@ public void periodic() { // automatic handoff boolean shouldHandoff = Arm.getInstance().getState() == Arm.State.FEED && Arm.getInstance().atValidFeedAngle() - && hasNote() + && !hasNote() && Intake.getInstance().hasNote() - && Intake.getInstance().getState() != Intake.State.DEACQUIRING; + && Intake.getInstance().getState() != Intake.State.DEACQUIRING + && getFeederState() != FeederState.DEACQUIRING; if (shouldHandoff) { setFeederState(FeederState.INTAKING); } - if (feederState == FeederState.INTAKING && !shouldHandoff) { + if (feederState == FeederState.INTAKING && (hasNote() || Arm.getInstance().getState() != Arm.State.FEED)) { setFeederState(FeederState.STOP); } From 4a326f9bbe6881da006d65b1d5248bf57753dc43 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 16:31:17 -0400 Subject: [PATCH 21/37] ferry alignment fixes --- .../java/com/stuypulse/robot/RobotContainer.java | 8 ++++---- .../robot/subsystems/swerve/SwerveDrive.java | 14 ++------------ 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 70bfbea..0109b47 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -177,7 +177,7 @@ private void configureDriverBindings() { .alongWith(new ArmToLobFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) @@ -191,7 +191,7 @@ private void configureDriverBindings() { .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) @@ -217,7 +217,7 @@ private void configureDriverBindings() { .alongWith(new ArmToLobFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) @@ -230,7 +230,7 @@ private void configureDriverBindings() { .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 905c4f3..8fec237 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -202,26 +202,16 @@ public boolean isAlignedToSpeaker() { return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); } - public boolean isAlignedToLowFerry() { + public boolean isAlignedToFerry() { Rotation2d targetAngle = getPose().getTranslation().minus(Field.getAmpCornerPose()).getAngle(); return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); } - public boolean isAlignedToLobFerry() { - Rotation2d targetAngle = getPose().getTranslation().minus(Field.getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); - return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); - } - - public boolean isAlignedToManualLowFerry() { + public boolean isAlignedToManualFerry() { Rotation2d targetAngle = Field.getManualFerryPosition().minus(Field.getAmpCornerPose()).getAngle(); return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); } - public boolean isAlignedToManualLobFerry() { - Rotation2d targetAngle = Field.getManualFerryPosition().minus(Field.getAmpCornerPose()).getAngle().plus(Rotation2d.fromDegrees(180)); - return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); - } - private void updateEstimatorWithVisionData(ArrayList outputs) { Pose2d poseSum = new Pose2d(); double timestampSum = 0; From dcda269e37ad2fd3af7efdcd12046a041bdc2503 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 16:56:01 -0400 Subject: [PATCH 22/37] skibidi (update autons to use new subsystem logic) --- .../com/stuypulse/robot/RobotContainer.java | 3 - .../commands/auton/ADEF/FivePieceADEF.java | 38 ++++++--- .../commands/auton/BCA/FourPieceBCA.java | 35 +++++---- .../auton/BF_Series/FivePieceBFAC.java | 78 +++++++++++-------- .../auton/BF_Series/FivePieceBFCA.java | 54 ++++++------- .../auton/BF_Series/FivePieceBFED.java | 54 ++++++------- .../auton/BF_Series/FivePieceBFGH.java | 54 ++++++------- .../commands/auton/FollowPathAndIntake.java | 31 -------- .../robot/commands/auton/ShootRoutine.java | 3 + .../shooter/ShooterAcquireFromIntake.java | 39 ---------- .../ShooterAcquireFromIntakeWithRetry.java | 64 --------------- 11 files changed, 183 insertions(+), 270 deletions(-) delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0109b47..d0c2e5d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -21,12 +21,9 @@ import com.stuypulse.robot.commands.leds.LEDDefaultMode; import com.stuypulse.robot.commands.leds.LEDReset; import com.stuypulse.robot.commands.leds.LEDSet; -import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; -import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntakeWithRetry; import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; -import com.stuypulse.robot.commands.shooter.ShooterManualIntake; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveRobotRelative; diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 1fc3b99..5c38511 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -1,11 +1,14 @@ package com.stuypulse.robot.commands.auton.ADEF; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class FivePieceADEF extends SequentialCommandGroup { @@ -13,24 +16,37 @@ public FivePieceADEF(PathPlannerPath... paths) { addCommands( ShootRoutine.fromSubwoofer(), + new ArmToFeed(), - new FollowPathAndIntake(paths[0]), - ShootRoutine.fromAnywhere(), + new IntakeAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), + ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), // Drive, Intake, Shoot D - new FollowPathAndIntake(paths[1]), + new IntakeAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), - ShootRoutine.fromAnywhere(), + ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), // Drive, Intake, Shoot E - new FollowPathAndIntake(paths[3]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), - ShootRoutine.fromAnywhere(), + new IntakeAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), + ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), // Drive, Intake, Shoot F - new FollowPathAndIntake(paths[5]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), - ShootRoutine.fromAnywhere() + new IntakeAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), + ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index d4745cd..339dcdf 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -2,35 +2,44 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.arm.ArmSetState; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; +import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class FourPieceBCA extends SequentialCommandGroup { - public FourPieceBCA(PathPlannerPath... path) { + public FourPieceBCA(PathPlannerPath... paths) { addCommands( // Preload Shot ShootRoutine.fromSubwoofer(), - new ArmSetState(Arm.State.FEED), + new ArmToFeed(), // Drive to B + Shoot B - new FollowPathAndIntake(path[0]), - new ShooterAcquireFromIntake(), - ShootRoutine.fromAnywhere().withTimeout(2.5), + new IntakeAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), + ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), // Drive to C + Shoot C - new FollowPathAndIntake(path[1]), - new ShooterAcquireFromIntake(), - ShootRoutine.fromAnywhere().withTimeout(2.5), + new IntakeAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), + ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), // Drive to A + Shoot A - new FollowPathAndIntake(path[2]), - new ShooterAcquireFromIntake(), - ShootRoutine.fromAnywhere().withTimeout(2.5) + new IntakeAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[2]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), + ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java index 12fe070..ac77b0c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java @@ -1,35 +1,51 @@ -package com.stuypulse.robot.commands.auton.BF_Series; +// package com.stuypulse.robot.commands.auton.BF_Series; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.stuypulse.robot.commands.arm.ArmToFeed; +// import com.stuypulse.robot.commands.auton.ShootRoutine; +// import com.stuypulse.robot.commands.intake.IntakeAcquire; +// import com.stuypulse.robot.subsystems.shooter.Shooter; +// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.WaitCommand; -public class FivePieceBFAC extends SequentialCommandGroup { +// public class FivePieceBFAC extends SequentialCommandGroup { - public FivePieceBFAC(PathPlannerPath... paths) { - - addCommands( - ShootRoutine.fromSubwoofer(), - - new FollowPathAndIntake(paths[0]), - ShootRoutine.fromAnywhere(), - - new FollowPathAndIntake(paths[1]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), - ShootRoutine.fromAnywhere(), - - new FollowPathAndIntake(paths[3]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), - ShootRoutine.fromAnywhere(), - - new FollowPathAndIntake(paths[5]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), - ShootRoutine.fromAnywhere() - ); - - } - -} +// public FivePieceBFAC(PathPlannerPath... paths) { + +// addCommands( +// ShootRoutine.fromSubwoofer(), +// new ArmToFeed(), + +// new IntakeAcquire(), +// SwerveDrive.getInstance().followPathCommand(paths[0]), +// new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), +// ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), +// new ArmToFeed(), + +// new IntakeAcquire(), +// SwerveDrive.getInstance().followPathCommand(paths[1]), +// new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), +// ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), +// new ArmToFeed(), + +// new IntakeAcquire(), +// SwerveDrive.getInstance().followPathCommand(paths[3]), +// new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), +// ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), +// new ArmToFeed(), + +// new IntakeAcquire(), +// SwerveDrive.getInstance().followPathCommand(paths[5]), +// new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), +// ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), +// new ArmToFeed() +// ); + +// } + +// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java index c2e0d4f..b81f408 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java @@ -1,37 +1,39 @@ -package com.stuypulse.robot.commands.auton.BF_Series; +// package com.stuypulse.robot.commands.auton.BF_Series; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.stuypulse.robot.commands.arm.ArmToFeed; +// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +// import com.stuypulse.robot.commands.auton.ShootRoutine; +// import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -public class FivePieceBFCA extends SequentialCommandGroup { +// public class FivePieceBFCA extends SequentialCommandGroup { - public FivePieceBFCA(PathPlannerPath... paths) { +// public FivePieceBFCA(PathPlannerPath... paths) { - addCommands( - ShootRoutine.fromSubwoofer(), +// addCommands( +// ShootRoutine.fromSubwoofer(), +// new ArmToFeed(), - new FollowPathAndIntake(paths[0]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[0]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[1]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[1]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[3]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[3]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[5]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), - ShootRoutine.fromAnywhere() - ); +// new FollowPathAndIntake(paths[5]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), +// ShootRoutine.fromAnywhere() +// ); - } +// } -} +// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java index 31284a4..c52f0b1 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java @@ -1,37 +1,39 @@ -package com.stuypulse.robot.commands.auton.BF_Series; +// package com.stuypulse.robot.commands.auton.BF_Series; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.stuypulse.robot.commands.arm.ArmToFeed; +// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +// import com.stuypulse.robot.commands.auton.ShootRoutine; +// import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -public class FivePieceBFED extends SequentialCommandGroup { +// public class FivePieceBFED extends SequentialCommandGroup { - public FivePieceBFED(PathPlannerPath... paths) { +// public FivePieceBFED(PathPlannerPath... paths) { - addCommands( - ShootRoutine.fromSubwoofer(), +// addCommands( +// ShootRoutine.fromSubwoofer(), +// new ArmToFeed(), - new FollowPathAndIntake(paths[0]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[0]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[1]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[1]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[3]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[3]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[5]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), - ShootRoutine.fromAnywhere() - ); +// new FollowPathAndIntake(paths[5]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), +// ShootRoutine.fromAnywhere() +// ); - } +// } -} +// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java index e2ef338..0b437ec 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java @@ -1,37 +1,39 @@ -package com.stuypulse.robot.commands.auton.BF_Series; +// package com.stuypulse.robot.commands.auton.BF_Series; -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.stuypulse.robot.commands.arm.ArmToFeed; +// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +// import com.stuypulse.robot.commands.auton.ShootRoutine; +// import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -public class FivePieceBFGH extends SequentialCommandGroup { +// public class FivePieceBFGH extends SequentialCommandGroup { - public FivePieceBFGH(PathPlannerPath... paths) { +// public FivePieceBFGH(PathPlannerPath... paths) { - addCommands( - ShootRoutine.fromSubwoofer(), +// addCommands( +// ShootRoutine.fromSubwoofer(), +// new ArmToFeed(), - new FollowPathAndIntake(paths[0]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[0]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[1]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[1]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[3]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), - ShootRoutine.fromAnywhere(), +// new FollowPathAndIntake(paths[3]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[5]), - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), - ShootRoutine.fromAnywhere() - ); +// new FollowPathAndIntake(paths[5]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), +// ShootRoutine.fromAnywhere() +// ); - } +// } -} +// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java deleted file mode 100644 index 5149e60..0000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java +++ /dev/null @@ -1,31 +0,0 @@ -package com.stuypulse.robot.commands.auton; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.intake.IntakeAcquire; -import com.stuypulse.robot.commands.swerve.SwerveDriveStop; -import com.stuypulse.robot.constants.Settings.Auton; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class FollowPathAndIntake extends SequentialCommandGroup { - - public FollowPathAndIntake(PathPlannerPath path) { - this(path, Auton.DEFAULT_INTAKE_TIMEOUT); - } - - public FollowPathAndIntake(PathPlannerPath path, double intakeTimeout) { - addCommands( - new ParallelRaceGroup( - new IntakeAcquire(), - - SwerveDrive.getInstance().followPathCommand(path) - .andThen(new WaitCommand(intakeTimeout)) - ), - - new SwerveDriveStop() - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java index 16f948f..7d226aa 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -13,7 +13,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; public abstract class ShootRoutine { @@ -32,6 +34,7 @@ public static Command fromAnywhere() { .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> SwerveDrive.getInstance().isAlignedToSpeaker())) .andThen(new ShooterFeederShoot()) + .andThen(new WaitCommand(0.25)) // give time for note to leave shooter ) .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)); } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java deleted file mode 100644 index a35138c..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java +++ /dev/null @@ -1,39 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.stuylib.util.StopWatch; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ShooterAcquireFromIntake extends Command { - - private final Shooter shooter; - private final Intake intake; - - public ShooterAcquireFromIntake() { - shooter = Shooter.getInstance(); - intake = Intake.getInstance(); - - addRequirements(shooter, intake); - } - - @Override - public void initialize() { - intake.setState(Intake.State.FEEDING); - shooter.setFeederState(Shooter.FeederState.INTAKING); - } - - @Override - public boolean isFinished() { - return shooter.hasNote(); - } - - @Override - public void end(boolean interrupted) { - shooter.setFeederState(Shooter.FeederState.STOP); - intake.setState(Intake.State.STOP); - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java deleted file mode 100644 index 3d6ceef..0000000 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntakeWithRetry.java +++ /dev/null @@ -1,64 +0,0 @@ -package com.stuypulse.robot.commands.shooter; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.stuylib.util.StopWatch; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ShooterAcquireFromIntakeWithRetry extends Command { - - private final Shooter shooter; - private final Intake intake; - - private final StopWatch stopWatch; - private boolean isFeeding; - - public ShooterAcquireFromIntakeWithRetry() { - shooter = Shooter.getInstance(); - intake = Intake.getInstance(); - - stopWatch = new StopWatch(); - isFeeding = true; - - addRequirements(shooter, intake); - } - - @Override - public void initialize() { - stopWatch.reset(); - intake.setState(Intake.State.FEEDING); - shooter.setFeederState(Shooter.FeederState.INTAKING); - } - - @Override - public void execute() { - if (isFeeding) { - if (stopWatch.getTime() > Settings.Intake.HANDOFF_TIMEOUT) { - intake.setState(Intake.State.DEACQUIRING); - isFeeding = false; - stopWatch.reset(); - } - } - else { - if (stopWatch.getTime() > Settings.Intake.MINIMUM_DEACQUIRE_TIME_WHEN_STUCK && intake.hasNote()) { - intake.setState(Intake.State.FEEDING); - isFeeding = true; - stopWatch.reset(); - } - } - } - - @Override - public boolean isFinished() { - return shooter.hasNote(); - } - - @Override - public void end(boolean interrupted) { - shooter.setFeederState(Shooter.FeederState.STOP); - intake.setState(Intake.State.STOP); - } - -} \ No newline at end of file From ca3834a0fc9a54c1d4fb0acbe964dab0c0850616 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 20:13:12 -0400 Subject: [PATCH 23/37] fix deacquire button --- src/main/java/com/stuypulse/robot/RobotContainer.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index d0c2e5d..a4c7da9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -146,9 +146,11 @@ private void configureDriverBindings() { // deacquire driver.getDPadLeft() - .whileTrue(new IntakeDeacquire()) - .whileTrue(new ShooterFeederDeacquire()) - .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)); + .onTrue(new IntakeDeacquire()) + .onTrue(new ShooterFeederDeacquire()) + .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)) + .onFalse(new IntakeStop()) + .onFalse(new ShooterFeederStop()); // speaker align and score // score amp From d083b15dac6c8e7c6b386ce60e190e07efba11cd Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 20:17:42 -0400 Subject: [PATCH 24/37] skibidi --- src/main/java/com/stuypulse/robot/RobotContainer.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a4c7da9..a16874b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -6,7 +6,9 @@ import com.stuypulse.robot.commands.arm.ArmToClimbing; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.arm.ArmToLobFerry; +import com.stuypulse.robot.commands.arm.ArmToLobFerryManual; import com.stuypulse.robot.commands.arm.ArmToLowFerry; +import com.stuypulse.robot.commands.arm.ArmToLowFerryManual; import com.stuypulse.robot.commands.arm.ArmToPreClimb; import com.stuypulse.robot.commands.arm.ArmToSpeaker; import com.stuypulse.robot.commands.arm.ArmToSubwooferShot; @@ -213,7 +215,7 @@ private void configureDriverBindings() { // manual lob ferry driver.getTopButton() .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) - .alongWith(new ArmToLobFerry() + .alongWith(new ArmToLobFerryManual() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualFerry())) @@ -226,7 +228,7 @@ private void configureDriverBindings() { // manual low ferry driver.getLeftButton() .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) - .alongWith(new ArmToLowFerry() + .alongWith(new ArmToLowFerryManual() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualFerry())) From 9e1d0a3a4a47fe0063b7e6a4b5b4a247e8051979 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Wed, 4 Sep 2024 20:25:34 -0400 Subject: [PATCH 25/37] automatically bring arm back to feed after letting go of speaker/ferry buttons --- .../com/stuypulse/robot/RobotContainer.java | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index a16874b..9415400 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -170,7 +170,8 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), () -> Arm.getInstance().getState() == Arm.State.AMP)) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed().onlyIf(() -> arm.getState() == Arm.State.SPEAKER)); // lob ferry align and shoot driver.getLeftStickButton() @@ -183,7 +184,8 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) ) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // low ferry align and shoot @@ -197,7 +199,8 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) ) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // arm to amp driver.getLeftBumper().onTrue(new ArmToAmp()); @@ -210,7 +213,8 @@ private void configureDriverBindings() { .andThen(new ShooterFeederShoot()) ) .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL)) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // manual lob ferry driver.getTopButton() @@ -223,7 +227,8 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) ) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // manual low ferry driver.getLeftButton() @@ -236,7 +241,8 @@ private void configureDriverBindings() { ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)) ) - .onFalse(new ShooterFeederStop()); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // human player attention button driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION)); From 651c9ee5d5abee2f425e8157b104a3234bf30181 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 5 Sep 2024 11:01:39 -0400 Subject: [PATCH 26/37] sus --- .../com/stuypulse/robot/RobotContainer.java | 9 +++++---- .../robot/commands/auton/ShootRoutine.java | 3 ++- .../commands/swerve/SwerveDriveDrive.java | 9 +++++++-- .../swerve/SwerveDriveDriveRobotRelative.java | 7 +++++-- .../driveAligned/SwerveDriveDriveAligned.java | 16 +++++++-------- .../robot/subsystems/swerve/SwerveDrive.java | 2 +- .../robot/subsystems/vision/PhotonVision.java | 20 ++++++++++--------- 7 files changed, 38 insertions(+), 28 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9415400..e82b0b2 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,6 +1,7 @@ package com.stuypulse.robot; import com.ctre.phoenix6.Utils; +import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.arm.ArmToAmp; import com.stuypulse.robot.commands.arm.ArmToClimbing; @@ -260,7 +261,7 @@ public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Mobility", new Mobility()); - AutonConfig BCA = new AutonConfig("4 BCA", FourPieceBCA::new, + AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, "Center to B", "B to C", "C to A"); AutonConfig BCA_RED = new AutonConfig("4 BCA RED", FourPieceBCA::new, "Center to B", "B to C", "C to A"); @@ -268,18 +269,18 @@ public void configureAutons() { // "Source to H", "H to Shoot", "H Shoot to G", "G to Shoot", "G Shoot to F", "F to Shoot"); // AutonConfig HGF_RED = new AutonConfig("4 HGF RED", FourPieceHGF::new, // "Source to H", "H to Shoot", "H Shoot to G", "G to Shoot", "G Shoot to F", "F to Shoot"); - AutonConfig ADEF = new AutonConfig("5 ADEF", FivePieceADEF::new, + AutonConfig ADEF_BLUE = new AutonConfig("5 ADEF", FivePieceADEF::new, "Amp to A", "A to D", "D to Shoot", "D Shoot to E", "E to Shoot", "E Shoot to F", "F to Shoot"); AutonConfig ADEF_RED = new AutonConfig("5 ADEF RED", FivePieceADEF::new, "Amp to A", "A to D", "D to Shoot", "D Shoot to E", "E to Shoot", "E Shoot to F", "F to Shoot"); - BCA.registerDefaultBlue(autonChooser); + BCA_BLUE.registerDefaultBlue(autonChooser); BCA_RED.registerRed(autonChooser); // HGF.registerBlue(autonChooser); // HGF_RED.registerRed(autonChooser); - ADEF.registerBlue(autonChooser); + ADEF_BLUE.registerBlue(autonChooser); ADEF_RED.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java index 7d226aa..eda3aeb 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -23,7 +23,8 @@ public static Command fromSubwoofer() { return new SequentialCommandGroup( new ArmToSubwooferShot(), new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), - new ShooterFeederShoot() + new ShooterFeederShoot(), + new WaitCommand(0.25) // give time for note to leave shooter ); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 537d18c..a59491a 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -2,6 +2,7 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.numbers.filters.RateLimit; @@ -11,11 +12,14 @@ import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Driver.Turn; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import edu.wpi.first.math.Vector; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveDrive extends Command { @@ -60,8 +64,9 @@ public SwerveDriveDrive(Gamepad driver) { @Override public void execute() { - swerve.setControl(drive.withVelocityX(speed.get().y) - .withVelocityY(-speed.get().x) + Vector2D velocity = Robot.isBlue() ? speed.get() : speed.get().mul(-1); + swerve.setControl(drive.withVelocityX(velocity.y) + .withVelocityY(-velocity.x) .withRotationalRate(turn.get()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java index 95f28a3..d8df368 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveRobotRelative.java @@ -2,6 +2,7 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.numbers.filters.RateLimit; @@ -11,6 +12,7 @@ import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Driver.Turn; @@ -61,8 +63,9 @@ public SwerveDriveDriveRobotRelative(Gamepad driver) { @Override public void execute() { - swerve.setControl(drive.withVelocityX(speed.get().y) - .withVelocityY(-speed.get().x) + Vector2D velocity = Robot.isBlue() ? speed.get() : speed.get().mul(-1); + swerve.setControl(drive.withVelocityX(velocity.y) + .withVelocityY(-velocity.x) .withRotationalRate(turn.get()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index aee4420..ff4caf5 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Driver.Drive; import com.stuypulse.robot.constants.Settings.Swerve.Assist; @@ -11,6 +12,7 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.numbers.filters.RateLimit; @@ -28,7 +30,7 @@ public abstract class SwerveDriveDriveAligned extends Command { private final SwerveDrive swerve; protected final Gamepad driver; - private final VStream velocity; + private final VStream speed; private final SwerveRequest.FieldCentric drive; @@ -39,7 +41,7 @@ public SwerveDriveDriveAligned(Gamepad driver) { swerve = SwerveDrive.getInstance(); this.driver = driver; - velocity = VStream.create(driver::getLeftStick) + speed = VStream.create(driver::getLeftStick) .filtered( new VDeadZone(Drive.DEADBAND), x -> x.clamp(1), @@ -80,16 +82,12 @@ protected double getAngleError() { return controller.getError().getRotation2d().getDegrees(); } - // @Override - // public boolean isFinished() { - // return Math.abs(driver.getRightX()) > Settings.Driver.Turn.DISABLE_ALIGNMENT_DEADBAND.getAsDouble(); - // } - @Override public void execute() { + Vector2D velocity = Robot.isBlue() ? speed.get() : speed.get().mul(-1); swerve.setControl( - drive.withVelocityX(velocity.get().y) - .withVelocityY(-velocity.get().x) + drive.withVelocityX(velocity.y) + .withVelocityY(-velocity.x) .withRotationalRate( SLMath.clamp(angleVelocity.get() + controller.update( diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 8fec237..1079c49 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -277,7 +277,7 @@ public void periodic() { field.setRobotPose(getPose()); - applyOperatorPerspective(); + // applyOperatorPerspective(); ArrayList outputs = AprilTagVision.getInstance().getOutputs(); if (Settings.Vision.IS_ACTIVE.get() && outputs.size() > 0) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java index cb2dc88..7122aa0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java @@ -104,15 +104,17 @@ public void periodic() { final int index = i; if (enabled[index]) { PhotonPipelineResult latestResult = cameras[index].getLatestResult(); - if (latestResult.getBestTarget().getPoseAmbiguity() < Settings.Vision.POSE_AMBIGUITY_RATIO_THRESHOLD) { - Optional estimatedRobotPose = poseEstimators[index].update(latestResult); - estimatedRobotPose.ifPresent( - (EstimatedRobotPose robotPose) -> { - VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea()); - outputs.add(data); - updateTelemetry("Vision/" + cameras[index].getName(), data); - } - ); + if (latestResult.hasTargets()) { + if (latestResult.getBestTarget().getPoseAmbiguity() < Settings.Vision.POSE_AMBIGUITY_RATIO_THRESHOLD) { + Optional estimatedRobotPose = poseEstimators[index].update(latestResult); + estimatedRobotPose.ifPresent( + (EstimatedRobotPose robotPose) -> { + VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea()); + outputs.add(data); + updateTelemetry("Vision/" + cameras[index].getName(), data); + } + ); + } } } } From 57d1fd58c241555c7cb2b5bd0eedd5ad9784973f Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 5 Sep 2024 14:56:52 -0400 Subject: [PATCH 27/37] add fake data for low ferry interpolation so it doesnt crash --- .../util/ShooterLowFerryInterpolation.java | 52 ++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java index 7276aeb..3f19aff 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java @@ -13,7 +13,57 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - + {4000, 551}, + {4000, 573}, + {4000, 546}, + {4000, 566}, + {4000, 579}, + {4000, 458}, + {4000, 533}, + {3800, 561}, + {3800, 499}, + {3800, 576}, + {3800, 517}, + {3800, 497}, + {3800, 522}, + {3600, 456}, + {3600, 516}, + {3600, 465}, + {3600, 422}, + {3600, 542}, + {3400, 435}, + {3400, 467}, + {3400, 496}, + {3400, 461}, + {3400, 470}, + {3400, 448}, + {3400, 516}, + {3200, 433}, + // {3200, 456}, + {3200, 447}, + {3200, 406}, + {3200, 424}, + {3200, 454}, + // {3000, 414}, + {3000, 388}, + {3000, 389}, + {3000, 392}, + // {3000, 405}, + {3000, 400}, + // {3000, 430}, + {2600, 316}, + // {2600, 354}, + // {2600, 339}, + // {2200, 288}, + {2200, 241}, + // {2200, 268}, + {2200, 218}, + {2200, 210}, + {1500, 145}, + {1500, 145}, + {1500, 136}, + // {1500, 184}, + // {1500, 178}, }; static { From 32216eb09b65fe3530bc733194e2769b5bc006cd Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 5 Sep 2024 15:21:55 -0400 Subject: [PATCH 28/37] skibidi (fix intake side as front, fix intake) --- .../com/stuypulse/robot/RobotContainer.java | 18 ++++++++---------- .../robot/subsystems/swerve/SwerveDrive.java | 2 +- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index e82b0b2..4e508e9 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -63,6 +63,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -133,19 +134,16 @@ private void configureDriverBindings() { driver.getRightTriggerButton() .onTrue(new ArmToFeed()) // .whileTrue(new SwerveDriveDriveToNote(driver)) - .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.FIELD_RELATIVE_INTAKING)) - .andThen(new BuzzController(driver)) - ); + .onTrue(new IntakeAcquire()) + .whileTrue((new LEDSet(LEDInstructions.FIELD_RELATIVE_INTAKING))) + .onFalse(new IntakeStop()); // intake robot relative driver.getLeftTriggerButton() .onTrue(new ArmToFeed()) - .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING)) - .andThen(new BuzzController(driver)) - ) - .whileTrue(new SwerveDriveDriveRobotRelative(driver)); + .onTrue(new IntakeAcquire()) + .whileTrue(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING)) + .onFalse(new IntakeStop()); // deacquire driver.getDPadLeft() @@ -195,7 +193,7 @@ private void configureDriverBindings() { .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry()).andThen(new WaitCommand(1.0))) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 1079c49..8fec237 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -277,7 +277,7 @@ public void periodic() { field.setRobotPose(getPose()); - // applyOperatorPerspective(); + applyOperatorPerspective(); ArrayList outputs = AprilTagVision.getInstance().getOutputs(); if (Settings.Vision.IS_ACTIVE.get() && outputs.size() > 0) { From 3dc37afd264f31222f25e881a37c53a03e77f3ba Mon Sep 17 00:00:00 2001 From: jopy-man Date: Thu, 5 Sep 2024 16:52:51 -0400 Subject: [PATCH 29/37] tests and stuff --- src/main/deploy/pathplanner/autos/BCA.auto | 2 +- src/main/deploy/pathplanner/autos/HGF.auto | 2 +- .../deploy/pathplanner/autos/Sraight Line.auto | 2 +- src/main/deploy/pathplanner/paths/B to C.path | 4 ++-- src/main/deploy/pathplanner/paths/C to A.path | 6 +++--- .../deploy/pathplanner/paths/Center to B.path | 11 +++++++---- .../deploy/pathplanner/paths/F to Shoot.path | 6 +++--- .../deploy/pathplanner/paths/G Shoot to F.path | 16 +++++----------- .../deploy/pathplanner/paths/G to Shoot.path | 6 +++--- .../deploy/pathplanner/paths/H Shoot to G.path | 10 +++++----- .../deploy/pathplanner/paths/H to Shoot.path | 12 ++++++------ .../deploy/pathplanner/paths/Source to H.path | 18 +++++++++--------- .../pathplanner/paths/Straight Line.path | 7 +++++-- .../com/stuypulse/robot/RobotContainer.java | 3 +++ .../robot/subsystems/vision/PhotonVision.java | 2 +- 15 files changed, 55 insertions(+), 52 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/BCA.auto b/src/main/deploy/pathplanner/autos/BCA.auto index c03eafe..de2733c 100644 --- a/src/main/deploy/pathplanner/autos/BCA.auto +++ b/src/main/deploy/pathplanner/autos/BCA.auto @@ -5,7 +5,7 @@ "x": 1.4053936459807443, "y": 5.5301848373414 }, - "rotation": 0 + "rotation": 179.43822267700324 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/HGF.auto b/src/main/deploy/pathplanner/autos/HGF.auto index dcb5cb0..ee71a20 100644 --- a/src/main/deploy/pathplanner/autos/HGF.auto +++ b/src/main/deploy/pathplanner/autos/HGF.auto @@ -5,7 +5,7 @@ "x": 0.7437038962619593, "y": 4.320740582515391 }, - "rotation": -59.682052822906385 + "rotation": 116.80625971086718 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/Sraight Line.auto b/src/main/deploy/pathplanner/autos/Sraight Line.auto index a8ab7a5..f1d9651 100644 --- a/src/main/deploy/pathplanner/autos/Sraight Line.auto +++ b/src/main/deploy/pathplanner/autos/Sraight Line.auto @@ -5,7 +5,7 @@ "x": 1.5, "y": 5.55 }, - "rotation": 0 + "rotation": -179.56356838292098 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/paths/B to C.path b/src/main/deploy/pathplanner/paths/B to C.path index 50204c9..272419c 100644 --- a/src/main/deploy/pathplanner/paths/B to C.path +++ b/src/main/deploy/pathplanner/paths/B to C.path @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -179.19416655403055, "rotateFast": false }, "reversed": false, "folder": "ABCDE", "previewStartingState": { - "rotation": 0.0, + "rotation": -179.44747299332943, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/C to A.path b/src/main/deploy/pathplanner/paths/C to A.path index fc7aa75..7f1e3ef 100644 --- a/src/main/deploy/pathplanner/paths/C to A.path +++ b/src/main/deploy/pathplanner/paths/C to A.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 63.01873139991995, + "rotationDegrees": -116.48352775095083, "rotateFast": false } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.6983315875415402, + "rotation": 178.9517141332874, "rotateFast": false }, "reversed": false, "folder": "ABCDE", "previewStartingState": { - "rotation": 0.0, + "rotation": -179.34585253591464, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Center to B.path b/src/main/deploy/pathplanner/paths/Center to B.path index 7e25762..52c2c21 100644 --- a/src/main/deploy/pathplanner/paths/Center to B.path +++ b/src/main/deploy/pathplanner/paths/Center to B.path @@ -20,8 +20,8 @@ "y": 5.55 }, "prevControl": { - "x": 2.456710172758143, - "y": 5.582738355097248 + "x": 2.363026588032343, + "y": 5.555664027064661 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,14 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -179.47943637878444, "rotateFast": false }, "reversed": false, "folder": "ABCDE", - "previewStartingState": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to Shoot.path b/src/main/deploy/pathplanner/paths/F to Shoot.path index d147ace..a96d0d4 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/F to Shoot.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -19.01631267638155, + "rotationDegrees": 147.06548083331427, "rotateFast": true } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -8.268297961765734, + "rotation": 170.34501153780897, "rotateFast": false }, "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": 0.0, + "rotation": 178.17768989239653, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/G Shoot to F.path b/src/main/deploy/pathplanner/paths/G Shoot to F.path index 6836437..e7d5397 100644 --- a/src/main/deploy/pathplanner/paths/G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/G Shoot to F.path @@ -20,21 +20,15 @@ "y": 4.1 }, "prevControl": { - "x": 7.066654028062521, - "y": 3.845370558203914 + "x": 7.033169768534478, + "y": 3.9595610497024567 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7, - "rotationDegrees": 7.29052801209588, - "rotateFast": false - } - ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -45,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.45368694477510463, + "rotation": 179.48407472803027, "rotateFast": false }, "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -9.481237644879682, + "rotation": 173.21363169926138, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/G to Shoot.path b/src/main/deploy/pathplanner/paths/G to Shoot.path index 287edd7..cc79226 100644 --- a/src/main/deploy/pathplanner/paths/G to Shoot.path +++ b/src/main/deploy/pathplanner/paths/G to Shoot.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -37.74307863582837, + "rotationDegrees": 138.52168950764727, "rotateFast": true } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -7.757990482222401, + "rotation": 171.1798219104725, "rotateFast": false }, "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": 0.0, + "rotation": 178.858578419353, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/H Shoot to G.path b/src/main/deploy/pathplanner/paths/H Shoot to G.path index 50e421e..bfd69f3 100644 --- a/src/main/deploy/pathplanner/paths/H Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/H Shoot to G.path @@ -20,8 +20,8 @@ "y": 2.44 }, "prevControl": { - "x": 7.729761200779842, - "y": 2.1500660505264464 + "x": 7.664236416642477, + "y": 2.2212553493187346 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -47.64200112069574, + "rotationDegrees": 127.53126500479296, "rotateFast": true } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -0.4937166178222, + "rotation": 178.56854890177945, "rotateFast": false }, "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -5.742284345235014, + "rotation": 173.9845066722469, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/H to Shoot.path b/src/main/deploy/pathplanner/paths/H to Shoot.path index 50f03a2..743f570 100644 --- a/src/main/deploy/pathplanner/paths/H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/H to Shoot.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 8.020720819990748, - "y": 1.7780393542037092 + "x": 7.1779684844711955, + "y": 2.4336941028792762 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.919501541012011 }, "prevControl": { - "x": 5.988426841995054, - "y": 4.331701441490681 + "x": 5.905931956023139, + "y": 4.587406743637892 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.75, - "rotationDegrees": -41.19150714584497, + "rotationDegrees": 158.01836985356624, "rotateFast": true } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -8.797211565554276, + "rotation": 175.17249248336842, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Source to H.path b/src/main/deploy/pathplanner/paths/Source to H.path index 4e0b4ae..e6a11a6 100644 --- a/src/main/deploy/pathplanner/paths/Source to H.path +++ b/src/main/deploy/pathplanner/paths/Source to H.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7437038962619593, - "y": 4.320740582515391 + "x": 0.7905759905641836, + "y": 4.310885424226206 }, "prevControl": null, "nextControl": { - "x": 1.5747100727832408, - "y": 3.3032555815855678 + "x": 3.1209603713850287, + "y": 3.2519606845389286 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 0.77 }, "prevControl": { - "x": 6.943160416439879, - "y": 1.1890605600333215 + "x": 6.869285697276752, + "y": 1.038924798273703 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.8, - "rotationDegrees": -36.85881982674874, + "rotationDegrees": 149.51442160606817, "rotateFast": true } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 1.8521041029649359, + "rotation": 176.99598612588744, "rotateFast": false }, "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -60.0, + "rotation": 117.79678461990976, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path index 3c285ee..3741cd8 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -39,11 +39,14 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -179.22842145964006, "rotateFast": false }, "reversed": false, "folder": "Tests", - "previewStartingState": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9415400..1899ab5 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -17,6 +17,7 @@ import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; +import com.stuypulse.robot.commands.auton.tests.StraightLine; import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeStop; @@ -259,6 +260,8 @@ private void configureOperatorBindings() { public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); autonChooser.addOption("Mobility", new Mobility()); + autonChooser.addOption("Straight Line", new StraightLine()); + AutonConfig BCA = new AutonConfig("4 BCA", FourPieceBCA::new, "Center to B", "B to C", "C to A"); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java index cb2dc88..c5b333a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java @@ -104,7 +104,7 @@ public void periodic() { final int index = i; if (enabled[index]) { PhotonPipelineResult latestResult = cameras[index].getLatestResult(); - if (latestResult.getBestTarget().getPoseAmbiguity() < Settings.Vision.POSE_AMBIGUITY_RATIO_THRESHOLD) { + if (latestResult.hasTargets()) { Optional estimatedRobotPose = poseEstimators[index].update(latestResult); estimatedRobotPose.ifPresent( (EstimatedRobotPose robotPose) -> { From ffe7cb71bac654105181b950f107902e552d5758 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 5 Sep 2024 16:59:20 -0400 Subject: [PATCH 30/37] tune alignment (sorta) --- src/main/java/com/stuypulse/robot/constants/Settings.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a1ded2e..7d0ad55 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -197,9 +197,9 @@ public interface Assist { double AMP_WALL_SCORE_DISTANCE = (Settings.LENGTH / 2) + Units.inchesToMeters(2.5); // angle PID - SmartNumber kP = new SmartNumber("SwerveAssist/kP", 5.0); + SmartNumber kP = new SmartNumber("SwerveAssist/kP", 8.5); SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0); - SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0); + SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.4); double ANGLE_DERIV_RC = 0.05; double REDUCED_FF_DIST = 0.75; @@ -310,7 +310,7 @@ public interface Alignment { SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 6); SmartNumber CLIMB_SETUP_DISTANCE = new SmartNumber("Alignment/Climb/Setup Distance", Units.inchesToMeters(21.0)); SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Climb/Into Chain Speed", 0.25); From 791c030d91dfb02566955ceeb1d03055df9701f3 Mon Sep 17 00:00:00 2001 From: jopy-man Date: Thu, 5 Sep 2024 17:13:59 -0400 Subject: [PATCH 31/37] straight line --- src/main/deploy/pathplanner/paths/Straight Line.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path index 3741cd8..4ee5c7f 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -179.22842145964006, + "rotation": 0.9338907055017344, "rotateFast": false }, "reversed": false, From a321a5532feb27d158562f30fe4fb986e3b75d1c Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Thu, 5 Sep 2024 19:31:15 -0400 Subject: [PATCH 32/37] buzz controller on intake --- src/main/java/com/stuypulse/robot/RobotContainer.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c10bede..ee61560 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -101,6 +101,9 @@ public RobotContainer() { swerve.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); } swerve.registerTelemetry(logger::telemeterize); + + new Trigger(() -> Intake.getInstance().getState() == Intake.State.ACQUIRING && Intake.getInstance().hasNote()) + .onTrue(new BuzzController(driver, 1)); } /****************/ @@ -165,7 +168,7 @@ private void configureDriverBindings() { .alongWith(new ArmToSpeaker() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker()).andThen(new WaitCommand(0.25))) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), @@ -194,7 +197,7 @@ private void configureDriverBindings() { .alongWith(new ArmToLowFerry() .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry()).andThen(new WaitCommand(1.0))) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) From 359646f82ed9cd583f5c48004eb75bde513772a2 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 7 Sep 2024 11:25:25 -0400 Subject: [PATCH 33/37] change name for intakeAcquire command --- src/main/java/com/stuypulse/robot/RobotContainer.java | 6 +++--- .../robot/commands/auton/ADEF/FivePieceADEF.java | 10 +++++----- .../robot/commands/auton/BCA/FourPieceBCA.java | 9 +++++---- .../{IntakeAcquire.java => IntakeSetAcquire.java} | 4 ++-- 4 files changed, 15 insertions(+), 14 deletions(-) rename src/main/java/com/stuypulse/robot/commands/intake/{IntakeAcquire.java => IntakeSetAcquire.java} (81%) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ee61560..1dcb3f7 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -19,8 +19,8 @@ import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; import com.stuypulse.robot.commands.auton.tests.StraightLine; -import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeDeacquire; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.leds.LEDDefaultMode; import com.stuypulse.robot.commands.leds.LEDReset; @@ -138,14 +138,14 @@ private void configureDriverBindings() { driver.getRightTriggerButton() .onTrue(new ArmToFeed()) // .whileTrue(new SwerveDriveDriveToNote(driver)) - .onTrue(new IntakeAcquire()) + .onTrue(new IntakeSetAcquire()) .whileTrue((new LEDSet(LEDInstructions.FIELD_RELATIVE_INTAKING))) .onFalse(new IntakeStop()); // intake robot relative driver.getLeftTriggerButton() .onTrue(new ArmToFeed()) - .onTrue(new IntakeAcquire()) + .onTrue(new IntakeSetAcquire()) .whileTrue(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING)) .onFalse(new IntakeStop()); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index 5c38511..dd39180 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -3,7 +3,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -18,14 +18,14 @@ public FivePieceADEF(PathPlannerPath... paths) { ShootRoutine.fromSubwoofer(), new ArmToFeed(), - new IntakeAcquire(), + new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[0]), new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), new ArmToFeed(), // Drive, Intake, Shoot D - new IntakeAcquire(), + new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[1]), new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), @@ -33,7 +33,7 @@ public FivePieceADEF(PathPlannerPath... paths) { new ArmToFeed(), // Drive, Intake, Shoot E - new IntakeAcquire(), + new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[1]), new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), @@ -41,7 +41,7 @@ public FivePieceADEF(PathPlannerPath... paths) { new ArmToFeed(), // Drive, Intake, Shoot F - new IntakeAcquire(), + new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[1]), new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index 339dcdf..8ff7324 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -4,7 +4,8 @@ import com.stuypulse.robot.commands.arm.ArmSetState; import com.stuypulse.robot.commands.arm.ArmToFeed; import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -21,21 +22,21 @@ public FourPieceBCA(PathPlannerPath... paths) { new ArmToFeed(), // Drive to B + Shoot B - new IntakeAcquire(), + new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[0]), new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), new ArmToFeed(), // Drive to C + Shoot C - new IntakeAcquire(), + new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[1]), new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), new ArmToFeed(), // Drive to A + Shoot A - new IntakeAcquire(), + new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[2]), new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetAcquire.java similarity index 81% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeSetAcquire.java index de45e84..e27db8a 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetAcquire.java @@ -4,11 +4,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; -public class IntakeAcquire extends InstantCommand { +public class IntakeSetAcquire extends InstantCommand { private final Intake intake; - public IntakeAcquire() { + public IntakeSetAcquire() { intake = Intake.getInstance(); addRequirements(intake); } From 4a3d2aa68952e619ca52755862bfdbae535e5909 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 7 Sep 2024 11:25:36 -0400 Subject: [PATCH 34/37] update intake led colors --- .../java/com/stuypulse/robot/constants/LEDInstructions.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index 9718f24..577c5c2 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -55,8 +55,8 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction FIELD_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE); - LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDRainbow(); + LEDInstruction FIELD_RELATIVE_INTAKING = new LEDRainbow(); + LEDInstruction ROBOT_RELATIVE_INTAKING = BLUE; LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.GOLD); LEDInstruction SPEAKER_ALIGN = GREEN; From b6ea2edcda90198f53bac59bd568812966e99737 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 7 Sep 2024 11:45:00 -0400 Subject: [PATCH 35/37] add low ferry data --- .../util/ShooterLowFerryInterpolation.java | 56 ++----------------- 1 file changed, 5 insertions(+), 51 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java index 3f19aff..5ae6e47 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java @@ -13,57 +13,11 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - {4000, 551}, - {4000, 573}, - {4000, 546}, - {4000, 566}, - {4000, 579}, - {4000, 458}, - {4000, 533}, - {3800, 561}, - {3800, 499}, - {3800, 576}, - {3800, 517}, - {3800, 497}, - {3800, 522}, - {3600, 456}, - {3600, 516}, - {3600, 465}, - {3600, 422}, - {3600, 542}, - {3400, 435}, - {3400, 467}, - {3400, 496}, - {3400, 461}, - {3400, 470}, - {3400, 448}, - {3400, 516}, - {3200, 433}, - // {3200, 456}, - {3200, 447}, - {3200, 406}, - {3200, 424}, - {3200, 454}, - // {3000, 414}, - {3000, 388}, - {3000, 389}, - {3000, 392}, - // {3000, 405}, - {3000, 400}, - // {3000, 430}, - {2600, 316}, - // {2600, 354}, - // {2600, 339}, - // {2200, 288}, - {2200, 241}, - // {2200, 268}, - {2200, 218}, - {2200, 210}, - {1500, 145}, - {1500, 145}, - {1500, 136}, - // {1500, 184}, - // {1500, 178}, + {2000, 204}, + {2000, 130}, + {2500, 235}, + {2500, 246}, + {2500, 284}, }; static { From 60f695598ded2bce0fa3d512a161e1c4708c559d Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 7 Sep 2024 11:46:14 -0400 Subject: [PATCH 36/37] configure auto builder, trust angle from vision data more --- .../robot/subsystems/swerve/SwerveDrive.java | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 8fec237..f3f07f6 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -9,10 +9,15 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; @@ -81,6 +86,8 @@ protected SwerveDrive(SwerveDrivetrainConstants driveTrainConstants, double Upda } modules2D = new FieldObject2d[Modules.length]; field = new Field2d(); + + configureAutoBuilder(); } public Command applyRequest(Supplier requestSupplier) { @@ -129,6 +136,25 @@ public void stop() { .withRotationalRate(0)); } + public void configureAutoBuilder() { + AutoBuilder.configureHolonomic( + this::getPose, + (Pose2d pose) -> seedFieldRelative(pose), + this::getChassisSpeeds, + this::setChassisSpeeds, + new HolonomicPathFollowerConfig( + Settings.Swerve.Motion.XY, + Settings.Swerve.Motion.THETA, + 4.9, + Settings.Swerve.WIDTH, + new ReplanningConfig(true, true)), + () -> false, + instance + ); + + PathPlannerLogging.setLogActivePathCallback((poses) -> getField().getObject("path").setPoses(poses)); + } + public Command followPathCommand(String pathName) { return followPathCommand(PathPlannerPath.fromPathFile(pathName)); } @@ -231,7 +257,7 @@ private void updateEstimatorWithVisionData(ArrayList outputs) { } addVisionMeasurement(poseSum.div(areaSum), timestampSum / areaSum, - DriverStation.isAutonomous() ? VecBuilder.fill(0.9, 0.9, 10) : VecBuilder.fill(0.7, 0.7, 10)); + DriverStation.isAutonomous() ? VecBuilder.fill(0.2, 0.2, 1) : VecBuilder.fill(0.2, 0.2, 1)); } From 86666b27d1a2ac250a4ef77cdd6b81167db56de6 Mon Sep 17 00:00:00 2001 From: Ian Shi Date: Sat, 7 Sep 2024 11:48:47 -0400 Subject: [PATCH 37/37] revert to original method to find arm angle --- .../robot/subsystems/arm/ArmImpl.java | 118 ++++++++++++------ 1 file changed, 80 insertions(+), 38 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index f157fd4..9166618 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -17,6 +17,7 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; @@ -118,43 +119,36 @@ private double getTargetDegrees() { } } - private double getNoteHeightAtSpeakerGivenArmAngle(double armAngle) { - Pose2d robotPose = SwerveDrive.getInstance().getPose(); - - Rotation2d robotAngleToSpeaker = Field.getAllianceSpeakerPose().minus(robotPose).getRotation(); - Translation2d noteStartingPose = SwerveDrive.getInstance().getPose() - .minus(new Pose2d(Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * -robotAngleToSpeaker.getCos(), Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * -robotAngleToSpeaker.getSin(), robotAngleToSpeaker)) - .getTranslation(); - Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); - - double horizontalDistance = noteStartingPose.getDistance(speakerPose); - - double shooterAngle = armAngle + 180 - Settings.ANGLE_BETWEEN_ARM_AND_SHOOTER; - - return -16 * Math.pow(horizontalDistance / (Settings.Shooter.SPEAKER_SHOT_VELOCITY * Math.cos(Angle.fromDegrees(shooterAngle).toRadians())), 2) - + Math.tan(Angle.fromDegrees(shooterAngle).toRadians()) * horizontalDistance - + Settings.HEIGHT_TO_ARM_PIVOT - (Math.sin(Angle.fromDegrees(armAngle).toRadians()) * Settings.Arm.LENGTH); - } - private double getSpeakerAngle() { try { - double angle = Settings.Arm.MIN_ANGLE.get(); - double angleIncrement = 0.4; - double heightToleranceAtSpeakerOpening = (Field.SPEAKER_MAX_HEIGHT - Field.SPEAKER_MIN_HEIGHT) / 4; - double lastError = Double.MAX_VALUE; - while (angle < Settings.Arm.SUBWOOFER_SHOT_ANGLE.get()) { - double error = Math.abs(getNoteHeightAtSpeakerGivenArmAngle(angle) - (Field.SPEAKER_MAX_HEIGHT +Field.SPEAKER_MIN_HEIGHT)/2); - if (error < heightToleranceAtSpeakerOpening) { - return angle; - } - - if (error > lastError) { - return angle - angleIncrement * 0.75; - } - - angle += angleIncrement; - } - return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); + Pose3d speakerPose = new Pose3d( + Field.getAllianceSpeakerPose().getX(), + Field.getAllianceSpeakerPose().getY(), + Field.SPEAKER_MAX_HEIGHT, + new Rotation3d() + ); + + Pose2d robotPose = SwerveDrive.getInstance().getPose(); + + Pose3d armPivotPose = new Pose3d( + robotPose.getX() + Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * robotPose.getRotation().getCos(), + robotPose.getY() + Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * robotPose.getRotation().getSin(), + Settings.HEIGHT_TO_ARM_PIVOT, + new Rotation3d() + ); + + Translation3d pivotToSpeaker = speakerPose.minus(armPivotPose).getTranslation(); + + double angleFromPivotToSpeaker = Units.radiansToDegrees( + Math.atan( + pivotToSpeaker.getZ() + / pivotToSpeaker.toTranslation2d().getNorm() + ) + ); + + double angleBetweenPivotToSpeakerAndArm = Units.radiansToDegrees(Math.acos(Settings.Arm.LENGTH / pivotToSpeaker.getNorm())); + + return -(angleBetweenPivotToSpeakerAndArm - angleFromPivotToSpeaker); } catch (Exception exception) { exception.printStackTrace(); @@ -162,11 +156,57 @@ private double getSpeakerAngle() { } } + // private double getNoteHeightAtSpeakerGivenArmAngle(double armAngle) { + // Pose2d robotPose = SwerveDrive.getInstance().getPose(); + + // Rotation2d robotAngleToSpeaker = Field.getAllianceSpeakerPose().minus(robotPose).getRotation(); + // Translation2d noteStartingPose = SwerveDrive.getInstance().getPose() + // .minus(new Pose2d(Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * -robotAngleToSpeaker.getCos(), Settings.DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT * -robotAngleToSpeaker.getSin(), robotAngleToSpeaker)) + // .getTranslation(); + // Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); + + // double horizontalDistance = noteStartingPose.getDistance(speakerPose); + + // double shooterAngle = armAngle + 180 - Settings.ANGLE_BETWEEN_ARM_AND_SHOOTER; + + // return -16 * Math.pow(horizontalDistance / (Settings.Shooter.SPEAKER_SHOT_VELOCITY * Math.cos(Angle.fromDegrees(shooterAngle).toRadians())), 2) + // + Math.tan(Angle.fromDegrees(shooterAngle).toRadians()) * horizontalDistance + // + Settings.HEIGHT_TO_ARM_PIVOT - (Math.sin(Angle.fromDegrees(armAngle).toRadians()) * Settings.Arm.LENGTH); + // } + + // tries to account for actual arm to shooter angle and gravity + // private double getSpeakerAngle() { + // try { + // double angle = Settings.Arm.MIN_ANGLE.get(); + // double angleIncrement = 0.4; + // double heightToleranceAtSpeakerOpening = (Field.SPEAKER_MAX_HEIGHT - Field.SPEAKER_MIN_HEIGHT) / 4; + // double lastError = Double.MAX_VALUE; + // while (angle < Settings.Arm.SUBWOOFER_SHOT_ANGLE.get()) { + // double error = Math.abs(getNoteHeightAtSpeakerGivenArmAngle(angle) - (Field.SPEAKER_MAX_HEIGHT +Field.SPEAKER_MIN_HEIGHT)/2); + // if (error < heightToleranceAtSpeakerOpening) { + // return angle; + // } + + // if (error > lastError) { + // return angle - angleIncrement * 0.75; + // } + + // angle += angleIncrement; + // } + // return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); + // } + // catch (Exception exception) { + // exception.printStackTrace(); + // return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); + // } + // } + + // accounts for actual shooter to arm angle and aims for the plane in front of speaker // private double getSpeakerAngle() { // try { // Pose3d speakerPose = new Pose3d( - // Field.getAllianceSpeakerPose().getX(), - // Field.getAllianceSpeakerPose().getY(), + // Field.getAllianceSpeakerPose().getX() + (Robot.isBlue() ? Units.feetToMeters(18.5 / 12.0 / 2.0) : -Units.feetToMeters(18.5 / 12.0 / 2.0)), + // Field.SPEAKER_MIN_HEIGHT + Units.feetToMeters(5.0 / 12.0 / 2.0), // Field.SPEAKER_MAX_HEIGHT, // new Rotation3d() // ); @@ -189,7 +229,9 @@ private double getSpeakerAngle() { // ) // ); - // double angleBetweenPivotToSpeakerAndArm = Units.radiansToDegrees(Math.acos(Settings.Arm.LENGTH / pivotToSpeaker.getNorm())); + // double angleFromPivotToSpeakerToShooter = Units.radiansToDegrees(Math.asin(Settings.Arm.LENGTH * Math.sin(Units.degreesToRadians(Settings.ANGLE_BETWEEN_ARM_AND_SHOOTER)) / pivotToSpeaker.getNorm())); + + // double angleBetweenPivotToSpeakerAndArm = 180 - angleFromPivotToSpeakerToShooter - Settings.ANGLE_BETWEEN_ARM_AND_SHOOTER; // return -(angleBetweenPivotToSpeakerAndArm - angleFromPivotToSpeaker); // }