diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..71dc1d9 --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,15 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [ + "ABCDE", + "HGF" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ADEF.auto b/src/main/deploy/pathplanner/autos/ADEF.auto new file mode 100644 index 0000000..5788e66 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ADEF.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.435487934219916, + "y": 6.595791855088175 + }, + "rotation": 1.3048503501930353 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Amp to A" + } + }, + { + "type": "path", + "data": { + "pathName": "A to D" + } + }, + { + "type": "path", + "data": { + "pathName": "D to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "D Shoot to E" + } + }, + { + "type": "path", + "data": { + "pathName": "E to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "E Shoot to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFAC.auto b/src/main/deploy/pathplanner/autos/BFAC.auto new file mode 100644 index 0000000..2293e32 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFAC.auto @@ -0,0 +1,49 @@ +{ + "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": "A to C" + } + } + ] + } + }, + "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 new file mode 100644 index 0000000..5652895 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFCA.auto @@ -0,0 +1,49 @@ +{ + "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/BFED.auto b/src/main/deploy/pathplanner/autos/BFED.auto new file mode 100644 index 0000000..47d69b9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFED.auto @@ -0,0 +1,61 @@ +{ + "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 Shoot ALT" + } + }, + { + "type": "path", + "data": { + "pathName": "F Shoot ALT to E" + } + }, + { + "type": "path", + "data": { + "pathName": "E to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "E Shoot to D" + } + }, + { + "type": "path", + "data": { + "pathName": "D to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BFGH.auto b/src/main/deploy/pathplanner/autos/BFGH.auto new file mode 100644 index 0000000..14f2696 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BFGH.auto @@ -0,0 +1,61 @@ +{ + "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 Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "F Shoot to G" + } + }, + { + "type": "path", + "data": { + "pathName": "G to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "G Shoot to H" + } + }, + { + "type": "path", + "data": { + "pathName": "H to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/CBA.auto b/src/main/deploy/pathplanner/autos/CBA.auto new file mode 100644 index 0000000..699f42d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CBA.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4365936349060198, + "y": 5.5179740558513855 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center to C" + } + }, + { + "type": "path", + "data": { + "pathName": "C to B" + } + }, + { + "type": "path", + "data": { + "pathName": "B 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.auto new file mode 100644 index 0000000..dcb5cb0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HGF.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7437038962619593, + "y": 4.320740582515391 + }, + "rotation": -59.682052822906385 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source to H" + } + }, + { + "type": "path", + "data": { + "pathName": "H to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "H Shoot to G" + } + }, + { + "type": "path", + "data": { + "pathName": "G to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "G Shoot to F" + } + }, + { + "type": "path", + "data": { + "pathName": "F to Shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A to C.path b/src/main/deploy/pathplanner/paths/A to C.path new file mode 100644 index 0000000..8ab12e3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A to C.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.49, + "y": 6.82 + }, + "prevControl": null, + "nextControl": { + "x": 1.8063404583877622, + "y": 5.684733839028691 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5041716843106308, + "y": 4.352132867212163 + }, + "prevControl": { + "x": 1.7061923065438394, + "y": 5.606093593139567 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -35.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.01025016685515, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 40.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path new file mode 100644 index 0000000..e37f2b3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5387368492370914, + "y": 6.712803832043677 + }, + "prevControl": null, + "nextControl": { + "x": 3.679387291810298, + "y": 6.898225029708988 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.80980431566518, + "y": 7.431413130125368 + }, + "prevControl": { + "x": 5.426682819511473, + "y": 7.094030199035101 + }, + "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": "ABCDE", + "previewStartingState": { + "rotation": 41.18784481523856, + "velocity": 0 + }, + "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/Amp to A.path new file mode 100644 index 0000000..769f691 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp to A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.435487934219916, + "y": 6.595791855088175 + }, + "prevControl": null, + "nextControl": { + "x": 2.26274434319702, + "y": 6.615980477868694 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5146518038571792, + "y": 6.712803832043677 + }, + "prevControl": { + "x": 1.7508529994738564, + "y": 6.559928693704114 + }, + "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.85599579668576, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "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/B to A.path new file mode 100644 index 0000000..3920a74 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.403841365532434, + "y": 5.547443382833194 + }, + "prevControl": null, + "nextControl": { + "x": 2.1124651977726576, + "y": 6.184182756200334 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.505085089225238, + "y": 6.749916915942463 + }, + "prevControl": { + "x": 1.9565133270891542, + "y": 6.234996913818232 + }, + "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.7991322249468, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": 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 new file mode 100644 index 0000000..d0cd21c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B to F.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.54, + "y": 5.53 + }, + "prevControl": null, + "nextControl": { + "x": 3.5538181530232116, + "y": 5.137757241660317 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": { + "x": 5.998955035484476, + "y": 3.6317929071870045 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": -24.38902882105597, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -19.61556919738271, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to A.path b/src/main/deploy/pathplanner/paths/C to A.path new file mode 100644 index 0000000..3ae2554 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C to A.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.52, + "y": 4.38 + }, + "prevControl": null, + "nextControl": { + "x": 1.9086797539294484, + "y": 5.314909721151288 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.47, + "y": 6.77 + }, + "prevControl": { + "x": 1.9818002210409187, + "y": 5.977080210298814 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 1.0388554318784187, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 38.18757752913914, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path new file mode 100644 index 0000000..073ac45 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.51, + "y": 4.29 + }, + "prevControl": null, + "nextControl": { + "x": 2.0727080470157966, + "y": 4.981565001262443 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.425282344054272, + "y": 5.545520425118232 + }, + "prevControl": { + "x": 1.6559550362404298, + "y": 5.38476116014732 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": -32.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to B.path b/src/main/deploy/pathplanner/paths/Center to B.path new file mode 100644 index 0000000..ea98b40 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center to B.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4053936459807443, + "y": 5.5301848373414 + }, + "prevControl": null, + "nextControl": { + "x": 2.179287978367624, + "y": 5.533261569685343 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.537823444322484, + "y": 5.5301848373414 + }, + "prevControl": { + "x": 2.0945336170806272, + "y": 5.562923192438648 + }, + "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": "ABCDE", + "previewStartingState": null, + "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 new file mode 100644 index 0000000..190fa1d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center to C.path @@ -0,0 +1,49 @@ +{ + "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.509507891969653, + "y": 4.287761857703775 + }, + "prevControl": { + "x": 2.1220319124045988, + "y": 4.703745685393156 + }, + "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": -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 Shoot to E.path b/src/main/deploy/pathplanner/paths/D Shoot to E.path new file mode 100644 index 0000000..a269951 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D Shoot to E.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 6.510461787664648, + "y": 6.264802758400161 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.834129730759462, + "y": 5.769064259482685 + }, + "prevControl": { + "x": 6.47604084456681, + "y": 6.294320159324842 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 16.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D to Shoot.path b/src/main/deploy/pathplanner/paths/D to Shoot.path new file mode 100644 index 0000000..63adf4e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D to Shoot.path @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.7855750484566455, + "y": 7.436124376527027 + }, + "prevControl": null, + "nextControl": { + "x": 6.609157592385126, + "y": 7.043167967474329 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.329525380962965, + "y": 6.789433696984953 + }, + "prevControl": { + "x": 6.628819835020623, + "y": 7.036774133072075 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 16.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 17.438531624411638, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "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/E Shoot to D.path new file mode 100644 index 0000000..71074da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E Shoot to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 6.421717289119103, + "y": 7.099702924294243 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.79, + "y": 7.44 + }, + "prevControl": { + "x": 6.876162419906684, + "y": 7.238618253300495 + }, + "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": 5.550205795548943, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 25.626758135321538, + "velocity": 0 + }, + "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/E Shoot to F.path new file mode 100644 index 0000000..95aa5b2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E Shoot to F.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": null, + "nextControl": { + "x": 6.704247851390048, + "y": 5.800312322350837 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.8917703882654795, + "y": 4.342229634980103 + }, + "prevControl": { + "x": 6.716218263165693, + "y": 5.79459152314882 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -39.907792341252474, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.96161156944142, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 11.872095567230494, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path new file mode 100644 index 0000000..a2fda6b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": null, + "nextControl": { + "x": 6.464262728562661, + "y": 6.586753953827849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.33, + "y": 6.79 + }, + "prevControl": { + "x": 6.487145925370721, + "y": 6.506326247399518 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 16.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 17.14111484001432, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 39.01725127228337, + "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 new file mode 100644 index 0000000..b0d2f33 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot ALT to E.path @@ -0,0 +1,52 @@ +{ + "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": 7.83, + "y": 5.77 + }, + "prevControl": { + "x": 6.77751254033014, + "y": 6.22081510067038 + }, + "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": "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 new file mode 100644 index 0000000..227629f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to A.path @@ -0,0 +1,52 @@ +{ + "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.487778469790571, + "y": 6.823421974596926 + }, + "prevControl": { + "x": 2.094966282566494, + "y": 6.342153732484548 + }, + "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 new file mode 100644 index 0000000..aeef997 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to C.path @@ -0,0 +1,58 @@ +{ + "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.5184015714013572, + "y": 4.376602504135068 + }, + "prevControl": { + "x": 1.7198933802629512, + "y": 5.131796072743931 + }, + "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 Shoot to E.path b/src/main/deploy/pathplanner/paths/F Shoot to E.path new file mode 100644 index 0000000..1ae9e0d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to E.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 5.607152401060755, + "y": 3.1411502462141825 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.83, + "y": 5.77 + }, + "prevControl": { + "x": 7.661789273883607, + "y": 5.857831093630937 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 17.078402120790262, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 7.5783021431903075, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -3.5408335937068656, + "velocity": 0 + }, + "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/F Shoot to G.path new file mode 100644 index 0000000..6dd5ba4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F Shoot to G.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": null, + "nextControl": { + "x": 5.415973831144092, + "y": 4.249032236330842 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": { + "x": 6.730603199234672, + "y": 3.645327872322412 + }, + "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": -38.92997350795309, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -6.680432083088365, + "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 new file mode 100644 index 0000000..2d63eaa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to A Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 4.9685862178644005, + "y": 4.268436132668396 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2530814806793305, + "y": 6.434696072517145 + }, + "prevControl": { + "x": 4.892244796580366, + "y": 4.086860850433009 + }, + "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": -28.880848497857865, + "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 new file mode 100644 index 0000000..03d2921 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to C Shoot.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 5.6385927597004, + "y": 3.2728247757462774 + }, + "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": -27.412130290314096, + "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 new file mode 100644 index 0000000..e119541 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to Shoot ALT.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 7.037832940993264, + "y": 5.5022538765315625 + }, + "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": -27.35687342191869, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F to Shoot.path b/src/main/deploy/pathplanner/paths/F to Shoot.path new file mode 100644 index 0000000..164c2fe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.89, + "y": 4.34 + }, + "prevControl": null, + "nextControl": { + "x": 5.625660869067274, + "y": 4.243149238716632 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.44, + "y": 4.92 + }, + "prevControl": { + "x": 5.723635564644642, + "y": 4.265840139753196 + }, + "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": -8.268297961765734, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -27.729825561933676, + "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 new file mode 100644 index 0000000..a52fbdf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FA Shoot to A.path @@ -0,0 +1,52 @@ +{ + "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.49, + "y": 6.82 + }, + "prevControl": { + "x": 2.395476499468936, + "y": 6.670691058088565 + }, + "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 new file mode 100644 index 0000000..accb863 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/FC Shoot to C.path @@ -0,0 +1,52 @@ +{ + "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.52, + "y": 4.35 + }, + "prevControl": { + "x": 2.368075591798505, + "y": 4.424436077294773 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -28.277746130553208, + "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 new file mode 100644 index 0000000..65685f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G Shoot to F.path @@ -0,0 +1,58 @@ +{ + "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": 7.89, + "y": 4.34 + }, + "prevControl": { + "x": 6.6666540280625215, + "y": 4.085370558203914 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -12.184647650336021, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.573918256423003, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -9.481237644879682, + "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 new file mode 100644 index 0000000..79b34d8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G Shoot to H.path @@ -0,0 +1,58 @@ +{ + "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.018108710138552, + "y": 1.1315632861920477 + }, + "prevControl": { + "x": 7.387955466945161, + "y": 2.4966709680443175 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -49.02018512209823, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -49.24596024004399, + "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 new file mode 100644 index 0000000..2519878 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/G to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.93, + "y": 2.73 + }, + "prevControl": null, + "nextControl": { + "x": 6.794920886590286, + "y": 3.474898154111784 + }, + "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": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -7.757990482222401, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -15.0, + "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 new file mode 100644 index 0000000..3ddeed4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H Shoot to G.path @@ -0,0 +1,58 @@ +{ + "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": 7.929219989764386, + "y": 2.728964260011853 + }, + "prevControl": { + "x": 6.772224406613993, + "y": 3.2354713221331206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -25.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -40.317362875171, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -5.742284345235014, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H to Shoot.path b/src/main/deploy/pathplanner/paths/H to Shoot.path new file mode 100644 index 0000000..5c284ec --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H to Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.03, + "y": 1.13 + }, + "prevControl": null, + "nextControl": { + "x": 7.760720819990748, + "y": 2.1380393542037086 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.439099811049325, + "y": 4.919501541012011 + }, + "prevControl": { + "x": 6.113755611067771, + "y": 4.501017868293177 + }, + "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": -8.797211565554276, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -57.0617176721166, + "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 new file mode 100644 index 0000000..2bcb647 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.897224241249078, + "y": 5.567009477582943 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.15, + "y": 5.55 + }, + "prevControl": { + "x": 2.896214688448722, + "y": 5.56249052695278 + }, + "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/Point Locations.path b/src/main/deploy/pathplanner/paths/Point Locations.path new file mode 100644 index 0000000..9ef4882 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Point Locations.path @@ -0,0 +1,161 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.8606755147290057, + "y": 6.160145784763296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.25, + "y": 5.55 + }, + "prevControl": { + "x": 0.23853011040910355, + "y": 5.685869940164359 + }, + "nextControl": { + "x": 0.26146988959089645, + "y": 5.4141300598356406 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": { + "x": 2.89563193569979, + "y": 5.358936627062242 + }, + "nextControl": { + "x": 2.90436806430021, + "y": 5.741063372937758 + }, + "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.28, + "y": 7.44 + }, + "prevControl": { + "x": 7.180548926469876, + "y": 6.917676610673163 + }, + "nextControl": { + "x": 8.69419010200492, + "y": 7.63677199205437 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 5.77 + }, + "prevControl": { + "x": 8.28838041697661, + "y": 6.311456818590719 + }, + "nextControl": { + "x": 8.269224286337115, + "y": 5.073783611933209 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 4.1 + }, + "prevControl": { + "x": 8.286182106596115, + "y": 4.544857124277373 + }, + "nextControl": { + "x": 8.271362241199185, + "y": 3.4784370294187017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 2.44 + }, + "prevControl": { + "x": 8.274967786914742, + "y": 3.4775236245040944 + }, + "nextControl": { + "x": 8.282900374354867, + "y": 1.842011230029793 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28, + "y": 0.77 + }, + "prevControl": { + "x": 8.266410625083155, + "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/Source to H.path b/src/main/deploy/pathplanner/paths/Source to H.path new file mode 100644 index 0000000..57f2fda --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source to H.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7437038962619593, + "y": 4.320740582515391 + }, + "prevControl": null, + "nextControl": { + "x": 1.5747100727832408, + "y": 3.3032555815855678 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.19535290219204, + "y": 2.5364461465297556 + }, + "prevControl": { + "x": 3.256787052187917, + "y": 3.2931577463451562 + }, + "nextControl": { + "x": 5.156266890844794, + "y": 1.76171652938544 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.03, + "y": 1.13 + }, + "prevControl": { + "x": 6.683160416439879, + "y": 1.5490605600333218 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": 2.3059547563747755, + "rotateFast": false + }, + { + "waypointRelativePos": 1.35, + "rotationDegrees": -23.99917974539861, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.9402672501694, + "rotateFast": false + }, + "reversed": false, + "folder": "HGF", + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ 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 new file mode 100644 index 0000000..dc8315c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/ADEF/FivePieceADEF.java @@ -0,0 +1,34 @@ +package com.stuypulse.robot.commands.auton.ADEF; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceADEF extends SequentialCommandGroup { + + public FivePieceADEF(PathPlannerPath... paths) { + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + } + +} 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 new file mode 100644 index 0000000..9eac536 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFAC extends SequentialCommandGroup { + + public FivePieceBFAC(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java new file mode 100644 index 0000000..46740f5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFCA extends SequentialCommandGroup { + + public FivePieceBFCA(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java new file mode 100644 index 0000000..08c6acc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFED extends SequentialCommandGroup { + + public FivePieceBFED(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java new file mode 100644 index 0000000..6474c31 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class FivePieceBFGH extends SequentialCommandGroup { + + public FivePieceBFGH(PathPlannerPath... paths) { + + addCommands( + new ShooterScoreSpeaker(), + + new ShooterWaitForTarget() + .withTimeout(1.0), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot(), + + new FollowPathAndIntake(paths[1]), + new FollowPathAlignAndShoot(paths[2], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[3]), + new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + + new FollowPathAndIntake(paths[5]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java new file mode 100644 index 0000000..a25756a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBA/FourPieceCBA.java @@ -0,0 +1,46 @@ +package com.stuypulse.robot.commands.auton.CBA; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.UntilNoteShot; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; +import com.stuypulse.robot.constants.Settings.Auton; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FourPieceCBA extends SequentialCommandGroup { + + public FourPieceCBA(PathPlannerPath... path) { + addCommands( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterScoreSpeaker()), + + SwerveDriveToPose.speakerRelative(-15) + .withTolerance(0.03, 0.03, 3) + ), + + new UntilNoteShot(0.7), + + new FollowPathAndIntake(path[0]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + + new FollowPathAndIntake(path[1]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + + new FollowPathAndIntake(path[2]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(0.7) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java new file mode 100644 index 0000000..9fd382d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAlignAndShoot.java @@ -0,0 +1,44 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathAlignAndShoot extends SequentialCommandGroup { + + public double getPathTime(PathPlannerPath path) { + return path.getTrajectory(new ChassisSpeeds(), path.getStartingDifferentialPose().getRotation()) + .getTotalTimeSeconds(); + } + + public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand) { + this(path, alignCommand, false); + } + + public FollowPathAlignAndShoot(PathPlannerPath path, Command alignCommand, boolean noteShot) { + addCommands( + new ParallelCommandGroup( + SwerveDrive.getInstance().followPathCommand(path), + new WaitCommand(getPathTime(path) - Auton.SHOOTER_START_PRE) + .andThen(new ShooterScoreSpeaker()) + ), + alignCommand, + new ShooterWaitForTarget() + .withTimeout(0.5) + ); + + if (noteShot) + addCommands(new UntilNoteShot(0.75)); + else + addCommands(new ShooterScoreSpeaker()); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java new file mode 100644 index 0000000..5149e60 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathAndIntake.java @@ -0,0 +1,31 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.swerve.SwerveDriveStop; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathAndIntake extends SequentialCommandGroup { + + public FollowPathAndIntake(PathPlannerPath path) { + this(path, Auton.DEFAULT_INTAKE_TIMEOUT); + } + + public FollowPathAndIntake(PathPlannerPath path, double intakeTimeout) { + addCommands( + new ParallelRaceGroup( + new IntakeAcquire(), + + SwerveDrive.getInstance().followPathCommand(path) + .andThen(new WaitCommand(intakeTimeout)) + ), + + new SwerveDriveStop() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java new file mode 100644 index 0000000..84370df --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShoot.java @@ -0,0 +1,23 @@ + +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathWithShoot extends ParallelCommandGroup { + + public FollowPathWithShoot(PathPlannerPath path, double shootTime) { + addCommands( + SwerveDrive.getInstance().followPathCommand(path), + new WaitCommand(shootTime) + .andThen(new ShooterWaitForTarget()) + .andThen(new ShooterScoreSpeaker()) + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java new file mode 100644 index 0000000..77c6afd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathWithShootAndIntake.java @@ -0,0 +1,35 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.constants.Settings.Auton; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class FollowPathWithShootAndIntake extends SequentialCommandGroup { + + public FollowPathWithShootAndIntake(PathPlannerPath path, double shootTime) { + this(path, shootTime, Auton.DEFAULT_INTAKE_TIMEOUT); + } + + public FollowPathWithShootAndIntake(PathPlannerPath path, double shootTime, double intakeTimeout) { + addCommands( + new ParallelCommandGroup( + SwerveDrive.getInstance().followPathCommand(path) + .until(() -> Intake.getInstance().hasNote()), + new SequentialCommandGroup( + new WaitCommand(shootTime), + new ShooterWaitForTarget(), + new ShooterScoreSpeaker(), + new IntakeAcquire() + ) + ) + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java new file mode 100644 index 0000000..e14f55a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -0,0 +1,48 @@ +package com.stuypulse.robot.commands.auton.HGF; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; +import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.auton.UntilNoteShot; +import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.shooter.SwerveDriveToShoot; +import com.stuypulse.robot.commands.swerve.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 { + + public FourPieceHGF(PathPlannerPath... paths) { + addCommands( + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterScoreSpeaker()), + + SwerveDriveToPose.speakerRelative(-15) + .withTolerance(0.03, 0.03, 3) + ), + + new UntilNoteShot(0.7), + + new FollowPathAndIntake(paths[0]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + + new FollowPathAndIntake(paths[1]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(), + + new FollowPathAndIntake(paths[2]), + new SwerveDriveToShoot() + .withTolerance(0.03, 3), + new UntilNoteShot(0.7) + ); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java new file mode 100644 index 0000000..d17c1a5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/Mobility.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.auton; + +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class Mobility extends SequentialCommandGroup { + + public Mobility() { + addCommands( + SwerveDrive.getInstance().followPathCommand("Mobility")); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java new file mode 100644 index 0000000..2fa0eb2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/UntilNoteShot.java @@ -0,0 +1,26 @@ +package com.stuypulse.robot.commands.auton; + +import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class UntilNoteShot extends SequentialCommandGroup { + + public UntilNoteShot(double timeout) { + addCommands( + new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), + new WaitUntilCommand(Shooter.getInstance()::hasNote) + .withTimeout(timeout), + new IntakeStop() + ); + } + + public UntilNoteShot() { + this(Settings.Auton.SHOOT_WAIT_DELAY.get()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java new file mode 100644 index 0000000..34b0bb9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/SwerveDriveToShoot.java @@ -0,0 +1,155 @@ +package com.stuypulse.robot.commands.shooter; + +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.math.kinematics.Odometry; +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); + } + + // NEED TO CHANGE SO THAT ALLOWED SHOOT DISTANCE IS ALL THE WAY FROM ALLIANCE WING LINE AND NOT AROUND PDOIUM + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java new file mode 100644 index 0000000..ed7fc49 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveStop.java @@ -0,0 +1,15 @@ +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 new file mode 100644 index 0000000..68bf9bd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -0,0 +1,183 @@ +package com.stuypulse.robot.commands.swerve; + +import java.util.function.Supplier; + +import com.pathplanner.lib.util.PIDConstants; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Alignment.Rotation; +import com.stuypulse.robot.constants.Settings.Alignment.Translation; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Swerve.Motion; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.robot.util.MirrorRotation2d; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +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.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveToPose extends Command { + + public static SwerveDriveToPose speakerRelative(double angleToSpeaker, double distanceToSpeaker) { + MirrorRotation2d angle = MirrorRotation2d.fromBlue( + Rotation2d.fromDegrees(SLMath.clamp( + angleToSpeaker, 80))); + + double distance = SLMath.clamp(distanceToSpeaker, 1, 5); + + return new SwerveDriveToPose(() -> { + return new Pose2d( + Field.getAllianceSpeakerPose().getTranslation() + .plus(new Translation2d(distance, angle.get())), + angle.get()); + } + ); + } + + public static SwerveDriveToPose speakerRelative(double angleToSpeaker) { + return speakerRelative(angleToSpeaker, Alignment.PODIUM_SHOT_DISTANCE.get()); + } + + private final SwerveDrive swerve; + + private final HolonomicController controller; + private final Supplier poseSupplier; + private final BStream isAligned; + private final IStream velocityError; + + private final FieldObject2d targetPose2d; + + private double xTolerance; + private double yTolerance; + private double thetaTolerance; + private double velocityTolerance; + private double maxSpeed; + + private Pose2d targetPose; + + public SwerveDriveToPose(Pose2d targetPose) { + this(() -> targetPose); + } + + public SwerveDriveToPose(Supplier poseSupplier) { + swerve = SwerveDrive.getInstance(); + + this.poseSupplier = poseSupplier; + + targetPose2d = swerve.getField().getObject("Target Pose"); + maxSpeed = Swerve.MAX_MODULE_SPEED; + + controller = new HolonomicController( + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new PIDController(Translation.kP, Translation.kI, Translation.kD), + new AnglePIDController(Rotation.kP, Rotation.kI, Rotation.kD)); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Both(Alignment.DEBOUNCE_TIME)); + + velocityError = IStream.create(() -> { + ChassisSpeeds speeds = controller.getError(); + + return new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond).getNorm(); + }) + .filtered(new LowPassFilter(0.05)) + .filtered(x -> Math.abs(x)); + + xTolerance = Alignment.X_TOLERANCE.get(); + yTolerance = Alignment.Y_TOLERANCE.get(); + thetaTolerance = Alignment.ANGLE_TOLERANCE.get(); + velocityTolerance = 0.15; + + addRequirements(swerve); + } + + public SwerveDriveToPose withTranslationConstants(PIDConstants pid) { + controller.setTranslationConstants(pid.kP, pid.kI, pid.kD); + return this; + } + + public SwerveDriveToPose withRotationConstants(PIDConstants pid) { + controller.setRotationConstants(pid.kP, pid.kI, pid.kD); + return this; + } + + public SwerveDriveToPose withTranslationConstants(double p, double i, double d) { + controller.setTranslationConstants(p, i, d); + return this; + } + + public SwerveDriveToPose withRotationConstants(double p, double i, double d) { + controller.setRotationConstants(p, i, d); + return this; + } + + public SwerveDriveToPose withTolerance(Number x, Number y, Number theta) { + xTolerance = x.doubleValue(); + yTolerance = y.doubleValue(); + thetaTolerance = theta.doubleValue(); + return this; + } + + public SwerveDriveToPose withMaxSpeed(double speed) { + maxSpeed = speed; + return this; + } + + @Override + public void initialize() { + targetPose = poseSupplier.get(); + SmartDashboard.putBoolean("AutonAlignment", true); + } + + private boolean isAligned() { + return controller.isDone(xTolerance, yTolerance, thetaTolerance) + && velocityError.get() < velocityTolerance; + } + + @Override + public void execute() { + targetPose2d.setPose(targetPose); + controller.update(targetPose, swerve.getPose()); + + Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond) + .clamp(maxSpeed); + double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get()); + + SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance()); + + if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + ChassisSpeeds clamped = new ChassisSpeeds( + speed.x, speed.y, rotation); + + swerve.setChassisSpeeds(clamped); + } + + @Override + public boolean isFinished() { + return isAligned.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + Field.clearFieldObject(targetPose2d); + SmartDashboard.putBoolean("AutonAlignment", false); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index d3d6556..f58040c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -169,6 +169,8 @@ public interface Swerve { String CAN_BUS_NAME = "swerve"; + SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); + // The stator current at which the wheels start to slip; double SLIP_CURRENT = 150.0; @@ -290,6 +292,8 @@ 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", 5); @@ -312,6 +316,20 @@ public interface Rotation { SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Alignment/Rotation/Omega Deadband", 0.05); } + + public interface Shoot { + public interface Translation { + SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); + SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); + SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.7); + } + + public interface Rotation { + SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); + SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); + SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); + } + } } public interface Driver { @@ -374,4 +392,14 @@ public interface Buzz { double BUZZ_DURATION = 0.2; double BUZZ_INTENSITY = 1; } + + public interface Auton { + double MAX_SHOT_DISTANCE = 3.1; + + SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/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/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index ba77864..7e5d990 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -6,17 +6,25 @@ import com.ctre.phoenix6.Utils; 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.SwerveModuleConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +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.vision.VisionData; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; @@ -113,6 +121,46 @@ public Field2d getField() { return field; } + public void stop() { + stop(); + } + + public Command followPathCommand(String pathName) { + return followPathCommand(PathPlannerPath.fromPathFile(pathName)); + } + + 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 Command followPathCommand(PathPlannerPath path) { + return new FollowPathHolonomic( + path, + () -> getPose(), + () -> m_kinematics.toChassisSpeeds(m_moduleStates), + this::setChassisSpeeds, + new HolonomicPathFollowerConfig( + Motion.XY, + Motion.THETA, + Settings.Swerve.MAX_MODULE_SPEED, + 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++) { diff --git a/src/main/java/com/stuypulse/robot/util/MirrorRotation2d.java b/src/main/java/com/stuypulse/robot/util/MirrorRotation2d.java new file mode 100644 index 0000000..6b167aa --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/MirrorRotation2d.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.robot.Robot; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class MirrorRotation2d { + + public static MirrorRotation2d fromBlue(Rotation2d blue) { + return new MirrorRotation2d(blue); + } + + private final Rotation2d red; + private final Rotation2d blue; + + private MirrorRotation2d(Rotation2d blue) { + this.blue = blue; + this.red = blue.times(-1); + } + + public Rotation2d get() { + if (Robot.isBlue()){ + return blue; + } + + return red; + } + +}