diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 56c73fa1..ba97465a 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,18 +1,23 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.92, + "robotLength": 0.82, "holonomicMode": true, "pathFolders": [ - "ABCDE", - "HGF", - "Tests", - "Reroutes" + "Blue To Notes", + "Blue To Shoots", + "Blue To Subwoofers", + "Misc", + "Red To Notes", + "Red To Shoots", + "Red To Subwoofers", + "Utils" ], "autoFolders": [ - "Tests" + "Blue Autons", + "Red Autons" ], "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 4.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/build.gradle b/build.gradle index aebb7295..af18a13b 100644 --- a/build.gradle +++ b/build.gradle @@ -38,6 +38,15 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + // Enable VisualVM connection + // jvmArgs.add("-Dcom.sun.management.jmxremote=true") + // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + // jvmArgs.add("-Djava.rmi.server.hostname=10.6.94.2") + jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError") + jvmArgs.add("-XX:HeapDumpPath=/u/frc-usercode.hprof") } // Static files artifact diff --git a/simgui-ds.json b/simgui-ds.json index 4fb780a7..ac6a2f42 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,24 +1,14 @@ { - "Joysticks": { - "window": { - "visible": false - } - }, - "System Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ { - "decKey": 65, - "incKey": 68 + "decKey": 83, + "incKey": 87 }, { - "decKey": 87, - "incKey": 83 + "decKey": 68, + "incKey": 65 }, { "decKey": 69, @@ -30,10 +20,10 @@ "axisCount": 3, "buttonCount": 4, "buttonKeys": [ - 90, - 88, - 67, - 86 + -1, + -1, + -1, + -1 ], "povConfig": [ { @@ -99,5 +89,10 @@ "povCount": 0 } ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ], "zeroDisconnectedJoysticks": false } diff --git a/simgui.json b/simgui.json index e61d9cdc..6dbfd9e4 100644 --- a/simgui.json +++ b/simgui.json @@ -16,6 +16,7 @@ "/FMSInfo": "FMSInfo", "/Pose": "Field2d", "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/Field": "Field2d", "/SmartDashboard/Module 0": "Mechanism2d", "/SmartDashboard/Module 1": "Mechanism2d", "/SmartDashboard/Module 2": "Mechanism2d", @@ -41,6 +42,17 @@ "window": { "visible": true } + }, + "/SmartDashboard/Field": { + "bottom": 1476, + "height": 8.210550308227539, + "left": 150, + "right": 2961, + "top": 79, + "width": 16.541748046875, + "window": { + "visible": true + } } } }, diff --git a/src/main/deploy/pathplanner/autos/ADEF.auto b/src/main/deploy/pathplanner/autos/ADEF BLUE.auto similarity index 64% rename from src/main/deploy/pathplanner/autos/ADEF.auto rename to src/main/deploy/pathplanner/autos/ADEF BLUE.auto index 5788e66b..0c9edd0e 100644 --- a/src/main/deploy/pathplanner/autos/ADEF.auto +++ b/src/main/deploy/pathplanner/autos/ADEF BLUE.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.435487934219916, - "y": 6.595791855088175 + "x": 1.3334750274411264, + "y": 7.002978151231599 }, - "rotation": 1.3048503501930353 + "rotation": 41.18642861701727 }, "command": { "type": "sequential", @@ -14,48 +14,48 @@ { "type": "path", "data": { - "pathName": "Amp to A" + "pathName": "Blue Amp to A" } }, { "type": "path", "data": { - "pathName": "A to D" + "pathName": "Blue A to D" } }, { "type": "path", "data": { - "pathName": "D to Shoot" + "pathName": "Blue D to Shoot" } }, { "type": "path", "data": { - "pathName": "D Shoot to E" + "pathName": "Blue D Shoot to E" } }, { "type": "path", "data": { - "pathName": "E to Shoot" + "pathName": "Blue E to Shoot" } }, { "type": "path", "data": { - "pathName": "E Shoot to F" + "pathName": "Blue E Shoot to F" } }, { "type": "path", "data": { - "pathName": "F to Shoot" + "pathName": "Blue F to Shoot" } } ] } }, - "folder": null, + "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFED.auto b/src/main/deploy/pathplanner/autos/ADEF RED.auto similarity index 67% rename from src/main/deploy/pathplanner/autos/BFED.auto rename to src/main/deploy/pathplanner/autos/ADEF RED.auto index 47d69b92..4ba3fb38 100644 --- a/src/main/deploy/pathplanner/autos/BFED.auto +++ b/src/main/deploy/pathplanner/autos/ADEF RED.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.4053936459807443, - "y": 5.5301848373414 + "x": 1.3334750274411264, + "y": 7.002978151231599 }, "rotation": 0 }, @@ -14,48 +14,48 @@ { "type": "path", "data": { - "pathName": "Center to B" + "pathName": "Red Amp to A" } }, { "type": "path", "data": { - "pathName": "B to F" + "pathName": "Red A to D" } }, { "type": "path", "data": { - "pathName": "F to Shoot ALT" + "pathName": "Red D to Shoot" } }, { "type": "path", "data": { - "pathName": "F Shoot ALT to E" + "pathName": "Red D Shoot to E" } }, { "type": "path", "data": { - "pathName": "E to Shoot" + "pathName": "Red E to Shoot" } }, { "type": "path", "data": { - "pathName": "E Shoot to D" + "pathName": "Red E Shoot to F" } }, { "type": "path", "data": { - "pathName": "D to Shoot" + "pathName": "Red F to Shoot" } } ] } }, - "folder": null, + "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFGH.auto b/src/main/deploy/pathplanner/autos/BCA BLUE.auto similarity index 62% rename from src/main/deploy/pathplanner/autos/BFGH.auto rename to src/main/deploy/pathplanner/autos/BCA BLUE.auto index 14f26960..4e561a59 100644 --- a/src/main/deploy/pathplanner/autos/BFGH.auto +++ b/src/main/deploy/pathplanner/autos/BCA BLUE.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.4053936459807443, + "x": 1.5588456716347956, "y": 5.5301848373414 }, - "rotation": 0 + "rotation": 0.7711444982014979 }, "command": { "type": "sequential", @@ -14,48 +14,42 @@ { "type": "path", "data": { - "pathName": "Center to B" + "pathName": "Blue Center to B" } }, { "type": "path", "data": { - "pathName": "B to F" + "pathName": "Blue B to Center" } }, { "type": "path", "data": { - "pathName": "F to Shoot" + "pathName": "Blue Center to C" } }, { "type": "path", "data": { - "pathName": "F Shoot to G" + "pathName": "Blue C to Shoot Before A" } }, { "type": "path", "data": { - "pathName": "G to Shoot" + "pathName": "Blue Center to A" } }, { "type": "path", "data": { - "pathName": "G Shoot to H" - } - }, - { - "type": "path", - "data": { - "pathName": "H to Shoot" + "pathName": "Blue A to Center" } } ] } }, - "folder": null, + "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BCA RED.auto b/src/main/deploy/pathplanner/autos/BCA RED.auto new file mode 100644 index 00000000..5db41b4e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BCA RED.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.5588456716347956, + "y": 5.5301848373414 + }, + "rotation": 0.7711444982014979 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "Red B to Center" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Center to C" + } + }, + { + "type": "path", + "data": { + "pathName": "Red C to Shoot Before A" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Center to A" + } + }, + { + "type": "path", + "data": { + "pathName": "Red A to Center" + } + } + ] + } + }, + "folder": "Red Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BCA.auto b/src/main/deploy/pathplanner/autos/BCA.auto deleted file mode 100644 index de2733c4..00000000 --- a/src/main/deploy/pathplanner/autos/BCA.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4053936459807443, - "y": 5.5301848373414 - }, - "rotation": 179.43822267700324 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center to B" - } - }, - { - "type": "path", - "data": { - "pathName": "B to C" - } - }, - { - "type": "path", - "data": { - "pathName": "C to A" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFAC.auto b/src/main/deploy/pathplanner/autos/BFAC.auto deleted file mode 100644 index e85fb935..00000000 --- a/src/main/deploy/pathplanner/autos/BFAC.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4053936459807443, - "y": 5.5301848373414 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center to B" - } - }, - { - "type": "path", - "data": { - "pathName": "B to F" - } - }, - { - "type": "path", - "data": { - "pathName": "F to A Shoot" - } - }, - { - "type": "path", - "data": { - "pathName": "FA Shoot to A" - } - }, - { - "type": "path", - "data": { - "pathName": "C to A" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFCA.auto b/src/main/deploy/pathplanner/autos/BFCA.auto deleted file mode 100644 index 56528954..00000000 --- a/src/main/deploy/pathplanner/autos/BFCA.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4053936459807443, - "y": 5.5301848373414 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center to B" - } - }, - { - "type": "path", - "data": { - "pathName": "B to F" - } - }, - { - "type": "path", - "data": { - "pathName": "F to C Shoot" - } - }, - { - "type": "path", - "data": { - "pathName": "FC Shoot to C" - } - }, - { - "type": "path", - "data": { - "pathName": "C to A" - } - } - ] - } - }, - "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 BLUE.auto similarity index 69% rename from src/main/deploy/pathplanner/autos/HGF.auto rename to src/main/deploy/pathplanner/autos/HGF BLUE.auto index ee71a20c..46b4a8ca 100644 --- a/src/main/deploy/pathplanner/autos/HGF.auto +++ b/src/main/deploy/pathplanner/autos/HGF BLUE.auto @@ -5,7 +5,7 @@ "x": 0.7437038962619593, "y": 4.320740582515391 }, - "rotation": 116.80625971086718 + "rotation": -61.150838114458885 }, "command": { "type": "sequential", @@ -14,42 +14,42 @@ { "type": "path", "data": { - "pathName": "Source to H" + "pathName": "Blue Source to H" } }, { "type": "path", "data": { - "pathName": "H to Shoot" + "pathName": "Blue H to Shoot" } }, { "type": "path", "data": { - "pathName": "H Shoot to G" + "pathName": "Blue H Shoot to G" } }, { "type": "path", "data": { - "pathName": "G to Shoot" + "pathName": "Blue G to Shoot" } }, { "type": "path", "data": { - "pathName": "G Shoot to F" + "pathName": "Blue G Shoot to F" } }, { "type": "path", "data": { - "pathName": "F to Shoot" + "pathName": "Blue F to Shoot" } } ] } }, - "folder": null, + "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HGF Red.auto b/src/main/deploy/pathplanner/autos/HGF Red.auto index dcb5cb07..570b049a 100644 --- a/src/main/deploy/pathplanner/autos/HGF Red.auto +++ b/src/main/deploy/pathplanner/autos/HGF Red.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7437038962619593, - "y": 4.320740582515391 + "x": 0.7905759905641836, + "y": 4.310885424226206 }, - "rotation": -59.682052822906385 + "rotation": -60.092064589198905 }, "command": { "type": "sequential", @@ -14,42 +14,42 @@ { "type": "path", "data": { - "pathName": "Source to H" + "pathName": "Red Source to H" } }, { "type": "path", "data": { - "pathName": "H to Shoot" + "pathName": "Red H to Shoot" } }, { "type": "path", "data": { - "pathName": "H Shoot to G" + "pathName": "Red H Shoot to G" } }, { "type": "path", "data": { - "pathName": "G to Shoot" + "pathName": "Red G to Shoot" } }, { "type": "path", "data": { - "pathName": "G Shoot to F" + "pathName": "Red G Shoot to F" } }, { "type": "path", "data": { - "pathName": "F to Shoot" + "pathName": "Red F to Shoot" } } ] } }, - "folder": null, + "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Square Test.auto b/src/main/deploy/pathplanner/autos/Square Test.auto deleted file mode 100644 index 35f5d714..00000000 --- a/src/main/deploy/pathplanner/autos/Square Test.auto +++ /dev/null @@ -1,51 +0,0 @@ -{ - "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 deleted file mode 100644 index f1d96517..00000000 --- a/src/main/deploy/pathplanner/autos/Sraight Line.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "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/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path deleted file mode 100644 index fc559e85..00000000 --- a/src/main/deploy/pathplanner/paths/B to F.path +++ /dev/null @@ -1,60 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.8982818679923077, - "y": 5.55 - }, - "prevControl": null, - "nextControl": { - "x": 4.242237014979983, - "y": 4.787595339960025 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 4.1 - }, - "prevControl": { - "x": 6.3594946760057205, - "y": 3.9070792672463317 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7, - "rotationDegrees": -1.764677017473934, - "rotateFast": true - }, - { - "waypointRelativePos": 0.25, - "rotationDegrees": -28.00487762270733, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.6307416329486492, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "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/Blue A to Center.path similarity index 65% rename from src/main/deploy/pathplanner/paths/Square Top.path rename to src/main/deploy/pathplanner/paths/Blue A to Center.path index 7364ccab..888b3d7e 100644 --- a/src/main/deploy/pathplanner/paths/Square Top.path +++ b/src/main/deploy/pathplanner/paths/Blue A to Center.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, + "x": 3.0, "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 3.118033988749895, - "y": 7.0 + "x": 2.2278907346133145, + "y": 6.402053865305648 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA A" }, { "anchor": { - "x": 3.5, - "y": 7.0 + "x": 2.1260220496631472, + "y": 6.298406444469139 }, "prevControl": { - "x": 2.9019564509574227, - "y": 7.0 + "x": 2.789538609211147, + "y": 6.851593305121134 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA A Shoot Pose" } ], "rotationTargets": [], @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 28.70481879170682, "rotateFast": false }, "reversed": false, - "folder": "Tests", + "folder": "Blue To Subwoofers", "previewStartingState": null, "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/Blue A to D.path similarity index 64% rename from src/main/deploy/pathplanner/paths/A to D.path rename to src/main/deploy/pathplanner/paths/Blue A to D.path index d9c2175a..61b60c25 100644 --- a/src/main/deploy/pathplanner/paths/A to D.path +++ b/src/main/deploy/pathplanner/paths/Blue A to D.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.9, - "y": 6.997401573858207 + "x": 2.954672602983599, + "y": 7.029418819812343 }, "prevControl": null, "nextControl": { - "x": 4.040650442573206, - "y": 7.182822771523519 + "x": 8.722728490843888, + "y": 7.477996780770348 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue ADEF A" }, { "anchor": { - "x": 8.29, - "y": 7.44 + "x": 9.27634801698175, + "y": 7.530253156674468 }, "prevControl": { - "x": 5.892648616755566, - "y": 7.181073743680225 + "x": 3.4714674888799184, + "y": 7.0783100197152775 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue D" } ], "rotationTargets": [], @@ -43,9 +43,9 @@ "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Blue To Notes", "previewStartingState": { - "rotation": 41.18784481523856, + "rotation": 34.908040095775505, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path new file mode 100644 index 00000000..3e8ae4ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.9075282655371586, + "y": 7.045342819474668 + }, + "isLocked": false, + "linkedName": "Blue BCA A" + }, + { + "anchor": { + "x": 2.6938714628917357, + "y": 5.631284339205584 + }, + "prevControl": { + "x": 2.6854239726791893, + "y": 5.63537084593235 + }, + "nextControl": { + "x": 2.702318953104282, + "y": 5.627197832478817 + }, + "isLocked": false, + "linkedName": "Blue BCA B" + }, + { + "anchor": { + "x": 2.8299207212253714, + "y": 4.577599659291581 + }, + "prevControl": { + "x": 2.8213830994377913, + "y": 4.581607916922298 + }, + "nextControl": { + "x": 2.8384583430129515, + "y": 4.573591401660863 + }, + "isLocked": false, + "linkedName": "Blue BCA C" + }, + { + "anchor": { + "x": 2.1260220496631472, + "y": 6.298406444469139 + }, + "prevControl": { + "x": 2.117196006835459, + "y": 6.302164304992502 + }, + "nextControl": { + "x": 2.1348480924908353, + "y": 6.294648583945776 + }, + "isLocked": false, + "linkedName": "Blue BCA A Shoot Pose" + }, + { + "anchor": { + "x": 1.95, + "y": 5.55 + }, + "prevControl": { + "x": 1.9402510098439658, + "y": 5.552956589779829 + }, + "nextControl": { + "x": 1.959748990156034, + "y": 5.54704341022017 + }, + "isLocked": false, + "linkedName": "Blue BCA B Shoot Pose" + }, + { + "anchor": { + "x": 1.5588456716347956, + "y": 5.5301848373414 + }, + "prevControl": { + "x": 1.5461432500280547, + "y": 5.530577360741923 + }, + "nextControl": { + "x": 1.5715480932415364, + "y": 5.529792313940877 + }, + "isLocked": false, + "linkedName": "Blue BCA Starting Pose" + }, + { + "anchor": { + "x": 2.4, + "y": 6.43 + }, + "prevControl": { + "x": 2.377846597750998, + "y": 6.4221875109867375 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue BCA C Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Utils", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Amp to A.path b/src/main/deploy/pathplanner/paths/Blue Amp to A.path new file mode 100644 index 00000000..c0dfb882 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Amp to A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3901542060896621, + "y": 6.569495408336055 + }, + "prevControl": null, + "nextControl": { + "x": 2.6604119986513783, + "y": 6.9303384235488705 + }, + "isLocked": false, + "linkedName": "Blue ADEF Starting Pose" + }, + { + "anchor": { + "x": 2.954672602983599, + "y": 7.029418819812343 + }, + "prevControl": { + "x": 1.6866262117940904, + "y": 6.629683984814398 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue ADEF A" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 10.233677721117825, + "rotateFast": false + }, + "reversed": false, + "folder": "Blue To Notes", + "previewStartingState": { + "rotation": 0.2809818448982869, + "velocity": 0 + }, + "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/Blue B to C.path similarity index 67% rename from src/main/deploy/pathplanner/paths/Square Left.path rename to src/main/deploy/pathplanner/paths/Blue B to C.path index 9b4fb714..08f3f22f 100644 --- a/src/main/deploy/pathplanner/paths/Square Left.path +++ b/src/main/deploy/pathplanner/paths/Blue B to C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 5.5 + "x": 2.6938714628917357, + "y": 5.631284339205584 }, "prevControl": null, "nextControl": { - "x": 2.0, - "y": 6.618033988749895 + "x": 2.0322297871158246, + "y": 5.373357077845108 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.8299207212253714, + "y": 4.58 }, "prevControl": { - "x": 2.0, - "y": 5.479309367425445 + "x": 2.3071646664126666, + "y": 4.837775838126081 }, "nextControl": null, "isLocked": false, @@ -33,17 +33,17 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -52.15221700176973, "rotateFast": false }, "reversed": false, - "folder": "Tests", + "folder": "Blue To Notes", "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/Blue B to Center.path similarity index 66% rename from src/main/deploy/pathplanner/paths/Square Right.path rename to src/main/deploy/pathplanner/paths/Blue B to Center.path index 52be2d59..6bcc666e 100644 --- a/src/main/deploy/pathplanner/paths/Square Right.path +++ b/src/main/deploy/pathplanner/paths/Blue B to Center.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5, - "y": 7.0 + "x": 2.6938714628917357, + "y": 5.631284339205584 }, "prevControl": null, "nextControl": { - "x": 3.5, - "y": 5.88 + "x": 2.113811268173132, + "y": 5.556096692550529 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA B" }, { "anchor": { - "x": 3.5, - "y": 5.5 + "x": 1.9507925278871394, + "y": 5.55 }, "prevControl": { - "x": 3.5, - "y": 7.020690632574555 + "x": 2.486143955732852, + "y": 5.598113318622471 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 1.0588668160039703, "rotateFast": false }, "reversed": false, - "folder": "Tests", + "folder": "Blue To Subwoofers", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to G.path b/src/main/deploy/pathplanner/paths/Blue C to A.path similarity index 65% rename from src/main/deploy/pathplanner/paths/F Shoot to G.path rename to src/main/deploy/pathplanner/paths/Blue C to A.path index f34c410c..df58c866 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/Blue C to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.44, - "y": 4.92 + "x": 2.8299207212253714, + "y": 4.58 }, "prevControl": null, "nextControl": { - "x": 6.0115504085174845, - "y": 4.516209234241385 + "x": 3.092452523260701, + "y": 6.699823867467676 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.29, - "y": 2.4413378597962554 + "x": 3.1548525011112525, + "y": 6.974422229164399 }, "prevControl": { - "x": 7.298378780336429, - "y": 2.3061058584914784 + "x": 2.9725080357898817, + "y": 5.774688910798953 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -40.973340418312205, + "rotationDegrees": 83.48461145679269, "rotateFast": true } ], @@ -39,19 +39,19 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.3103702110425773, + "rotation": 85.39557844916084, "rotateFast": false }, "reversed": false, - "folder": "HGF", + "folder": "Blue To Notes", "previewStartingState": { - "rotation": -6.680432083088365, + "rotation": 83.35190685163934, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path new file mode 100644 index 00000000..eed01821 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8299207212253714, + "y": 4.577599659291581 + }, + "prevControl": null, + "nextControl": { + "x": 2.454095504946963, + "y": 6.165533422800476 + }, + "isLocked": false, + "linkedName": "Blue BCA C" + }, + { + "anchor": { + "x": 2.4, + "y": 6.43 + }, + "prevControl": { + "x": 2.7701167362900203, + "y": 4.7639342618714995 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue BCA C Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 38.766552008153084, + "rotateFast": false + }, + "reversed": false, + "folder": "Blue To Shoots", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to E.path b/src/main/deploy/pathplanner/paths/Blue Center to A.path similarity index 62% rename from src/main/deploy/pathplanner/paths/F Shoot to E.path rename to src/main/deploy/pathplanner/paths/Blue Center to A.path index ff5fe7da..3a6e3b0e 100644 --- a/src/main/deploy/pathplanner/paths/F Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to A.path @@ -3,35 +3,35 @@ "waypoints": [ { "anchor": { - "x": 4.44, - "y": 4.92 + "x": 2.4, + "y": 6.43 }, "prevControl": null, "nextControl": { - "x": 5.590230373169081, - "y": 2.934143848198411 + "x": 2.8901573716465885, + "y": 6.887841058048188 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA C Shoot Pose" }, { "anchor": { - "x": 8.29, - "y": 5.78 + "x": 3.0, + "y": 7.0 }, "prevControl": { - "x": 7.126129843018615, - "y": 6.127863050636817 + "x": 2.5359966344932694, + "y": 6.550650422729418 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA A" } ], "rotationTargets": [ { - "waypointRelativePos": 0.4, - "rotationDegrees": 27.738650602589555, + "waypointRelativePos": 1, + "rotationDegrees": 0, "rotateFast": false } ], @@ -45,13 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 2.0861407973812502, + "rotation": 32.52489028779099, "rotateFast": false }, "reversed": false, - "folder": "HGF", + "folder": "Blue To Notes", "previewStartingState": { - "rotation": -3.5408335937068656, + "rotation": 31.8239059025087, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/B to C.path b/src/main/deploy/pathplanner/paths/Blue Center to B.path similarity index 61% rename from src/main/deploy/pathplanner/paths/B to C.path rename to src/main/deploy/pathplanner/paths/Blue Center to B.path index 272419ce..a6f83ffd 100644 --- a/src/main/deploy/pathplanner/paths/B to C.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to B.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.9, - "y": 5.55 + "x": 1.5588456716347956, + "y": 5.5301848373414 }, "prevControl": null, "nextControl": { - "x": 1.979636893611585, - "y": 4.568946349574247 + "x": 2.401533816274478, + "y": 5.599459389022944 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA Starting Pose" }, { "anchor": { - "x": 2.5, - "y": 4.1 + "x": 2.6938714628917357, + "y": 5.631284339205584 }, "prevControl": { - "x": 2.0252109914562095, - "y": 4.045661481389927 + "x": 1.848971916879846, + "y": 5.5467703476329575 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue BCA B" } ], "rotationTargets": [], @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -179.19416655403055, + "rotation": -0.14529343403600373, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Blue To Notes", "previewStartingState": { - "rotation": -179.44747299332943, + "rotation": 0.4328405361826138, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Blue Center to C.path b/src/main/deploy/pathplanner/paths/Blue Center to C.path new file mode 100644 index 00000000..e6ae60d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Center to C.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.95, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.5935411441135385, + "y": 4.824459355949962 + }, + "isLocked": false, + "linkedName": "Blue BCA B Shoot Pose" + }, + { + "anchor": { + "x": 2.8299207212253714, + "y": 4.577599659291581 + }, + "prevControl": { + "x": 2.111455644972302, + "y": 5.35043636493523 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue BCA C" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -44.3462417321064, + "rotateFast": false + }, + "reversed": false, + "folder": "Blue To Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D Shoot to E.path b/src/main/deploy/pathplanner/paths/Blue D Shoot to E.path similarity index 63% rename from src/main/deploy/pathplanner/paths/D Shoot to E.path rename to src/main/deploy/pathplanner/paths/Blue D Shoot to E.path index a6b0dc62..bc6114c7 100644 --- a/src/main/deploy/pathplanner/paths/D Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/Blue D Shoot to E.path @@ -3,35 +3,35 @@ "waypoints": [ { "anchor": { - "x": 5.33, - "y": 6.79 + "x": 4.844074705820543, + "y": 6.341528771227186 }, "prevControl": null, "nextControl": { - "x": 6.510461787664648, - "y": 6.264802758400161 + "x": 6.579303673860324, + "y": 6.340999957855571 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue Midline Shoot Pose" }, { "anchor": { - "x": 8.29, - "y": 5.777236829771279 + "x": 8.398526797053595, + "y": 5.762294601512281 }, "prevControl": { - "x": 6.89080789265001, - "y": 6.152405879960569 + "x": 5.532878159320307, + "y": 6.17528907676863 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue E" } ], "rotationTargets": [ { - "waypointRelativePos": 0.45, - "rotationDegrees": -49.26302603704863, + "waypointRelativePos": 0.5, + "rotationDegrees": -29.576784217189907, "rotateFast": true } ], @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Blue To Notes", "previewStartingState": { "rotation": 16.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/D to Shoot.path b/src/main/deploy/pathplanner/paths/Blue D to Shoot.path similarity index 69% rename from src/main/deploy/pathplanner/paths/D to Shoot.path rename to src/main/deploy/pathplanner/paths/Blue D to Shoot.path index 3b653a76..1af267be 100644 --- a/src/main/deploy/pathplanner/paths/D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue D to Shoot.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.29, - "y": 7.44 + "x": 9.27634801698175, + "y": 7.530253156674468 }, "prevControl": null, "nextControl": { - "x": 7.113582543928479, - "y": 7.047043590947302 + "x": 6.325377107599455, + "y": 6.478010695046689 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue D" }, { "anchor": { - "x": 5.329525380962965, - "y": 6.789433696984953 + "x": 4.844074705820543, + "y": 6.341528771227186 }, "prevControl": { - "x": 6.628819835020623, - "y": 7.036774133072075 + "x": 7.70048416956953, + "y": 7.366224863588121 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue Midline Shoot Pose" } ], "rotationTargets": [ @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Blue To Shoots", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E Shoot to F.path b/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path similarity index 60% rename from src/main/deploy/pathplanner/paths/E Shoot to F.path rename to src/main/deploy/pathplanner/paths/Blue E Shoot to F.path index a5f95372..98be21fe 100644 --- a/src/main/deploy/pathplanner/paths/E Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path @@ -3,43 +3,43 @@ "waypoints": [ { "anchor": { - "x": 5.33, - "y": 6.79 + "x": 4.844074705820543, + "y": 6.341528771227186 }, "prevControl": null, "nextControl": { - "x": 7.290846102339526, - "y": 6.4834430505914575 + "x": 7.865711885269925, + "y": 7.084464938160646 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue Midline Shoot Pose" }, { "anchor": { - "x": 8.29, - "y": 4.1 + "x": 9.32365277676984, + "y": 4.877773358597312 }, "prevControl": { - "x": 6.771776810093797, - "y": 4.0354847686629896 + "x": 6.422771590392083, + "y": 4.690301445191947 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue F" } ], "rotationTargets": [ { - "waypointRelativePos": 0.7, - "rotationDegrees": -21.216895027434855, - "rotateFast": true + "waypointRelativePos": 0.5, + "rotationDegrees": -19.34074487837396, + "rotateFast": false } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Blue To Notes", "previewStartingState": { "rotation": 11.872095567230494, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/Blue E to Shoot.path similarity index 61% rename from src/main/deploy/pathplanner/paths/E to Shoot.path rename to src/main/deploy/pathplanner/paths/Blue E to Shoot.path index 677f2839..31864c75 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue E to Shoot.path @@ -3,43 +3,37 @@ "waypoints": [ { "anchor": { - "x": 8.29, - "y": 5.779496305086361 + "x": 8.398526797053595, + "y": 5.762294601512281 }, "prevControl": null, "nextControl": { - "x": 6.92426272856266, - "y": 6.59625025891421 + "x": 5.445623953003859, + "y": 6.495269240538481 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue E" }, { "anchor": { - "x": 5.33, - "y": 6.79 + "x": 4.844074705820543, + "y": 6.341528771227186 }, "prevControl": { - "x": 6.487145925370721, - "y": 6.506326247399518 + "x": 5.895884501962457, + "y": 6.343980542313763 }, "nextControl": null, "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 16.0, - "rotateFast": true + "linkedName": "Blue Midline Shoot Pose" } ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -49,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Blue To Shoots", "previewStartingState": { "rotation": -0.49534996263591174, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Blue F to Shoot.path b/src/main/deploy/pathplanner/paths/Blue F to Shoot.path new file mode 100644 index 00000000..e4d79aa6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue F to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.32365277676984, + "y": 4.877773358597312 + }, + "prevControl": null, + "nextControl": { + "x": 6.497337601260025, + "y": 6.27989797646262 + }, + "isLocked": false, + "linkedName": "Blue F" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 6.271245847923244, + "y": 6.380372517069439 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Midline Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 11.738504411842449, + "rotateFast": false + }, + "reversed": false, + "folder": "Blue To Shoots", + "previewStartingState": { + "rotation": 1.6826420893398992, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path b/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path new file mode 100644 index 00000000..c2a68084 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": null, + "nextControl": { + "x": 6.712180051964271, + "y": 6.800875295388985 + }, + "isLocked": false, + "linkedName": "Blue Midline Shoot Pose" + }, + { + "anchor": { + "x": 9.32365277676984, + "y": 4.877773358597312 + }, + "prevControl": { + "x": 8.60893746806095, + "y": 4.491979967032851 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue F" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -1.0333876477886335, + "rotateFast": false + }, + "reversed": false, + "folder": "Blue To Notes", + "previewStartingState": { + "rotation": 5.880944330975369, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to Shoot.path b/src/main/deploy/pathplanner/paths/Blue G to Shoot.path similarity index 59% rename from src/main/deploy/pathplanner/paths/F to Shoot.path rename to src/main/deploy/pathplanner/paths/Blue G to Shoot.path index a96d0d43..09286e45 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue G to Shoot.path @@ -3,35 +3,35 @@ "waypoints": [ { "anchor": { - "x": 8.29, - "y": 4.102965621295826 + "x": 9.17904635660462, + "y": 3.0775484197917797 }, "prevControl": null, "nextControl": { - "x": 6.0256608690672735, - "y": 4.006114860012458 + "x": 8.083681568218793, + "y": 4.805277852743206 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue G" }, { "anchor": { - "x": 4.44, - "y": 4.92 + "x": 4.844074705820543, + "y": 6.341528771227186 }, "prevControl": { - "x": 5.723635564644642, - "y": 4.265840139753196 + "x": 6.4047471872845545, + "y": 6.780924609096242 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue Midline Shoot Pose" } ], "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 147.06548083331427, + "rotationDegrees": -4.159684543530611, "rotateFast": true } ], @@ -39,19 +39,19 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 170.34501153780897, + "rotation": 7.509278390683297, "rotateFast": false }, "reversed": false, - "folder": "HGF", + "folder": "Blue To Shoots", "previewStartingState": { - "rotation": 178.17768989239653, + "rotation": -0.14051537896562244, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path b/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path new file mode 100644 index 00000000..6e6dc490 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": null, + "nextControl": { + "x": 7.574338143467954, + "y": 7.5179462272987045 + }, + "isLocked": false, + "linkedName": "Blue Midline Shoot Pose" + }, + { + "anchor": { + "x": 9.17904635660462, + "y": 3.0775484197917797 + }, + "prevControl": { + "x": 7.344448548644122, + "y": 5.242269993468559 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue G" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.224027189600015, + "rotateFast": false + }, + "reversed": false, + "folder": "Blue To Notes", + "previewStartingState": { + "rotation": 4.704435901357846, + "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/Blue H to Shoot.path similarity index 65% rename from src/main/deploy/pathplanner/paths/H to Shoot.path rename to src/main/deploy/pathplanner/paths/Blue H to Shoot.path index 743f5702..2328680d 100644 --- a/src/main/deploy/pathplanner/paths/H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue H to Shoot.path @@ -12,44 +12,38 @@ "y": 2.4336941028792762 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue H" }, { "anchor": { - "x": 4.439099811049325, - "y": 4.919501541012011 + "x": 4.844074705820543, + "y": 6.341528771227186 }, "prevControl": { - "x": 5.905931956023139, - "y": 4.587406743637892 + "x": 6.985047751717528, + "y": 7.12657625836085 }, "nextControl": null, "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.75, - "rotationDegrees": 158.01836985356624, - "rotateFast": true + "linkedName": "Blue Midline Shoot Pose" } ], + "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 175.17249248336842, + "rotation": 7.957397728998733, "rotateFast": false }, "reversed": false, - "folder": "HGF", + "folder": "Blue To Shoots", "previewStartingState": { "rotation": 0.0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path new file mode 100644 index 00000000..2786ac73 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path @@ -0,0 +1,161 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.27634801698175, + "y": 7.530253156674468 + }, + "prevControl": null, + "nextControl": { + "x": 10.27634801698175, + "y": 7.030253156674468 + }, + "isLocked": false, + "linkedName": "Blue D" + }, + { + "anchor": { + "x": 8.398526797053595, + "y": 5.762294601512281 + }, + "prevControl": { + "x": 8.489336670358856, + "y": 5.716898759806668 + }, + "nextControl": { + "x": 8.307716923748334, + "y": 5.8076904432178935 + }, + "isLocked": false, + "linkedName": "Blue E" + }, + { + "anchor": { + "x": 9.32365277676984, + "y": 4.877773358597312 + }, + "prevControl": { + "x": 9.414244371346676, + "y": 4.832506665139351 + }, + "nextControl": { + "x": 9.233061182193005, + "y": 4.923040052055273 + }, + "isLocked": false, + "linkedName": "Blue F" + }, + { + "anchor": { + "x": 9.17904635660462, + "y": 3.0775484197917797 + }, + "prevControl": { + "x": 9.268939459250493, + "y": 3.0326950007263043 + }, + "nextControl": { + "x": 9.089153253958745, + "y": 3.122401838857255 + }, + "isLocked": false, + "linkedName": "Blue G" + }, + { + "anchor": { + "x": 8.29, + "y": 0.77 + }, + "prevControl": { + "x": 8.377657928466796, + "y": 0.7264690589904781 + }, + "nextControl": { + "x": 8.202342071533202, + "y": 0.8135309410095211 + }, + "isLocked": false, + "linkedName": "Blue H" + }, + { + "anchor": { + "x": 1.3901542060896621, + "y": 6.569495408336055 + }, + "prevControl": { + "x": 1.4706595771834121, + "y": 6.530196397105586 + }, + "nextControl": { + "x": 1.3096488349959121, + "y": 6.608794419566523 + }, + "isLocked": false, + "linkedName": "Blue ADEF Starting Pose" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 4.901691893320543, + "y": 6.315771935289686 + }, + "nextControl": { + "x": 4.786457518320543, + "y": 6.367285607164686 + }, + "isLocked": false, + "linkedName": "Blue Midline Shoot Pose" + }, + { + "anchor": { + "x": 2.954672602983599, + "y": 7.029418819812343 + }, + "prevControl": { + "x": 3.9529715982039386, + "y": 7.541832769355557 + }, + "nextControl": { + "x": 1.956373607763259, + "y": 6.517004870269129 + }, + "isLocked": false, + "linkedName": "Blue ADEF A" + }, + { + "anchor": { + "x": 0.7905759905641836, + "y": 4.310885424226206 + }, + "prevControl": { + "x": 0.7749509905641836, + "y": 4.328463549226206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue HGF Starting Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Utils", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H Up.path b/src/main/deploy/pathplanner/paths/Blue Source to H.path similarity index 63% rename from src/main/deploy/pathplanner/paths/H Up.path rename to src/main/deploy/pathplanner/paths/Blue Source to H.path index 5f01f568..7ab4d6bf 100644 --- a/src/main/deploy/pathplanner/paths/H Up.path +++ b/src/main/deploy/pathplanner/paths/Blue Source to H.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.29, - "y": 0.77 + "x": 0.7905759905641836, + "y": 4.310885424226206 }, "prevControl": null, "nextControl": { - "x": 8.294726177430418, - "y": 4.093543128491389 + "x": 4.366652379138092, + "y": 1.9121879705812976 }, "isLocked": false, - "linkedName": null + "linkedName": "Blue HGF Starting Pose" }, { "anchor": { "x": 8.29, - "y": 7.44 + "y": 0.77 }, "prevControl": { - "x": 8.285399832512846, - "y": 4.093495054548515 + "x": 5.675946213313558, + "y": 1.5535082827978157 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Blue H" } ], "rotationTargets": [], @@ -33,19 +33,19 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 90.0, + "rotation": -1.5944180626014446, "rotateFast": false }, "reversed": false, - "folder": "Reroutes", + "folder": "Blue To Notes", "previewStartingState": { - "rotation": 90.0, + "rotation": -60.7473892158758, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/C to A.path b/src/main/deploy/pathplanner/paths/C to A.path deleted file mode 100644 index 7f1e3efb..00000000 --- a/src/main/deploy/pathplanner/paths/C to A.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.5, - "y": 4.1 - }, - "prevControl": null, - "nextControl": { - "x": 2.1540599070781474, - "y": 4.355657228204337 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.8953493574769893, - "y": 6.995863207686237 - }, - "prevControl": { - "x": 1.8478662161937423, - "y": 7.153016926941592 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -116.48352775095083, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 178.9517141332874, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": { - "rotation": -179.34585253591464, - "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 deleted file mode 100644 index 0ed52eca..00000000 --- a/src/main/deploy/pathplanner/paths/Center to C.path +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.4365936349060198, - "y": 5.5179740558513855 - }, - "prevControl": null, - "nextControl": { - "x": 1.9887709427576594, - "y": 4.858543781447682 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9, - "y": 4.1 - }, - "prevControl": { - "x": 2.5125240204349457, - "y": 4.5159838276893804 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -49.766999639914516, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -25.389005943465087, - "rotateFast": false - }, - "reversed": false, - "folder": "ABCDE", - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D Down.path b/src/main/deploy/pathplanner/paths/D Down.path deleted file mode 100644 index 6b7cd025..00000000 --- a/src/main/deploy/pathplanner/paths/D Down.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.29, - "y": 7.44 - }, - "prevControl": null, - "nextControl": { - "x": 8.29, - "y": 4.0982082285165475 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 0.77 - }, - "prevControl": { - "x": 8.29, - "y": 4.089534458398493 - }, - "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": -90.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Reroutes", - "previewStartingState": { - "rotation": -90.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file 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 deleted file mode 100644 index ff2701ee..00000000 --- a/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 5.33, - "y": 6.79 - }, - "prevControl": null, - "nextControl": { - "x": 6.282639372384399, - "y": 6.412389763023573 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 5.78 - }, - "prevControl": { - "x": 7.115890093186435, - "y": 5.924606420165222 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7, - "rotationDegrees": -20.76066485737566, - "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": "HGF", - "previewStartingState": { - "rotation": 17.734765504348854, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to A.path b/src/main/deploy/pathplanner/paths/F Shoot to A.path deleted file mode 100644 index aa8c1e55..00000000 --- a/src/main/deploy/pathplanner/paths/F Shoot to A.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 4.44, - "y": 4.92 - }, - "prevControl": null, - "nextControl": { - "x": 2.9511160941071886, - "y": 5.54126539972574 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9, - "y": 7.0 - }, - "prevControl": { - "x": 1.2419777842159854, - "y": 6.566132665561462 - }, - "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": 34.162913907056854, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": -16.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F Shoot to C.path b/src/main/deploy/pathplanner/paths/F Shoot to C.path deleted file mode 100644 index a094e841..00000000 --- a/src/main/deploy/pathplanner/paths/F Shoot to C.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 4.44, - "y": 4.92 - }, - "prevControl": null, - "nextControl": { - "x": 3.0066405352304755, - "y": 5.122613949654983 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9, - "y": 4.103734804381811 - }, - "prevControl": { - "x": 1.1951537638566347, - "y": 4.875658105110853 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -3.9091747518495517, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -31.82130002339912, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": -3.382897056838466, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to A Shoot.path b/src/main/deploy/pathplanner/paths/F to A Shoot.path deleted file mode 100644 index b72b5a37..00000000 --- a/src/main/deploy/pathplanner/paths/F to A Shoot.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.29, - "y": 4.102244512152715 - }, - "prevControl": null, - "nextControl": { - "x": 5.295522172316666, - "y": 3.8711530687570304 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.2530814806793305, - "y": 6.434696072517145 - }, - "prevControl": { - "x": 5.215157470865537, - "y": 4.363382169844697 - }, - "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.67316277210924, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 0.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to C Shoot.path b/src/main/deploy/pathplanner/paths/F to C Shoot.path deleted file mode 100644 index 1c12f0b7..00000000 --- a/src/main/deploy/pathplanner/paths/F to C Shoot.path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.29, - "y": 4.1 - }, - "prevControl": null, - "nextControl": { - "x": 6.0385927597004, - "y": 3.032824775746277 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.2317366500432407, - "y": 4.6325001020537755 - }, - "prevControl": { - "x": 3.0671656293089375, - "y": 6.355710584375037 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -42.94374675747001, - "rotateFast": true - }, - { - "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": -35.108347979019236, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 0.0, - "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 deleted file mode 100644 index 9d0249a0..00000000 --- a/src/main/deploy/pathplanner/paths/F to Shoot ALT.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.29, - "y": 4.1 - }, - "prevControl": null, - "nextControl": { - "x": 7.437832940993263, - "y": 5.262253876531562 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.33, - "y": 6.79 - }, - "prevControl": { - "x": 6.695690689558464, - "y": 5.875163451405772 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -53.74657859666923, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 13.23836155662926, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 0.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FA Shoot to A.path b/src/main/deploy/pathplanner/paths/FA Shoot to A.path deleted file mode 100644 index 9142313d..00000000 --- a/src/main/deploy/pathplanner/paths/FA Shoot to A.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.25, - "y": 6.43 - }, - "prevControl": null, - "nextControl": { - "x": 2.356103940255067, - "y": 6.680882733977868 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9, - "y": 7.0 - }, - "prevControl": { - "x": 2.8054764994689356, - "y": 6.8506910580885645 - }, - "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": 37.107670410433414, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 40.36814255534238, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FC Shoot to C.path b/src/main/deploy/pathplanner/paths/FC Shoot to C.path deleted file mode 100644 index ef5a379c..00000000 --- a/src/main/deploy/pathplanner/paths/FC Shoot to C.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.23, - "y": 4.63 - }, - "prevControl": null, - "nextControl": { - "x": 2.371295306206684, - "y": 4.45587643593442 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9, - "y": 4.1 - }, - "prevControl": { - "x": 2.748075591798505, - "y": 4.1744360772947715 - }, - "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": -34.89230355927469, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": -32.54514019016325, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G Shoot to F.path b/src/main/deploy/pathplanner/paths/G Shoot to F.path deleted file mode 100644 index e7d5397b..00000000 --- a/src/main/deploy/pathplanner/paths/G Shoot to F.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 4.44, - "y": 4.92 - }, - "prevControl": null, - "nextControl": { - "x": 5.614603862206237, - "y": 4.242043538030528 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 4.1 - }, - "prevControl": { - "x": 7.033169768534478, - "y": 3.9595610497024567 - }, - "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": 179.48407472803027, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 173.21363169926138, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/G Shoot to H.path b/src/main/deploy/pathplanner/paths/G Shoot to H.path deleted file mode 100644 index d78d8a2f..00000000 --- a/src/main/deploy/pathplanner/paths/G Shoot to H.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 4.44, - "y": 4.92 - }, - "prevControl": null, - "nextControl": { - "x": 5.64412126313092, - "y": 4.370449039447186 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 0.77 - }, - "prevControl": { - "x": 7.6598467568066075, - "y": 2.1351076818522676 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -58.70191149373465, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -56.9138613699413, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": -8.909069649342515, - "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 deleted file mode 100644 index cc792261..00000000 --- a/src/main/deploy/pathplanner/paths/G to Shoot.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.29, - "y": 2.44 - }, - "prevControl": null, - "nextControl": { - "x": 7.154920886590285, - "y": 3.184898154111784 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.44, - "y": 4.92 - }, - "prevControl": { - "x": 5.047523632063633, - "y": 4.447367348045708 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 138.52168950764727, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 171.1798219104725, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 178.858578419353, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H Shoot to G.path b/src/main/deploy/pathplanner/paths/H Shoot to G.path deleted file mode 100644 index bfd69f3e..00000000 --- a/src/main/deploy/pathplanner/paths/H Shoot to G.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 4.44, - "y": 4.92 - }, - "prevControl": null, - "nextControl": { - "x": 5.583645205891603, - "y": 4.669973259075983 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.287322790233379, - "y": 2.44 - }, - "prevControl": { - "x": 7.664236416642477, - "y": 2.2212553493187346 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 127.53126500479296, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 178.56854890177945, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 173.9845066722469, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path index 2bcb647a..b48fc1e2 100644 --- a/src/main/deploy/pathplanner/paths/Mobility.path +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.5, - "y": 5.55 + "x": 1.41, + "y": 5.6 }, "prevControl": null, "nextControl": { - "x": 2.897224241249078, - "y": 5.567009477582943 + "x": 2.7005544812853595, + "y": 5.6 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.15, - "y": 5.55 + "x": 3.0, + "y": 5.6 }, "prevControl": { - "x": 2.896214688448722, - "y": 5.56249052695278 + "x": 1.6580381329233314, + "y": 5.6 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Point Locations.path b/src/main/deploy/pathplanner/paths/Point Locations.path deleted file mode 100644 index aed960cf..00000000 --- a/src/main/deploy/pathplanner/paths/Point Locations.path +++ /dev/null @@ -1,161 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.9, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.2258235550697294, - "y": 6.68477672334809 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.25, - "y": 5.55 - }, - "prevControl": { - "x": 0.11367740203340196, - "y": 5.552889340951021 - }, - "nextControl": { - "x": 0.40237890185601805, - "y": 5.5467703476329575 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9, - "y": 5.55 - }, - "prevControl": { - "x": 2.904530075373245, - "y": 5.358940399662314 - }, - "nextControl": { - "x": 2.8948686180482484, - "y": 5.7664201926246115 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9, - "y": 4.1 - }, - "prevControl": { - "x": 2.910352370185796, - "y": 4.231098373366762 - }, - "nextControl": { - "x": 2.889647629814204, - "y": 3.9689016266332375 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.3, - "y": 7.44 - }, - "prevControl": { - "x": 7.283808318646552, - "y": 6.7699486192294955 - }, - "nextControl": { - "x": 8.351646965563278, - "y": 7.474054717454673 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 5.78 - }, - "prevControl": { - "x": 8.29838041697661, - "y": 6.32145681859072 - }, - "nextControl": { - "x": 8.279224286337115, - "y": 5.083783611933209 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 4.1 - }, - "prevControl": { - "x": 8.296182106596115, - "y": 4.544857124277373 - }, - "nextControl": { - "x": 8.281362241199185, - "y": 3.4784370294187017 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 2.44 - }, - "prevControl": { - "x": 8.284967786914741, - "y": 3.4775236245040944 - }, - "nextControl": { - "x": 8.292900374354867, - "y": 1.842011230029793 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 0.77 - }, - "prevControl": { - "x": 8.276410625083155, - "y": 1.8410385351276657 - }, - "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": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red A to Center.path b/src/main/deploy/pathplanner/paths/Red A to Center.path new file mode 100644 index 00000000..0f89149a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red A to Center.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1548525011112525, + "y": 6.974422229164399 + }, + "prevControl": null, + "nextControl": { + "x": 2.1577027780171636, + "y": 6.548006355871343 + }, + "isLocked": false, + "linkedName": "Red BCA A" + }, + { + "anchor": { + "x": 1.964541675549125, + "y": 6.441955237891131 + }, + "prevControl": { + "x": 2.9477980291526062, + "y": 6.8998595437667065 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red BCA A Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 28.70481879170682, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Subwoofers", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red A to D.path b/src/main/deploy/pathplanner/paths/Red A to D.path new file mode 100644 index 00000000..f1b8c633 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red A to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.001400475457201, + "y": 7.146094279167726 + }, + "prevControl": null, + "nextControl": { + "x": 8.876949699583925, + "y": 7.932247466987493 + }, + "isLocked": false, + "linkedName": "Red ADEF A" + }, + { + "anchor": { + "x": 9.583540511947096, + "y": 8.029356831593127 + }, + "prevControl": { + "x": 3.4529590208733993, + "y": 7.195273922727905 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Notes", + "previewStartingState": { + "rotation": 24.479897927083506, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red ABC Waypoints.path b/src/main/deploy/pathplanner/paths/Red ABC Waypoints.path new file mode 100644 index 00000000..80b04665 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red ABC Waypoints.path @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1548525011112525, + "y": 6.974422229164399 + }, + "prevControl": null, + "nextControl": { + "x": 3.053464555589826, + "y": 5.956552636691583 + }, + "isLocked": false, + "linkedName": "Red BCA A" + }, + { + "anchor": { + "x": 3.148650962480497, + "y": 5.5301848373414 + }, + "prevControl": { + "x": 3.220906098620233, + "y": 6.95980775053068 + }, + "nextControl": { + "x": 3.0960436896664576, + "y": 4.489309867984168 + }, + "isLocked": false, + "linkedName": "Red BCA B" + }, + { + "anchor": { + "x": 2.6557969001354675, + "y": 4.098014005179796 + }, + "prevControl": { + "x": 2.7444979098858338, + "y": 4.011846192832384 + }, + "nextControl": { + "x": 2.5670958903851013, + "y": 4.184181817527208 + }, + "isLocked": false, + "linkedName": "Red BCA C" + }, + { + "anchor": { + "x": 1.964541675549125, + "y": 6.441955237891131 + }, + "prevControl": { + "x": 2.0483849067502966, + "y": 6.366218238379412 + }, + "nextControl": { + "x": 1.8806984443479529, + "y": 6.51769223740285 + }, + "isLocked": false, + "linkedName": "Red BCA C Shoot Pose" + }, + { + "anchor": { + "x": 1.5588456716347956, + "y": 5.5301848373414 + }, + "prevControl": { + "x": 1.6271440114785456, + "y": 5.4878264389039 + }, + "nextControl": { + "x": 1.4905473317910456, + "y": 5.5725432357789 + }, + "isLocked": false, + "linkedName": "Red BCA Starting Pose" + }, + { + "anchor": { + "x": 1.95, + "y": 5.55 + }, + "prevControl": { + "x": 1.9685546875000002, + "y": 5.614453125 + }, + "nextControl": { + "x": 1.9314453125000002, + "y": 5.485546875 + }, + "isLocked": false, + "linkedName": "Red BCA B Shoot Pose" + }, + { + "anchor": { + "x": 1.964541675549125, + "y": 6.441955237891131 + }, + "prevControl": { + "x": 1.8239166755491247, + "y": 6.848205237891131 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red BCA A Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Utils", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Amp to A.path b/src/main/deploy/pathplanner/paths/Red Amp to A.path new file mode 100644 index 00000000..bee56d65 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Amp to A.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3824623752298102, + "y": 6.555650112788321 + }, + "prevControl": null, + "nextControl": { + "x": 2.759059729428983, + "y": 7.0723488507988925 + }, + "isLocked": false, + "linkedName": "Red ADEF Starting Pose" + }, + { + "anchor": { + "x": 3.001400475457201, + "y": 7.146094279167726 + }, + "prevControl": { + "x": 1.6141307059399834, + "y": 6.624203555326754 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red ADEF A" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.32633732715843, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Square Bottom.path b/src/main/deploy/pathplanner/paths/Red B to C.path similarity index 67% rename from src/main/deploy/pathplanner/paths/Square Bottom.path rename to src/main/deploy/pathplanner/paths/Red B to C.path index 4f00d91c..100cc706 100644 --- a/src/main/deploy/pathplanner/paths/Square Bottom.path +++ b/src/main/deploy/pathplanner/paths/Red B to C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5, - "y": 5.5 + "x": 2.6938714628917357, + "y": 5.631284339205584 }, "prevControl": null, "nextControl": { - "x": 2.381966011250105, - "y": 5.5 + "x": 2.0322297871158246, + "y": 5.373357077845108 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0, - "y": 5.5 + "x": 2.8299207212253714, + "y": 4.58 }, "prevControl": { - "x": 3.5206906325745555, - "y": 5.5 + "x": 2.0304029772866095, + "y": 5.007284560700074 }, "nextControl": null, "isLocked": false, @@ -33,17 +33,17 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -52.15221700176973, "rotateFast": false }, "reversed": false, - "folder": "Tests", + "folder": "Red To Notes", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red B to Center.path b/src/main/deploy/pathplanner/paths/Red B to Center.path new file mode 100644 index 00000000..0a17a60a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red B to Center.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.148650962480497, + "y": 5.5301848373414 + }, + "prevControl": null, + "nextControl": { + "x": 2.2524565194219677, + "y": 5.561625195981047 + }, + "isLocked": false, + "linkedName": "Red BCA B" + }, + { + "anchor": { + "x": 1.95, + "y": 5.55 + }, + "prevControl": { + "x": 2.7986726583572215, + "y": 5.559173424894468 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red BCA B Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Subwoofers", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/Red C to A.path similarity index 66% rename from src/main/deploy/pathplanner/paths/B to A.path rename to src/main/deploy/pathplanner/paths/Red C to A.path index bcbb7d64..4360467a 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/Red C to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.9, - "y": 5.547731826490439 + "x": 2.8299207212253714, + "y": 4.58 }, "prevControl": null, "nextControl": { - "x": 2.401088620852837, - "y": 6.1130813946895755 + "x": 3.092452523260701, + "y": 6.699823867467676 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9, - "y": 7.002641633631481 + "x": 3.1548525011112525, + "y": 6.974422229164399 }, "prevControl": { - "x": 1.7418025682777556, - "y": 6.382103179012906 + "x": 2.9725080357898817, + "y": 5.774688910798953 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 40.15509788025759, + "rotationDegrees": 83.48461145679269, "rotateFast": true } ], @@ -39,17 +39,17 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 34.7991322249468, + "rotation": 85.39557844916084, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Red To Notes", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path new file mode 100644 index 00000000..495bbeeb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6557969001354675, + "y": 4.098014005179796 + }, + "prevControl": null, + "nextControl": { + "x": 2.099677528968153, + "y": 5.947322439659761 + }, + "isLocked": false, + "linkedName": "Red BCA C" + }, + { + "anchor": { + "x": 1.964541675549125, + "y": 6.441955237891131 + }, + "prevControl": { + "x": 2.5069599729973273, + "y": 4.456982136620524 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red BCA C Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 38.766552008153084, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Shoots", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp to A.path b/src/main/deploy/pathplanner/paths/Red Center to A.path similarity index 61% rename from src/main/deploy/pathplanner/paths/Amp to A.path rename to src/main/deploy/pathplanner/paths/Red Center to A.path index dc47433d..4e1b5791 100644 --- a/src/main/deploy/pathplanner/paths/Amp to A.path +++ b/src/main/deploy/pathplanner/paths/Red Center to A.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.44, - "y": 6.595791855088175 + "x": 1.964541675549125, + "y": 6.441955237891131 }, "prevControl": null, "nextControl": { - "x": 1.8597816691764337, - "y": 7.114461624756583 + "x": 2.914867378283864, + "y": 6.852122118492749 }, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA C Shoot Pose" }, { "anchor": { - "x": 2.9, - "y": 7.002978151231599 + "x": 3.1548525011112525, + "y": 6.974422229164399 }, "prevControl": { - "x": 2.0125550145445525, - "y": 6.9965362428864735 + "x": 2.1927967563152393, + "y": 6.550986940329536 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA A" } ], "rotationTargets": [], @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.9005777857670384, + "rotation": 32.52489028779099, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Red To Notes", "previewStartingState": { - "rotation": 0.0, + "rotation": 31.8239059025087, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Center to B.path b/src/main/deploy/pathplanner/paths/Red Center to B.path similarity index 65% rename from src/main/deploy/pathplanner/paths/Center to B.path rename to src/main/deploy/pathplanner/paths/Red Center to B.path index 52c2c21a..5709f4d3 100644 --- a/src/main/deploy/pathplanner/paths/Center to B.path +++ b/src/main/deploy/pathplanner/paths/Red Center to B.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.4053936459807443, + "x": 1.5588456716347956, "y": 5.5301848373414 }, "prevControl": null, "nextControl": { - "x": 2.179287978367624, - "y": 5.533261569685343 + "x": 2.5783978521082047, + "y": 5.532684682370853 }, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA Starting Pose" }, { "anchor": { - "x": 2.9, - "y": 5.55 + "x": 3.148650962480497, + "y": 5.5301848373414 }, "prevControl": { - "x": 2.363026588032343, - "y": 5.555664027064661 + "x": 2.146405401441756, + "y": 5.54350131951752 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Red BCA B" } ], "rotationTargets": [], @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -179.47943637878444, + "rotation": -0.14529343403600373, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Red To Notes", "previewStartingState": { - "rotation": 180.0, + "rotation": 0.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Red Center to C.path b/src/main/deploy/pathplanner/paths/Red Center to C.path new file mode 100644 index 00000000..682b66e1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Center to C.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.95, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.5243146663748686, + "y": 4.364968609959541 + }, + "isLocked": false, + "linkedName": "Red BCA B Shoot Pose" + }, + { + "anchor": { + "x": 2.6557969001354675, + "y": 4.098014005179796 + }, + "prevControl": { + "x": 2.0502575156936027, + "y": 5.261018831189449 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red BCA C" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -32.73734304607968, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E Shoot to D.path b/src/main/deploy/pathplanner/paths/Red D Shoot to E.path similarity index 60% rename from src/main/deploy/pathplanner/paths/E Shoot to D.path rename to src/main/deploy/pathplanner/paths/Red D Shoot to E.path index c4506a08..c4804438 100644 --- a/src/main/deploy/pathplanner/paths/E Shoot to D.path +++ b/src/main/deploy/pathplanner/paths/Red D Shoot to E.path @@ -3,35 +3,35 @@ "waypoints": [ { "anchor": { - "x": 5.33, - "y": 6.79 + "x": 4.844074705820543, + "y": 6.341528771227186 }, "prevControl": null, "nextControl": { - "x": 6.421717289119103, - "y": 7.099702924294243 + "x": 6.678528291952418, + "y": 6.478106842932436 }, "isLocked": false, - "linkedName": null + "linkedName": "Red Midline Shoot Pose" }, { "anchor": { - "x": 8.29, - "y": 7.45 + "x": 8.398526797053595, + "y": 5.762294601512281 }, "prevControl": { - "x": 7.3720253966575395, - "y": 7.247706580388155 + "x": 5.501437800680661, + "y": 6.388737383129529 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Red E" } ], "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 35.0358419299735, + "rotationDegrees": -20.607589471852908, "rotateFast": true } ], @@ -39,19 +39,19 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -0.9439960906702931, + "rotation": 0, "rotateFast": false }, "reversed": false, - "folder": "ABCDE", + "folder": "Red To Notes", "previewStartingState": { - "rotation": 25.626758135321538, + "rotation": 9.121601501824317, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Red D to Shoot.path b/src/main/deploy/pathplanner/paths/Red D to Shoot.path new file mode 100644 index 00000000..205b4b09 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red D to Shoot.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.583540511947096, + "y": 8.029356831593127 + }, + "prevControl": null, + "nextControl": { + "x": 5.792092859297326, + "y": 6.64574068173434 + }, + "isLocked": false, + "linkedName": "Red D" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 6.9995180085226245, + "y": 7.030476446555571 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 12.244059995551996, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Shoots", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red E Shoot to F.path b/src/main/deploy/pathplanner/paths/Red E Shoot to F.path new file mode 100644 index 00000000..cc97361d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red E Shoot to F.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": null, + "nextControl": { + "x": 5.985494331479735, + "y": 6.6556919879092735 + }, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + }, + { + "anchor": { + "x": 9.32365277676984, + "y": 4.877773358597312 + }, + "prevControl": { + "x": 7.457228766578071, + "y": 5.583772188191697 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red F" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -26.70800563120147, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red E to Shoot.path b/src/main/deploy/pathplanner/paths/Red E to Shoot.path new file mode 100644 index 00000000..f791ec24 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red E to Shoot.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.398526797053595, + "y": 5.762294601512281 + }, + "prevControl": null, + "nextControl": { + "x": 5.564270444017079, + "y": 6.2968680782971695 + }, + "isLocked": false, + "linkedName": "Red E" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 6.214855112931953, + "y": 6.6096371506359075 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 11.086538893198085, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Shoots", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red F to Shoot.path b/src/main/deploy/pathplanner/paths/Red F to Shoot.path new file mode 100644 index 00000000..8271d6a1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red F to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.32365277676984, + "y": 4.877773358597312 + }, + "prevControl": null, + "nextControl": { + "x": 6.064864411164835, + "y": 6.154521133447029 + }, + "isLocked": false, + "linkedName": "Red F" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 6.927118650554267, + "y": 6.5047398072846745 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 13.775816001890945, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Shoots", + "previewStartingState": { + "rotation": -35.112332977419605, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G Shoot to F.path b/src/main/deploy/pathplanner/paths/Red G Shoot to F.path new file mode 100644 index 00000000..9fa76c14 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red G Shoot to F.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": null, + "nextControl": { + "x": 6.737563093801783, + "y": 6.744051894911826 + }, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + }, + { + "anchor": { + "x": 9.32365277676984, + "y": 4.877773358597312 + }, + "prevControl": { + "x": 8.698835741235472, + "y": 4.399629922771751 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red F" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Notes", + "previewStartingState": { + "rotation": 4.900031264073487, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G to Shoot.path b/src/main/deploy/pathplanner/paths/Red G to Shoot.path new file mode 100644 index 00000000..025c558d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red G to Shoot.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.17904635660462, + "y": 3.0775484197917797 + }, + "prevControl": null, + "nextControl": { + "x": 7.379638674827945, + "y": 5.380722948945898 + }, + "isLocked": false, + "linkedName": "Red G" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 6.933704780728014, + "y": 6.492384803966036 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 10.292841181604183, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Shoots", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red H Shoot to G.path b/src/main/deploy/pathplanner/paths/Red H Shoot to G.path new file mode 100644 index 00000000..343945d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red H Shoot to G.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": null, + "nextControl": { + "x": 6.3702300963009675, + "y": 6.766887017777012 + }, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + }, + { + "anchor": { + "x": 9.17904635660462, + "y": 3.0775484197917797 + }, + "prevControl": { + "x": 7.9253260003915855, + "y": 4.7072550832229645 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red G" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -45.01939380931683, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Notes", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red H to Shoot.path b/src/main/deploy/pathplanner/paths/Red H to Shoot.path new file mode 100644 index 00000000..a5748f47 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red H to Shoot.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.08448491097131, + "y": 1.3743847716490074 + }, + "prevControl": null, + "nextControl": { + "x": 7.278971838449629, + "y": 4.65908499246314 + }, + "isLocked": false, + "linkedName": "Red H" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 7.026968229903722, + "y": 6.600647323318456 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 9.740747376516406, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Shoots", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path new file mode 100644 index 00000000..617d74ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path @@ -0,0 +1,161 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.583540511947096, + "y": 8.029356831593127 + }, + "prevControl": null, + "nextControl": { + "x": 10.583540511947096, + "y": 7.529356831593127 + }, + "isLocked": false, + "linkedName": "Red D" + }, + { + "anchor": { + "x": 8.398526797053595, + "y": 5.762294601512281 + }, + "prevControl": { + "x": 8.48940488246149, + "y": 5.716858400979277 + }, + "nextControl": { + "x": 8.307648711645701, + "y": 5.807730802045285 + }, + "isLocked": false, + "linkedName": "Red E" + }, + { + "anchor": { + "x": 9.32365277676984, + "y": 4.877773358597312 + }, + "prevControl": { + "x": 9.414462650075102, + "y": 4.832377516891699 + }, + "nextControl": { + "x": 9.23284290346458, + "y": 4.923169200302925 + }, + "isLocked": false, + "linkedName": "Red F" + }, + { + "anchor": { + "x": 9.17904635660462, + "y": 3.0775484197917797 + }, + "prevControl": { + "x": 9.269637951181455, + "y": 3.0322817263338186 + }, + "nextControl": { + "x": 9.088454762027784, + "y": 3.122815113249741 + }, + "isLocked": false, + "linkedName": "Red G" + }, + { + "anchor": { + "x": 9.08448491097131, + "y": 1.3743847716490074 + }, + "prevControl": { + "x": 9.174378013617185, + "y": 1.3295313525835315 + }, + "nextControl": { + "x": 8.994591808325437, + "y": 1.4192381907144824 + }, + "isLocked": false, + "linkedName": "Red H" + }, + { + "anchor": { + "x": 1.3824623752298102, + "y": 6.555650112788321 + }, + "prevControl": { + "x": 1.4701203036966073, + "y": 6.512119171778799 + }, + "nextControl": { + "x": 1.2948044467630135, + "y": 6.599181053797842 + }, + "isLocked": false, + "linkedName": "Red ADEF Starting Pose" + }, + { + "anchor": { + "x": 4.844074705820543, + "y": 6.341528771227186 + }, + "prevControl": { + "x": 4.924580076914293, + "y": 6.302229759996718 + }, + "nextControl": { + "x": 4.763569334726793, + "y": 6.380827782457655 + }, + "isLocked": false, + "linkedName": "Red Midline Shoot Pose" + }, + { + "anchor": { + "x": 3.001400475457201, + "y": 7.146094279167726 + }, + "prevControl": { + "x": 3.9890657714661404, + "y": 7.669002598379403 + }, + "nextControl": { + "x": 2.013735179448261, + "y": 6.623185959956049 + }, + "isLocked": false, + "linkedName": "Red ADEF A" + }, + { + "anchor": { + "x": 0.7495689172925966, + "y": 4.3183368853716875 + }, + "prevControl": { + "x": 0.8071861047925966, + "y": 4.2925800494341875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red HGF Starting Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Utils", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Source to H.path b/src/main/deploy/pathplanner/paths/Red Source to H.path new file mode 100644 index 00000000..aabb5e95 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Source to H.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7495689172925966, + "y": 4.3183368853716875 + }, + "prevControl": null, + "nextControl": { + "x": 1.7495689172925961, + "y": 3.8183368853716875 + }, + "isLocked": false, + "linkedName": "Red HGF Starting Pose" + }, + { + "anchor": { + "x": 4.368911854453174, + "y": 2.305913562719982 + }, + "prevControl": { + "x": 3.0634548695792625, + "y": 3.070445327974576 + }, + "nextControl": { + "x": 5.75651814157051, + "y": 1.4932716323765978 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.08448491097131, + "y": 1.3743847716490074 + }, + "prevControl": { + "x": 7.065187014488613, + "y": 1.1927133415278737 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red H" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Red To Notes", + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source to H.path b/src/main/deploy/pathplanner/paths/Source to H.path deleted file mode 100644 index e6a11a6c..00000000 --- a/src/main/deploy/pathplanner/paths/Source to H.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7905759905641836, - "y": 4.310885424226206 - }, - "prevControl": null, - "nextControl": { - "x": 3.1209603713850287, - "y": 3.2519606845389286 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.29, - "y": 0.77 - }, - "prevControl": { - "x": 6.869285697276752, - "y": 1.038924798273703 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8, - "rotationDegrees": 149.51442160606817, - "rotateFast": true - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 176.99598612588744, - "rotateFast": false - }, - "reversed": false, - "folder": "HGF", - "previewStartingState": { - "rotation": 117.79678461990976, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path deleted file mode 100644 index 4ee5c7f1..00000000 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.5, - "y": 5.552154629234854 - }, - "prevControl": null, - "nextControl": { - "x": 2.9, - "y": 5.552154629234854 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.15, - "y": 5.55 - }, - "prevControl": { - "x": 2.9000000000000004, - "y": 5.55 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.9338907055017344, - "rotateFast": false - }, - "reversed": false, - "folder": "Tests", - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index cf181adb..2b6a9ea2 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -4,8 +4,9 @@ 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.RobotType; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.util.PixelFormat; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; @@ -14,15 +15,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; public class Robot extends TimedRobot { - - public static final RobotType ROBOT; - - static { - if (Robot.isSimulation()) - ROBOT = RobotType.SIM; - else - ROBOT = RobotType.fromString(System.getenv("serialnum")); - } private RobotContainer robot; private CommandScheduler scheduler; @@ -42,10 +34,9 @@ public void robotInit() { robot = new RobotContainer(); - SmartDashboard.putString("Robot State", "DISABLED"); - SmartDashboard.putString("Robot", ROBOT.name()); + if (Robot.isReal()) CameraServer.startAutomaticCapture().setVideoMode(PixelFormat.kMJPEG, 80, 60, 30); - SmartDashboard.putData(CommandScheduler.getInstance()); + SmartDashboard.putString("Robot State", "DISABLED"); } @Override @@ -63,7 +54,9 @@ public static boolean isBlue() { /*********************/ @Override - public void disabledInit() {} + public void disabledInit() { + SmartDashboard.putString("Robot State", "DISABLED"); + } @Override public void disabledPeriodic() { @@ -86,7 +79,6 @@ public void autonomousInit() { scheduler.schedule(new LEDReset()); SmartDashboard.putString("Robot State", "AUTON"); - } @Override @@ -104,6 +96,7 @@ public void teleopInit() { if (auto != null) { auto.cancel(); } + SmartDashboard.putString("Robot State", "TELEOP"); } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 1dcb3f7a..cf302b42 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -1,5 +1,7 @@ package com.stuypulse.robot; +import java.util.concurrent.atomic.AtomicBoolean; + import com.ctre.phoenix6.Utils; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.BuzzController; @@ -17,8 +19,11 @@ import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; +import com.stuypulse.robot.commands.auton.BCA.AltFourPieceBCA; import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; -import com.stuypulse.robot.commands.auton.tests.StraightLine; +import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF; +import com.stuypulse.robot.commands.auton.SideAutons.OnePieceAmpSide; +import com.stuypulse.robot.commands.auton.SideAutons.OnePieceSourceSide; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.intake.IntakeStop; @@ -35,7 +40,6 @@ 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; import com.stuypulse.robot.constants.LEDInstructions; import com.stuypulse.robot.constants.Ports; @@ -46,7 +50,6 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.swerve.Telemetry; import com.stuypulse.robot.subsystems.vision.AprilTagVision; -import com.stuypulse.robot.subsystems.vision.NoteVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; import com.stuypulse.robot.util.ShooterLobFerryInterpolation; import com.stuypulse.robot.util.ShooterSpeeds; @@ -60,6 +63,7 @@ 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.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -76,7 +80,6 @@ public class RobotContainer { // Subsystem public final AprilTagVision vision = AprilTagVision.getInstance(); - public final NoteVision noteVision = NoteVision.getInstance(); public final Intake intake = Intake.getInstance(); public final Shooter shooter = Shooter.getInstance(); @@ -85,7 +88,7 @@ public class RobotContainer { public final LEDController leds = LEDController.getInstance(); - private final Telemetry logger = new Telemetry(); + // private final Telemetry logger = new Telemetry(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -98,9 +101,11 @@ public RobotContainer() { configureAutons(); if (Utils.isSimulation()) { - swerve.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); + swerve.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(0))); } - swerve.registerTelemetry(logger::telemeterize); + // swerve.registerTelemetry(logger::telemeterize); + + LiveWindow.disableAllTelemetry(); new Trigger(() -> Intake.getInstance().getState() == Intake.State.ACQUIRING && Intake.getInstance().hasNote()) .onTrue(new BuzzController(driver, 1)); @@ -261,34 +266,43 @@ private void configureOperatorBindings() { public void configureAutons() { autonChooser.addOption("Do Nothing", new DoNothingAuton()); - autonChooser.addOption("Mobility", new Mobility()); - autonChooser.addOption("Straight Line", new StraightLine()); - + + + // Mobility + AutonConfig MOBILITY_BLUE = new AutonConfig("Mobility", Mobility::new, "Mobility"); + AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, "Mobility"); + // BCA 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"); + "Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center"); + AutonConfig BCA_RED = new AutonConfig("4 BCA", FourPieceBCA::new, + "Red Center to B", "Red B to Center", "Red Center to C", "Red C to Shoot Before A", "Red Center to A", "Red A to Center"); + + // HGF + AutonConfig HGF_BLUE = new AutonConfig("4 HGF", FourPieceHGF::new, + "Blue Source to H", "Blue H to Shoot", "Blue H Shoot to G", "Blue G to Shoot", "Blue G Shoot to F", "Blue F to Shoot"); + AutonConfig HGF_RED = new AutonConfig("4 HGF", FourPieceHGF::new, + "Red Source to H", "Red H to Shoot", "Red H Shoot to G", "Red G to Shoot", "Red G Shoot to F", "Red F to Shoot"); + + // ADEF 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"); + "Blue Amp to A", "Blue A to D", "Blue D to Shoot", "Blue D Shoot to E", "Blue E to Shoot", "Blue E Shoot to F", "Blue F to Shoot"); + AutonConfig ADEF_RED = new AutonConfig("5 ADEF", FivePieceADEF::new, + "Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F", "Red F to Shoot"); + + MOBILITY_BLUE.registerBlue(autonChooser); + MOBILITY_RED.registerRed(autonChooser); BCA_BLUE.registerDefaultBlue(autonChooser); - BCA_RED.registerRed(autonChooser); + BCA_RED.registerDefaultRed(autonChooser); - // HGF.registerBlue(autonChooser); - // HGF_RED.registerRed(autonChooser); + HGF_BLUE.registerBlue(autonChooser); + HGF_RED.registerRed(autonChooser); ADEF_BLUE.registerBlue(autonChooser); ADEF_RED.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); - } public Command getAutonomousCommand() { @@ -304,4 +318,4 @@ public static String getAutonomousCommandNameStatic() { } -} +} \ 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 dd391809..0207f46b 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 @@ -2,6 +2,7 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.FollowPathThenShoot; import com.stuypulse.robot.commands.auton.ShootRoutine; import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.subsystems.shooter.Shooter; @@ -15,37 +16,32 @@ public class FivePieceADEF extends SequentialCommandGroup { public FivePieceADEF(PathPlannerPath... paths) { addCommands( - ShootRoutine.fromSubwoofer(), + // Preload Shot + ShootRoutine.fromAnywhere(), new ArmToFeed(), + // Drive to A + Shoot A new IntakeSetAcquire(), SwerveDrive.getInstance().followPathCommand(paths[0]), - new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote()), - ShootRoutine.fromAnywhere().withTimeout(2.5).onlyIf(() -> Shooter.getInstance().hasNote()), + ShootRoutine.fromAnywhere(), new ArmToFeed(), - // Drive, Intake, Shoot D + // Drive to D + 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 FollowPathThenShoot(paths[2], false), new ArmToFeed(), - // Drive, Intake, Shoot E + // Drive to E + 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()), + SwerveDrive.getInstance().followPathCommand(paths[3]), + new FollowPathThenShoot(paths[4], false), new ArmToFeed(), - // Drive, Intake, Shoot F + // Drive to F + 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()), + SwerveDrive.getInstance().followPathCommand(paths[5]), + new FollowPathThenShoot(paths[6], true), new ArmToFeed() ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/AltFourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/AltFourPieceBCA.java new file mode 100644 index 00000000..d976f278 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/AltFourPieceBCA.java @@ -0,0 +1,50 @@ +package com.stuypulse.robot.commands.auton.BCA; + +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.IntakeSetAcquire; +import com.stuypulse.robot.commands.shooter.ShooterFeederStop; +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 AltFourPieceBCA extends SequentialCommandGroup { + + public AltFourPieceBCA(PathPlannerPath... paths) { + addCommands( + // Preload Shot + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + // Drive to B + Shoot B + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + new WaitCommand(0).until(() -> Shooter.getInstance().hasNote()), + ShootRoutine.fromAnywhere().withTimeout(1), + new ShooterFeederStop(), + new ArmToFeed(), + + // Drive to C + Drive to Shoot + Shoot C + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new WaitCommand(0).until(() -> Shooter.getInstance().hasNote()), + SwerveDrive.getInstance().followPathCommand(paths[2]), + ShootRoutine.fromAnywhere().withTimeout(1), + new ShooterFeederStop(), + new ArmToFeed(), + + // Drive to A + Drive to Shoot + Shoot A + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[3]), + new WaitCommand(0).until(() -> Shooter.getInstance().hasNote()), + SwerveDrive.getInstance().followPathCommand(paths[4]), + ShootRoutine.fromAnywhere().withTimeout(1), + new ShooterFeederStop(), + new ArmToFeed() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java index 8ff7324c..576303e3 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BCA/FourPieceBCA.java @@ -3,9 +3,13 @@ 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.arm.ArmToSubwooferShot; +import com.stuypulse.robot.commands.auton.FollowPathThenShoot; 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.commands.shooter.ShooterFeederStop; +import com.stuypulse.robot.constants.Settings.Alignment.Shoot; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -24,22 +28,19 @@ public FourPieceBCA(PathPlannerPath... paths) { // 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 FollowPathThenShoot(paths[1], false), 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()), + SwerveDrive.getInstance().followPathCommand(paths[2]), + new FollowPathThenShoot(paths[3], false), 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()), + SwerveDrive.getInstance().followPathCommand(paths[4]), + new FollowPathThenShoot(paths[5], true), 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 ac77b0c5..9db1f028 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,51 +1,53 @@ -// package com.stuypulse.robot.commands.auton.BF_Series; - -// 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.WaitCommand; - -// public class FivePieceBFAC extends SequentialCommandGroup { +package com.stuypulse.robot.commands.auton.BF_Series; + +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.IntakeSetAcquire; +import com.stuypulse.robot.commands.shooter.ShooterFeederStop; +import com.stuypulse.robot.subsystems.intake.Intake; +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; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class FivePieceBFAC extends SequentialCommandGroup { -// 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() -// ); - -// } - -// } + public FivePieceBFAC(PathPlannerPath... paths) { + + addCommands( + // Preload Shot + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote() || Intake.getInstance().hasNote()), + new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).onlyIf(() -> Intake.getInstance().hasNote()).withTimeout(1.0), + ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), + + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + SwerveDrive.getInstance().followPathCommand(paths[1]), + ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), + + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[2]), + SwerveDrive.getInstance().followPathCommand(paths[3]), + ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed(), + + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[2]), + SwerveDrive.getInstance().followPathCommand(paths[3]), + ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), + new ArmToFeed() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathThenShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathThenShoot.java new file mode 100644 index 00000000..1e1cb728 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathThenShoot.java @@ -0,0 +1,34 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPlannerTrajectory; +import com.stuypulse.robot.commands.arm.ArmToSpeaker; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +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.WaitUntilCommand; + +public class FollowPathThenShoot extends SequentialCommandGroup{ + + private final double totalPathTime; + + public FollowPathThenShoot(PathPlannerPath path, boolean isLastShot) { + totalPathTime = path.getTrajectory(new ChassisSpeeds(), path.getPathPoses().get(0).getRotation()).getTotalTimeSeconds(); + addCommands( + new ParallelCommandGroup( + SwerveDrive.getInstance().followPathCommand(path), + new WaitCommand(totalPathTime > 1.5 ? totalPathTime - 1.0 : 0) + // wait for handoff + .andThen(new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).onlyIf(() -> Intake.getInstance().hasNote()).alongWith(new WaitCommand(0.75))) + .andThen(new ArmToSpeaker().onlyIf(() -> Shooter.getInstance().hasNote())) + ), + isLastShot ? ShootRoutine.fromAnywhereLastShot() + : ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()) + ); + } +} 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 9d1ec0a7..94b56d9e 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,44 @@ -// package com.stuypulse.robot.commands.auton.HGF; - -// import com.pathplanner.lib.path.PathPlannerPath; -// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -// import com.stuypulse.robot.commands.auton.ShootRoutine; -// import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; -// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -// import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; -// import com.stuypulse.robot.constants.Settings.Auton; - -// import 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 { +package com.stuypulse.robot.commands.auton.HGF; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.FollowPathThenShoot; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FourPieceHGF extends SequentialCommandGroup { -// public FourPieceHGF(PathPlannerPath... paths) { -// addCommands( -// new ParallelCommandGroup( -// new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) -// .andThen(new ShooterScoreSpeaker()), - -// SwerveDriveToPose.speakerRelative(-15) -// .withTolerance(0.03, 0.03, 3) -// ), - -// new ShootRoutine(0.7), - -// new FollowPathAndIntake(paths[0]), -// new SwerveDriveToShoot() -// .withTolerance(0.03, 3), -// new ShootRoutine(), - -// new FollowPathAndIntake(paths[1]), -// new SwerveDriveToShoot() -// .withTolerance(0.03, 3), -// new ShootRoutine(), - -// new FollowPathAndIntake(paths[2]), -// new SwerveDriveToShoot() -// .withTolerance(0.03, 3), -// new ShootRoutine(0.7) -// ); -// } - -// } \ No newline at end of file + public FourPieceHGF(PathPlannerPath... paths) { + addCommands( + // Preload Shot + ShootRoutine.fromAnywhere(), + new ArmToFeed(), + + // Drive to H + Shoot H + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + new FollowPathThenShoot(paths[1], false), + new ArmToFeed(), + + // Drive to G + Shoot G + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[2]), + new FollowPathThenShoot(paths[3], false), + new ArmToFeed(), + + // Drive to F + Shoot F + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[4]), + new FollowPathThenShoot(paths[5], true), + new ArmToFeed() + ); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java index d17c1a58..06c03b1b 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java @@ -1,12 +1,15 @@ package com.stuypulse.robot.commands.auton; +import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; public class Mobility extends SequentialCommandGroup { - - public Mobility() { + + public Mobility(PathPlannerPath... paths) { addCommands( - SwerveDrive.getInstance().followPathCommand("Mobility")); + SwerveDrive.getInstance().followPathCommand(paths[0]) + ); } -} \ No newline at end of file +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java index eda3aeb3..f7751eaf 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/ShootRoutine.java @@ -5,39 +5,76 @@ 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.ShooterFeederStop; 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.intake.Intake; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; 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 ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) + .alongWith(new ShooterWaitForTarget().withTimeout(1.0)), new ShooterFeederShoot(), - new WaitCommand(0.25) // give time for note to leave shooter + new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()), + new ShooterFeederStop() ); } 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)); + return new SequentialCommandGroup( + new WaitUntilCommand(() -> Shooter.getInstance().hasNote()) + .onlyIf(() -> Intake.getInstance().hasNote()) + .withTimeout(2.0), + new ArmToSpeaker(), + new ParallelCommandGroup( + new SwerveDriveAlignToSpeaker(), + new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET), + new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET) + ), + new ShooterFeederShoot(), + new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()), + new ShooterFeederStop() + ); + } + + public static Command fromAnywhereLastShot() { + return new SequentialCommandGroup( + new ArmToSpeaker(), + new ParallelCommandGroup( + new SwerveDriveAlignToSpeaker(), + new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET), + new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET) + ), + new ShooterFeederShoot(), + new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()) + .alongWith(new WaitCommand(2.5)), + new ShooterFeederStop() + ); } + // public static Command fromLastShot() { + // return new SequentialCommandGroup( + // new ArmToSpeaker().onlyIf(() -> Shooter.getInstance().hasNote()), + // new ParallelCommandGroup( + // new SwerveDriveAlignToSpeaker(), + // new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET), + // new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET) + // ), + // new ShooterFeederShoot(), + // new WaitUntilCommand(() -> !Shooter.getInstance().hasNote()) + // ); + // } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java new file mode 100644 index 00000000..5f8102bb --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceAmpSide.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.commands.auton.SideAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class OnePieceAmpSide extends SequentialCommandGroup { + + public OnePieceAmpSide(PathPlannerPath... paths) { + addCommands( + // Preload Shot + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + // Mobility + SwerveDrive.getInstance().followPathCommand(paths[0]) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java new file mode 100644 index 00000000..405651c3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/SideAutons/OnePieceSourceSide.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.commands.auton.SideAutons; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class OnePieceSourceSide extends SequentialCommandGroup { + + public OnePieceSourceSide(PathPlannerPath... paths) { + addCommands( + // Preload Shot + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + // Mobility + SwerveDrive.getInstance().followPathCommand(paths[0]) + ); + } + +} 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 deleted file mode 100644 index 49835197..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/tests/SquareTest.java +++ /dev/null @@ -1,18 +0,0 @@ -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 deleted file mode 100644 index 84685bac..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java +++ /dev/null @@ -1,15 +0,0 @@ -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/swerve/SwerveDriveAlignToSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java index 09a45dc6..2c33f898 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignToSpeaker.java @@ -11,6 +11,7 @@ import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; 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.util.AngleVelocity; @@ -78,20 +79,18 @@ public boolean isFinished() { @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()); + swerve.drive(new Vector2D(new Translation2d()), SLMath.clamp(angleVelocity.get() + + controller.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(swerve.getPose().getRotation())), + -Settings.Swerve.MAX_ANGULAR_VELOCITY, + Settings.Swerve.MAX_ANGULAR_VELOCITY + ) + ); + } + + @Override + public void end(boolean interrupted) { + swerve.drive(new Vector2D(new Translation2d()), 0); } } 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 a59491aa..c22f55e3 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -2,24 +2,19 @@ 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; import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VMotionProfile; 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.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveDrive extends Command { @@ -28,8 +23,6 @@ public class SwerveDriveDrive extends Command { private final Gamepad driver; - private final SwerveRequest.FieldCentric drive; - private final VStream speed; private final IStream turn; @@ -38,36 +31,27 @@ public SwerveDriveDrive(Gamepad driver) { speed = VStream.create(driver::getLeftStick) .filtered( + new VDeadZone(Drive.DEADBAND), x -> x.clamp(1), x -> x.pow(Drive.POWER.get()), x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), - new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), - new VLowPassFilter(Drive.RC.get())); + new VRateLimit(Drive.MAX_TELEOP_ACCEL), + new VLowPassFilter(Drive.RC)); turn = IStream.create(driver::getRightX) .filtered( - x -> SLMath.clamp(x, -1, 1), + x -> SLMath.deadband(x, Turn.DEADBAND.get()), x -> SLMath.spow(x, Turn.POWER.get()), x -> x * Turn.MAX_TELEOP_TURN_SPEED.get(), - new RateLimit(Turn.MAX_TELEOP_TURN_ACCEL.get()), - new LowPassFilter(Turn.RC.get())); + new LowPassFilter(Turn.RC)); this.driver = driver; - 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); - addRequirements(swerve); } @Override public void execute() { - Vector2D velocity = Robot.isBlue() ? speed.get() : speed.get().mul(-1); - swerve.setControl(drive.withVelocityX(velocity.y) - .withVelocityY(-velocity.x) - .withRotationalRate(turn.get()) - ); + swerve.drive(speed.get(), turn.get()); } -} \ No newline at end of file +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java deleted file mode 100644 index ed7fc495..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class SwerveDriveStop extends InstantCommand { - - public SwerveDriveStop() { - super(() -> SwerveDrive.getInstance().stop()); - - addRequirements(SwerveDrive.getInstance()); - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java index 436bb9fd..c1693346 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -47,10 +47,6 @@ public static SwerveDriveToPose speakerRelative(double angleToSpeaker, double di ); } - public static SwerveDriveToPose speakerRelative(double angleToSpeaker) { - return speakerRelative(angleToSpeaker, Alignment.PODIUM_SHOT_DISTANCE.get()); - } - private final SwerveDrive swerve; private final HolonomicController controller; @@ -174,7 +170,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - swerve.stop(); Field.clearFieldObject(targetPose2d); SmartDashboard.putBoolean("AutonAlignment", false); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java deleted file mode 100644 index 639b0ad6..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java +++ /dev/null @@ -1,151 +0,0 @@ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings.Alignment; -import com.stuypulse.robot.constants.Settings.Alignment.Shoot; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.control.Controller; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.control.feedback.PIDController; -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.BDebounceRC; -import com.stuypulse.stuylib.streams.numbers.IStream; -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; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class SwerveDriveToShoot extends Command { - - private final SwerveDrive swerve; - - private final Controller distanceController; - private final AnglePIDController angleController; - private final IStream velocityError; - - private final IStream distanceToSpeaker; - private final BStream isAligned; - - private final Number targetDistance; - - private double distanceTolerance; - private double angleTolerance; - private double velocityTolerance; - - - public SwerveDriveToShoot() { - this(Alignment.PODIUM_SHOT_DISTANCE); - } - - public SwerveDriveToShoot(Number targetDistance) { - this(targetDistance, Alignment.DEBOUNCE_TIME); - } - - public SwerveDriveToShoot(Number targetDistance, double debounce) { - this.targetDistance = targetDistance; - - swerve = SwerveDrive.getInstance(); - - distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) - .setOutputFilter( - x -> -x, - x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) - ); - - angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); - - velocityError = IStream.create(distanceController::getError) - .filtered(new Derivative()) - .filtered(new LowPassFilter(0.05)) - .filtered(x -> Math.abs(x)); - - distanceToSpeaker = IStream.create(() -> getTranslationToSpeaker().getNorm()) - .filtered(new LowPassFilter(0.05)); - - isAligned = BStream.create(this::isAligned) - .filtered(new BDebounceRC.Rising(debounce)); - - distanceTolerance = 0.05; - angleTolerance = Alignment.ANGLE_TOLERANCE.get(); - velocityTolerance = 0.1; - - addRequirements(swerve); - - } - - private double getTargetDistance() { - return SLMath.clamp(targetDistance.doubleValue(), 1, 5); - } - - public SwerveDriveToShoot withTolerance(double distanceTolerance, double angleTolerance) { - this.distanceTolerance = distanceTolerance; - this.angleTolerance = angleTolerance; - return this; - } - - public SwerveDriveToShoot withRotationConstants(double p, double i, double d) { - angleController.setPID(p, i, d); - return this; - } - - private boolean isAligned() { - return distanceController.isDone(distanceTolerance) - && angleController.isDoneDegrees(angleTolerance) - && velocityError.get() < velocityTolerance; - } - - private Translation2d getTranslationToSpeaker() { - return Field.getAllianceSpeakerPose().getTranslation().minus(swerve.getPose().getTranslation()); - } - - @Override - public void initialize() { - SmartDashboard.putBoolean("AutonAlignment", true); - } - - @Override - public void execute() { - Rotation2d toSpeaker = getTranslationToSpeaker().getAngle(); - - double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); - double rotation = angleController.update( - Angle.fromRotation2d(toSpeaker).add(Angle.k180deg), - Angle.fromRotation2d(swerve.getPose().getRotation())); - - Translation2d speeds = new Translation2d( - speed, - toSpeaker); - - if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) - rotation = 0; - - swerve.setFieldRelativeSpeeds( - new ChassisSpeeds( - speeds.getX(), - speeds.getY(), - rotation)); - - SmartDashboard.putNumber("Alignment/Velocity Error", velocityError.get()); - SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.plus(Rotation2d.fromDegrees(180)).getDegrees()); - } - - @Override - public boolean isFinished() { - return isAligned.get(); - } - - @Override - public void end(boolean interrupted) { - swerve.stop(); - SmartDashboard.putBoolean("AutonAlignment", false); - } - -} 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 ff4caf58..d69e511e 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 @@ -1,9 +1,5 @@ package com.stuypulse.robot.commands.swerve.driveAligned; -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; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -11,11 +7,8 @@ import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; 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; import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; @@ -23,25 +16,20 @@ import com.stuypulse.stuylib.util.AngleVelocity; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public abstract class SwerveDriveDriveAligned extends Command { private final SwerveDrive swerve; - protected final Gamepad driver; - private final VStream speed; - - private final SwerveRequest.FieldCentric drive; + private final VStream drive; private final AngleController controller; private final IStream angleVelocity; public SwerveDriveDriveAligned(Gamepad driver) { swerve = SwerveDrive.getInstance(); - this.driver = driver; - speed = VStream.create(driver::getLeftStick) + drive = VStream.create(driver::getLeftStick) .filtered( new VDeadZone(Drive.DEADBAND), x -> x.clamp(1), @@ -49,27 +37,18 @@ public SwerveDriveDriveAligned(Gamepad driver) { x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), new VLowPassFilter(Drive.RC.get())); - - 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) + controller = 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 - 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 - ); + .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); addRequirements(swerve); } @@ -78,27 +57,12 @@ public SwerveDriveDriveAligned(Gamepad driver) { protected abstract double getDistanceToTarget(); - protected double getAngleError() { - return controller.getError().getRotation2d().getDegrees(); - } - @Override public void execute() { - Vector2D velocity = Robot.isBlue() ? speed.get() : speed.get().mul(-1); - swerve.setControl( - drive.withVelocityX(velocity.y) - .withVelocityY(-velocity.x) - .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()); + swerve.drive( + drive.get(), + angleVelocity.get() + controller.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(swerve.getPose().getRotation()))); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java index d6a93797..8d52c8c7 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedAmp.java @@ -12,7 +12,6 @@ public SwerveDriveDriveAlignedAmp(Gamepad driver) { super(driver); } - @Override protected Rotation2d getTargetAngle() { return Rotation2d.fromDegrees(Robot.isBlue() ? 90 : 270); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java deleted file mode 100644 index a193189f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/noteAlignment/SwerveDriveDriveToNote.java +++ /dev/null @@ -1,123 +0,0 @@ -package com.stuypulse.robot.commands.swerve.noteAlignment; - -import static com.stuypulse.robot.constants.Settings.Alignment.*; -import static com.stuypulse.robot.constants.Settings.NoteDetection.*; - -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.NoteDetection; -import com.stuypulse.robot.constants.Settings.Driver.Drive; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.control.feedback.PIDController; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.math.SLMath; -import com.stuypulse.stuylib.math.Vector2D; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; -import com.stuypulse.stuylib.streams.vectors.VStream; -import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; -import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; -import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.subsystems.vision.NoteVision; -import com.stuypulse.robot.util.HolonomicController; - -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.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class SwerveDriveDriveToNote extends Command { - - private final SwerveDrive swerve; - private final NoteVision vision; - private final Intake intake; - - private final SwerveRequest.FieldCentric drive; - - private final VStream speed; - - private final HolonomicController controller; - private final BStream aligned; - - public SwerveDriveDriveToNote(Gamepad driver) { - this.swerve = SwerveDrive.getInstance(); - this.vision = NoteVision.getInstance(); - this.intake = Intake.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); - - speed = VStream.create(driver::getLeftStick) - .filtered( - new VDeadZone(Drive.DEADBAND), - x -> x.clamp(1), - x -> x.pow(Drive.POWER.get()), - x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), - new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()), - new VLowPassFilter(Drive.RC.get())); - - controller = new HolonomicController( - new PIDController(NoteDetection.Translation.kP, NoteDetection.Translation.kI, NoteDetection.Translation.kD), - new PIDController(NoteDetection.Translation.kP, NoteDetection.Translation.kI, NoteDetection.Translation.kD), - new AnglePIDController(NoteDetection.Rotation.kP, NoteDetection.Rotation.kI, NoteDetection.Rotation.kD)); - - SmartDashboard.putData("Note Detection/Controller", controller); - - aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); - - addRequirements(swerve); - } - - private boolean isAligned() { - return controller.isDone(THRESHOLD_X.get(), THRESHOLD_Y.get(), THRESHOLD_ANGLE.get()); - } - - @Override - public void execute() { - Translation2d targetTranslation = swerve.getPose() - .getTranslation().plus( - new Translation2d(Settings.CENTER_TO_FRONT_OF_INTAKE, 0) - .rotateBy(swerve.getPose().getRotation())); - - Rotation2d targetRotation = vision.getEstimatedNotePose().minus(targetTranslation).getAngle(); - - Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); - - if (vision.hasNoteData()) { - ChassisSpeeds speeds = controller.update(targetPose, swerve.getPose()); - - if (vision.withinIntakePath()) - speeds.omegaRadiansPerSecond = Math.signum(speeds.omegaRadiansPerSecond) * Math.toRadians(5.0); - - Vector2D velocity = new Vector2D(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - double angularVelocity = speeds.omegaRadiansPerSecond; - - velocity = velocity.clamp(Settings.Swerve.MAX_LINEAR_VELOCITY); - angularVelocity = SLMath.clamp(angularVelocity, -Settings.Swerve.MAX_ANGULAR_VELOCITY, Settings.Swerve.MAX_ANGULAR_VELOCITY); - // drive to note - swerve.setControl(drive.withVelocityX(velocity.x) - .withVelocityY(velocity.y) - .withRotationalRate(angularVelocity) - ); - } else { - swerve.setControl(drive.withVelocityX(speed.get().y) - .withVelocityY(-speed.get().x) - .withRotationalRate(0) - ); - } - - SmartDashboard.putBoolean("Note Detection/Is Aligned", aligned.get()); - } - - @Override - public boolean isFinished() { - return aligned.get() || intake.hasNote(); - } -} diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 1e388bbb..016ee054 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -39,20 +39,20 @@ public static void disableStatusFrames(CANSparkBase motor, StatusFrame... ids) { /** Classes to store all of the values a motor needs */ public interface Arm { - CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25, false); - CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, false); + CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.35, true); + CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.35, true); } public interface Intake { - CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kCoast, 500, 0.25, true); - CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true); - CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true); + CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kCoast, 60, 0.35, false); + CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 60, 0.35, false); + CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 60, 0.25, false); } public interface Shooter { - CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5, true); - CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5, true); - CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, true); + CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5, false); + CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5, false); + CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, false); } /* Configurations */ diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7d0ad555..6f1725bb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -2,6 +2,7 @@ import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.util.PIDConstants; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.util.ShooterSpeeds; import com.stuypulse.stuylib.network.SmartBoolean; @@ -32,29 +33,6 @@ public interface Settings { double DISTANCE_FROM_TOWER_TO_CENTER_OF_ROBOT = Units.inchesToMeters(Units.metersToInches(LENGTH) / 2 - 14.875); double ANGLE_BETWEEN_ARM_AND_SHOOTER = 84; // shooter is tilted up - // checks the current RIO's serial number to determine which robot is running - public enum RobotType { - // TODO: Add serial numbers from RIOs - SELF_REINFORCED_VELVEETA_CHEESE_POLYPROPYLENE_GOOBER(""), - SIM(""); - - public final String serialNum; - - RobotType(String serialNum) { - this.serialNum = serialNum; - } - - public static RobotType fromString(String serialNum) { - for (RobotType robot : RobotType.values()) { - if (robot.serialNum.equals(serialNum.toUpperCase())) { - return robot; - } - } - - return RobotType.SIM; - } - } - public interface Arm { double LENGTH = Units.inchesToMeters(16.5); @@ -107,11 +85,10 @@ public interface Intake { double INTAKE_FEED_SPEED = 0.4; - double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 25; + double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 20; double ARM_SPEED_THRESHOLD_TO_FEED = 1.75; // degrees per second double INTAKE_SHOOT_SPEED = 0.9; - double INTAKE_SHOOT_TIME = 0.75; double FUNNEL_ACQUIRE = 1.0; double FUNNEL_DEACQUIRE = 1.0; @@ -127,7 +104,7 @@ public interface Shooter { double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; - double TARGET_RPM_THRESHOLD = 200; + double TARGET_RPM_THRESHOLD = 250; double MAX_WAIT_TO_REACH_TARGET = 2.0; ShooterSpeeds SPEAKER = new ShooterSpeeds( @@ -138,9 +115,8 @@ public interface Shooter { // 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 + SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.0); + SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.0); // left runs faster than right public interface LEFT { @@ -218,8 +194,8 @@ public interface Motion { MAX_ANGULAR_VELOCITY.get(), MAX_ANGULAR_ACCELERATION.get()); - PIDConstants XY = new PIDConstants(2.5, 0, 0.02); - PIDConstants THETA = new PIDConstants(4, 0, 0.1); + PIDConstants XY = new PIDConstants(1.0, 0, 0.02); + PIDConstants THETA = new PIDConstants(2.0, 0, 0.02); } public interface Encoder { @@ -239,13 +215,13 @@ 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.2); + SmartNumber kP = new SmartNumber("Swerve/Turn/PID/kP", Robot.isReal() ? 9.0 : 9.0); + SmartNumber kI = new SmartNumber("Swerve/Turn/PID/kI", Robot.isReal() ? 0.0 : 0.0); + SmartNumber kD = new SmartNumber("Swerve/Turn/PID/kD", Robot.isReal() ? 0.2 : 0.0); - SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", 0.30718); - SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 1.42659); - SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", 0.0036513); + SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", Robot.isReal() ? 0.30718 : Simulation.TURN_FRICTION_VOLTAGE); + SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", Robot.isReal() ? 1.42659 : 0.0); + SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", Robot.isReal() ? 0.0036513 : 0.0); boolean INVERTED = true; @@ -253,13 +229,13 @@ public interface Turn { } public interface Drive { - SmartNumber kP = new SmartNumber("Swerve/Drive/PID/kP", 9.0); - SmartNumber kI = new SmartNumber("Swerve/Drive/PID/kI", 0); - SmartNumber kD = new SmartNumber("Swerve/Drive/PID/kD", 0); + SmartNumber kP = new SmartNumber("Swerve/Turn/PID/kP", Robot.isReal() ? 9.0 : 1.0); + SmartNumber kI = new SmartNumber("Swerve/Turn/PID/kI", Robot.isReal() ? 0.0 : 0.0); + SmartNumber kD = new SmartNumber("Swerve/Turn/PID/kD", Robot.isReal() ? 0.0 : 0.1); - SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", 0.31007); - SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", 1.62153); - SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", 0.0048373); + SmartNumber kS = new SmartNumber("Swerve/Drive/FF/kS", Robot.isReal() ? 0.31007 : Simulation.DRIVE_FRICTION_VOLTAGE); + SmartNumber kV = new SmartNumber("Swerve/Drive/FF/kV", Robot.isReal() ? 1.62153 : 0.25); + SmartNumber kA = new SmartNumber("Swerve/Drive/FF/kA", Robot.isReal() ? 0.0048373 : 0.01); double L2 = ((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)); // 6.74607175 double L3 = ((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 6.12244898 @@ -296,7 +272,7 @@ public interface BackRight { public interface Simulation { double TURN_INERTIA = 0.00001; - double DRIVE_INERTIA = 0.001; + double DRIVE_INERTIA = 0.00001; // Simulated voltage necessary to overcome friction double TURN_FRICTION_VOLTAGE = 0.25; double DRIVE_FRICTION_VOLTAGE = 0.25; @@ -306,8 +282,6 @@ public interface Simulation { public interface Alignment { double DEBOUNCE_TIME = 0.05; - SmartNumber PODIUM_SHOT_DISTANCE = new SmartNumber("Shooter/Podium Distance", 2.85); - 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", 6); @@ -347,22 +321,20 @@ public interface Rotation { } public interface LED { - int LED_LENGTH = 61; - SmartNumber BLINK_TIME = new SmartNumber("LED/LED Blink Time", .15); + int LED_LENGTH = 106; + double BLINK_TIME = .15; - SmartNumber TRANSLATION_SPREAD = new SmartNumber("LED/LED Translation Spread (m)", 0.5); - SmartNumber ROTATION_SPREAD = new SmartNumber("LED/LED Rotation Spread (deg)", 15); + double TRANSLATION_SPREAD = 0.5; + double ROTATION_SPREAD = 15; SmartBoolean LED_AUTON_TOGGLE = new SmartBoolean("LED/Auton Align Display", false); } public interface Driver { - double TIME_UNTIL_HOLD = 0.7; - public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.015); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); - SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.01); + SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_LINEAR_VELOCITY); @@ -371,7 +343,6 @@ public interface Drive { public interface Turn { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.03); - SmartNumber DISABLE_ALIGNMENT_DEADBAND = new SmartNumber("Driver Settings/Turn/Disable Alignment Deadband", 0.08); SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); @@ -381,51 +352,13 @@ public interface Turn { } } - public interface NoteDetection { - double X_ANGLE_RC = 0.05; - - SmartNumber HAS_NOTE_DEBOUNCE = new SmartNumber("Note Detection/Has Note Debounce", 0.2); - - SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.2); - SmartNumber THRESHOLD_Y = new SmartNumber("Note Detection/Y Threshold", Units.inchesToMeters(2)); - SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 1); - - SmartNumber DRIVE_SPEED = new SmartNumber("Note Detection/Drive Speed", 1); - - SmartNumber INTAKE_THRESHOLD_DISTANCE = new SmartNumber("Note Detection/In Intake Path Distance", 0.9); - - double MAX_FULLY_IN_VIEW_ANGLE = 20; - - public interface Translation { - SmartNumber kP = new SmartNumber("Note Detection/Translation/kP", 8.0); - SmartNumber kI = new SmartNumber("Note Detection/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("Note Detection/Translation/kD", 0.0); - } - - public interface Rotation { - SmartNumber kP = new SmartNumber("Note Detection/Rotation/kP", 2.0); - SmartNumber kI = new SmartNumber("Note Detection/Rotation/kI", 0.0); - SmartNumber kD = new SmartNumber("Note Detection/Rotation/kD", 0.0); - } - } - public interface Vision { SmartBoolean IS_ACTIVE = new SmartBoolean("Vision/Is Active", true); - double POSE_AMBIGUITY_RATIO_THRESHOLD = 0.25; + double POSE_AMBIGUITY_RATIO_THRESHOLD = 0.50; } public interface Buzz { double BUZZ_DURATION = 1.0; double BUZZ_INTENSITY = 1.0; } - - public interface Auton { - double MAX_SHOT_DISTANCE = 3.1; - - SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Shoot Wait Delay", 0.45); - - double SHOOTER_STARTUP_DELAY = 0.5; - double DEFAULT_INTAKE_TIMEOUT = 0.75; - double SHOOTER_START_PRE = 1.0; - } } 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 8be89ff1..ad11229a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -1,5 +1,7 @@ package com.stuypulse.robot.subsystems.arm; +import com.stuypulse.robot.Robot; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -33,11 +35,18 @@ public enum State { protected State state; protected Arm() { - state = State.RESETTING; + if (Robot.isReal()) { + state = State.RESETTING; + } + else { + state = State.FEED; + } } public void setState(State state) { - this.state = state; + if (state != State.RESETTING) { + this.state = state; + } } public State getState() { @@ -53,6 +62,6 @@ public State getState() { @Override public void periodic() { - SmartDashboard.putString("Arm/State", state.toString()); + // SmartDashboard.putString("Arm/State", state.toString()); } } \ No newline at end of file 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 91666188..8358e7de 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -286,22 +286,17 @@ else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTrigger SmartDashboard.putNumber("Arm/Setpoint (deg)", controller.getSetpoint()); SmartDashboard.putNumber("Arm/Error (deg)", controller.getError()); - SmartDashboard.putNumber("Arm/Output (V)", controller.getOutput()); SmartDashboard.putBoolean("Arm/Bump Switch Triggered?", !bumpSwitch.get()); - SmartDashboard.putNumber("Arm/Encoder Angle (deg))", getDegrees()); SmartDashboard.putNumber("Arm/Raw Encoder Angle (rot)", armEncoder.getPosition()); - SmartDashboard.putNumber("Arm/Left Bus Voltage (V)", leftMotor.getBusVoltage()); - SmartDashboard.putNumber("Arm/Right Bus Voltage (V)", rightMotor.getBusVoltage()); + SmartDashboard.putNumber("Arm/Left Voltage (V)", leftMotor.getBusVoltage()); + SmartDashboard.putNumber("Arm/Right Voltage (V)", rightMotor.getBusVoltage()); SmartDashboard.putNumber("Arm/Left Current (amps)", leftMotor.getOutputCurrent()); SmartDashboard.putNumber("Arm/Right Current (amps)", rightMotor.getOutputCurrent()); - SmartDashboard.putNumber("Arm/Left Duty Cycle", leftMotor.get()); - SmartDashboard.putNumber("Arm/Right Duty Cycle", rightMotor.get()); - SmartDashboard.putNumber("Arm/Arm Angle", getDegrees()); 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 124ef191..e423cbd8 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -78,6 +78,6 @@ && getState() != Intake.State.DEACQUIRING setState(State.STOP); } - SmartDashboard.putString("Intake/State", state.name()); + // SmartDashboard.putString("Intake/State", state.name()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 01948ed0..6582df43 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -98,12 +98,10 @@ public void periodic() { break; } - SmartDashboard.putNumber("Intake/Intake Speed", intakeMotor.get()); - SmartDashboard.putNumber("Intake/Funnel/Right Funnel Speed", funnelMotorLeft.get()); - SmartDashboard.putNumber("Intake/Funnel/Left Funnel Speed", funnelMotorRight.get()); - SmartDashboard.putBoolean("Intake/Has Note", hasNote()); - SmartDashboard.putBoolean("Intake/IR Sensor", !IRSensor.get()); + + SmartDashboard.putNumber("Intake/Left Funnel Current", funnelMotorLeft.getOutputCurrent()); + SmartDashboard.putNumber("Intake/Left Funnel Current", funnelMotorRight.getOutputCurrent()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java index ad027032..c6db1587 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/LEDController.java @@ -4,7 +4,6 @@ import com.stuypulse.robot.constants.LEDInstructions; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.RobotType; import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction; import edu.wpi.first.wpilibj.AddressableLED; diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAlign.java b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAlign.java index 16b1a49a..44f2528b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAlign.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAlign.java @@ -70,18 +70,18 @@ public void setLED(AddressableLEDBuffer ledsBuffer) { if (!isXAligned.get()) { ledController.runLEDInstruction(LEDInstructions.RED); - index = linearInterp(pose.getX(), startPose.getX(), LED.TRANSLATION_SPREAD.get()); + index = linearInterp(pose.getX(), startPose.getX(), LED.TRANSLATION_SPREAD); } if (!isYAligned.get() && isXAligned()) { ledController.runLEDInstruction(LEDInstructions.GREEN); - index = linearInterp(pose.getY(), startPose.getY(), LED.TRANSLATION_SPREAD.get()); + index = linearInterp(pose.getY(), startPose.getY(), LED.TRANSLATION_SPREAD); } if (!isThetaAligned.get() && isXAligned() && isYAligned()) { ledController.runLEDInstruction(LEDInstructions.DARK_BLUE); index = linearInterp( pose.getRotation().getDegrees(), startPose.getRotation().getDegrees(), - LED.ROTATION_SPREAD.get()); + LED.ROTATION_SPREAD); } ledsBuffer.setLED(index, Color.kWhite); diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDPulseColor.java b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDPulseColor.java index 29326a87..280ad130 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDPulseColor.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDPulseColor.java @@ -15,7 +15,7 @@ public class LEDPulseColor implements LEDInstruction { private final double pulseTime; public LEDPulseColor(SLColor color1, SLColor color2) { - this(color1, color2, Settings.LED.BLINK_TIME.get()); + this(color1, color2, Settings.LED.BLINK_TIME); } public LEDPulseColor(SLColor color1, SLColor color2, double pulseTime) { @@ -30,7 +30,7 @@ public LEDPulseColor(SLColor color, double pulseTime) { } public LEDPulseColor(SLColor color) { - this(color, SLColor.BLACK, Settings.LED.BLINK_TIME.get()); + this(color, SLColor.BLACK, Settings.LED.BLINK_TIME); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java index 0f83dd96..1452be69 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java +++ b/src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java @@ -44,7 +44,7 @@ public LEDSection(boolean isBlinking, SLColor... sections) { public void setLED(AddressableLEDBuffer ledBuffer) { if (isBlinking){ double time = stopWatch.getTime(); - double pulseTime = Settings.LED.BLINK_TIME.get(); + double pulseTime = Settings.LED.BLINK_TIME; if (time < pulseTime) { setColorSections(ledBuffer, this.sections, this.separatorIndexes); 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 e1f39f73..d48779cc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -62,6 +62,8 @@ public FlywheelState getFlywheelState() { @Override public void periodic() { + // SmartDashboard.putString("Shooter/Feeder State", getFeederState().toString()); + // 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; 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 0fd72cbc..be7ec9db 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -226,8 +226,6 @@ public void periodic () { SmartDashboard.putNumber("Shooter/Left Current", leftMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Right Current", rightMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Feeder Current", feederMotor.getOutputCurrent()); - - SmartDashboard.putString("Shooter/Feeder State", getFeederState().toString()); } } \ No newline at end of file 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 f3f07f66..c4e97eb3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -4,9 +4,12 @@ import java.util.function.Supplier; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.MountPoseConfigs; +import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.SteerRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -19,20 +22,21 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; +import com.stuypulse.robot.Robot; 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 com.stuypulse.stuylib.math.Vector2D; + 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.geometry.Twist2d; 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; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -69,9 +73,13 @@ public static SwerveDrive getInstance() { private final Field2d field; private final FieldObject2d[] modules2D; + private final Translation2d[] moduleOffsets; + private Notifier m_simNotifier = null; private double m_lastSimTime; + private SwerveRequest.ApplyChassisSpeeds drive = new SwerveRequest.ApplyChassisSpeeds(); + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private final Rotation2d BlueAlliancePerspectiveRotation = Rotation2d.fromDegrees(0); /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ @@ -85,13 +93,94 @@ protected SwerveDrive(SwerveDrivetrainConstants driveTrainConstants, double Upda startSimThread(); } modules2D = new FieldObject2d[Modules.length]; + + moduleOffsets = new Translation2d[] { + Settings.Swerve.FrontLeft.MODULE_OFFSET, + Settings.Swerve.FrontRight.MODULE_OFFSET, + Settings.Swerve.BackLeft.MODULE_OFFSET, + Settings.Swerve.BackRight.MODULE_OFFSET, + }; + field = new Field2d(); + initFieldObject(); + SmartDashboard.putData("Field", field); configureAutoBuilder(); } - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); + /*** PATH FOLLOWING ***/ + + public Command followPathCommand(String pathName) { + return followPathCommand(PathPlannerPath.fromPathFile(pathName)); + } + + public Command followPathCommand(PathPlannerPath path) { + return new FollowPathHolonomic( + path, + this::getPose, + this::getChassisSpeeds, + this::setChassisSpeeds, + new HolonomicPathFollowerConfig( + Motion.XY, + Motion.THETA, + 4.9, + Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH), + new ReplanningConfig(false, false) + ), + () -> false, + this + ); + } + + public Command followPathWithSpeakerAlignCommand(PathPlannerPath path) { + return new FollowPathPointSpeakerCommand( + path, + this::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 ChassisSpeeds getChassisSpeeds() { + return m_kinematics.toChassisSpeeds(m_moduleStates); + } + + public void setChassisSpeeds(ChassisSpeeds robotSpeeds) { + SmartDashboard.putNumber("Swerve/Chassis Target X", robotSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Swerve/Chassis Target Y", robotSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber("Swerve/Chassis Target Omega", robotSpeeds.omegaRadiansPerSecond); + + ChassisSpeeds speeds = new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, -robotSpeeds.omegaRadiansPerSecond); + setControl(drive.withSpeeds(speeds)); + } + + public void drive(Vector2D velocity, double rotation) { + ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + Robot.isBlue() ? velocity.y : -velocity.y, + Robot.isBlue() ? -velocity.x : velocity.x, + -rotation, + getPose().getRotation()); + + Pose2d robotVel = new Pose2d( + Settings.DT * speeds.vxMetersPerSecond, + Settings.DT * speeds.vyMetersPerSecond, + Rotation2d.fromRadians(Settings.DT * speeds.omegaRadiansPerSecond)); + Twist2d twistVel = new Pose2d().log(robotVel); + + setChassisSpeeds(new ChassisSpeeds( + twistVel.dx / Settings.DT, + twistVel.dy / Settings.DT, + twistVel.dtheta / Settings.DT + )); } private void startSimThread() { @@ -104,23 +193,15 @@ private void startSimThread() { m_lastSimTime = currentTime; /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); + updateSimState(Settings.DT, RobotController.getBatteryVoltage()); }); m_simNotifier.startPeriodic(0.005); } - public SwerveDriveKinematics getKinematics() { - return m_kinematics; - } - public Rotation2d getGyroAngle() { return Rotation2d.fromRotations(m_yawGetter.getValueAsDouble()); } - public SwerveModulePosition[] getModulePositions() { - return m_modulePositions; - } - public Pose2d getPose() { return m_odometry.getEstimatedPosition(); } @@ -129,13 +210,6 @@ public Field2d getField() { return field; } - public void stop() { - setControl(new SwerveRequest.FieldCentric() - .withVelocityX(0) - .withVelocityY(0) - .withRotationalRate(0)); - } - public void configureAutoBuilder() { AutoBuilder.configureHolonomic( this::getPose, @@ -146,7 +220,7 @@ public void configureAutoBuilder() { Settings.Swerve.Motion.XY, Settings.Swerve.Motion.THETA, 4.9, - Settings.Swerve.WIDTH, + Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH), new ReplanningConfig(true, true)), () -> false, instance @@ -155,64 +229,6 @@ public void configureAutoBuilder() { 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) - .withVelocityY(chassisSpeeds.vyMetersPerSecond) - .withRotationalRate(chassisSpeeds.omegaRadiansPerSecond) - ); - } - - public ChassisSpeeds getChassisSpeeds() { - return m_kinematics.toChassisSpeeds(m_moduleStates); - } - - public Command followPathCommand(PathPlannerPath path) { - return new FollowPathHolonomic( - path, - () -> getPose(), - () -> m_kinematics.toChassisSpeeds(m_moduleStates), - this::setChassisSpeeds, - new HolonomicPathFollowerConfig( - Motion.XY, - Motion.THETA, - 4.9, - Math.hypot(Settings.Swerve.LENGTH, Settings.Swerve.WIDTH), - new ReplanningConfig(false, false) - ), - () -> false, - this - ); - } - - public void setFieldRelativeSpeeds(ChassisSpeeds chassisSpeeds) { - setChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( - chassisSpeeds, - getPose().getRotation())); - } - public void initFieldObject() { String[] ids = {"Front Left", "Front Right", "Back Left", "Back Right"}; for (int i = 0; i < Modules.length; i++) { @@ -274,16 +290,16 @@ public void setVisionEnabled(boolean enabled) { * *

