From 9726dd9000526ab3556a7863b6cd59e3ac5e07c3 Mon Sep 17 00:00:00 2001 From: Ian Shi <69531533+IanShiii@users.noreply.github.com> Date: Sat, 7 Sep 2024 11:55:43 -0400 Subject: [PATCH] Sus (#42) * fix swerveDrive.stop(), fix naming and place SwerveDriveToShoot in correct directory * changed points locations on pathplanner, fixed imports, added last path that i forgot :skull: * changing pathplanner paths * first redirection added, changed pathplanner robot locations to center of note, might change how redirection is done. experimenting with these... * name change * trying out simulation stuff * auton stuff bca * 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 * hgf auton chooser * added test autons, made tweaks to adef, hgf, and bca auton code. * n/a * fix ShootRoutine, add FollowPathPointSpeaker * fix subsystem requirement conflicts with automatic command scheduling stuff * sussy * oopsies! * 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 * automatic state machine for flywheel, move some triggers to subsystem periodics * resolve errors * move handoff automation to subsystem periodics * remove unecessary commands that are automated with intake logic, improve handoff logic * ferry alignment fixes * skibidi (update autons to use new subsystem logic) * fix deacquire button * skibidi * automatically bring arm back to feed after letting go of speaker/ferry buttons * sus * add fake data for low ferry interpolation so it doesnt crash * skibidi (fix intake side as front, fix intake) * tests and stuff * tune alignment (sorta) * straight line * buzz controller on intake * change name for intakeAcquire command * update intake led colors * add low ferry data * configure auto builder, trust angle from vision data more * revert to original method to find arm angle --------- Co-authored-by: jopy-man Co-authored-by: Tmanxyz --- .pathplanner/settings.json | 5 +- networktables.json | 1 + simgui-ds.json | 103 +++++++ simgui.json | 74 +++++ .../pathplanner/autos/{CBA.auto => BCA.auto} | 12 +- src/main/deploy/pathplanner/autos/BFAC.auto | 2 +- .../deploy/pathplanner/autos/HGF Red.auto | 55 ++++ src/main/deploy/pathplanner/autos/HGF.auto | 2 +- .../deploy/pathplanner/autos/Square Test.auto | 51 +++ .../pathplanner/autos/Sraight Line.auto | 25 ++ src/main/deploy/pathplanner/paths/A to C.path | 58 ---- src/main/deploy/pathplanner/paths/A to D.path | 16 +- .../deploy/pathplanner/paths/Amp to A.path | 16 +- src/main/deploy/pathplanner/paths/B to A.path | 24 +- .../paths/{F to E.path => B to C.path} | 22 +- src/main/deploy/pathplanner/paths/B to F.path | 27 +- src/main/deploy/pathplanner/paths/C to A.path | 24 +- src/main/deploy/pathplanner/paths/C to B.path | 58 ---- .../deploy/pathplanner/paths/Center to B.path | 15 +- .../deploy/pathplanner/paths/Center to C.path | 16 +- .../paths/{D to E.path => D Down.path} | 18 +- .../pathplanner/paths/D Shoot to E.path | 12 +- .../deploy/pathplanner/paths/D to Shoot.path | 8 +- .../pathplanner/paths/E Shoot to D.path | 18 +- .../pathplanner/paths/E Shoot to F.path | 18 +- .../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 +- .../pathplanner/paths/F to C Shoot.path | 10 +- 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 | 20 +- .../pathplanner/paths/FA Shoot to A.path | 8 +- .../pathplanner/paths/FC Shoot to C.path | 10 +- .../pathplanner/paths/G Shoot to F.path | 20 +- .../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 | 20 +- .../pathplanner/paths/H Shoot to G.path | 14 +- .../paths/{E to D.path => H Up.path} | 18 +- src/main/deploy/pathplanner/paths/H to G.path | 58 ---- .../deploy/pathplanner/paths/H to Shoot.path | 24 +- .../pathplanner/paths/Point Locations.path | 58 ++-- .../deploy/pathplanner/paths/Source to H.path | 47 +-- .../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 +++ .../paths/{E to F.path => Straight Line.path} | 22 +- src/main/java/com/stuypulse/robot/Robot.java | 30 +- .../com/stuypulse/robot/RobotContainer.java | 191 +++++------- .../commands/arm/ArmToLobFerryManual.java | 10 + .../commands/arm/ArmToLowFerryManual.java | 10 + .../commands/auton/ADEF/FivePieceADEF.java | 59 ++-- .../auton/ADEF/ReroutableFivePieceADEF.java | 62 ++++ .../commands/auton/BCA/FourPieceBCA.java | 47 +++ .../auton/BF_Series/FivePieceBFAC.java | 79 +++-- .../auton/BF_Series/FivePieceBFCA.java | 53 ++-- .../auton/BF_Series/FivePieceBFED.java | 53 ++-- .../auton/BF_Series/FivePieceBFGH.java | 53 ++-- .../commands/auton/CBA/FourPieceCBA.java | 46 --- .../auton/FollowPathAlignAndShoot.java | 44 --- .../commands/auton/FollowPathAndIntake.java | 31 -- .../commands/auton/FollowPathWithShoot.java | 23 -- .../auton/FollowPathWithShootAndIntake.java | 35 --- .../commands/auton/HGF/FourPieceHGF.java | 82 ++--- .../robot/commands/auton/ShootRoutine.java | 43 +++ .../robot/commands/auton/UntilNoteShot.java | 26 -- .../commands/auton/tests/SquareTest.java | 18 ++ .../commands/auton/tests/StraightLine.java | 15 + .../robot/commands/intake/IntakeAcquire.java | 30 -- .../commands/intake/IntakeDeacquire.java | 4 +- .../robot/commands/intake/IntakeFeed.java | 31 -- ...uireForever.java => IntakeSetAcquire.java} | 4 +- .../robot/commands/intake/IntakeShoot.java | 19 -- .../intake/IntakeWaitUntilHasNote.java | 12 + .../shooter/ShooterAcquireFromIntake.java | 39 --- .../ShooterAcquireFromIntakeWithRetry.java | 64 ---- .../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 -- .../swerve/SwerveDriveAlignToSpeaker.java | 97 ++++++ .../commands/swerve/SwerveDriveDrive.java | 9 +- .../swerve/SwerveDriveDriveRobotRelative.java | 7 +- .../SwerveDriveToShoot.java | 5 +- .../driveAligned/SwerveDriveDriveAligned.java | 16 +- ...java => SwerveDriveDriveAlignedFerry.java} | 4 +- .../SwerveDriveDriveAlignedLobFerry.java | 24 -- ...> SwerveDriveDriveAlignedManualFerry.java} | 4 +- ...SwerveDriveDriveAlignedManualLobFerry.java | 23 -- .../robot/constants/LEDInstructions.java | 4 +- .../stuypulse/robot/constants/Settings.java | 18 +- .../stuypulse/robot/subsystems/arm/Arm.java | 2 + .../robot/subsystems/arm/ArmImpl.java | 95 +++++- .../robot/subsystems/intake/Intake.java | 36 +++ .../robot/subsystems/shooter/Shooter.java | 96 ++++-- .../robot/subsystems/shooter/ShooterImpl.java | 96 ++++-- .../robot/subsystems/swerve/SwerveDrive.java | 72 ++++- .../robot/subsystems/vision/PhotonVision.java | 19 +- .../util/FollowPathPointSpeakerCommand.java | 291 ++++++++++++++++++ .../com/stuypulse/robot/util/PathUtil.java | 220 +++++++++++++ .../util/ShooterLowFerryInterpolation.java | 6 +- 110 files changed, 2369 insertions(+), 1591 deletions(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json rename src/main/deploy/pathplanner/autos/{CBA.auto => BCA.auto} (69%) 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 delete mode 100644 src/main/deploy/pathplanner/paths/A to C.path rename src/main/deploy/pathplanner/paths/{F to E.path => B to C.path} (71%) delete mode 100644 src/main/deploy/pathplanner/paths/C to B.path rename src/main/deploy/pathplanner/paths/{D to E.path => D Down.path} (78%) 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} (77%) delete mode 100644 src/main/deploy/pathplanner/paths/H to G.path 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 rename src/main/deploy/pathplanner/paths/{E to F.path => Straight Line.path} (72%) 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 create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java 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 create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java 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 delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java delete mode 100644 src/main/java/com/stuypulse/robot/commands/intake/IntakeFeed.java rename src/main/java/com/stuypulse/robot/commands/intake/{IntakeAcquireForever.java => IntakeSetAcquire.java} (79%) 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 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 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 create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java rename src/main/java/com/stuypulse/robot/commands/{shooter => swerve}/SwerveDriveToShoot.java (96%) 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 create mode 100644 src/main/java/com/stuypulse/robot/util/FollowPathPointSpeakerCommand.java create mode 100644 src/main/java/com/stuypulse/robot/util/PathUtil.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 75a4120e..56c73fa1 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/networktables.json b/networktables.json new file mode 100644 index 00000000..fe51488c --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..4fb780a7 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,103 @@ +{ + "Joysticks": { + "window": { + "visible": false + } + }, + "System Joysticks": { + "window": { + "visible": false + } + }, + "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 + } + ], + "zeroDisconnectedJoysticks": false +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 00000000..e61d9cdc --- /dev/null +++ b/simgui.json @@ -0,0 +1,74 @@ +{ + "HALProvider": { + "Addressable LEDs": { + "0": { + "serpentine": true + } + }, + "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": { + "Alignment": { + "open": true + }, + "Autonomous": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/deploy/pathplanner/autos/CBA.auto b/src/main/deploy/pathplanner/autos/BCA.auto similarity index 69% rename from src/main/deploy/pathplanner/autos/CBA.auto rename to src/main/deploy/pathplanner/autos/BCA.auto index 699f42d1..de2733c4 100644 --- a/src/main/deploy/pathplanner/autos/CBA.auto +++ b/src/main/deploy/pathplanner/autos/BCA.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.4365936349060198, - "y": 5.5179740558513855 + "x": 1.4053936459807443, + "y": 5.5301848373414 }, - "rotation": 0 + "rotation": 179.43822267700324 }, "command": { "type": "sequential", @@ -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 C" } }, { "type": "path", "data": { - "pathName": "B to A" + "pathName": "C to A" } } ] diff --git a/src/main/deploy/pathplanner/autos/BFAC.auto b/src/main/deploy/pathplanner/autos/BFAC.auto index 2293e32a..e85fb935 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/autos/HGF Red.auto b/src/main/deploy/pathplanner/autos/HGF Red.auto new file mode 100644 index 00000000..dcb5cb07 --- /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/HGF.auto b/src/main/deploy/pathplanner/autos/HGF.auto index dcb5cb07..ee71a20c 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/Square Test.auto b/src/main/deploy/pathplanner/autos/Square Test.auto new file mode 100644 index 00000000..35f5d714 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Square Test.auto @@ -0,0 +1,51 @@ +{ + "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": { +<<<<<<<< HEAD:src/main/deploy/pathplanner/autos/Square Test.auto + "pathName": "Square Right" +======== + "pathName": "B to C" +>>>>>>>> 7a9b4902c6973903cb64552237778325b95eeb47:src/main/deploy/pathplanner/autos/BCA Red.auto + } + }, + { + "type": "path", + "data": { +<<<<<<<< HEAD:src/main/deploy/pathplanner/autos/Square Test.auto + "pathName": "Square Bottom" + } + }, + { + "type": "path", + "data": { + "pathName": "Square Left" +======== + "pathName": "C to A" +>>>>>>>> 7a9b4902c6973903cb64552237778325b95eeb47:src/main/deploy/pathplanner/autos/BCA Red.auto + } + } + ] + } + }, + "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 00000000..f1d96517 --- /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": -179.56356838292098 + }, + "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/A to C.path b/src/main/deploy/pathplanner/paths/A to C.path deleted file mode 100644 index 8ab12e3c..00000000 --- a/src/main/deploy/pathplanner/paths/A to C.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.49, - "y": 6.82 - }, - "prevControl": null, - "nextControl": { - "x": 1.8063404583877622, - "y": 5.684733839028691 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5041716843106308, - "y": 4.352132867212163 - }, - "prevControl": { - "x": 1.7061923065438394, - "y": 5.606093593139567 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -35.0, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -35.01025016685515, - "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/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path index e37f2b30..d9c2175a 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.9, + "y": 6.997401573858207 }, "prevControl": null, "nextControl": { - "x": 3.679387291810298, - "y": 6.898225029708988 + "x": 4.040650442573206, + "y": 7.182822771523519 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.80980431566518, - "y": 7.431413130125368 + "x": 8.29, + "y": 7.44 }, "prevControl": { - "x": 5.426682819511473, - "y": 7.094030199035101 + "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 769f6912..dc47433d 100644 --- a/src/main/deploy/pathplanner/paths/Amp to A.path +++ b/src/main/deploy/pathplanner/paths/Amp to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.435487934219916, + "x": 1.44, "y": 6.595791855088175 }, "prevControl": null, "nextControl": { - "x": 2.26274434319702, - "y": 6.615980477868694 + "x": 1.8597816691764337, + "y": 7.114461624756583 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5146518038571792, - "y": 6.712803832043677 + "x": 2.9, + "y": 7.002978151231599 }, "prevControl": { - "x": 1.7508529994738564, - "y": 6.559928693704114 + "x": 2.0125550145445525, + "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 3920a744..bcbb7d64 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 2.403841365532434, - "y": 5.547443382833194 + "x": 2.9, + "y": 5.547731826490439 }, "prevControl": null, "nextControl": { - "x": 2.1124651977726576, - "y": 6.184182756200334 + "x": 2.401088620852837, + "y": 6.1130813946895755 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.505085089225238, - "y": 6.749916915942463 + "x": 2.9, + "y": 7.002641633631481 }, "prevControl": { - "x": 1.9565133270891542, - "y": 6.234996913818232 + "x": 1.7418025682777556, + "y": 6.382103179012906 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 40.15509788025759, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/F to E.path b/src/main/deploy/pathplanner/paths/B to C.path similarity index 71% rename from src/main/deploy/pathplanner/paths/F to E.path rename to src/main/deploy/pathplanner/paths/B to C.path index 586f73ae..272419ce 100644 --- a/src/main/deploy/pathplanner/paths/F to E.path +++ b/src/main/deploy/pathplanner/paths/B to C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 2.9, + "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 7.538234612369523, - "y": 5.378126956030698 + "x": 1.979636893611585, + "y": 4.568946349574247 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.83, - "y": 5.77 + "x": 2.5, + "y": 4.1 }, "prevControl": { - "x": 7.287673222109838, - "y": 5.7519018618766395 + "x": 2.0252109914562095, + "y": 4.045661481389927 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -179.19416655403055, "rotateFast": false }, "reversed": false, - "folder": "Reroutes", + "folder": "ABCDE", "previewStartingState": { - "rotation": -29.424188910993948, + "rotation": -179.44747299332943, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path index d0cd21c6..fc559e85 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.55 }, "prevControl": null, "nextControl": { - "x": 3.5538181530232116, - "y": 5.137757241660317 + "x": 4.242237014979983, + "y": 4.787595339960025 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.29, + "y": 4.1 }, "prevControl": { - "x": 5.998955035484476, - "y": 3.6317929071870045 + "x": 6.3594946760057205, + "y": 3.9070792672463317 }, "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 3ae25545..7f1e3efb 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.5, + "y": 4.1 }, "prevControl": null, "nextControl": { - "x": 1.9086797539294484, - "y": 5.314909721151288 + "x": 2.1540599070781474, + "y": 4.355657228204337 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.47, - "y": 6.77 + "x": 2.8953493574769893, + "y": 6.995863207686237 }, "prevControl": { - "x": 1.9818002210409187, - "y": 5.977080210298814 + "x": 1.8478662161937423, + "y": 7.153016926941592 }, "nextControl": null, "isLocked": false, @@ -31,8 +31,8 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 1.0388554318784187, - "rotateFast": true + "rotationDegrees": -116.48352775095083, + "rotateFast": false } ], "constraintZones": [], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 38.18757752913914, + "rotation": 178.9517141332874, "rotateFast": false }, "reversed": false, "folder": "ABCDE", "previewStartingState": { - "rotation": -35.0, + "rotation": -179.34585253591464, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path deleted file mode 100644 index 073ac455..00000000 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.51, - "y": 4.29 - }, - "prevControl": null, - "nextControl": { - "x": 2.0727080470157966, - "y": 4.981565001262443 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.425282344054272, - "y": 5.545520425118232 - }, - "prevControl": { - "x": 1.6559550362404298, - "y": 5.38476116014732 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0, - "rotateFast": true - } - ], - "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": "ABCDE", - "previewStartingState": { - "rotation": -32.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to B.path b/src/main/deploy/pathplanner/paths/Center to B.path index ea98b40c..52c2c21a 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.537823444322484, - "y": 5.5301848373414 + "x": 2.9, + "y": 5.55 }, "prevControl": { - "x": 2.0945336170806272, - "y": 5.562923192438648 + "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/Center to C.path b/src/main/deploy/pathplanner/paths/Center to C.path index 190fa1d3..0ed52eca 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.9, + "y": 4.1 }, "prevControl": { - "x": 2.1220319124045988, - "y": 4.703745685393156 + "x": 2.5125240204349457, + "y": 4.5159838276893804 }, "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 to E.path b/src/main/deploy/pathplanner/paths/D Down.path similarity index 78% rename from src/main/deploy/pathplanner/paths/D to E.path rename to src/main/deploy/pathplanner/paths/D Down.path index 9e2d9a50..6b7cd025 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", "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 a2699513..a6b0dc62 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.29, + "y": 5.777236829771279 }, "prevControl": { - "x": 6.47604084456681, - "y": 6.294320159324842 + "x": 6.89080789265001, + "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 63adf4ed..3b653a76 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.29, + "y": 7.44 }, "prevControl": null, "nextControl": { - "x": 6.609157592385126, - "y": 7.043167967474329 + "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 71074dab..c4506a08 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": 7.79, - "y": 7.44 + "x": 8.29, + "y": 7.45 }, "prevControl": { - "x": 6.876162419906684, - "y": 7.238618253300495 + "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": { @@ -39,7 +45,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 95aa5b23..a5f95372 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.29, + "y": 4.1 }, "prevControl": { - "x": 6.716218263165693, - "y": 5.79459152314882 + "x": 6.771776810093797, + "y": 4.0354847686629896 }, "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 Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index 9902b876..677f2839 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.29, + "y": 5.779496305086361 }, "prevControl": null, "nextControl": { - "x": 6.464262728562661, - "y": 6.586753953827849 + "x": 6.92426272856266, + "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 b0d2f337..ff2701ee 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.29, + "y": 5.78 }, "prevControl": { - "x": 6.77751254033014, - "y": 6.22081510067038 + "x": 7.115890093186435, + "y": 5.924606420165222 }, "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 227629f6..aa8c1e55 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.9, + "y": 7.0 }, "prevControl": { - "x": 2.094966282566494, - "y": 6.342153732484548 + "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 aeef9976..a094e841 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.9, + "y": 4.103734804381811 }, "prevControl": { - "x": 1.7198933802629512, - "y": 5.131796072743931 + "x": 1.1951537638566347, + "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 1ae9e0dc..ff5fe7da 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.29, + "y": 5.78 }, "prevControl": { - "x": 7.661789273883607, - "y": 5.857831093630937 + "x": 7.126129843018615, + "y": 6.127863050636817 }, "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 6dd5ba45..f34c410c 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.29, + "y": 2.4413378597962554 }, "prevControl": { - "x": 6.730603199234672, - "y": 3.645327872322412 + "x": 7.298378780336429, + "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 2d63eaa8..b72b5a37 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.29, + "y": 4.102244512152715 }, "prevControl": null, "nextControl": { - "x": 4.9685862178644005, - "y": 4.268436132668396 + "x": 5.295522172316666, + "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/deploy/pathplanner/paths/F to C Shoot.path b/src/main/deploy/pathplanner/paths/F to C Shoot.path index 03d2921b..1c12f0b7 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 G.path b/src/main/deploy/pathplanner/paths/F to G.path deleted file mode 100644 index 0dba821b..00000000 --- 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 e1195418..9d0249a0 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 164c2fe6..a96d0d43 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": 147.06548083331427, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -8.268297961765734, + "rotation": 170.34501153780897, "rotateFast": false }, "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -27.729825561933676, + "rotation": 178.17768989239653, "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 a52fbdf2..9142313d 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 accb8635..ef5a379c 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 65685f5c..e7d5397b 100644 --- a/src/main/deploy/pathplanner/paths/G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/G Shoot to F.path @@ -16,25 +16,19 @@ }, { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 8.29, + "y": 4.1 }, "prevControl": { - "x": 6.6666540280625215, - "y": 4.085370558203914 + "x": 7.033169768534478, + "y": 3.9595610497024567 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.4, - "rotationDegrees": -12.184647650336021, - "rotateFast": true - } - ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -45,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -27.573918256423003, + "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 Shoot to H.path b/src/main/deploy/pathplanner/paths/G Shoot to H.path index 79b34d8e..d78d8a2f 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 58f3cbd5..00000000 --- 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 b7848fb0..00000000 --- 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 25198788..cc792261 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": 138.52168950764727, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -7.757990482222401, + "rotation": 171.1798219104725, "rotateFast": false }, "reversed": false, "folder": "HGF", "previewStartingState": { - "rotation": -15.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 3ddeed4f..bfd69f3e 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.664236416642477, + "y": 2.2212553493187346 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -25.0, + "rotationDegrees": 127.53126500479296, "rotateFast": true } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -40.317362875171, + "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/E to D.path b/src/main/deploy/pathplanner/paths/H Up.path similarity index 77% rename from src/main/deploy/pathplanner/paths/E to D.path rename to src/main/deploy/pathplanner/paths/H Up.path index 11739768..5f01f568 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", "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 8949975d..00000000 --- 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 5c284ece..743f5702 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": 7.1779684844711955, + "y": 2.4336941028792762 }, "isLocked": false, "linkedName": null @@ -20,15 +20,21 @@ "y": 4.919501541012011 }, "prevControl": { - "x": 6.113755611067771, - "y": 4.501017868293177 + "x": 5.905931956023139, + "y": 4.587406743637892 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 158.01836985356624, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -8.797211565554276, + "rotation": 175.17249248336842, "rotateFast": false }, "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 9ef48822..aed960cf 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 @@ -64,47 +64,47 @@ }, { "anchor": { - "x": 8.28, + "x": 8.3, "y": 7.44 }, "prevControl": { - "x": 7.180548926469876, - "y": 6.917676610673163 + "x": 7.283808318646552, + "y": 6.7699486192294955 }, "nextControl": { - "x": 8.69419010200492, - "y": 7.63677199205437 + "x": 8.351646965563278, + "y": 7.474054717454673 }, "isLocked": false, "linkedName": null }, { "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 57f2fdad..e6a11a6c 100644 --- a/src/main/deploy/pathplanner/paths/Source to H.path +++ b/src/main/deploy/pathplanner/paths/Source to H.path @@ -3,41 +3,25 @@ "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 }, { "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.869285697276752, + "y": 1.038924798273703 }, "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": 149.51442160606817, + "rotateFast": true } ], "constraintZones": [], @@ -66,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -35.9402672501694, + "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/Square Bottom.path b/src/main/deploy/pathplanner/paths/Square Bottom.path new file mode 100644 index 00000000..4f00d91c --- /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 00000000..9b4fb714 --- /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 00000000..52be2d59 --- /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 00000000..7364ccab --- /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/E to F.path b/src/main/deploy/pathplanner/paths/Straight Line.path similarity index 72% rename from src/main/deploy/pathplanner/paths/E to F.path rename to src/main/deploy/pathplanner/paths/Straight Line.path index 0611a22d..4ee5c7f1 100644 --- a/src/main/deploy/pathplanner/paths/E to F.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.83, - "y": 5.77 + "x": 1.5, + "y": 5.552154629234854 }, "prevControl": null, "nextControl": { - "x": 8.037386361231057, - "y": 5.1801103853323776 + "x": 2.9, + "y": 5.552154629234854 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.89, - "y": 4.34 + "x": 4.15, + "y": 5.55 }, "prevControl": { - "x": 7.447759451880511, - "y": 4.608703500331107 + "x": 2.9000000000000004, + "y": 5.55 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -28.367522393402137, + "rotation": 0.9338907055017344, "rotateFast": false }, "reversed": false, - "folder": "Reroutes", + "folder": "Tests", "previewStartingState": { - "rotation": 0, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 419cc381..cf181adb 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -1,18 +1,15 @@ package com.stuypulse.robot; -import com.stuypulse.robot.commands.BuzzController; -import com.stuypulse.robot.commands.intake.IntakeShoot; -import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; -import com.stuypulse.robot.commands.shooter.ShooterManualIntake; +import com.pathplanner.lib.pathfinding.LocalADStar; +import com.pathplanner.lib.pathfinding.Pathfinding; +import com.stuypulse.robot.commands.leds.LEDReset; 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; 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; @@ -28,6 +25,7 @@ public class Robot extends TimedRobot { } private RobotContainer robot; + private CommandScheduler scheduler; private Command auto; /*************************/ @@ -36,7 +34,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 @@ -73,6 +82,11 @@ public void autonomousInit() { if (auto != null) { auto.schedule(); } + + scheduler.schedule(new LEDReset()); + + 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 91dec589..1dcb3f7a 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,43 +1,39 @@ 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; 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; import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.intake.IntakeAcquire; -import com.stuypulse.robot.commands.intake.IntakeAcquireForever; +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.IntakeDeacquire; -import com.stuypulse.robot.commands.intake.IntakeFeed; -import com.stuypulse.robot.commands.intake.IntakeShoot; +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; 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.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; 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; @@ -51,7 +47,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.SLColor; +import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.robot.subsystems.arm.Arm; @@ -68,6 +64,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; @@ -104,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)); } /****************/ @@ -112,7 +112,6 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - intake.setDefaultCommand(new IntakeStop()); leds.setDefaultCommand(new LEDDefaultMode()); } @@ -123,7 +122,6 @@ private void configureDefaultCommands() { private void configureButtonBindings() { configureOperatorBindings(); configureDriverBindings(); - configureAutomaticCommandScheduling(); } private void configureDriverBindings() { @@ -131,7 +129,7 @@ private void configureDriverBindings() { driver.getDPadUp() .onTrue(new ArmToPreClimb()) - .onTrue(new ShooterStop()) + .onTrue(new ShooterFeederStop()) .onTrue(new IntakeStop()); driver.getDPadDown().onTrue(new ArmToClimbing()); @@ -140,25 +138,24 @@ 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 IntakeSetAcquire()) + .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 IntakeSetAcquire()) + .whileTrue(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING)) + .onFalse(new IntakeStop()); // 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 @@ -168,101 +165,87 @@ 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())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker()).andThen(new WaitCommand(0.25))) .andThen(new ShooterFeederShoot()) ) .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()) + .onFalse(new ArmToFeed().onlyIf(() -> arm.getState() == Arm.State.SPEAKER)); // lob ferry align and shoot driver.getLeftStickButton() - .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver) - .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .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))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // low ferry align and shoot driver.getRightStickButton() - .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver) - .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .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))) - .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // 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()) + .onFalse(new ArmToFeed()); // manual lob ferry driver.getTopButton() - .whileTrue(new SwerveDriveDriveAlignedManualLobFerry(driver) - .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) + .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.isAlignedToManualLobFerry())) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualFerry())) .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // manual low ferry driver.getLeftButton() - .whileTrue(new SwerveDriveDriveAlignedManualLowFerry(driver) - .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) + .whileTrue(new SwerveDriveDriveAlignedManualFerry(driver) + .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.isAlignedToManualLowFerry())) - .andThen(new ShooterFeederShoot() - .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) - ) + .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualFerry())) + .andThen(new ShooterFeederShoot()) ) .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)) ) - .onFalse(new ConditionalCommand( - new ShooterFeederStop(), - new ShooterStop(), - () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + .onFalse(new ShooterFeederStop()) + .onFalse(new ArmToFeed()); // human player attention button driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION)); @@ -272,46 +255,40 @@ 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 ***/ /**************/ public void configureAutons() { - autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + autonChooser.addOption("Do Nothing", new DoNothingAuton()); + autonChooser.addOption("Mobility", new Mobility()); + autonChooser.addOption("Straight Line", new StraightLine()); + + + 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"); + // 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_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_BLUE.registerDefaultBlue(autonChooser); + BCA_RED.registerRed(autonChooser); + + // HGF.registerBlue(autonChooser); + // HGF_RED.registerRed(autonChooser); + + ADEF_BLUE.registerBlue(autonChooser); + ADEF_RED.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); + } public Command getAutonomousCommand() { @@ -322,7 +299,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/arm/ArmToLobFerryManual.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToLobFerryManual.java new file mode 100644 index 00000000..666dda20 --- /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 00000000..e2b98a1e --- /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/auton/ADEF/FivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java index dc8315c8..dd391809 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,33 +1,52 @@ 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.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +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 { public FivePieceADEF(PathPlannerPath... paths) { + addCommands( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), - - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), - - new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), - - new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - - new FollowPathAndIntake(paths[5]) + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + 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 IntakeSetAcquire(), + 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 E + new IntakeSetAcquire(), + 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 IntakeSetAcquire(), + 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/ADEF/ReroutableFivePieceADEF.java b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.java new file mode 100644 index 00000000..d61a03f3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/ReroutableFivePieceADEF.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 ReroutableFivePieceADEF extends SequentialCommandGroup { + +// public ReroutableFivePieceADEF(PathPlannerPath... paths) { + +// PathPlannerPath D_DOWN = 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(D_DOWN), + +// 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/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java new file mode 100644 index 00000000..8ff7324c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -0,0 +1,47 @@ +package com.stuypulse.robot.commands.auton.BCA; + +import com.pathplanner.lib.path.PathPlannerPath; +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.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; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FourPieceBCA extends SequentialCommandGroup { + + public FourPieceBCA(PathPlannerPath... paths) { + addCommands( + // Preload Shot + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + // Drive to B + Shoot B + 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 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 IntakeSetAcquire(), + 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 9eac5362..ac77b0c5 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,36 +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.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.shooter.SwerveDriveToShoot; +// 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( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), - - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), - - new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), - - new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), - - new FollowPathAndIntake(paths[5]) - ); - - } - -} +// 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 46740f52..b81f4081 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,36 +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.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.shooter.SwerveDriveToShoot; +// 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( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), +// addCommands( +// ShootRoutine.fromSubwoofer(), +// new ArmToFeed(), - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), +// new FollowPathAndIntake(paths[0]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), +// new FollowPathAndIntake(paths[1]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), +// new FollowPathAndIntake(paths[3]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[5]) - ); +// 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 08c6acc1..c52f0b1f 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,36 +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.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.shooter.SwerveDriveToShoot; +// 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( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), +// addCommands( +// ShootRoutine.fromSubwoofer(), +// new ArmToFeed(), - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), +// new FollowPathAndIntake(paths[0]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), +// new FollowPathAndIntake(paths[1]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), +// new FollowPathAndIntake(paths[3]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[5]) - ); +// 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 6474c310..0b437ec8 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,36 +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.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.shooter.SwerveDriveToShoot; +// 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( - new ShooterScoreSpeaker(), - - new ShooterWaitForTarget() - .withTimeout(1.0), +// addCommands( +// ShootRoutine.fromSubwoofer(), +// new ArmToFeed(), - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), +// new FollowPathAndIntake(paths[0]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[1]), - new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), +// new FollowPathAndIntake(paths[1]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), +// new FollowPathAndIntake(paths[3]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), +// ShootRoutine.fromAnywhere(), - new FollowPathAndIntake(paths[5]) - ); +// new FollowPathAndIntake(paths[5]), +// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), +// ShootRoutine.fromAnywhere() +// ); - } +// } -} +// } 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 deleted file mode 100644 index a25756a0..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java +++ /dev/null @@ -1,46 +0,0 @@ -package com.stuypulse.robot.commands.auton.CBA; - -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.shooter.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 FourPieceCBA extends SequentialCommandGroup { - - public FourPieceCBA(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/FollowPathAlignAndShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java deleted file mode 100644 index 9fd382d5..00000000 --- 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 UntilNoteShot(0.75)); - else - addCommands(new ShooterScoreSpeaker()); - } - -} \ No newline at end of file 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 5149e604..00000000 --- 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/FollowPathWithShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java deleted file mode 100644 index 84370dfa..00000000 --- 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 77c6afd1..00000000 --- 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/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index eacb6a37..9d1ec0a7 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.UntilNoteShot; -import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -import com.stuypulse.robot.commands.shooter.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 UntilNoteShot(0.7), - - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new UntilNoteShot(), - - new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new UntilNoteShot(), - - new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot() - .withTolerance(0.03, 3), - new UntilNoteShot(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 new file mode 100644 index 00000000..eda3aeb3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -0,0 +1,43 @@ +package com.stuypulse.robot.commands.auton; + +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.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.swerve.SwerveDrive; + +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 { + + public static Command fromSubwoofer() { + return new SequentialCommandGroup( + new ArmToSubwooferShot(), + new ArmWaitUntilAtTarget().alongWith(new ShooterWaitForTarget()).withTimeout(1.0), + new ShooterFeederShoot(), + new WaitCommand(0.25) // give time for note to leave shooter + ); + } + + public static Command fromAnywhere() { + return new SwerveDriveAlignToSpeaker() + .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())) + .andThen(new ShooterFeederShoot()) + .andThen(new WaitCommand(0.25)) // give time for note to leave shooter + ) + .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java deleted file mode 100644 index 2fa0eb24..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java +++ /dev/null @@ -1,26 +0,0 @@ -package com.stuypulse.robot.commands.auton; - -import com.stuypulse.robot.commands.intake.IntakeStop; -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; - -public class UntilNoteShot extends SequentialCommandGroup { - - public UntilNoteShot(double timeout) { - addCommands( - new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), - new WaitUntilCommand(Shooter.getInstance()::hasNote) - .withTimeout(timeout), - new IntakeStop() - ); - } - - public UntilNoteShot() { - this(Settings.Auton.SHOOT_WAIT_DELAY.get()); - } -} \ No newline at end of file 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 00000000..49835197 --- /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 00000000..84685bac --- /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") + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java deleted file mode 100644 index 7fb25d0f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.stuypulse.robot.commands.intake; - -import com.stuypulse.robot.subsystems.intake.Intake; - -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeAcquire extends Command { - - private final Intake intake; - - public IntakeAcquire() { - intake = Intake.getInstance(); - addRequirements(intake); - } - - @Override - 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/IntakeDeacquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java index 4c8ecdbc..335cfbc7 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 1d4f2895..00000000 --- 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/IntakeAcquireForever.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetAcquire.java similarity index 79% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeSetAcquire.java index ce357698..e27db8a6 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.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 IntakeAcquireForever extends InstantCommand { +public class IntakeSetAcquire extends InstantCommand { private final Intake intake; - public IntakeAcquireForever() { + public IntakeSetAcquire() { intake = Intake.getInstance(); addRequirements(intake); } 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 a0afb129..00000000 --- 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 00000000..73a4b53a --- /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/commands/shooter/ShooterAcquireFromIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java deleted file mode 100644 index a35138ce..00000000 --- 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 3d6ceef8..00000000 --- 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 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 76b2d342..00000000 --- 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 31928d8e..00000000 --- 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 5c1e2365..00000000 --- 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 c9224a1f..00000000 --- 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 104daa3d..00000000 --- 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 db0bd361..00000000 --- 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/commands/swerve/SwerveDriveAlignToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java new file mode 100644 index 00000000..09a45dc6 --- /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/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index 537d18c2..a59491aa 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 95f28a38..d8df3682 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/shooter/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java similarity index 96% 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 d500dbac..639b0ad6 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; @@ -149,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/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index aee44209..ff4caf58 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/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 70d14458..2acf4d22 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 6b928604..00000000 --- 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 a4a71e8c..44ffb4e9 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 614d976d..00000000 --- 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/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index 9718f243..577c5c2f 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; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index f1b4daea..7d0ad555 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -127,16 +127,17 @@ 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), 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 @@ -196,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; @@ -240,7 +241,7 @@ public interface Turn { public interface Turn { 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.30718); SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 1.42659); @@ -309,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); @@ -410,6 +411,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 { @@ -420,7 +422,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/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index a3547b70..8be89ff1 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 005c34a2..91666188 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -12,17 +12,22 @@ 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; 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; 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; @@ -95,8 +100,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: @@ -147,6 +156,91 @@ 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() + (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() + // ); + + // 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 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); + // } + // catch (Exception exception) { + // exception.printStackTrace(); + // return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); + // } + // } + private double getDegrees() { return 360 * armEncoder.getPosition(); } @@ -209,7 +303,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 43b7ddd2..124ef191 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,10 @@ 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 com.stuypulse.robot.subsystems.shooter.Shooter.FeederState; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -42,6 +47,37 @@ 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 + && Shooter.getInstance().getFeederState() != Shooter.FeederState.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()) + || (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 6f5fad3b..e1f39f73 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,8 @@ 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; @@ -25,34 +27,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 +52,64 @@ 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; + } + + // automatic handoff + boolean shouldHandoff = Arm.getInstance().getState() == Arm.State.FEED + && Arm.getInstance().atValidFeedAngle() + && !hasNote() + && Intake.getInstance().hasNote() + && Intake.getInstance().getState() != Intake.State.DEACQUIRING + && getFeederState() != FeederState.DEACQUIRING; + + if (shouldHandoff) { + setFeederState(FeederState.INTAKING); + } + if (feederState == FeederState.INTAKING && (hasNote() || Arm.getInstance().getState() != Arm.State.FEED)) { + setFeederState(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 65bf49e8..0fd72cbc 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()); 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 b1b6b8a4..f3f07f66 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -9,20 +9,28 @@ 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; 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; @@ -78,6 +86,8 @@ protected SwerveDrive(SwerveDrivetrainConstants driveTrainConstants, double Upda } modules2D = new FieldObject2d[Modules.length]; field = new Field2d(); + + configureAutoBuilder(); } public Command applyRequest(Supplier requestSupplier) { @@ -120,13 +130,53 @@ public Field2d getField() { } public void stop() { - stop(); + setControl(new SwerveRequest.FieldCentric() + .withVelocityX(0) + .withVelocityY(0) + .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)); } + 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) @@ -135,6 +185,10 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { ); } + public ChassisSpeeds getChassisSpeeds() { + return m_kinematics.toChassisSpeeds(m_moduleStates); + } + public Command followPathCommand(PathPlannerPath path) { return new FollowPathHolonomic( path, @@ -174,26 +228,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; @@ -213,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)); } 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 1e645aa0..7122aa0f 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,18 @@ 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.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); + } + ); } - ); + } } } 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 00000000..f0cdc24d --- /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); + } +} 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 00000000..1f5eb703 --- /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 diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java index 7276aeb6..5ae6e472 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java @@ -13,7 +13,11 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - + {2000, 204}, + {2000, 130}, + {2500, 235}, + {2500, 246}, + {2500, 284}, }; static {