From 559e338e798dc84dde57ffa1a3e1043497bdf123 Mon Sep 17 00:00:00 2001 From: Matthew Milunic <62996888+mmilunicmobile@users.noreply.github.com> Date: Sun, 23 Feb 2025 21:41:06 -0500 Subject: [PATCH] Adding all paths (#217) * so close to a working left side 4. * oh i didnt need those commands * pushing this so matthew can fix it later * wewawewawewa * auto changes yay!! * saturday auto changes * ugh somebody fix this * final auto changes before 103 * format * lets gooooooooooooooo --------- Co-authored-by: trainrechs Co-authored-by: AlexNunemacher --- .../{test matthew yay.auto => Left3.auto} | 42 ++++- .../{Test Half Field.auto => PushAuto.auto} | 16 +- src/main/deploy/pathplanner/autos/Right3.auto | 154 ++++++++++++++++++ .../autos/Spinny Difficult Auto Choreo.auto | 19 --- .../pathplanner/autos/Test Choreo Auto.auto | 19 --- .../pathplanner/autos/Test-Auto Auto.auto | 55 ------- .../autos/{Test 1M.auto => leaveAuto.auto} | 2 +- .../deploy/pathplanner/autos/pleasework.auto | 26 +++ src/main/deploy/pathplanner/paths/B-RCS.path | 54 ++++++ src/main/deploy/pathplanner/paths/C-RCS.path | 79 +++++++++ src/main/deploy/pathplanner/paths/D-RCS.path | 79 +++++++++ src/main/deploy/pathplanner/paths/E-RCS.path | 79 +++++++++ src/main/deploy/pathplanner/paths/F-RCS.path | 79 +++++++++ src/main/deploy/pathplanner/paths/J-LCS.path | 79 +++++++++ src/main/deploy/pathplanner/paths/K-LCS.path | 79 +++++++++ src/main/deploy/pathplanner/paths/L-LCS.path | 79 +++++++++ src/main/deploy/pathplanner/paths/LCS-A.path | 105 ++++++++++++ .../{Half-Field Sprint.path => LCS-B.path} | 28 ++-- src/main/deploy/pathplanner/paths/LCS-K.path | 92 +++++++++++ src/main/deploy/pathplanner/paths/LCS-L.path | 92 +++++++++++ .../paths/{1M.path => LCorner-Leave.path} | 26 +-- .../paths/{ab to bc.path => LStart-I.path} | 28 ++-- .../deploy/pathplanner/paths/LStart-J.path | 66 ++++++++ .../deploy/pathplanner/paths/LStart-K.path | 54 ++++++ src/main/deploy/pathplanner/paths/RCS-C.path | 104 ++++++++++++ src/main/deploy/pathplanner/paths/RCS-D.path | 104 ++++++++++++ .../deploy/pathplanner/paths/RStart-D.path | 54 ++++++ .../deploy/pathplanner/paths/RStart-E.path | 85 ++++++++++ .../deploy/pathplanner/paths/RStart-F.path | 54 ++++++ .../deploy/pathplanner/paths/RStart-G.path | 54 ++++++ .../paths/Red C_L4-SourceRight.path | 4 +- .../paths/Red SourceRight-C_L4.path | 4 +- .../paths/Red StartRight-SourceRight .path | 4 +- .../deploy/pathplanner/paths/pushEnd-H.path | 66 ++++++++ ...urce to ab.path => pushStart-pushEnd.path} | 28 ++-- src/main/deploy/pathplanner/settings.json | 29 +++- src/main/java/frc/robot/Auto.java | 51 +++--- src/main/java/frc/robot/RobotContainer.java | 19 ++- .../java/frc/robot/commands/AlignToReef.java | 4 + .../robot/constants/AligningConstants.java | 23 ++- .../SuperstructureStateManager.java | 20 +-- .../robot/subsystems/arm/ArmPivotIOSim.java | 2 +- 42 files changed, 1928 insertions(+), 212 deletions(-) rename src/main/deploy/pathplanner/autos/{test matthew yay.auto => Left3.auto} (56%) rename src/main/deploy/pathplanner/autos/{Test Half Field.auto => PushAuto.auto} (53%) create mode 100644 src/main/deploy/pathplanner/autos/Right3.auto delete mode 100644 src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto delete mode 100644 src/main/deploy/pathplanner/autos/Test Choreo Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/Test-Auto Auto.auto rename src/main/deploy/pathplanner/autos/{Test 1M.auto => leaveAuto.auto} (86%) create mode 100644 src/main/deploy/pathplanner/autos/pleasework.auto create mode 100644 src/main/deploy/pathplanner/paths/B-RCS.path create mode 100644 src/main/deploy/pathplanner/paths/C-RCS.path create mode 100644 src/main/deploy/pathplanner/paths/D-RCS.path create mode 100644 src/main/deploy/pathplanner/paths/E-RCS.path create mode 100644 src/main/deploy/pathplanner/paths/F-RCS.path create mode 100644 src/main/deploy/pathplanner/paths/J-LCS.path create mode 100644 src/main/deploy/pathplanner/paths/K-LCS.path create mode 100644 src/main/deploy/pathplanner/paths/L-LCS.path create mode 100644 src/main/deploy/pathplanner/paths/LCS-A.path rename src/main/deploy/pathplanner/paths/{Half-Field Sprint.path => LCS-B.path} (64%) create mode 100644 src/main/deploy/pathplanner/paths/LCS-K.path create mode 100644 src/main/deploy/pathplanner/paths/LCS-L.path rename src/main/deploy/pathplanner/paths/{1M.path => LCorner-Leave.path} (65%) rename src/main/deploy/pathplanner/paths/{ab to bc.path => LStart-I.path} (62%) create mode 100644 src/main/deploy/pathplanner/paths/LStart-J.path create mode 100644 src/main/deploy/pathplanner/paths/LStart-K.path create mode 100644 src/main/deploy/pathplanner/paths/RCS-C.path create mode 100644 src/main/deploy/pathplanner/paths/RCS-D.path create mode 100644 src/main/deploy/pathplanner/paths/RStart-D.path create mode 100644 src/main/deploy/pathplanner/paths/RStart-E.path create mode 100644 src/main/deploy/pathplanner/paths/RStart-F.path create mode 100644 src/main/deploy/pathplanner/paths/RStart-G.path create mode 100644 src/main/deploy/pathplanner/paths/pushEnd-H.path rename src/main/deploy/pathplanner/paths/{source to ab.path => pushStart-pushEnd.path} (62%) diff --git a/src/main/deploy/pathplanner/autos/test matthew yay.auto b/src/main/deploy/pathplanner/autos/Left3.auto similarity index 56% rename from src/main/deploy/pathplanner/autos/test matthew yay.auto rename to src/main/deploy/pathplanner/autos/Left3.auto index 69b78923..f29af43e 100644 --- a/src/main/deploy/pathplanner/autos/test matthew yay.auto +++ b/src/main/deploy/pathplanner/autos/Left3.auto @@ -4,16 +4,40 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "height L4" + } + }, { "type": "path", "data": { - "pathName": "source to ab" + "pathName": "LStart-J" } }, { "type": "named", "data": { - "name": "location B" + "name": "place" + } + }, + { + "type": "path", + "data": { + "pathName": "J-LCS" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "LCS-K" } }, { @@ -31,19 +55,25 @@ { "type": "path", "data": { - "pathName": "ab to bc" + "pathName": "K-LCS" } }, { - "type": "named", + "type": "wait", "data": { - "name": "location C" + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "LCS-L" } }, { "type": "named", "data": { - "name": "height L2" + "name": "height L4" } }, { diff --git a/src/main/deploy/pathplanner/autos/Test Half Field.auto b/src/main/deploy/pathplanner/autos/PushAuto.auto similarity index 53% rename from src/main/deploy/pathplanner/autos/Test Half Field.auto rename to src/main/deploy/pathplanner/autos/PushAuto.auto index 5581ca3c..9eeb52e2 100644 --- a/src/main/deploy/pathplanner/autos/Test Half Field.auto +++ b/src/main/deploy/pathplanner/autos/PushAuto.auto @@ -7,13 +7,25 @@ { "type": "path", "data": { - "pathName": "Half-Field Sprint" + "pathName": "pushStart-pushEnd" + } + }, + { + "type": "path", + "data": { + "pathName": "pushEnd-H" + } + }, + { + "type": "named", + "data": { + "name": "height L1" } }, { "type": "named", "data": { - "name": "height L4" + "name": "score" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right3.auto b/src/main/deploy/pathplanner/autos/Right3.auto new file mode 100644 index 00000000..b578fb3d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right3.auto @@ -0,0 +1,154 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height L3" + } + }, + { + "type": "path", + "data": { + "pathName": "RStart-E" + } + }, + { + "type": "named", + "data": { + "name": "height L3" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "wait prep" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "place" + } + }, + { + "type": "path", + "data": { + "pathName": "E-RCS" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "RCS-D" + } + }, + { + "type": "named", + "data": { + "name": "height L3" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "wait prep" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "place" + } + }, + { + "type": "path", + "data": { + "pathName": "D-RCS" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "RCS-C" + } + }, + { + "type": "named", + "data": { + "name": "height L3" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "wait prep" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "place" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto b/src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto deleted file mode 100644 index ec5f6f58..00000000 --- a/src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Spinny Path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Choreo Auto.auto b/src/main/deploy/pathplanner/autos/Test Choreo Auto.auto deleted file mode 100644 index 7845498b..00000000 --- a/src/main/deploy/pathplanner/autos/Test Choreo Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "QuarterSprint" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test-Auto Auto.auto b/src/main/deploy/pathplanner/autos/Test-Auto Auto.auto deleted file mode 100644 index 70025bc0..00000000 --- a/src/main/deploy/pathplanner/autos/Test-Auto Auto.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Red StartRight-SourceRight " - } - }, - { - "type": "path", - "data": { - "pathName": "Red SourceRight-C_L4" - } - }, - { - "type": "named", - "data": { - "name": "wait" - } - }, - { - "type": "path", - "data": { - "pathName": "Red C_L4-SourceRight" - } - }, - { - "type": "named", - "data": { - "name": "wait" - } - }, - { - "type": "path", - "data": { - "pathName": "Red SourceRight-C_L4" - } - }, - { - "type": "named", - "data": { - "name": "wait" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test 1M.auto b/src/main/deploy/pathplanner/autos/leaveAuto.auto similarity index 86% rename from src/main/deploy/pathplanner/autos/Test 1M.auto rename to src/main/deploy/pathplanner/autos/leaveAuto.auto index ae5c9873..fb595255 100644 --- a/src/main/deploy/pathplanner/autos/Test 1M.auto +++ b/src/main/deploy/pathplanner/autos/leaveAuto.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "1M" + "pathName": "LCorner-Leave" } } ] diff --git a/src/main/deploy/pathplanner/autos/pleasework.auto b/src/main/deploy/pathplanner/autos/pleasework.auto new file mode 100644 index 00000000..8a373503 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/pleasework.auto @@ -0,0 +1,26 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RStart-E" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-RCS.path b/src/main/deploy/pathplanner/paths/B-RCS.path new file mode 100644 index 00000000..c7923340 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B-RCS.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.1809375, + "y": 3.8754261363636364 + }, + "prevControl": null, + "nextControl": { + "x": 2.8119886363636364, + "y": 3.6759943181818175 + }, + "isLocked": false, + "linkedName": "B" + }, + { + "anchor": { + "x": 1.1148565573770493, + "y": 0.7523565573770474 + }, + "prevControl": { + "x": 1.1196332646514404, + "y": 1.002310919346617 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "RCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 48.5 + }, + "reversed": false, + "folder": "Reef-RCS", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C-RCS.path b/src/main/deploy/pathplanner/paths/C-RCS.path new file mode 100644 index 00000000..d8724600 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C-RCS.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5598579545454543, + "y": 2.71872159090909 + }, + "prevControl": null, + "nextControl": { + "x": 3.217040663308801, + "y": 2.362630667264994 + }, + "isLocked": false, + "linkedName": "C" + }, + { + "anchor": { + "x": 1.1148565573770493, + "y": 0.7523565573770474 + }, + "prevControl": { + "x": 2.5507656482861405, + "y": 1.4703111028315923 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "RCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "height Handoff", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height Handoff" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 48.5 + }, + "reversed": false, + "folder": "Reef-RCS", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D-RCS.path b/src/main/deploy/pathplanner/paths/D-RCS.path new file mode 100644 index 00000000..400715d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D-RCS.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8290909090909087, + "y": 2.489375 + }, + "prevControl": null, + "nextControl": { + "x": 3.4003125000000005, + "y": 2.050625000000001 + }, + "isLocked": false, + "linkedName": "D" + }, + { + "anchor": { + "x": 1.1148565573770493, + "y": 0.7523565573770474 + }, + "prevControl": { + "x": 1.802896330104322, + "y": 1.2210213301043198 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "RCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "height Handoff", + "waypointRelativePos": 0.19730077120822576, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height Handoff" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 48.5 + }, + "reversed": false, + "folder": "Reef-RCS", + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E-RCS.path b/src/main/deploy/pathplanner/paths/E-RCS.path new file mode 100644 index 00000000..01775292 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E-RCS.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.16528409090909, + "y": 2.4794034090909087 + }, + "prevControl": null, + "nextControl": { + "x": 4.739794753511753, + "y": 2.206545069670984 + }, + "isLocked": false, + "linkedName": "E" + }, + { + "anchor": { + "x": 1.1148565573770493, + "y": 0.7523565573770474 + }, + "prevControl": { + "x": 0.7723599240160615, + "y": 0.6067997862542978 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "RCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "height Handoff", + "waypointRelativePos": 0.19987146529561958, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height Handoff" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 48.5 + }, + "reversed": false, + "folder": "Reef-RCS", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F-RCS.path b/src/main/deploy/pathplanner/paths/F-RCS.path new file mode 100644 index 00000000..9b163340 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F-RCS.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.434517045454545, + "y": 2.6987784090909086 + }, + "prevControl": null, + "nextControl": { + "x": 6.222272727272727, + "y": 2.2301136363636367 + }, + "isLocked": false, + "linkedName": "F" + }, + { + "anchor": { + "x": 1.1148565573770493, + "y": 0.7523565573770474 + }, + "prevControl": { + "x": 2.002328148286141, + "y": 0.89195883010432 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "RCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "height Handoff", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height Handoff" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 48.5 + }, + "reversed": false, + "folder": "Reef-RCS", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/J-LCS.path b/src/main/deploy/pathplanner/paths/J-LCS.path new file mode 100644 index 00000000..36ce2e6f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/J-LCS.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.175255681818181, + "y": 5.620454545454545 + }, + "prevControl": null, + "nextControl": { + "x": 5.055596590909091, + "y": 6.388267045454544 + }, + "isLocked": false, + "linkedName": "J" + }, + { + "anchor": { + "x": 1.545596590909091, + "y": 6.836988636363635 + }, + "prevControl": { + "x": 1.3073758686858423, + "y": 6.912823242602699 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "height Handoff", + "waypointRelativePos": 0.2, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height Handoff" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.0 + }, + "reversed": false, + "folder": "Reef-LCS", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/K-LCS.path b/src/main/deploy/pathplanner/paths/K-LCS.path new file mode 100644 index 00000000..90638ede --- /dev/null +++ b/src/main/deploy/pathplanner/paths/K-LCS.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.789204545454545, + "y": 5.6703125 + }, + "prevControl": null, + "nextControl": { + "x": 4.127313841897648, + "y": 5.514901492390936 + }, + "isLocked": false, + "linkedName": "K" + }, + { + "anchor": { + "x": 1.545596590909091, + "y": 6.836988636363635 + }, + "prevControl": { + "x": 1.3324851977442531, + "y": 6.967692630577056 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "height Handoff", + "waypointRelativePos": 0.2, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height Handoff" + } + }, + { + "type": "named", + "data": { + "name": "armprep" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.0 + }, + "reversed": false, + "folder": "Reef-LCS", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L-LCS.path b/src/main/deploy/pathplanner/paths/L-LCS.path new file mode 100644 index 00000000..38a137ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L-LCS.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4701136363636365, + "y": 5.421022727272727 + }, + "prevControl": null, + "nextControl": { + "x": 3.7792947020883836, + "y": 5.32028677002405 + }, + "isLocked": false, + "linkedName": "L" + }, + { + "anchor": { + "x": 1.545596590909091, + "y": 6.836988636363635 + }, + "prevControl": { + "x": 1.3421898950574207, + "y": 6.982845441435263 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "height Handoff", + "waypointRelativePos": 0.2002249718785083, + "endWaypointRelativePos": null, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height Handoff" + } + }, + { + "type": "named", + "data": { + "name": "armprep" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.0 + }, + "reversed": false, + "folder": "Reef-LCS", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS-A.path b/src/main/deploy/pathplanner/paths/LCS-A.path new file mode 100644 index 00000000..f28f688d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS-A.path @@ -0,0 +1,105 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.545596590909091, + "y": 6.836988636363635 + }, + "prevControl": null, + "nextControl": { + "x": 1.6716760756489928, + "y": 6.52798671224065 + }, + "isLocked": false, + "linkedName": "LCS" + }, + { + "anchor": { + "x": 3.200880681818182, + "y": 4.194517045454545 + }, + "prevControl": { + "x": 2.233636363636364, + "y": 4.104772727272727 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "A" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "arm" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "height L4" + } + }, + { + "type": "named", + "data": { + "name": "armprep" + } + } + ] + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "LCS-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -55.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Half-Field Sprint.path b/src/main/deploy/pathplanner/paths/LCS-B.path similarity index 64% rename from src/main/deploy/pathplanner/paths/Half-Field Sprint.path rename to src/main/deploy/pathplanner/paths/LCS-B.path index 234946c2..1c00bc5a 100644 --- a/src/main/deploy/pathplanner/paths/Half-Field Sprint.path +++ b/src/main/deploy/pathplanner/paths/LCS-B.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 0.3722130667010144, - "y": 5.573373042480681 + "x": 1.545596590909091, + "y": 6.836988636363635 }, "prevControl": null, "nextControl": { - "x": 1.3722130667010144, - "y": 5.573373042480681 + "x": 1.7448868011642042, + "y": 6.534880919615796 }, "isLocked": false, - "linkedName": null + "linkedName": "LCS" }, { "anchor": { - "x": 8.28970984956188, - "y": 5.573373042480681 + "x": 3.1809375, + "y": 3.8754261363636364 }, "prevControl": { - "x": 7.289709849561881, - "y": 5.573373042480681 + "x": 3.001448863636364, + "y": 4.503636363636364 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "B" } ], "rotationTargets": [], @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 5.2, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": null, + "folder": "LCS-Reef", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS-K.path b/src/main/deploy/pathplanner/paths/LCS-K.path new file mode 100644 index 00000000..60b02ec2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS-K.path @@ -0,0 +1,92 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.545596590909091, + "y": 6.836988636363635 + }, + "prevControl": null, + "nextControl": { + "x": 2.3033072612205023, + "y": 6.3605093487385345 + }, + "isLocked": false, + "linkedName": "LCS" + }, + { + "anchor": { + "x": 3.789204545454545, + "y": 5.6703125 + }, + "prevControl": { + "x": 3.0411148746385517, + "y": 6.160049264888443 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "K" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake & prep", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "arm" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "location K" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "LCS-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -55.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS-L.path b/src/main/deploy/pathplanner/paths/LCS-L.path new file mode 100644 index 00000000..37666cd4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS-L.path @@ -0,0 +1,92 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.545596590909091, + "y": 6.836988636363635 + }, + "prevControl": null, + "nextControl": { + "x": 2.3298048273372616, + "y": 6.255424452987281 + }, + "isLocked": false, + "linkedName": "LCS" + }, + { + "anchor": { + "x": 3.4701136363636365, + "y": 5.421022727272727 + }, + "prevControl": { + "x": 2.9925063176600646, + "y": 5.76603633822357 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "L" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake & prep", + "waypointRelativePos": 0.0809898762654653, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "arm" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "location L" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "LCS-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": -55.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1M.path b/src/main/deploy/pathplanner/paths/LCorner-Leave.path similarity index 65% rename from src/main/deploy/pathplanner/paths/1M.path rename to src/main/deploy/pathplanner/paths/LCorner-Leave.path index 04db582d..e6dbe8c8 100644 --- a/src/main/deploy/pathplanner/paths/1M.path +++ b/src/main/deploy/pathplanner/paths/LCorner-Leave.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.0, - "y": 0.0 + "x": 7.139659090909091, + "y": 7.5250284090909085 }, "prevControl": null, "nextControl": { - "x": 0.9673213526060513, - "y": 0.0 + "x": 6.890918582322183, + "y": 7.499965300985049 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.0, - "y": 0.0 + "x": 5.364715909090909, + "y": 7.405369318181817 }, "prevControl": { - "x": -0.27527965608765825, - "y": 1.561767148845488e-16 + "x": 6.092138107374886, + "y": 7.464308243535791 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 5.2, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -178.18169703554804 }, "reversed": false, - "folder": null, + "folder": "misc", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ab to bc.path b/src/main/deploy/pathplanner/paths/LStart-I.path similarity index 62% rename from src/main/deploy/pathplanner/paths/ab to bc.path rename to src/main/deploy/pathplanner/paths/LStart-I.path index 5394e284..dcf91bce 100644 --- a/src/main/deploy/pathplanner/paths/ab to bc.path +++ b/src/main/deploy/pathplanner/paths/LStart-I.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.2307954545454542, - "y": 4.000071022727272 + "x": 7.129687499999999, + "y": 5.421022727272727 }, "prevControl": null, "nextControl": { - "x": 2.3532954545454547, - "y": 3.3419460227272717 + "x": 5.997936231484273, + "y": 5.382326228371569 }, "isLocked": false, - "linkedName": null + "linkedName": "LStart" }, { "anchor": { - "x": 3.6795170454545456, - "y": 2.663877840909091 + "x": 5.474403409090908, + "y": 5.331278409090909 }, "prevControl": { - "x": 2.6795170454545456, - "y": 2.663877840909091 + "x": 5.773670166441749, + "y": 5.299178128626464 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 5.2, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.06848815949221 + "rotation": -119.99999999999999 }, "reversed": false, - "folder": null, + "folder": "LStart-Reef", "idealStartingState": { "velocity": 0, - "rotation": -0.9710219310791925 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LStart-J.path b/src/main/deploy/pathplanner/paths/LStart-J.path new file mode 100644 index 00000000..49c8b9e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LStart-J.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.129687499999999, + "y": 5.421022727272727 + }, + "prevControl": null, + "nextControl": { + "x": 6.0627272727272725, + "y": 5.8398295454545455 + }, + "isLocked": false, + "linkedName": "LStart" + }, + { + "anchor": { + "x": 5.175255681818181, + "y": 5.620454545454545 + }, + "prevControl": { + "x": 5.574119318181818, + "y": 5.650369318181817 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "J" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "location J", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "location J" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "LStart-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LStart-K.path b/src/main/deploy/pathplanner/paths/LStart-K.path new file mode 100644 index 00000000..74f2566c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LStart-K.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.129687499999999, + "y": 5.421022727272727 + }, + "prevControl": null, + "nextControl": { + "x": 5.294914772727273, + "y": 5.819886363636364 + }, + "isLocked": false, + "linkedName": "LStart" + }, + { + "anchor": { + "x": 3.789204545454545, + "y": 5.6703125 + }, + "prevControl": { + "x": 4.527102272727273, + "y": 5.809914772727272 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "K" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "LStart-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RCS-C.path b/src/main/deploy/pathplanner/paths/RCS-C.path new file mode 100644 index 00000000..5575022e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RCS-C.path @@ -0,0 +1,104 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1148565573770493, + "y": 0.7523565573770474 + }, + "prevControl": null, + "nextControl": { + "x": 1.4706650219232016, + "y": 1.1011238331307638 + }, + "isLocked": false, + "linkedName": "RCS" + }, + { + "anchor": { + "x": 3.5598579545454543, + "y": 2.71872159090909 + }, + "prevControl": { + "x": 3.2736459589095848, + "y": 2.4679886587730784 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "C" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "location C" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "arm" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "height L3" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "RCS-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 48.5 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RCS-D.path b/src/main/deploy/pathplanner/paths/RCS-D.path new file mode 100644 index 00000000..c68575b6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RCS-D.path @@ -0,0 +1,104 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1148565573770493, + "y": 0.7523565573770474 + }, + "prevControl": null, + "nextControl": { + "x": 1.904938524590164, + "y": 1.4519262295081958 + }, + "isLocked": false, + "linkedName": "RCS" + }, + { + "anchor": { + "x": 3.8290909090909087, + "y": 2.489375 + }, + "prevControl": { + "x": 3.24016668535489, + "y": 2.0557231083684036 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "arm" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "location D" + } + }, + { + "type": "named", + "data": { + "name": "height L3" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "RCS-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 48.5 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RStart-D.path b/src/main/deploy/pathplanner/paths/RStart-D.path new file mode 100644 index 00000000..515a27a8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RStart-D.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.139659090909091, + "y": 0.5149999999999989 + }, + "prevControl": null, + "nextControl": { + "x": 7.7007353642089456, + "y": 0.24454137518329144 + }, + "isLocked": false, + "linkedName": "RStart" + }, + { + "anchor": { + "x": 3.8290909090909087, + "y": 2.489375 + }, + "prevControl": { + "x": 3.898892045454545, + "y": 1.5420738636363636 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "RStart-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RStart-E.path b/src/main/deploy/pathplanner/paths/RStart-E.path new file mode 100644 index 00000000..be7ef6f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RStart-E.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.139659090909091, + "y": 0.5149999999999989 + }, + "prevControl": null, + "nextControl": { + "x": 6.022840909090909, + "y": 1.3226988636363626 + }, + "isLocked": false, + "linkedName": "RStart" + }, + { + "anchor": { + "x": 5.16528409090909, + "y": 2.4794034090909087 + }, + "prevControl": { + "x": 5.584090909090909, + "y": 2.0905113636363626 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "E" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "location E", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "location E" + } + }, + { + "type": "named", + "data": { + "name": "height L4" + } + }, + { + "type": "named", + "data": { + "name": "preparm" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "RStart-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RStart-F.path b/src/main/deploy/pathplanner/paths/RStart-F.path new file mode 100644 index 00000000..0a9fcc7e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RStart-F.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.139659090909091, + "y": 0.5149999999999989 + }, + "prevControl": null, + "nextControl": { + "x": 6.780681818181818, + "y": 0.9437784090909088 + }, + "isLocked": false, + "linkedName": "RStart" + }, + { + "anchor": { + "x": 5.434517045454545, + "y": 2.6987784090909086 + }, + "prevControl": { + "x": 5.207455432735299, + "y": 2.895571103265361 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "RStart-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RStart-G.path b/src/main/deploy/pathplanner/paths/RStart-G.path new file mode 100644 index 00000000..a48401fa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RStart-G.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.139659090909091, + "y": 0.5149999999999989 + }, + "prevControl": null, + "nextControl": { + "x": 7.384811125136247, + "y": 0.4660053052434517 + }, + "isLocked": false, + "linkedName": "RStart" + }, + { + "anchor": { + "x": 5.8134375, + "y": 3.8754261363636364 + }, + "prevControl": { + "x": 6.4615909090909085, + "y": 3.6959375 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "G" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "RStart-Reef", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path b/src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path index 50254b82..43a90d9e 100644 --- a/src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path +++ b/src/main/deploy/pathplanner/paths/Red C_L4-SourceRight.path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 5.2, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path b/src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path index 928b695d..facdf332 100644 --- a/src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path +++ b/src/main/deploy/pathplanner/paths/Red SourceRight-C_L4.path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 5.2, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path b/src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path index 6ed916fe..73fbeb29 100644 --- a/src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path +++ b/src/main/deploy/pathplanner/paths/Red StartRight-SourceRight .path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 5.2, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/pushEnd-H.path b/src/main/deploy/pathplanner/paths/pushEnd-H.path new file mode 100644 index 00000000..678d66ed --- /dev/null +++ b/src/main/deploy/pathplanner/paths/pushEnd-H.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.438806818186804, + "y": 5.560624999995014 + }, + "prevControl": null, + "nextControl": { + "x": 7.853252089318251, + "y": 5.824631312690614 + }, + "isLocked": false, + "linkedName": "pushEnd" + }, + { + "anchor": { + "x": 6.092642045454545, + "y": 4.2244318181818175 + }, + "prevControl": { + "x": 6.096356070139283, + "y": 4.474404228700741 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "H" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "location H", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "location H" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.2, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "misc", + "idealStartingState": { + "velocity": 0, + "rotation": 111.0375110254219 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/source to ab.path b/src/main/deploy/pathplanner/paths/pushStart-pushEnd.path similarity index 62% rename from src/main/deploy/pathplanner/paths/source to ab.path rename to src/main/deploy/pathplanner/paths/pushStart-pushEnd.path index bcc9fa63..9cc054bf 100644 --- a/src/main/deploy/pathplanner/paths/source to ab.path +++ b/src/main/deploy/pathplanner/paths/pushStart-pushEnd.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.0270738636363637, - "y": 1.0484801136363637 + "x": 7.977272727277714, + "y": 3.0477840909041043 }, "prevControl": null, "nextControl": { - "x": 1.3760795454545454, - "y": 2.3447869318181827 + "x": 7.749531234789036, + "y": 4.145238876648082 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.8319318181818183, - "y": 3.940241477272727 + "x": 7.438806818186804, + "y": 5.560624999995014 }, "prevControl": { - "x": 1.8319318181818183, - "y": 3.940241477272727 + "x": 7.380103573979456, + "y": 5.818645192686005 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "pushEnd" } ], "rotationTargets": [], @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 3.0, + "maxVelocity": 5.2, + "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.4320961841646833 + "rotation": 111.0375110254219 }, "reversed": false, - "folder": null, + "folder": "misc", "idealStartingState": { "velocity": 0, - "rotation": 55.09241514460653 + "rotation": 108.18904569965132 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index c52b9e29..0b2702bb 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,22 +3,29 @@ "robotLength": 0.914, "holonomicMode": true, "pathFolders": [ - "AutoTest" + "AutoTest", + "LCS-Reef", + "LStart-Reef", + "misc", + "RCS-Reef", + "RStart-Reef", + "Reef-LCS", + "Reef-RCS" ], "autoFolders": [], - "defaultMaxVel": 4.5, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 5.2, + "defaultMaxAccel": 8.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 68.0389, - "robotMOI": 11.61, + "robotMass": 64.0, + "robotMOI": 11.9, "robotTrackwidth": 0.508, "driveWheelRadius": 0.0508, - "driveGearing": 5.142857, - "maxDriveSpeed": 6.21, + "driveGearing": 5.143, + "maxDriveSpeed": 5.75, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 85.0, + "driveCurrentLimit": 102.0, "wheelCOF": 1.0, "flModuleX": 0.2921, "flModuleY": 0.292, @@ -30,5 +37,9 @@ "brModuleY": -0.2921, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [] + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.3,\"y\":0.0},\"size\":{\"width\":0.2,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.3,\"y\":0.0},\"size\":{\"width\":0.4,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.0,\"y\":-0.3},\"size\":{\"width\":0.2,\"length\":0.05},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index 215f72a2..3cddde2f 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -233,7 +233,8 @@ public enum ArmHeight { L2(Position.L2, Position.L2Prep), L3(Position.L3, Position.L3Prep), L4(Position.L4, Position.L4Prep), - Source(Position.Source, Position.Source); + Source(Position.Source, Position.Source), + Handoff(Position.Handoff, Position.HandoffPrep); public Position position; public double armMotorSpeed; @@ -252,6 +253,12 @@ public void configureBindings() { NamedCommands.registerCommand( "wait", Commands.waitUntil(() -> armInPlace() && robotInPlace())); + NamedCommands.registerCommand("wait arm", Commands.waitUntil(() -> armInPlace())); + + NamedCommands.registerCommand("wait prep", Commands.waitUntil(() -> armInPrep())); + + NamedCommands.registerCommand("wait pose", Commands.waitUntil(() -> robotInPlace())); + Command alignCommand = Commands.defer( () -> @@ -284,22 +291,40 @@ public void configureBindings() { robotContainer.gripperSubsystem.ejectSpinCoral().withTimeout(placeTimeout); NamedCommands.registerCommand("score", scoreCommand.asProxy()); - Command intakeCommand = robotContainer.intakeSubsystem.intake(); + Command intakeCommand = + robotContainer + .gripperSubsystem + .intakeSpinCoral() + .withDeadline( + Commands.waitUntil(() -> robotContainer.gripperSubsystem.hasPiece()) + .andThen(Commands.waitSeconds(0.75)) + .withTimeout(5)); // TODO: TIMEOUT NamedCommands.registerCommand("intake", intakeCommand.asProxy()); + NamedCommands.registerCommand( + "placenoalign", + Commands.race( + armCommand.asProxy(), + Commands.sequence( + Commands.waitUntil(() -> armInPlace()), scoreCommand.asProxy()))); + // spotless:off Command placeCommand = prepArmCommand.asProxy() .until(() -> robotInPlace()) // Wait until arm and align are in position .andThen( - ( - Commands.waitUntil(() -> armInPlace()) + Commands.waitSeconds(1).andThen(( + Commands.waitSeconds(0.5).andThen(Commands.waitUntil(() -> armInPlace())) .andThen(scoreCommand.asProxy()) ) - .deadlineFor(armCommand.asProxy()) + .deadlineFor(armCommand.asProxy())) ) .deadlineFor(alignCommand); + + NamedCommands.registerCommand("place", placeCommand); + + // spotless:on // #region Auto create locations and heights @@ -347,7 +372,8 @@ private boolean robotInPlace() { .plus( new Transform2d( new Translation2d( - Units.feetToMeters(3) / 2, targetLocation.offset), + AligningConstants.reefDistance, + targetLocation.offset), Rotation2d.k180deg)); Logger.recordOutput("Auto/Physical Target Pose", alignmentPose); Pose2d currentPose = robotContainer.drivetrain.getRobotPose(); @@ -358,17 +384,4 @@ private boolean robotInPlace() { && ((Math.abs(relativePos.getRotation().getRadians()) % Math.PI) < Units.degreesToRadians(2)); } - - public static boolean robotInPlace(Pose2d robotPose, Pose2d pose, double offset) { - Pose2d alignmentPose = - pose.plus( - new Transform2d( - new Translation2d(Units.feetToMeters(3) / 2, offset), - Rotation2d.k180deg)); - Pose2d offsetPose = robotPose.relativeTo(alignmentPose); - return (offsetPose.getX() > -Units.inchesToMeters(0.7)) - && (Math.abs(offsetPose.getY()) < Units.inchesToMeters(0.7)) - && ((Math.abs(offsetPose.getRotation().getRadians()) % (2 * Math.PI)) - < Units.degreesToRadians(2)); - } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 901aeeb6..2f37b360 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -108,9 +107,6 @@ public RobotContainer() { vision = new Vision( drivetrain::addVisionMeasurement, - // new DummyPhotonCamera(), - // new DummyPhotonCamera(), - // new DummyPhotonCamera()); new VisionIOLimelight( VisionConstants.camera0Name, () -> drivetrain.getRobotPose().getRotation()), @@ -226,7 +222,7 @@ private void configureBindings() { AUTO_ALIGNED = new Trigger( () -> { - return Auto.robotInPlace( + return AligningConstants.robotInPlace( drivetrain.getRobotPose(), stateManager.getLastScoringPose(), stateManager.getLastScoringOffset()); @@ -476,7 +472,12 @@ private void configureBindings() { ALGAE.and(operatorController.getDPadRight()) .onTrue(stateManager.moveToPosition(Position.Quick23)); - // operatorController.getBack().onTrue(wristSubsystem.flipWristPosition()); + operatorController + .getBack() + .whileTrue( + stateManager + .moveToPosition(Position.HandoffSus) + .alongWith(gripperSubsystem.intakeSpinCoral())); // Driver Align Bindings, for a different/later day // CORAL.and(leftDriveController.getTrigger()).whileTrue(alignToReef(9, 0)); @@ -674,7 +675,7 @@ public Command alignToReef(int tag, double offset, Rotation2d rotOffset) { .toPose2d() .plus( new Transform2d( - new Translation2d(Units.feetToMeters(3) / 2, offset), + new Translation2d(AligningConstants.reefDistance, offset), rotOffset)); return new AlignToReef( drivetrain, @@ -699,7 +700,7 @@ public Command alignToReef(double offset) { .plus( new Transform2d( new Translation2d( - Units.feetToMeters(3) / 2, + AligningConstants.reefDistance, offset), new Rotation2d())); // return new AlignAndDriveToReef(drivetrain, 0, alignmentPose, @@ -724,7 +725,7 @@ public Command alignAndDriveToReef(int tag, double offset) { .toPose2d() .plus( new Transform2d( - new Translation2d(Units.feetToMeters(3) / 2, offset), + new Translation2d(AligningConstants.reefDistance, offset), new Rotation2d())); return new AlignAndDriveToReef(drivetrain, 0, alignmentPose, Rotation2d.kPi); } diff --git a/src/main/java/frc/robot/commands/AlignToReef.java b/src/main/java/frc/robot/commands/AlignToReef.java index 1febed5f..7c584739 100644 --- a/src/main/java/frc/robot/commands/AlignToReef.java +++ b/src/main/java/frc/robot/commands/AlignToReef.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; import java.util.function.DoubleSupplier; @@ -23,6 +24,8 @@ public class AlignToReef extends Command { private double offset; private Rotation2d rotationOffset; + private Timer slewerTimer = new Timer(); + public AlignToReef( CommandSwerveDrivetrain drivetrain, DoubleSupplier xVelocity, @@ -44,6 +47,7 @@ public void initialize() { // Camera id // tagId // Rotation to face the tag + slewerTimer.restart(); Logger.recordOutput("/AutoAlign/Pose", targetPose); Logger.recordOutput("/AutoAlign/Offset", offset); diff --git a/src/main/java/frc/robot/constants/AligningConstants.java b/src/main/java/frc/robot/constants/AligningConstants.java index bcb496c2..f46ac035 100644 --- a/src/main/java/frc/robot/constants/AligningConstants.java +++ b/src/main/java/frc/robot/constants/AligningConstants.java @@ -1,9 +1,12 @@ package frc.robot.constants; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; public class AligningConstants { - public static final double aligningDeadband = Units.degreesToRadians(3); public static final double Kp = 0.9; public static final double Ki = 0.0; public static final double Kd = 0.0; @@ -11,4 +14,22 @@ public class AligningConstants { public static final double leftOffset = -0.18 + 0.08; public static final double rightOffset = 0.2 + 0.03; public static final double centerOffset = 0 + 0.05; + public static final double reefDistance = Units.feetToMeters(3) / 2; + + public static final double aligningAngleTolerance = Units.degreesToRadians(2); + public static final double aligningXTolerance = Units.inchesToMeters(0.7); + public static final double aligningYTolerance = Units.inchesToMeters(0.7); + + public static boolean robotInPlace(Pose2d robotPose, Pose2d targetPose, double offset) { + Pose2d alignmentPose = + targetPose.plus( + new Transform2d( + new Translation2d(AligningConstants.reefDistance, offset), + Rotation2d.k180deg)); + Pose2d offsetPose = robotPose.relativeTo(alignmentPose); + return (offsetPose.getX() > -aligningXTolerance) + && (Math.abs(offsetPose.getY()) < aligningYTolerance) + && ((Math.abs(offsetPose.getRotation().getRadians()) % (2 * Math.PI)) + < aligningAngleTolerance); + } } diff --git a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java index 59c529bf..db4163cd 100644 --- a/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java +++ b/src/main/java/frc/robot/subsystems/ModeManager/SuperstructureStateManager.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.ModeManager; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; @@ -130,7 +129,8 @@ public static enum Position { (a, s, e) -> !s.chuteSubsystem.DOWN.getAsBoolean()), ChuteDownNull(0, 0, 0, ChuteDown, TRUE, FALSE), HandoffPrep(165, -0.45, 1.58, ChuteDownNull), - Handoff(130, -0.45, 1.58, HandoffPrep), + HandoffSus(160, -0.55, 0.0, HandoffPrep), + Handoff(133, -0.45, 1.58, HandoffPrep), PointUpPrep( 170, 0.5, @@ -190,9 +190,9 @@ public static enum Position { Quick34(250, 0.7, 0, UpZoneNull), Quick23(150, 0.7, 0, UpZoneNull), // L2AlgaePrep(297, 2.15, -1.58, UpZoneNull), - L2Algae(170, 1.2, -1.58, UpZoneNull), + L2Algae(90, 2.2, -1.58, UpZoneNull), // L3AlgaePrep(297, 2.15, -1.58, UpZoneNull), - L3Algae(250, 1.2, -1.58, UpZoneNull), + L3Algae(170, 2.2, -1.58, UpZoneNull), // NetAlgaePrep(297, 2.15, -1.58, UpZoneNull), // NetAlgae(297, 2.15, -1.58, NetAlgaePrep), Source(130, 2.15, 0, PointUp), @@ -205,8 +205,8 @@ public static enum Position { // IcecreamAlgae(170, 0, 1.58, AlgaeHome), GroundAlgaePrep(65, 1.58, 1.58, AlgaeHomeNull), GroundAlgae(20, 1.58, 1.58, GroundAlgaePrep), - StartPrepPrepPrep(170, 2.15, 1.58, ChuteUpNull), - StartPrepPrep(140, 0, 1.58, StartPrepPrepPrep), + StartPrepPrepPrep(170, 0.65, -1.58, ChuteUpNull), + StartPrepPrep(140, 0, -1.58, StartPrepPrepPrep), StartPrep(140, 0, -1.58, StartPrepPrep), Start(0, 0, -1.58, StartPrep), Climb(10, 0, -1.58, StartPrep), @@ -659,10 +659,10 @@ public SuperstructureStateManager( SmartDashboard.putData("Mech2d", mech); Command defaultcom = - Commands.either( - Commands.idle(), - moveToPosition(Position.Home).asProxy(), - () -> !RobotState.isAutonomous()); + // Commands.either( + Commands.idle(); + // moveToPosition(Position.Home).asProxy(), + // () -> !RobotState.isAutonomous()); defaultcom.addRequirements(this); setDefaultCommand(defaultcom); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java index 61387f99..f9944076 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmPivotIOSim.java @@ -5,7 +5,7 @@ public class ArmPivotIOSim implements ArmPivotIO { private double voltage = 0; public void updateInputs(ArmPivotIOInputs inputs) { - position += 0.02 * voltage * 0.5; + position += 0.02 * voltage * 0.6; inputs.position = position; inputs.throughboreEncoderPosition = position;