From 5783595d1cc5e282aeb7ddeaa8100e131de29ad2 Mon Sep 17 00:00:00 2001 From: Richie Xue Date: Tue, 26 Mar 2024 17:49:00 -0400 Subject: [PATCH] Auton Fixes (#91) * 4ADE ampside auton * Changed paths involving A, B, and C so that they were straighter than the original arcs. Affects 5 CBAE and 5 CBAE Podium routines. * Change B to A path * make HGF and CHGF shoot immediately, remove alignment Co-authored-by: BrianZ60 Co-authored-by: overseeroverseer Co-authored-by: Ardian Agoes Co-authored-by: Alan Liang * Remove some shooter startup waits, make sure waiting for shooter RPM * Actually, remove shooter rpm check in conveyor shoot routine * Add back rpm check in align and shoot * Delete a ton of unused autos and paths * Lowercase To * Remove more unused autons, fix CBAED auto file, clean robotcontainer * update auton tab on pathplanner * made f to shoot other side of podium * Modify f to shoot slightly --------- Co-authored-by: Rahuldeb75 Co-authored-by: Owen-Zeng Co-authored-by: BenG49 Co-authored-by: BrianZ60 Co-authored-by: overseeroverseer Co-authored-by: Ardian Agoes Co-authored-by: Alan Liang --- .pathplanner/settings.json | 1 - .../deploy/pathplanner/autos/4.5 ADEF.auto | 14 ++--- .../deploy/pathplanner/autos/4.5 CHGF.auto | 12 ++-- .../deploy/pathplanner/autos/6 CBAED.auto | 20 +++---- src/main/deploy/pathplanner/paths/B to A.path | 31 +++++------ .../pathplanner/paths/C Shoot to B.path | 49 ----------------- .../pathplanner/paths/C to B Close.path | 22 ++++---- src/main/deploy/pathplanner/paths/C to B.path | 31 +++++++---- .../deploy/pathplanner/paths/C to Shoot.path | 49 ----------------- ... H (CHGF).path => CShoot to H (CHGF).path} | 0 .../pathplanner/paths/Close Preload to C.path | 10 ++-- ...oot (ADEF).path => F to Shoot (ADEF).path} | 0 .../pathplanner/paths/F to Shoot (HGF).path | 14 ++--- .../pathplanner/paths/G to GShoot (GHF).path | 55 ------------------- .../pathplanner/paths/GShoot to H (GHF).path | 49 ----------------- .../pathplanner/paths/H to HShoot (HGF).path | 8 +-- .../pathplanner/paths/HShoot to F (GHF).path | 49 ----------------- ... Intermediate D.path => Preload to A.path} | 24 ++++---- .../pathplanner/paths/Preload to C.path | 26 +++++---- ...o F (ADEF).path => Shoot to F (ADEF).path} | 0 .../pathplanner/paths/Start to G (GHF).path | 55 ------------------- .../pathplanner/paths/Start to H (HGF).path | 16 +++--- .../paths/TopShoot to Intermediate E.path | 52 ------------------ .../paths/TopShoot to Intermediate F.path | 52 ------------------ .../com/stuypulse/robot/RobotContainer.java | 52 ++++-------------- .../auton/CBADE/BlayFivePieceCBAE.java | 47 ---------------- .../commands/auton/CBADE/FivePieceCBAD.java | 46 ---------------- .../commands/auton/CBADE/SixPieceCBADE.java | 49 ----------------- .../commands/auton/CBADE/SixPieceCBAED.java | 5 +- .../commands/auton/CHFG/FivePieceCHGF.java | 15 ++--- .../auton/FollowPathAlignAndShoot.java | 10 +--- .../commands/auton/GHF/FourPieceGHF.java | 38 ------------- .../commands/auton/HGF/FourPieceHGF.java | 17 +----- .../conveyor/ConveyorShootRoutine.java | 1 + .../leds/instructions/LEDAutonChooser.java | 1 - 35 files changed, 140 insertions(+), 780 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/C Shoot to B.path delete mode 100644 src/main/deploy/pathplanner/paths/C to Shoot.path rename src/main/deploy/pathplanner/paths/{CShoot To H (CHGF).path => CShoot to H (CHGF).path} (100%) rename src/main/deploy/pathplanner/paths/{F To Shoot (ADEF).path => F to Shoot (ADEF).path} (100%) delete mode 100644 src/main/deploy/pathplanner/paths/G to GShoot (GHF).path delete mode 100644 src/main/deploy/pathplanner/paths/GShoot to H (GHF).path delete mode 100644 src/main/deploy/pathplanner/paths/HShoot to F (GHF).path rename src/main/deploy/pathplanner/paths/{A to Intermediate D.path => Preload to A.path} (66%) rename src/main/deploy/pathplanner/paths/{Shoot To F (ADEF).path => Shoot to F (ADEF).path} (100%) delete mode 100644 src/main/deploy/pathplanner/paths/Start to G (GHF).path delete mode 100644 src/main/deploy/pathplanner/paths/TopShoot to Intermediate E.path delete mode 100644 src/main/deploy/pathplanner/paths/TopShoot to Intermediate F.path delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAD.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBADE.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/GHF/FourPieceGHF.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 6be05877..5303a099 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,7 +4,6 @@ "holonomicMode": true, "pathFolders": [ "ABCDE", - "ABCDE (ND)", "HGF", "Tests" ], diff --git a/src/main/deploy/pathplanner/autos/4.5 ADEF.auto b/src/main/deploy/pathplanner/autos/4.5 ADEF.auto index 538b430c..7d9d6c98 100644 --- a/src/main/deploy/pathplanner/autos/4.5 ADEF.auto +++ b/src/main/deploy/pathplanner/autos/4.5 ADEF.auto @@ -8,43 +8,43 @@ { "type": "path", "data": { - "pathName": "First Piece To A" + "pathName": "Preload to A" } }, { "type": "path", "data": { - "pathName": "A To D" + "pathName": "A to D" } }, { "type": "path", "data": { - "pathName": "D To Shoot" + "pathName": "D to Shoot" } }, { "type": "path", "data": { - "pathName": "Shoot To E" + "pathName": "Shoot to E" } }, { "type": "path", "data": { - "pathName": "E To Shoot" + "pathName": "E to Shoot" } }, { "type": "path", "data": { - "pathName": "Shoot To F (ADEF)" + "pathName": "Shoot to F (ADEF)" } }, { "type": "path", "data": { - "pathName": "F To Shoot (ADEF)" + "pathName": "F to Shoot (ADEF)" } } ] diff --git a/src/main/deploy/pathplanner/autos/4.5 CHGF.auto b/src/main/deploy/pathplanner/autos/4.5 CHGF.auto index 7a6daf28..f5b7b233 100644 --- a/src/main/deploy/pathplanner/autos/4.5 CHGF.auto +++ b/src/main/deploy/pathplanner/autos/4.5 CHGF.auto @@ -8,37 +8,37 @@ { "type": "path", "data": { - "pathName": "Forward First Piece to C" + "pathName": "Close Preload to C" } }, { "type": "path", "data": { - "pathName": "CShoot To H (CHGF)" + "pathName": "CShoot to H (CHGF)" } }, { "type": "path", "data": { - "pathName": "H To HShoot (HGF)" + "pathName": "H to HShoot (HGF)" } }, { "type": "path", "data": { - "pathName": "HShoot To G (HGF)" + "pathName": "HShoot to G (HGF)" } }, { "type": "path", "data": { - "pathName": "G To Shoot (HGF)" + "pathName": "G to Shoot (HGF)" } }, { "type": "path", "data": { - "pathName": "GShoot To F (HGF)" + "pathName": "GShoot to F (HGF)" } } ] diff --git a/src/main/deploy/pathplanner/autos/6 CBAED.auto b/src/main/deploy/pathplanner/autos/6 CBAED.auto index e60ed5b2..37eed8f9 100644 --- a/src/main/deploy/pathplanner/autos/6 CBAED.auto +++ b/src/main/deploy/pathplanner/autos/6 CBAED.auto @@ -1,12 +1,6 @@ { "version": 1.0, - "startingPose": { - "position": { - "x": 1.3103352321354325, - "y": 5.375733057397644 - }, - "rotation": 0 - }, + "startingPose": null, "command": { "type": "sequential", "data": { @@ -14,31 +8,31 @@ { "type": "path", "data": { - "pathName": "Forward First Piece to C" + "pathName": "Preload to C" } }, { "type": "path", "data": { - "pathName": "C to B 2" + "pathName": "C to B" } }, { "type": "path", "data": { - "pathName": "B To A" + "pathName": "B to A" } }, { "type": "path", "data": { - "pathName": "A To E" + "pathName": "A to E" } }, { "type": "path", "data": { - "pathName": "E To Shoot" + "pathName": "E to Shoot" } }, { @@ -50,7 +44,7 @@ { "type": "path", "data": { - "pathName": "D To Shoot" + "pathName": "D to Shoot" } } ] diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index 2740adcb..ffa08e70 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.81, - "y": 5.472864279061892 + "x": 2.78765406878382, + "y": 5.239147655045014 }, "prevControl": null, "nextControl": { - "x": 1.8152627375287849, - "y": 5.5043194331858505 + "x": 2.6183483616967234, + "y": 5.861535033255252 }, "isLocked": false, "linkedName": "B" @@ -20,28 +20,23 @@ "y": 6.94 }, "prevControl": { - "x": 1.6454530467410546, - "y": 6.691707053395119 + "x": 2.646912247528199, + "y": 6.50431154103049 }, "nextControl": null, "isLocked": false, "linkedName": "A" } ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ + "rotationTargets": [ { - "name": "IntakeToShooter", - "waypointRelativePos": 0.3, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } + "waypointRelativePos": 0.5, + "rotationDegrees": 60.0, + "rotateFast": true } ], + "constraintZones": [], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 4.0, @@ -50,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 23.336949256100596, + "rotation": 51.76617482255292, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C Shoot to B.path b/src/main/deploy/pathplanner/paths/C Shoot to B.path deleted file mode 100644 index f2609de5..00000000 --- a/src/main/deploy/pathplanner/paths/C Shoot to B.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.86, - "y": 4.84 - }, - "prevControl": null, - "nextControl": { - "x": 1.7588179877852295, - "y": 4.764330651795632 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.4882832788053633, - "y": 5.5447211491437915 - }, - "prevControl": { - "x": 1.3630468119145196, - "y": 5.581766365025268 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to B Close.path b/src/main/deploy/pathplanner/paths/C to B Close.path index 291ee93a..01b578f6 100644 --- a/src/main/deploy/pathplanner/paths/C to B Close.path +++ b/src/main/deploy/pathplanner/paths/C to B Close.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.3938754127508255, - "y": 4.103044831089663 + "x": 2.52, + "y": 4.1 }, "prevControl": null, "nextControl": { - "x": 1.5151395724967984, - "y": 4.789588512576437 + "x": 2.2862694665305834, + "y": 4.4310887318157315 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.81, - "y": 5.472864279061892 + "x": 2.78765406878382, + "y": 5.239147655045014 }, "prevControl": { - "x": 1.9184388425219452, - "y": 5.403007877888045 + "x": 2.4857700960953526, + "y": 5.063860187032356 }, "nextControl": null, "isLocked": false, @@ -30,8 +30,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0, - "rotationDegrees": 0, + "waypointRelativePos": 0.5, + "rotationDegrees": 72.16956019523882, "rotateFast": false } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.3821062752111186, + "rotation": 70.55996517182386, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 68920424..4d2cbcf0 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 2.5635091726023904, - "y": 4.034483282641779 + "x": 2.5, + "y": 4.265328388308022 }, "prevControl": null, "nextControl": { - "x": 1.7117636877122235, - "y": 4.2792545853082995 + "x": 2.5, + "y": 4.878834526352327 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 2.81, - "y": 5.472864279061892 + "x": 2.78765406878382, + "y": 5.239147655045014 }, "prevControl": { - "x": 1.9184388425219452, - "y": 5.403007877888045 + "x": 2.5539374447669423, + "y": 4.8009289850133685 }, "nextControl": null, "isLocked": false, "linkedName": "B" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 60.0, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,11 +45,14 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.3821062752111186, + "rotation": 60.71172956223985, "rotateFast": false }, "reversed": false, "folder": "ABCDE", - "previewStartingState": null, + "previewStartingState": { + "rotation": -20.0, + "velocity": 0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to Shoot.path b/src/main/deploy/pathplanner/paths/C to Shoot.path deleted file mode 100644 index 3fc3c897..00000000 --- a/src/main/deploy/pathplanner/paths/C to Shoot.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.86, - "y": 4.110774549907309 - }, - "prevControl": null, - "nextControl": { - "x": 1.8125191922025634, - "y": 4.0865084976438615 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.86, - "y": 4.84 - }, - "prevControl": { - "x": 1.7345230447936435, - "y": 4.811904277746514 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CShoot To H (CHGF).path b/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path similarity index 100% rename from src/main/deploy/pathplanner/paths/CShoot To H (CHGF).path rename to src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path diff --git a/src/main/deploy/pathplanner/paths/Close Preload to C.path b/src/main/deploy/pathplanner/paths/Close Preload to C.path index 9c99f4a1..35835595 100644 --- a/src/main/deploy/pathplanner/paths/Close Preload to C.path +++ b/src/main/deploy/pathplanner/paths/Close Preload to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.665075171110164, - "y": 4.100282002640273 + "x": 2.5247228667648325, + "y": 4.099779112962734 }, "prevControl": { - "x": 2.4985470329943635, - "y": 4.170894102576896 + "x": 2.4932833677369035, + "y": 4.167946461634323 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/F To Shoot (ADEF).path b/src/main/deploy/pathplanner/paths/F to Shoot (ADEF).path similarity index 100% rename from src/main/deploy/pathplanner/paths/F To Shoot (ADEF).path rename to src/main/deploy/pathplanner/paths/F to Shoot (ADEF).path diff --git a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path index 82964740..c31efa7e 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.559068829433955, - "y": 4.954498043255346 + "x": 5.508704809076285, + "y": 4.189449101120425 }, "isLocked": false, "linkedName": "F" }, { "anchor": { - "x": 2.18, - "y": 3.1400677569873663 + "x": 3.338562098420493, + "y": 5.052304113867487 }, "prevControl": { - "x": 3.3717256982662116, - "y": 1.8074229296606745 + "x": 4.81527342684315, + "y": 4.724383578396787 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -52.04098183775284, + "rotation": -20.559668324391616, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/G to GShoot (GHF).path b/src/main/deploy/pathplanner/paths/G to GShoot (GHF).path deleted file mode 100644 index 49846389..00000000 --- a/src/main/deploy/pathplanner/paths/G to GShoot (GHF).path +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.23517407718576, - "y": 2.71 - }, - "prevControl": null, - "nextControl": { - "x": 6.1908162722429365, - "y": 1.978112460378566 - }, - "isLocked": false, - "linkedName": "G" - }, - { - "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 - }, - "prevControl": { - "x": 4.183366623322263, - "y": 2.0791050812816336 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "HShoot" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.0, - "rotationDegrees": 0.8290217277682228, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -38.01026976540658, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GShoot to H (GHF).path b/src/main/deploy/pathplanner/paths/GShoot to H (GHF).path deleted file mode 100644 index 5a62ce01..00000000 --- a/src/main/deploy/pathplanner/paths/GShoot to H (GHF).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 - }, - "prevControl": null, - "nextControl": { - "x": 4.025243185262777, - "y": 2.4415905342679474 - }, - "isLocked": false, - "linkedName": "HShoot" - }, - { - "anchor": { - "x": 8.290000000000001, - "y": 0.4 - }, - "prevControl": { - "x": 6.563179294170366, - "y": 1.152045534129408 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "H" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -21.356126487835258, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path b/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path index 4e9eac65..6cce4637 100644 --- a/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.290000000000001, - "y": 0.4 + "x": 8.14162084164624, + "y": -0.14032985330927936 }, "prevControl": null, "nextControl": { - "x": 6.080497457486887, - "y": 0.7886650812581135 + "x": 5.932118299133126, + "y": 0.24833522794883411 }, "isLocked": false, "linkedName": "H" diff --git a/src/main/deploy/pathplanner/paths/HShoot to F (GHF).path b/src/main/deploy/pathplanner/paths/HShoot to F (GHF).path deleted file mode 100644 index 73afee94..00000000 --- a/src/main/deploy/pathplanner/paths/HShoot to F (GHF).path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 - }, - "prevControl": null, - "nextControl": { - "x": 4.575519250617969, - "y": 3.870125043665526 - }, - "isLocked": false, - "linkedName": "HShoot" - }, - { - "anchor": { - "x": 8.23, - "y": 3.9030382458913073 - }, - "prevControl": { - "x": 6.093336946970118, - "y": 3.527328454893818 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "F" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 0.515203372540753, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A to Intermediate D.path b/src/main/deploy/pathplanner/paths/Preload to A.path similarity index 66% rename from src/main/deploy/pathplanner/paths/A to Intermediate D.path rename to src/main/deploy/pathplanner/paths/Preload to A.path index 68ec0852..89aec544 100644 --- a/src/main/deploy/pathplanner/paths/A to Intermediate D.path +++ b/src/main/deploy/pathplanner/paths/Preload to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.8, - "y": 6.94 + "x": 1.3235291280709125, + "y": 6.988799251934787 }, "prevControl": null, "nextControl": { - "x": 3.846861284496223, - "y": 6.987958009273224 + "x": 2.606899400757718, + "y": 6.707805863084916 }, "isLocked": false, - "linkedName": "A" + "linkedName": null }, { "anchor": { - "x": 7.12, - "y": 7.016405622302347 + "x": 2.6068811631294135, + "y": 6.718114094456037 }, "prevControl": { - "x": 6.161376716515369, - "y": 6.967973856330742 + "x": 1.6820363767309796, + "y": 6.910357111853464 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 42.3, "rotateFast": false }, "reversed": false, - "folder": "ABCDE (ND)", + "folder": "ABCDE", "previewStartingState": { - "rotation": 0, + "rotation": 42.37477264972295, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Preload to C.path b/src/main/deploy/pathplanner/paths/Preload to C.path index c766f186..0ca02525 100644 --- a/src/main/deploy/pathplanner/paths/Preload to C.path +++ b/src/main/deploy/pathplanner/paths/Preload to C.path @@ -8,27 +8,33 @@ }, "prevControl": null, "nextControl": { - "x": 2.086577210467854, - "y": 4.913516618078012 + "x": 2.2131007014089956, + "y": 4.985954645693396 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5635091726023904, - "y": 4.034483282641779 + "x": 2.52, + "y": 4.1 }, "prevControl": { - "x": 1.8543373848558296, - "y": 4.232536387703213 + "x": 2.4857700960953526, + "y": 4.323757544312242 }, "nextControl": null, "isLocked": false, - "linkedName": "C" + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": -40.0, + "rotateFast": false } ], - "rotationTargets": [], "constraintZones": [], "eventMarkers": [ { @@ -49,8 +55,8 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, - "rotation": -0.8174456275588874, + "velocity": 0.0, + "rotation": -20.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Shoot To F (ADEF).path b/src/main/deploy/pathplanner/paths/Shoot to F (ADEF).path similarity index 100% rename from src/main/deploy/pathplanner/paths/Shoot To F (ADEF).path rename to src/main/deploy/pathplanner/paths/Shoot to F (ADEF).path diff --git a/src/main/deploy/pathplanner/paths/Start to G (GHF).path b/src/main/deploy/pathplanner/paths/Start to G (GHF).path deleted file mode 100644 index 03b0f916..00000000 --- a/src/main/deploy/pathplanner/paths/Start to G (GHF).path +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3992020999924704, - "y": 2.850407598330388 - }, - "prevControl": null, - "nextControl": { - "x": 3.7018969140206073, - "y": 2.095600533296336 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.23517407718576, - "y": 2.71 - }, - "prevControl": { - "x": 6.020547926742053, - "y": 2.139721213771854 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "G" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3, - "rotationDegrees": 10.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -0.15960239286688718, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": null, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to H (HGF).path b/src/main/deploy/pathplanner/paths/Start to H (HGF).path index 2bbdf4a1..15eec615 100644 --- a/src/main/deploy/pathplanner/paths/Start to H (HGF).path +++ b/src/main/deploy/pathplanner/paths/Start to H (HGF).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.87, - "y": 3.264181107065918 + "x": 1.76, + "y": 3.262 }, "prevControl": null, "nextControl": { - "x": 3.661009849273592, - "y": 1.8329545447689473 + "x": 2.639123454924323, + "y": 1.1446271693862373 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.290000000000001, - "y": 0.4 + "x": 8.14162084164624, + "y": -0.14032985330927936 }, "prevControl": { - "x": 5.268463833761794, - "y": 0.4 + "x": 5.120084675408034, + "y": -0.14032985330927938 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/TopShoot to Intermediate E.path b/src/main/deploy/pathplanner/paths/TopShoot to Intermediate E.path deleted file mode 100644 index 35b93731..00000000 --- a/src/main/deploy/pathplanner/paths/TopShoot to Intermediate E.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.729397148609112, - "y": 6.342607879660204 - }, - "prevControl": null, - "nextControl": { - "x": 4.341960448460829, - "y": 6.0979296735358535 - }, - "isLocked": false, - "linkedName": "Top Side Shoot" - }, - { - "anchor": { - "x": 7.12, - "y": 5.4 - }, - "prevControl": { - "x": 6.055093976145707, - "y": 6.024835113725493 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE (ND)", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TopShoot to Intermediate F.path b/src/main/deploy/pathplanner/paths/TopShoot to Intermediate F.path deleted file mode 100644 index 8841ddcc..00000000 --- a/src/main/deploy/pathplanner/paths/TopShoot to Intermediate F.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.729397148609112, - "y": 6.342607879660204 - }, - "prevControl": null, - "nextControl": { - "x": 3.679574105603651, - "y": 3.6353337471865927 - }, - "isLocked": false, - "linkedName": "Top Side Shoot" - }, - { - "anchor": { - "x": 7.908637226273295, - "y": 4.112193736829604 - }, - "prevControl": { - "x": 6.242703144549808, - "y": 4.112085174763521 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE (ND)", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ 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 9393070c..53c7f35e 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -8,16 +8,13 @@ import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import com.fasterxml.jackson.databind.introspect.AnnotatedField; import com.stuypulse.robot.commands.*; import com.stuypulse.robot.commands.amper.*; import com.stuypulse.robot.commands.auton.*; import com.stuypulse.robot.commands.auton.ADE.*; -import com.stuypulse.robot.commands.auton.ADEF.FourPieceADEF; import com.stuypulse.robot.commands.auton.CBADE.*; import com.stuypulse.robot.commands.auton.CHFG.FivePieceCHGF; import com.stuypulse.robot.commands.auton.DE.*; -import com.stuypulse.robot.commands.auton.GHF.*; import com.stuypulse.robot.commands.auton.HGF.*; import com.stuypulse.robot.commands.auton.tests.*; import com.stuypulse.robot.commands.climber.*; @@ -298,69 +295,40 @@ public void configureAutons() { autonChooser.addOption("Mobility", new Mobility()); - AutonConfig HGF = new AutonConfig("3.5 HGF", FourPieceHGF::new, + AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new, "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)"); AutonConfig TrackingCBAE = new AutonConfig("Tracking 5 CBAE Podium", FivePieceTrackingCBAE::new, "Preload to C", "C to B", "B to A", "A to E", "E to Shoot"); - AutonConfig PodiumCBAE = new AutonConfig("5 CBAE", FivePiecePodiumCBAE::new, - "Preload to C", "C to B", "B to A","A to E", "E to Shoot"); - AutonConfig CBAED = new AutonConfig("6 CBAED", SixPieceCBAED::new, - "Close Preload to C", "C to B Close", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); + "Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); AutonConfig CHGF = new AutonConfig("4.5 Piece CHGF", FivePieceCHGF::new, - "Close Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)"); + "Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)"); - AutonConfig ADEF = new AutonConfig("4.5 Piece ADEF", FourPieceADEF::new, - "Preload Shot to A", "A to D", "D to Shoot", "Shoot to E", "E to Shoot", "Shoot To F (ADEF)", "F To Shoot (ADEF)"); + // AutonConfig ADEF = new AutonConfig("4.5 Piece ADEF", FourPieceADEF::new, + // "Preload to A", "A to D", "D to Shoot", "Shoot to E", "E to Shoot", "Shoot To F (ADEF)", "F To Shoot (ADEF)"); - AutonConfig ADE = new AutonConfig("3 ADE", ThreePieceADE::new, - "Preload Shot to A", "A to D", "D to Ferry Shot", "Ferry Shot to E", "E to Shoot"); + // AutonConfig ADE = new AutonConfig("3 ADE", ThreePieceADE::new, + // "Preload to A", "A to D", "D to Ferry Shot", "Ferry Shot to E", "E to Shoot"); - AutonConfig DE = new AutonConfig("2 DE", TwoPieceDE::new, - "Preload Shot to D", "D to Ferry Shot", "Ferry Shot to E", "E to Shoot"); + // AutonConfig DE = new AutonConfig("2 DE", TwoPieceDE::new, + // "Preload Shot to D", "D to Ferry Shot", "Ferry Shot to E", "E to Shoot"); // AutonConfig PodiumCloseCBAE = new AutonConfig("Podium Close 5 Piece CBAE", FivePiecePodiumForwardCBAE::new, // "Forward First Piece to C", "C to B 2", "B to A","A to E", "E to Shoot"); - - // CBAE.registerBlue(autonChooser) - // .registerRed(autonChooser); - - // BLAY_CBAE - // .registerBlue(autonChooser) - // .registerRed(autonChooser); HGF.registerBlue(autonChooser) .registerRed(autonChooser); - // TrackingCBAE - // .registerBlue(autonChooser) - // .registerRed(autonChooser); - - PodiumCBAE - .registerDefaultBlue(autonChooser) - .registerDefaultRed(autonChooser); - - // ADE.registerBlue(autonChooser) - // .registerRed(autonChooser); - - // DE.registerBlue(autonChooser) - // .registerRed(autonChooser); - CBAED - .registerBlue(autonChooser) + .registerDefaultBlue(autonChooser) .registerRed(autonChooser); CHGF .registerBlue(autonChooser) .registerRed(autonChooser); - - - ADEF - .registerBlue(autonChooser) - .registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java deleted file mode 100644 index a63de216..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/BlayFivePieceCBAE.java +++ /dev/null @@ -1,47 +0,0 @@ -package com.stuypulse.robot.commands.auton.CBADE; - -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.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -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 BlayFivePieceCBAE extends SequentialCommandGroup { - - public BlayFivePieceCBAE(PathPlannerPath... paths) { - addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterPodiumShot()), - - SwerveDriveToPose.speakerRelative(-18) - ), - - new ConveyorShootRoutine(), - - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(2.9), - new ConveyorShootRoutine(), - - new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot(), - new ConveyorShootRoutine(), - - new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot(2.9) - .withTolerance(0.05, 3), - new ConveyorShootRoutine(), - - new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()) - ); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAD.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAD.java deleted file mode 100644 index db932b07..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/FivePieceCBAD.java +++ /dev/null @@ -1,46 +0,0 @@ -package com.stuypulse.robot.commands.auton.CBADE; - -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -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 FivePieceCBAD extends SequentialCommandGroup { - - public FivePieceCBAD() { - addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterPodiumShot()), - - SwerveDriveToPose.speakerRelative(-45) - ), - - new ConveyorShootRoutine() - - // new FollowPathAndIntake("First Piece to C"), - // new SwerveDriveToShoot(2.9), - // new ConveyorShootRoutine(), - - // new FollowPathAndIntake("C to B"), - // new SwerveDriveToShoot(), - // new ConveyorShootRoutine(), - - // new FollowPathAndIntake("B to A"), - // new SwerveDriveToShoot(2.9) - // .withTolerance(0.05, 3), - // new ConveyorShootRoutine(), - - // new FollowPathAndIntake("A to D"), - // new FollowPathAlignAndShoot("D to Shoot", new SwerveDriveToShoot()) - ); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBADE.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBADE.java deleted file mode 100644 index a9a39b40..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBADE.java +++ /dev/null @@ -1,49 +0,0 @@ -package com.stuypulse.robot.commands.auton.CBADE; - -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -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 SixPieceCBADE extends SequentialCommandGroup { - - public SixPieceCBADE() { - addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterPodiumShot()), - - SwerveDriveToPose.speakerRelative(-45) - ), - - new ConveyorShootRoutine() - - // new FollowPathAndIntake("First Piece to C"), - // new SwerveDriveToShoot(2.9) - // .withTolerance(0.1, 5), - // new ConveyorShootRoutine(), - - // new FollowPathAndIntake("C to B"), - // SwerveDriveToPose.speakerRelative(5), - // new ConveyorShootRoutine(), - - // new FollowPathAndIntake("B to A"), - // SwerveDriveToPose.speakerRelative(35), - // new ConveyorShootRoutine(), - - // new FollowPathAndIntake("A to D"), - // new FollowPathAlignAndShoot("D to Shoot", SwerveDriveToPose.speakerRelative(30)), - - // new FollowPathAndIntake("A to E"), - // new FollowPathAlignAndShoot("E to Shoot", SwerveDriveToPose.speakerRelative(30)) - ); - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java index f02fb1bd..3cf5b4b7 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java @@ -21,9 +21,10 @@ public SixPieceCBAED(PathPlannerPath... paths) { addCommands( new ParallelCommandGroup( new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterPodiumShot()), + .andThen(new ShooterPodiumShot()), - SwerveDriveToPose.speakerRelative(-18) + SwerveDriveToPose.speakerRelative(-15) + .withTolerance(0.1, 0.1, 3) ), new ConveyorShootRoutine(), diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CHFG/FivePieceCHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/CHFG/FivePieceCHGF.java index e41b031b..0c79aeb0 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CHFG/FivePieceCHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CHFG/FivePieceCHGF.java @@ -6,26 +6,19 @@ import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -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 FivePieceCHGF extends SequentialCommandGroup { public FivePieceCHGF(PathPlannerPath... paths) { addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterPodiumShot()), - - SwerveDriveToPose.speakerRelative(-18) - ), - + new ShooterPodiumShot(), + //shoot preload + new ShooterWaitForTarget() + .withTimeout(1.0), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[0]), 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 278f942d..117bbb6c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java @@ -32,13 +32,9 @@ public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand) { .andThen(new ShooterPodiumShot()) ), alignCommand, - new ShooterWaitForTarget(), - new ConveyorShoot(), - new WaitCommand(Settings.Conveyor.SHOOT_WAIT_DELAY.get()), - // ConveyorShoot.untilDone() - // .withTimeout(Settings.Conveyor.SHOOT_WAIT_DELAY.get()), - new ConveyorStop(), - new IntakeStop() + new ShooterWaitForTarget() + .withTimeout(0.5), + new ConveyorShootRoutine() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/GHF/FourPieceGHF.java b/src/main/java/com/stuypulse/robot/commands/auton/GHF/FourPieceGHF.java deleted file mode 100644 index 2c9fd84f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/GHF/FourPieceGHF.java +++ /dev/null @@ -1,38 +0,0 @@ -package com.stuypulse.robot.commands.auton.GHF; - -import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; -import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; -import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -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 FourPieceGHF extends SequentialCommandGroup { - - public FourPieceGHF() { - addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterPodiumShot()), - - SwerveDriveToPose.speakerRelative(-45) - .withTolerance(0.1, 0.1, 2) - ), - - new ConveyorShootRoutine() - - // new FollowPathAndIntake("Start to G (GHF)"), - // new FollowPathAlignAndShoot("G to GShoot (GHF)", SwerveDriveToPose.speakerRelative(-45)), - // new FollowPathAndIntake("GShoot to H (GHF)"), - // new FollowPathAlignAndShoot("H to HShoot (HGF)", SwerveDriveToPose.speakerRelative(-45)), - // new FollowPathAndIntake("HShoot to F (GHF)"), - // new FollowPathAlignAndShoot("F to Shoot (HGF)", SwerveDriveToPose.speakerRelative(-7)) - ); - } - -} 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 a43ff052..dd01cce4 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 @@ -6,30 +6,19 @@ import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -import com.stuypulse.robot.constants.Settings.Alignment; -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 FourPieceHGF extends SequentialCommandGroup { public FourPieceHGF(PathPlannerPath... paths) { addCommands( - new ParallelCommandGroup( - new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) - .andThen(new ShooterPodiumShot()), + new ShooterPodiumShot(), - SwerveDriveToPose.speakerRelative(-45) - .withTolerance(0.1, 0.1, 2) - ), - - new ShooterWaitForTarget(), + new ShooterWaitForTarget() + .withTimeout(1.0), new ConveyorShootRoutine(), - // new ShooterStop(), new FollowPathAndIntake(paths[0]), new FollowPathAlignAndShoot(paths[1], SwerveDriveToShoot.withHigherDebounce()), diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java index 62ec75fe..adaf6b79 100644 --- a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java @@ -1,6 +1,7 @@ package com.stuypulse.robot.commands.conveyor; import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java index 9db6915b..980718f2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java @@ -25,7 +25,6 @@ public enum AutonLEDColors { OCB("ThreePieceCB"), OHG("ThreePieceHG"), OGH("ThreePieceGH"), - OGHF("FourPieceGHF"), OHGF("FourPieceHGF"), OCBA("FourPieceCBA"), OCBAD("FivePieceCBAD"),