Should call this periodically */ - private void applyOperatorPerspective() { - if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent((allianceColor) -> { - this.setOperatorPerspectiveForward( - allianceColor == Alliance.Red ? RedAlliancePerspectiveRotation - : BlueAlliancePerspectiveRotation); - hasAppliedOperatorPerspective = true; - }); - } - } + // private void applyOperatorPerspective() { + // if (!hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + // DriverStation.getAlliance().ifPresent((allianceColor) -> { + // this.setOperatorPerspectiveForward( + // allianceColor == Alliance.Red ? RedAlliancePerspectiveRotation + // : BlueAlliancePerspectiveRotation); + // hasAppliedOperatorPerspective = true; + // }); + // } + // } @Override public void periodic() { @@ -291,8 +307,8 @@ public void periodic() { for (int i = 0; i < Modules.length; i++) { SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Target Angle (deg)", Modules[i].getTargetState().angle.getDegrees()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Angle (deg)", Modules[i].getCurrentState().angle.getDegrees()); - SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Target Velocity (m/s)", Modules[i].getTargetState().speedMetersPerSecond); - SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Velocity (m/s)", Modules[i].getCurrentState().speedMetersPerSecond); + SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Target Velocity (meters per s)", Modules[i].getTargetState().speedMetersPerSecond); + SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Velocity (meters per s)", Modules[i].getCurrentState().speedMetersPerSecond); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Angle Error", Modules[i].getTargetState().angle.minus(Modules[i].getCurrentState().angle).getDegrees()); SmartDashboard.putNumber("Swerve/Modules/" + moduleIds[i] + "/Drive Current", Modules[i].getDriveMotor().getSupplyCurrent().getValueAsDouble()); @@ -303,12 +319,16 @@ public void periodic() { field.setRobotPose(getPose()); - applyOperatorPerspective(); - ArrayList outputs = AprilTagVision.getInstance().getOutputs(); if (Settings.Vision.IS_ACTIVE.get() && outputs.size() > 0) { updateEstimatorWithVisionData(outputs); } + for (int i = 0; i < Modules.length; i++) { + modules2D[i].setPose(new Pose2d( + getPose().getTranslation().plus(moduleOffsets[i].rotateBy(getPose().getRotation())), + getModule(i).getCurrentState().angle.plus(getPose().getRotation()) + )); + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java index 16fac0cd..f26655d3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/Telemetry.java @@ -41,30 +41,32 @@ public Telemetry() {} private double lastTime = Utils.getCurrentTimeSeconds(); /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - }; + // private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + // new Mechanism2d(1, 1), + // new Mechanism2d(1, 1), + // new Mechanism2d(1, 1), + // new Mechanism2d(1, 1), + // }; + /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; + // private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + // m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + // m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + // m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + // m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + // }; + /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; + // private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + // m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + // m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + // m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + // m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + // .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + // }; /* Accept the swerve drive state and telemeterize it to smartdashboard */ public void telemeterize(SwerveDriveState state) { @@ -92,12 +94,12 @@ public void telemeterize(SwerveDriveState state) { odomPeriod.set(state.OdometryPeriod); /* Telemeterize the module's states */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * Settings.Swerve.MAX_LINEAR_VELOCITY)); + // for (int i = 0; i < 4; ++i) { + // m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + // m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + // m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * Settings.Swerve.MAX_LINEAR_VELOCITY)); - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } + // SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + // } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java deleted file mode 100644 index 9920ca15..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LLNoteVision.java +++ /dev/null @@ -1,157 +0,0 @@ -package com.stuypulse.robot.subsystems.vision; - -import static com.stuypulse.robot.constants.Cameras.Limelight.*; - -import com.stuypulse.robot.constants.Settings.NoteDetection; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.util.vision.Limelight; -import com.stuypulse.stuylib.math.Vector2D; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; -import com.stuypulse.stuylib.streams.vectors.VStream; -import com.stuypulse.stuylib.streams.vectors.filters.VTimedMovingAverage; - -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.net.PortForwarder; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class LLNoteVision extends NoteVision { - - private final Limelight[] limelights; - - private final FieldObject2d note; - - private final VStream notePose; - private final BStream noteData; - - private Vector2D notePoseRaw; - - protected LLNoteVision() { - String[] hostNames = LIMELIGHTS; - - limelights = new Limelight[hostNames.length]; - - if (hostNames.length != POSITIONS.length) { - throw new IllegalArgumentException("Number of Limelights and Positions must be the same"); - } - - for (int i = 0; i < hostNames.length; i++) { - limelights[i] = new Limelight(hostNames[i], POSITIONS[i]); - - for (int port : PORTS) { - PortForwarder.add(port + i * 10, hostNames[i] + ".local", port); - } - } - - note = SwerveDrive.getInstance().getField().getObject("Note"); - - notePose = VStream.create(() -> notePoseRaw) - .filtered(new VTimedMovingAverage(0.5)); - - noteData = BStream.create(this::hasNoteDataRaw) - .filtered(new BDebounceRC.Both(NoteDetection.HAS_NOTE_DEBOUNCE)); - - notePoseRaw = Vector2D.kOrigin; - } - - /** - * Get whether the Limelight has data. - * - * @return whether the Limelight has data - */ - @Override - public boolean hasNoteData() { - return noteData.get(); - } - - private boolean hasNoteDataRaw() { - for (Limelight limelight : limelights) { - if (limelight.hasNoteData()) { - return true; - } - } - return false; - } - - /** - * Get the estimated pose of the note. - * - * @return the estimated pose of the note - */ - @Override - public Translation2d getEstimatedNotePose() { - return notePose.get().getTranslation2d(); - } - - @Override - public Translation2d getRobotRelativeNotePose() { - Translation2d sum = new Translation2d(); - - for (Limelight limelight : limelights) { - sum = sum.plus(new Translation2d( - limelight.getDistanceToNote(), - Rotation2d.fromDegrees(limelight.getXAngle()))); - } - - return sum.div(limelights.length); - } - - /** - * @Calculates the estimated pose of the note by averaging data from all available limelights. - * Sets the pose to `notePose` that can be accessed with `getEstimatedNotePose()`. - */ - private void updateNotePose() { - Translation2d sum = new Translation2d(); - - for (Limelight limelight : limelights) { - SwerveDrive swerve = SwerveDrive.getInstance(); - - Translation2d limelightToNote = new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle())); - - Translation2d robotToNote = limelightToNote - .minus(limelight.getRobotRelativePose().getTranslation().toTranslation2d()) - .rotateBy(limelight.getRobotRelativePose().getRotation().toRotation2d()); - - Translation2d fieldToNote = robotToNote - .rotateBy(swerve.getPose().getRotation()) - .plus(swerve.getPose().getTranslation()); - - sum = sum.plus(fieldToNote); - } - - notePoseRaw = new Vector2D(sum.div(limelights.length)); - } - - @Override - public void periodic() { - super.periodic(); - - for (int i = 0; i < limelights.length; ++i) { - limelights[i].updateData(); - } - - note.setPose(new Pose2d(getEstimatedNotePose(), new Rotation2d())); - - if (hasNoteDataRaw()) updateNotePose(); - updateTelemetry(); - } - - private void updateTelemetry() { - if (hasNoteDataRaw()) { - SmartDashboard.putNumber("Note Detection/X Angle", limelights[0].getXAngle()); - SmartDashboard.putNumber("Note Detection/Y Angle", limelights[0].getYAngle()); - SmartDashboard.putNumber("Note Detection/Distance", limelights[0].getDistanceToNote()); - SmartDashboard.putNumber("Note Detection/Estimated X", getEstimatedNotePose().getX()); - SmartDashboard.putNumber("Note Detection/Estimated Y", getEstimatedNotePose().getY()); - } else { - SmartDashboard.putNumber("Note Detection/X Angle", 0); - SmartDashboard.putNumber("Note Detection/Y Angle", 0); - SmartDashboard.putNumber("Note Detection/Distance", 0); - SmartDashboard.putNumber("Note Detection/Estimated X", 0); - SmartDashboard.putNumber("Note Detection/Estimated Y", 0); - } - } -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java deleted file mode 100644 index 813a2531..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/NoteVision.java +++ /dev/null @@ -1,47 +0,0 @@ -package com.stuypulse.robot.subsystems.vision; - -import com.stuypulse.robot.constants.Settings; - -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.SubsystemBase; - -public abstract class NoteVision extends SubsystemBase { - - private static final NoteVision instance; - - static { - instance = new LLNoteVision(); - } - - public static NoteVision getInstance() { - return instance; - } - - public final boolean withinIntakePath() { - if (!hasNoteData()) return false; - - Translation2d robotRelative = getRobotRelativeNotePose(); - - return robotRelative.getX() > 0 - && robotRelative.getX() < Settings.NoteDetection.INTAKE_THRESHOLD_DISTANCE.get() - && Math.abs(robotRelative.getY()) < Settings.Swerve.WIDTH / 2.0; - } - - public abstract boolean hasNoteData(); - - public abstract Translation2d getEstimatedNotePose(); - - public abstract Translation2d getRobotRelativeNotePose(); - - public final Rotation2d getRotationToNote() { - return getRobotRelativeNotePose().getAngle(); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Note Detection/Has Note Data", hasNoteData()); - SmartDashboard.putBoolean("Note Detection/Is in Intake Path", withinIntakePath()); - } -} 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 7122aa0f..bfd6de97 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java @@ -126,10 +126,10 @@ private void updateTelemetry(String prefix, VisionData data) { SmartDashboard.putNumber(prefix + "/Pose X", data.getPose().getX()); SmartDashboard.putNumber(prefix + "/Pose Y", data.getPose().getY()); SmartDashboard.putNumber(prefix + "/Pose Z", data.getPose().getZ()); + SmartDashboard.putNumber(prefix + "/Pose Rotation", Units.radiansToDegrees(data.getPose().getRotation().getAngle())); SmartDashboard.putNumber(prefix + "/Distance to Tag", data.getDistanceToPrimaryTag()); - SmartDashboard.putNumber(prefix + "/Pose Rotation", Units.radiansToDegrees(data.getPose().getRotation().getAngle())); SmartDashboard.putNumber(prefix + "/Timestamp", data.getTimestamp()); robot.setPose(data.getPose().toPose2d()); diff --git a/src/main/java/com/stuypulse/robot/util/PathUtil.java b/src/main/java/com/stuypulse/robot/util/PathUtil.java index 1f5eb703..3a1d824d 100644 --- a/src/main/java/com/stuypulse/robot/util/PathUtil.java +++ b/src/main/java/com/stuypulse/robot/util/PathUtil.java @@ -100,11 +100,11 @@ public static PathPlannerPath loadRed(String name) { /*** PATH MIRRORING ***/ public static Translation2d flipFieldTranslation(Translation2d pose) { - return new Translation2d(pose.getX(), Field.WIDTH - pose.getY()); + return new Translation2d(Field.LENGTH-pose.getX(), pose.getY()); } public static Rotation2d flipFieldRotation(Rotation2d rotation) { - return rotation.times(-1); + return Rotation2d.fromDegrees(180).minus(rotation); } public static Pose2d flipFieldPose(Pose2d pose) { diff --git a/src/main/java/com/stuypulse/robot/util/vision/Limelight.java b/src/main/java/com/stuypulse/robot/util/vision/Limelight.java deleted file mode 100644 index 35b3e113..00000000 --- a/src/main/java/com/stuypulse/robot/util/vision/Limelight.java +++ /dev/null @@ -1,132 +0,0 @@ -package com.stuypulse.robot.util.vision; - -import static com.stuypulse.robot.constants.Cameras.Limelight.*; - -import com.stuypulse.stuylib.math.SLMath; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; -import com.stuypulse.stuylib.streams.numbers.IStream; -import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings.NoteDetection; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.IntegerEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; - -/** - * This class handles interactions between the robot code and the Limelight system through the - * NetworkTables. - */ -public class Limelight { - - private final DoubleEntry txEntry; - private final DoubleEntry tyEntry; - private final IntegerEntry tvEntry; - - private int limelightID; - private Pose3d robotRelativePose; - - private double txData; - private double tyData; - - private IStream xAngle; - private BStream noteData; - - public Limelight(String tableName, Pose3d robotRelativePose) { - - this.robotRelativePose = robotRelativePose; - - for (int i = 0; i < LIMELIGHTS.length; i++) { - if (LIMELIGHTS[i].equals(tableName)) { - limelightID = i; - } - } - - NetworkTable limelight = NetworkTableInstance.getDefault().getTable(tableName); - - txEntry = limelight.getDoubleTopic("tx").getEntry(0.0); - tyEntry = limelight.getDoubleTopic("ty").getEntry(0.0); - tvEntry = limelight.getIntegerTopic("tv").getEntry(0); - - xAngle = IStream.create(() -> txData).filtered(new LowPassFilter(NoteDetection.X_ANGLE_RC)); - noteData = BStream.create(() -> tvEntry.get() == 1) - .filtered(new BDebounceRC.Falling(.05)); - } - - /** - * Returnns whether or not there is data from the Limelight. - * - * @return whether or not there is data from the Limelight - */ - public boolean hasNoteData() { - return noteData.get(); - } - - /** Updates the data from the Limelight. */ - public void updateData() { - if (tvEntry.get() == 1) { - txData = txEntry.get(); - tyData = tyEntry.get(); - } - } - - /** - * Returns the position of the note relative to the robot. - * - * @return the position of the note relative to the robot - */ - public Pose3d getRobotRelativePose() { - return robotRelativePose; - } - - private double getRawXAngle() { - return SLMath.clamp(-(xAngle.get() - Units.radiansToDegrees(POSITIONS[limelightID].getRotation().getZ())), 30); - } - - /** - * Returns the x angle of the note relative to the robot. - * - * @return the x angle of the note relative to the robot - */ - public double getXAngle() { - double deg = getRawXAngle(); - - if (Math.abs(deg) < NoteDetection.MAX_FULLY_IN_VIEW_ANGLE) - return deg; - - if (deg < 0) - deg = -Math.pow(deg + NoteDetection.MAX_FULLY_IN_VIEW_ANGLE, 1.3) - NoteDetection.MAX_FULLY_IN_VIEW_ANGLE; - else - deg = +Math.pow(deg - NoteDetection.MAX_FULLY_IN_VIEW_ANGLE, 1.3) + NoteDetection.MAX_FULLY_IN_VIEW_ANGLE; - - return deg; - } - - /** - * Returns the y angle of the note relative to the robot. - * - * @return the y angle of the note relative to the robot - */ - public double getYAngle() { - return tyData - Units.radiansToDegrees(POSITIONS[limelightID].getRotation().getY()); - } - - /** - * Calculates the distance from the robot's center to the note's center. Limelight targets far - * end of note, so half of note length is substracted. - * - * @return distance from robot center to note center. - */ - public double getDistanceToNote() { - Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); - return POSITIONS[limelightID].getZ() / -yRotation.getTan() - + POSITIONS[limelightID].getX() - - Field.NOTE_LENGTH / 2.0; - } -}