diff --git a/logs/FRC_20241023_005628.wpilog b/logs/FRC_20241023_005628.wpilog new file mode 100644 index 00000000..21fac318 Binary files /dev/null and b/logs/FRC_20241023_005628.wpilog differ diff --git a/logs/FRC_TBD_25d35e33ade850f9.wpilog b/logs/FRC_TBD_25d35e33ade850f9.wpilog new file mode 100644 index 00000000..536a67e3 Binary files /dev/null and b/logs/FRC_TBD_25d35e33ade850f9.wpilog differ diff --git a/src/main/deploy/pathplanner/autos/ADEF BLUE.auto b/src/main/deploy/pathplanner/autos/ADEF BLUE.auto index a2958c3b..56ad792a 100644 --- a/src/main/deploy/pathplanner/autos/ADEF BLUE.auto +++ b/src/main/deploy/pathplanner/autos/ADEF BLUE.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -50,6 +49,7 @@ ] } }, + "resetOdom": true, "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ADEF RED.auto b/src/main/deploy/pathplanner/autos/ADEF RED.auto index 68c483cc..d5948d65 100644 --- a/src/main/deploy/pathplanner/autos/ADEF RED.auto +++ b/src/main/deploy/pathplanner/autos/ADEF RED.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -50,6 +49,7 @@ ] } }, + "resetOdom": true, "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BCA BLUE.auto b/src/main/deploy/pathplanner/autos/BCA BLUE.auto index 532db4f7..24d9090d 100644 --- a/src/main/deploy/pathplanner/autos/BCA BLUE.auto +++ b/src/main/deploy/pathplanner/autos/BCA BLUE.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -44,6 +43,7 @@ ] } }, + "resetOdom": true, "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BCA RED.auto b/src/main/deploy/pathplanner/autos/BCA RED.auto index 70d34ac6..9f95d67a 100644 --- a/src/main/deploy/pathplanner/autos/BCA RED.auto +++ b/src/main/deploy/pathplanner/autos/BCA RED.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -8,13 +7,13 @@ { "type": "path", "data": { - "pathName": null + "pathName": "Red Center to B" } }, { "type": "path", "data": { - "pathName": null + "pathName": "Red B to Center" } }, { @@ -26,7 +25,7 @@ { "type": "path", "data": { - "pathName": null + "pathName": "Red C to Shoot Before A" } }, { @@ -44,6 +43,7 @@ ] } }, + "resetOdom": true, "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue 90 Deg BCA.auto b/src/main/deploy/pathplanner/autos/Blue 90 Deg BCA.auto index f80012b1..d46db594 100644 --- a/src/main/deploy/pathplanner/autos/Blue 90 Deg BCA.auto +++ b/src/main/deploy/pathplanner/autos/Blue 90 Deg BCA.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -44,6 +43,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue BDEFA.auto b/src/main/deploy/pathplanner/autos/Blue BDEFA.auto new file mode 100644 index 00000000..3da5cc28 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue BDEFA.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue B to D" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue D to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue D Shoot to E" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue E to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue E Shoot to F" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue F to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue F Shoot to A" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Center to A" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue BFCAD.auto b/src/main/deploy/pathplanner/autos/Blue BFCAD.auto new file mode 100644 index 00000000..852f2422 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue BFCAD.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue F to Close C Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue FC Shoot to C" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue C to Shoot Before A" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Center to A" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue A to Center" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue A Shoot to D" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue D to Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue BFGH.auto b/src/main/deploy/pathplanner/autos/Blue BFGH.auto new file mode 100644 index 00000000..776109fc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue BFGH.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue F to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue F Shoot to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue G to F Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue GF Shoot to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue H to F Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue GH.auto b/src/main/deploy/pathplanner/autos/Blue GH.auto index 561daa2c..95a24329 100644 --- a/src/main/deploy/pathplanner/autos/Blue GH.auto +++ b/src/main/deploy/pathplanner/autos/Blue GH.auto @@ -1,12 +1,5 @@ { "version": 1.0, - "startingPose": { - "position": { - "x": 0.8795509328910394, - "y": 4.209895599278378 - }, - "rotation": -59.036243467926475 - }, "command": { "type": "sequential", "data": { @@ -38,6 +31,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue HG.auto b/src/main/deploy/pathplanner/autos/Blue HG.auto index 3a8b5888..3fa035a6 100644 --- a/src/main/deploy/pathplanner/autos/Blue HG.auto +++ b/src/main/deploy/pathplanner/autos/Blue HG.auto @@ -1,12 +1,5 @@ { "version": 1.0, - "startingPose": { - "position": { - "x": 0.8795509328910394, - "y": 4.209895599278378 - }, - "rotation": -59.19855402386314 - }, "command": { "type": "sequential", "data": { @@ -38,6 +31,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HGF BLUE.auto b/src/main/deploy/pathplanner/autos/HGF BLUE.auto index cc7d16c1..0be249e8 100644 --- a/src/main/deploy/pathplanner/autos/HGF BLUE.auto +++ b/src/main/deploy/pathplanner/autos/HGF BLUE.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -44,6 +43,7 @@ ] } }, + "resetOdom": true, "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HGF Red.auto b/src/main/deploy/pathplanner/autos/HGF Red.auto index 858c26e8..d5b36085 100644 --- a/src/main/deploy/pathplanner/autos/HGF Red.auto +++ b/src/main/deploy/pathplanner/autos/HGF Red.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -44,6 +43,7 @@ ] } }, + "resetOdom": true, "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red 90 Deg BCA.auto b/src/main/deploy/pathplanner/autos/Red 90 Deg BCA.auto index 6f038011..ff1a8dc2 100644 --- a/src/main/deploy/pathplanner/autos/Red 90 Deg BCA.auto +++ b/src/main/deploy/pathplanner/autos/Red 90 Deg BCA.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -44,6 +43,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red BDEFA.auto b/src/main/deploy/pathplanner/autos/Red BDEFA.auto new file mode 100644 index 00000000..a604eccc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red BDEFA.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "Red B to D" + } + }, + { + "type": "path", + "data": { + "pathName": "Red D to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Red D Shoot to E" + } + }, + { + "type": "path", + "data": { + "pathName": "Red E to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Red E Shoot to F" + } + }, + { + "type": "path", + "data": { + "pathName": "Red F to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Red F Shoot to A" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Center to A" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red BFCAD.auto b/src/main/deploy/pathplanner/autos/Red BFCAD.auto new file mode 100644 index 00000000..42afa962 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red BFCAD.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "Red B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "Red F to Close C Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Red FC Shoot to C" + } + }, + { + "type": "path", + "data": { + "pathName": "Red C to Shoot Before A" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Center to A" + } + }, + { + "type": "path", + "data": { + "pathName": "Red A to D" + } + }, + { + "type": "path", + "data": { + "pathName": "Red D to Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red BFGH.auto b/src/main/deploy/pathplanner/autos/Red BFGH.auto new file mode 100644 index 00000000..f2117751 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red BFGH.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Center to B" + } + }, + { + "type": "path", + "data": { + "pathName": "Red B to F" + } + }, + { + "type": "path", + "data": { + "pathName": "Red F to Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Red F Shoot to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Red G to F Shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Red GF Shoot to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Red H to F Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red GH.auto b/src/main/deploy/pathplanner/autos/Red GH.auto index 55b4ee63..12efadab 100644 --- a/src/main/deploy/pathplanner/autos/Red GH.auto +++ b/src/main/deploy/pathplanner/autos/Red GH.auto @@ -1,12 +1,5 @@ { "version": 1.0, - "startingPose": { - "position": { - "x": 0.88, - "y": 4.21 - }, - "rotation": -59.417977335920554 - }, "command": { "type": "sequential", "data": { @@ -38,6 +31,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red HG.auto b/src/main/deploy/pathplanner/autos/Red HG.auto index 5e28d14e..0f5513bc 100644 --- a/src/main/deploy/pathplanner/autos/Red HG.auto +++ b/src/main/deploy/pathplanner/autos/Red HG.auto @@ -1,12 +1,5 @@ { "version": 1.0, - "startingPose": { - "position": { - "x": 0.88, - "y": 4.21 - }, - "rotation": -62.664411916191156 - }, "command": { "type": "sequential", "data": { @@ -38,6 +31,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Reroute Test Blue.auto b/src/main/deploy/pathplanner/autos/Reroute Test Blue.auto index 6c853975..887920f2 100644 --- a/src/main/deploy/pathplanner/autos/Reroute Test Blue.auto +++ b/src/main/deploy/pathplanner/autos/Reroute Test Blue.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -20,6 +19,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Blue Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Reroute Test Red.auto b/src/main/deploy/pathplanner/autos/Reroute Test Red.auto index aa6fa334..4c162754 100644 --- a/src/main/deploy/pathplanner/autos/Reroute Test Red.auto +++ b/src/main/deploy/pathplanner/autos/Reroute Test Red.auto @@ -1,6 +1,5 @@ { "version": 1.0, - "startingPose": null, "command": { "type": "sequential", "data": { @@ -20,6 +19,7 @@ ] } }, - "folder": null, + "resetOdom": true, + "folder": "Red Autons", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B to A Reroute Test.path b/src/main/deploy/pathplanner/paths/B to A Reroute Test.path index d3cf6ac8..f51a82d5 100644 --- a/src/main/deploy/pathplanner/paths/B to A Reroute Test.path +++ b/src/main/deploy/pathplanner/paths/B to A Reroute Test.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 90.0, - "rotateFast": false + "rotation": 90.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path b/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path index 43d2e2fc..77a92caf 100644 --- a/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/Blue 90 Deg B Shoot to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9026396514331485, - "y": 4.291754680375458 + "x": 2.6238277281250206, + "y": 4.378717757621604 }, "prevControl": { - "x": 1.8369869877204144, - "y": 4.291576527241374 + "x": 1.5581750644122865, + "y": 4.3785396044875196 }, "nextControl": null, "isLocked": false, @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue A Shoot to D.path b/src/main/deploy/pathplanner/paths/Blue A Shoot to D.path new file mode 100644 index 00000000..87341f5e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue A Shoot to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.43, + "y": 6.58 + }, + "prevControl": null, + "nextControl": { + "x": 3.523916160555536, + "y": 6.803855879847177 + }, + "isLocked": false, + "linkedName": "Blue BCA A Shoot Pose" + }, + { + "anchor": { + "x": 8.303868098476938, + "y": 7.429163844438801 + }, + "prevControl": { + "x": 7.303868098476938, + "y": 7.429163844438801 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 28.731253180883208 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue A to Center.path b/src/main/deploy/pathplanner/paths/Blue A to Center.path index ac2e9da4..18646e91 100644 --- a/src/main/deploy/pathplanner/paths/Blue A to Center.path +++ b/src/main/deploy/pathplanner/paths/Blue A to Center.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.968997465060457, - "y": 7.021291176485487 + "x": 2.9106477302293583, + "y": 6.9718848238973115 }, "isLocked": false, "linkedName": "Blue BCA A" @@ -20,8 +20,8 @@ "y": 6.58 }, "prevControl": { - "x": 2.5167325548326307, - "y": 6.630585568863446 + "x": 2.6459538508152494, + "y": 6.705952111209242 }, "nextControl": null, "isLocked": false, @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 14.613680543382463, - "rotateFast": false + "rotation": 14.613680543382463 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path index 632132be..2c3f5073 100644 --- a/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue ABC Waypoints.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 2.9026396514331485, - "y": 4.291754680375458 + "x": 2.6238277281250206, + "y": 4.378717757621604 }, "prevControl": { - "x": 2.676338597987036, - "y": 4.39799889531999 + "x": 2.3975266746789083, + "y": 4.484961972566135 }, "nextControl": { - "x": 3.128940704879261, - "y": 4.1855104654309265 + "x": 2.850128781571133, + "y": 4.272473542677072 }, "isLocked": false, "linkedName": "Blue BCA C" @@ -110,6 +110,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -119,11 +120,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path index 9a248f49..c850fa4c 100644 --- a/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path +++ b/src/main/deploy/pathplanner/paths/Blue Amp Side Mobility.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue B to Center.path b/src/main/deploy/pathplanner/paths/Blue B to Center.path index 2b146569..8e828743 100644 --- a/src/main/deploy/pathplanner/paths/Blue B to Center.path +++ b/src/main/deploy/pathplanner/paths/Blue B to Center.path @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 1.0588668160039703, - "rotateFast": false + "rotation": 1.0588668160039703 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue B to D.path b/src/main/deploy/pathplanner/paths/Blue B to D.path new file mode 100644 index 00000000..ba11d6d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue B to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 4.09653456185508, + "y": 5.917887071242202 + }, + "isLocked": false, + "linkedName": "Blue BCA B" + }, + { + "anchor": { + "x": 8.303868098476938, + "y": 7.429163844438801 + }, + "prevControl": { + "x": 7.3081731942977575, + "y": 7.066410696939982 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 16.281060042006033 + }, + "reversed": false, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue B to F.path b/src/main/deploy/pathplanner/paths/Blue B to F.path new file mode 100644 index 00000000..36dbdbb7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue B to F.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 5.044398825776819, + "y": 4.782298508049397 + }, + "isLocked": false, + "linkedName": "Blue BCA B" + }, + { + "anchor": { + "x": 5.381541387152715, + "y": 4.0969083044936925 + }, + "prevControl": { + "x": 4.692305268167084, + "y": 4.115224476728716 + }, + "nextControl": { + "x": 6.112392080426523, + "y": 4.077486241709475 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.288957304291097, + "y": 4.0969083044936925 + }, + "prevControl": { + "x": 6.643001648168476, + "y": 4.0836879702033215 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue F" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path index 19cd60a8..94d21d45 100644 --- a/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path +++ b/src/main/deploy/pathplanner/paths/Blue C to Shoot Before A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.9026396514331485, - "y": 4.291754680375458 + "x": 2.6238277281250206, + "y": 4.378717757621604 }, "prevControl": null, "nextControl": { - "x": 2.3802855262405695, - "y": 4.240099407549243 + "x": 2.1014736029324417, + "y": 4.3270624847953885 }, "isLocked": false, "linkedName": "Blue BCA C" @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 38.766552008153084, - "rotateFast": false + "rotation": 38.766552008153084 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Center to A.path b/src/main/deploy/pathplanner/paths/Blue Center to A.path index 22e0006c..4b24808f 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to A.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to A.path @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 32.52489028779099, - "rotateFast": false + "rotation": 32.52489028779099 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Center to B.path b/src/main/deploy/pathplanner/paths/Blue Center to B.path index 972f192f..b22a760e 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to B.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to B.path @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.5, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -0.14529343403600373, - "rotateFast": false + "rotation": -0.14529343403600373 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Center to C.path b/src/main/deploy/pathplanner/paths/Blue Center to C.path index 2e95d553..cbc05c12 100644 --- a/src/main/deploy/pathplanner/paths/Blue Center to C.path +++ b/src/main/deploy/pathplanner/paths/Blue Center to C.path @@ -8,28 +8,34 @@ }, "prevControl": null, "nextControl": { - "x": 2.5935411441135385, - "y": 4.824459355949962 + "x": 2.0924424154876817, + "y": 5.333196225699311 }, "isLocked": false, "linkedName": "Blue BCA B Shoot Pose" }, { "anchor": { - "x": 2.9026396514331485, - "y": 4.291754680375458 + "x": 2.6238277281250206, + "y": 4.378717757621604 }, "prevControl": { - "x": 2.184174575180079, - "y": 5.064591386019107 + "x": 1.758976899669116, + "y": 5.811554088964557 }, "nextControl": null, "isLocked": false, "linkedName": "Blue BCA C" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": -52.33367496345772 + } + ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -44.3462417321064, - "rotateFast": false + "rotation": -44.3462417321064 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path b/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path index 3cc3dac7..088a7e84 100644 --- a/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Blue E Shoot to F.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.978996157066389, - "y": 6.932057215966284 + "x": 4.683796180278373, + "y": 3.722508437390387 }, "isLocked": false, "linkedName": "Blue DEF Shoot" }, { "anchor": { - "x": 7.740726041637269, - "y": 3.815446288299984 + "x": 8.288957304291097, + "y": 4.0969083044936925 }, "prevControl": { - "x": 6.065382226457245, - "y": 6.0808024600713555 + "x": 5.226887512926812, + "y": 4.141857441080955 }, "nextControl": null, "isLocked": false, @@ -40,7 +40,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -67.94269283671294 + "rotation": 0.0 }, "reversed": false, "folder": "Blue To Note", diff --git a/src/main/deploy/pathplanner/paths/Blue E to Shoot.path b/src/main/deploy/pathplanner/paths/Blue E to Shoot.path index e562d1db..02c2ef04 100644 --- a/src/main/deploy/pathplanner/paths/Blue E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue E to Shoot.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.35132156069537, - "y": 6.507392789714355 + "x": 5.292652666778547, + "y": 6.843132291175296 }, "isLocked": false, "linkedName": "Blue E" diff --git a/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path b/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path new file mode 100644 index 00000000..a8da4d1c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue F Shoot to A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.412562994582834, + "y": 4.983968698406147 + }, + "prevControl": null, + "nextControl": { + "x": 2.9498537080711227, + "y": 6.132174369658296 + }, + "isLocked": false, + "linkedName": "Blue F Shoot" + }, + { + "anchor": { + "x": 2.4, + "y": 6.43 + }, + "prevControl": { + "x": 3.9604601919295908, + "y": 5.356198902739307 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue BCA C Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 32.332853404618355 + }, + "reversed": false, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue F Shoot to G.path b/src/main/deploy/pathplanner/paths/Blue F Shoot to G.path new file mode 100644 index 00000000..ac784374 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue F Shoot to G.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.412562994582834, + "y": 4.983968698406147 + }, + "prevControl": null, + "nextControl": { + "x": 5.062859219840464, + "y": 3.896488036651668 + }, + "isLocked": false, + "linkedName": "Blue F Shoot" + }, + { + "anchor": { + "x": 7.454009064454142, + "y": 2.7589143264224028 + }, + "prevControl": { + "x": 6.038712186241336, + "y": 4.161231240059206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue G" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.75099449320135 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue F to Close C Shoot.path b/src/main/deploy/pathplanner/paths/Blue F to Close C Shoot.path new file mode 100644 index 00000000..4afc0bbf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue F to Close C Shoot.path @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.288957304291097, + "y": 4.0969083044936925 + }, + "prevControl": null, + "nextControl": { + "x": 4.74491958541272, + "y": 3.9026503308971727 + }, + "isLocked": false, + "linkedName": "Blue F" + }, + { + "anchor": { + "x": 4.590444617119941, + "y": 4.527401755529436 + }, + "prevControl": { + "x": 4.985219802098831, + "y": 3.937158592323137 + }, + "nextControl": { + "x": 4.1083373280769155, + "y": 5.248218418068103 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3521618769428017, + "y": 4.5938005780401445 + }, + "prevControl": { + "x": 3.225184679536026, + "y": 5.982512565843585 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue FC Shoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.25, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.204 + }, + "reversed": false, + "folder": "Blue To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue F to Shoot.path b/src/main/deploy/pathplanner/paths/Blue F to Shoot.path index 304f0f2f..a46366b2 100644 --- a/src/main/deploy/pathplanner/paths/Blue F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue F to Shoot.path @@ -3,38 +3,43 @@ "waypoints": [ { "anchor": { - "x": 7.740726041637269, - "y": 3.815446288299984 + "x": 8.288957304291097, + "y": 4.0969083044936925 }, "prevControl": null, "nextControl": { - "x": 4.196688322758893, - "y": 3.6211883147034643 + "x": 4.677883085304861, + "y": 4.005135147547081 }, "isLocked": false, "linkedName": "Blue F" }, { "anchor": { - "x": 4.24738092686751, - "y": 5.167803455956615 + "x": 4.412562994582834, + "y": 4.983968698406147 }, "prevControl": { - "x": 5.436537977800659, - "y": 4.006336996118933 + "x": 5.074637335844613, + "y": 4.2721378262697005 }, "nextControl": null, "isLocked": false, "linkedName": "Blue F Shoot" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.25, - "maxAcceleration": 2.0, + "maxVelocity": 3.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path b/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path new file mode 100644 index 00000000..be2ff57e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue FC Shoot to C.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.3521618769428017, + "y": 4.5938005780401445 + }, + "prevControl": null, + "nextControl": { + "x": 2.693159268226654, + "y": 4.429742058561564 + }, + "isLocked": false, + "linkedName": "Blue FC Shoot" + }, + { + "anchor": { + "x": 2.6238277281250206, + "y": 4.378717757621604 + }, + "prevControl": { + "x": 2.315502052772566, + "y": 4.530128331558909 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue BCA C" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.204 + }, + "reversed": false, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": -36.204 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path b/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path index b15ad50a..71f0688a 100644 --- a/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Blue G Shoot to F.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.740726041637269, - "y": 3.815446288299984 + "x": 8.288957304291097, + "y": 4.0969083044936925 }, "prevControl": { - "x": 6.485102752612913, - "y": 0.9461119107423408 + "x": 7.033334015266741, + "y": 1.2275739269360493 }, "nextControl": null, "isLocked": false, @@ -31,11 +31,11 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 26.187378361039833, - "rotateFast": false + "rotationDegrees": 26.187378361039833 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.25, @@ -45,11 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 61.30469161044191, - "rotateFast": false + "rotation": 61.30469161044191 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue G Shoot to H.path b/src/main/deploy/pathplanner/paths/Blue G Shoot to H.path index 0c5514bb..6423b558 100644 --- a/src/main/deploy/pathplanner/paths/Blue G Shoot to H.path +++ b/src/main/deploy/pathplanner/paths/Blue G Shoot to H.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.7951402231152036, - "y": 1.6228745548659187 + "y": 1.622874554865919 }, "isLocked": false, "linkedName": "Blue Midline Shoot Pose" @@ -21,7 +21,7 @@ }, "prevControl": { "x": 6.6387722006213234, - "y": 1.0875563631469138 + "y": 1.087556363146914 }, "nextControl": null, "isLocked": false, @@ -31,28 +31,27 @@ "rotationTargets": [ { "waypointRelativePos": 0.1, - "rotationDegrees": 0, - "rotateFast": false + "rotationDegrees": 0.0 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": "Utils", - "previewStartingState": { - "rotation": -42.734040696705854, - "velocity": 0 + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue G to F Reroute.path b/src/main/deploy/pathplanner/paths/Blue G to F Reroute.path index a3f0ee13..895fed5c 100644 --- a/src/main/deploy/pathplanner/paths/Blue G to F Reroute.path +++ b/src/main/deploy/pathplanner/paths/Blue G to F Reroute.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.740726041637269, - "y": 3.815446288299984 + "x": 8.288957304291097, + "y": 4.0969083044936925 }, "prevControl": { - "x": 7.740117446954639, - "y": 3.1961214581034154 + "x": 8.288348709608467, + "y": 3.477583474297124 }, "nextControl": null, "isLocked": false, @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 90.0, - "rotateFast": false + "rotation": 90.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue Reroutes", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue G to F Shoot.path b/src/main/deploy/pathplanner/paths/Blue G to F Shoot.path new file mode 100644 index 00000000..039fef7a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue G to F Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.454009064454142, + "y": 2.7589143264224028 + }, + "prevControl": null, + "nextControl": { + "x": 6.055153474704271, + "y": 4.402322063572698 + }, + "isLocked": false, + "linkedName": "Blue G" + }, + { + "anchor": { + "x": 4.412562994582834, + "y": 4.983968698406147 + }, + "prevControl": { + "x": 5.011852766451069, + "y": 4.0666697944258985 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue F Shoot" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": -36.99723984606199 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue G to Shoot.path b/src/main/deploy/pathplanner/paths/Blue G to Shoot.path index 01b85ad2..ebdf8b71 100644 --- a/src/main/deploy/pathplanner/paths/Blue G to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue G to Shoot.path @@ -20,7 +20,7 @@ "y": 3.8122943482354206 }, "prevControl": { - "x": 3.9645556673592006, + "x": 3.9645556673592, "y": 2.242664740315236 }, "nextControl": null, @@ -31,11 +31,11 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -4.159684543530611, - "rotateFast": false + "rotationDegrees": -4.159684543530611 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.5, @@ -45,11 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -42.20729763428664, - "rotateFast": false + "rotation": -42.20729763428664 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue GF Shoot to H.path b/src/main/deploy/pathplanner/paths/Blue GF Shoot to H.path new file mode 100644 index 00000000..2642c39e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue GF Shoot to H.path @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.412562994582834, + "y": 4.983968698406147 + }, + "prevControl": null, + "nextControl": { + "x": 4.914743401845435, + "y": 4.56615806088755 + }, + "isLocked": false, + "linkedName": "Blue F Shoot" + }, + { + "anchor": { + "x": 5.94073749066397, + "y": 3.992059035085332 + }, + "prevControl": { + "x": 5.364715507146789, + "y": 4.060900921281009 + }, + "nextControl": { + "x": 6.675720902241656, + "y": 3.9042192549668844 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.6387722006213234, + "y": 1.0875563631469138 + }, + "prevControl": { + "x": 6.853853961614173, + "y": 2.7999213996939902 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue H" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1, + "rotationDegrees": -39.22308339213737 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -64.40320817809109 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path b/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path index f491389b..194a7217 100644 --- a/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/Blue H Shoot to G.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 5.299941312133336, - "y": 0.9589206642800752 + "y": 0.9589206642800754 }, "nextControl": null, "isLocked": false, @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 37.58348553725079, - "rotateFast": false + "rotation": 37.58348553725079 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue H to F Shoot.path b/src/main/deploy/pathplanner/paths/Blue H to F Shoot.path new file mode 100644 index 00000000..b3484dbc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue H to F Shoot.path @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.6387722006213234, + "y": 1.0875563631469138 + }, + "prevControl": null, + "nextControl": { + "x": 6.625166215362193, + "y": 3.524491866692066 + }, + "isLocked": false, + "linkedName": "Blue H" + }, + { + "anchor": { + "x": 6.120437889127267, + "y": 3.907016230141091 + }, + "prevControl": { + "x": 6.480328574548539, + "y": 3.7267985048182815 + }, + "nextControl": { + "x": 5.451547194750325, + "y": 4.2419678260377776 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.412562994582834, + "y": 4.983968698406147 + }, + "prevControl": { + "x": 4.985075580270209, + "y": 4.100706145980745 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue F Shoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.85, + "rotationDegrees": -53.447147122261576 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue H to G Reroute.path b/src/main/deploy/pathplanner/paths/Blue H to G Reroute.path index 9e226c7a..1562383a 100644 --- a/src/main/deploy/pathplanner/paths/Blue H to G Reroute.path +++ b/src/main/deploy/pathplanner/paths/Blue H to G Reroute.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 90.0, - "rotateFast": false + "rotation": 90.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue Reroutes", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue H to Shoot.path b/src/main/deploy/pathplanner/paths/Blue H to Shoot.path index a463ca90..ea86ae51 100644 --- a/src/main/deploy/pathplanner/paths/Blue H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Blue H to Shoot.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -45.93919094573563, - "rotateFast": false + "rotation": -45.93919094573563 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path index 7ec0e527..0c2f87e9 100644 --- a/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Blue Midline Waypoints.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 7.740726041637269, - "y": 3.815446288299984 + "x": 8.288957304291097, + "y": 4.0969083044936925 }, "prevControl": { - "x": 7.964361570430268, - "y": 3.703700369978659 + "x": 8.512592833084096, + "y": 3.9851623861723673 }, "nextControl": { - "x": 7.517090512844271, - "y": 3.9271922066213087 + "x": 8.065321775498099, + "y": 4.208654222815017 }, "isLocked": false, "linkedName": "Blue F" @@ -69,7 +69,7 @@ }, "prevControl": { "x": 7.862682673242908, - "y": 0.9763623873641472 + "y": 0.976362387364147 }, "nextControl": { "x": 7.414861727999737, @@ -142,6 +142,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -151,11 +152,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path b/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path index 44d0057f..7dfc97e4 100644 --- a/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path +++ b/src/main/deploy/pathplanner/paths/Blue Source Side Mobility.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Blue Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Source to G.path b/src/main/deploy/pathplanner/paths/Blue Source to G.path index a35d06c1..60722566 100644 --- a/src/main/deploy/pathplanner/paths/Blue Source to G.path +++ b/src/main/deploy/pathplanner/paths/Blue Source to G.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 4.42287972895034, - "y": 1.6488757763840323 + "y": 1.648875776384032 }, "isLocked": false, "linkedName": "Blue HGF Starting Pose" @@ -20,8 +20,8 @@ "y": 2.7589143264224028 }, "prevControl": { - "x": 5.510436092097255, - "y": 0.3625187877156391 + "x": 5.510436092097254, + "y": 0.3625187877156386 }, "nextControl": null, "isLocked": false, @@ -30,23 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 47.89574324189973, - "rotateFast": false + "rotation": 47.89574324189973 }, "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -59.59000000000003, - "velocity": 0 + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Source to H.path b/src/main/deploy/pathplanner/paths/Blue Source to H.path index 9bc345a7..ed760dd3 100644 --- a/src/main/deploy/pathplanner/paths/Blue Source to H.path +++ b/src/main/deploy/pathplanner/paths/Blue Source to H.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.639371381306865, - "y": 0.7952025020859156 + "x": 3.6393713813068667, + "y": 0.7952025020859157 }, "isLocked": false, "linkedName": "Blue HGF Starting Pose" @@ -21,7 +21,7 @@ }, "prevControl": { "x": 3.251736469704623, - "y": 0.9992926040301093 + "y": 0.9992926040301094 }, "nextControl": null, "isLocked": false, @@ -31,28 +31,27 @@ "rotationTargets": [ { "waypointRelativePos": 0.35, - "rotationDegrees": 0.0, - "rotateFast": false + "rotationDegrees": 0.0 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -1.5944180626014446, - "rotateFast": false + "rotation": -1.5944180626014446 }, "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -59.588918732874625, - "velocity": 0 + "folder": "Blue To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red 90 Deg B Shoot to C.path b/src/main/deploy/pathplanner/paths/Red 90 Deg B Shoot to C.path index 89749c0d..b79afcae 100644 --- a/src/main/deploy/pathplanner/paths/Red 90 Deg B Shoot to C.path +++ b/src/main/deploy/pathplanner/paths/Red 90 Deg B Shoot to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.8441688792209487, - "y": 4.081259900411539 + "x": 2.6088286579474946, + "y": 4.298674642736191 }, "prevControl": { - "x": 1.8441688792209487, - "y": 4.081259900411539 + "x": 1.6088286579474946, + "y": 4.298674642736191 }, "nextControl": null, "isLocked": false, @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red A Shoot to D.path b/src/main/deploy/pathplanner/paths/Red A Shoot to D.path new file mode 100644 index 00000000..59cd1999 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red A Shoot to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.4348734737355513, + "y": 6.583808951093687 + }, + "prevControl": null, + "nextControl": { + "x": 3.528789634291087, + "y": 6.807664830940864 + }, + "isLocked": false, + "linkedName": "Red BCA A Shoot Pose" + }, + { + "anchor": { + "x": 8.281950694955519, + "y": 7.472564688719121 + }, + "prevControl": { + "x": 7.281950694955519, + "y": 7.472564688719121 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 28.731253180883208 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red A to Center.path b/src/main/deploy/pathplanner/paths/Red A to Center.path index 7cc9fb00..ab6ee477 100644 --- a/src/main/deploy/pathplanner/paths/Red A to Center.path +++ b/src/main/deploy/pathplanner/paths/Red A to Center.path @@ -20,8 +20,8 @@ "y": 6.583808951093687 }, "prevControl": { - "x": 2.5284267092750707, - "y": 6.642279723305886 + "x": 2.6468730497368242, + "y": 6.7163086860944805 }, "nextControl": null, "isLocked": false, @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 27.59729586864363, - "rotateFast": false + "rotation": 27.59729586864363 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path index 70b0ab58..ee5bd5bc 100644 --- a/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path +++ b/src/main/deploy/pathplanner/paths/Red Amp Side Mobility.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red B to Center.path b/src/main/deploy/pathplanner/paths/Red B to Center.path index dc4216cd..a862b988 100644 --- a/src/main/deploy/pathplanner/paths/Red B to Center.path +++ b/src/main/deploy/pathplanner/paths/Red B to Center.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.9011993744785207, - "y": 5.5430292057165325 + "x": 2.852269430395628, + "y": 5.536390999962906 }, "isLocked": false, "linkedName": "Red BCA B" @@ -20,8 +20,8 @@ "y": 5.55 }, "prevControl": { - "x": 2.142519612674553, - "y": 5.5430292057165325 + "x": 2.1998362814792927, + "y": 5.5409538706288455 }, "nextControl": null, "isLocked": false, @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red B to D.path b/src/main/deploy/pathplanner/paths/Red B to D.path new file mode 100644 index 00000000..f56a88ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red B to D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1, + "y": 5.57 + }, + "prevControl": null, + "nextControl": { + "x": 4.09653456185508, + "y": 5.937887071242202 + }, + "isLocked": false, + "linkedName": "Red BCA B" + }, + { + "anchor": { + "x": 8.281950694955519, + "y": 7.472564688719121 + }, + "prevControl": { + "x": 7.286255790776338, + "y": 7.109811541220302 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red D" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 16.281060042006033 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red B to F.path b/src/main/deploy/pathplanner/paths/Red B to F.path new file mode 100644 index 00000000..bfaebd4d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red B to F.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.1, + "y": 5.57 + }, + "prevControl": null, + "nextControl": { + "x": 5.044398825776819, + "y": 4.802298508049398 + }, + "isLocked": false, + "linkedName": "Red BCA B" + }, + { + "anchor": { + "x": 5.381541387152715, + "y": 4.0969083044936925 + }, + "prevControl": { + "x": 4.692305268167084, + "y": 4.115224476728716 + }, + "nextControl": { + "x": 6.112392080426523, + "y": 4.077486241709475 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.285159462804046, + "y": 4.107340350097367 + }, + "prevControl": { + "x": 6.639203806681425, + "y": 4.094120015806996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red F" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path index 9a661d2c..867bdb6b 100644 --- a/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path +++ b/src/main/deploy/pathplanner/paths/Red C to Shoot Before A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.8441688792209487, - "y": 4.081259900411539 + "x": 2.6088286579474946, + "y": 4.298674642736191 }, "prevControl": null, "nextControl": { - "x": 2.294543620426272, - "y": 4.163118981508619 + "x": 2.059203399152818, + "y": 4.38053372383327 }, "isLocked": false, "linkedName": "Red BCA C" @@ -20,8 +20,8 @@ "y": 6.075378178534135 }, "prevControl": { - "x": 2.130825458232113, - "y": 5.905547993432169 + "x": 2.1187232445698765, + "y": 5.828398456474928 }, "nextControl": null, "isLocked": false, @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 38.19580416075567, - "rotateFast": false + "rotation": 38.19580416075567 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Center to B.path b/src/main/deploy/pathplanner/paths/Red Center to B.path index 4971cbe9..2c905a8d 100644 --- a/src/main/deploy/pathplanner/paths/Red Center to B.path +++ b/src/main/deploy/pathplanner/paths/Red Center to B.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.7403760112078623, - "y": 5.578111669043852 + "x": 1.779814367462399, + "y": 5.579632331460321 }, "isLocked": false, "linkedName": "Red BCA Starting Pose" @@ -20,8 +20,8 @@ "y": 5.57 }, "prevControl": { - "x": 2.971364301133161, - "y": 5.566417514601411 + "x": 2.8500968954326606, + "y": 5.563040235089783 }, "nextControl": null, "isLocked": false, @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Center to C.path b/src/main/deploy/pathplanner/paths/Red Center to C.path index 0ee8f569..43bdaa0d 100644 --- a/src/main/deploy/pathplanner/paths/Red Center to C.path +++ b/src/main/deploy/pathplanner/paths/Red Center to C.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.5243146663748686, - "y": 4.364968609959541 + "x": 2.62397646657746, + "y": 4.229222674810491 }, "isLocked": false, "linkedName": "Red BCA B Shoot Pose" }, { "anchor": { - "x": 2.8441688792209487, - "y": 4.081259900411539 + "x": 2.6088286579474946, + "y": 4.298674642736191 }, "prevControl": { - "x": 2.238629494779084, - "y": 5.244264726421192 + "x": 1.9319651290827706, + "y": 5.399632142610049 }, "nextControl": null, "isLocked": false, @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -32.73734304607968, - "rotateFast": false + "rotation": -32.73734304607968 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red D Shoot to E.path b/src/main/deploy/pathplanner/paths/Red D Shoot to E.path index 78bc9a0b..bcb4ab54 100644 --- a/src/main/deploy/pathplanner/paths/Red D Shoot to E.path +++ b/src/main/deploy/pathplanner/paths/Red D Shoot to E.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 4.174260459756039, + "y": 6.097216993541131 }, "prevControl": null, "nextControl": { - "x": 3.7080076466303087, - "y": 4.06581396436507 + "x": 5.678734502000255, + "y": 7.013746714435394 }, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": null }, { "anchor": { @@ -20,8 +20,8 @@ "y": 5.762294601512281 }, "prevControl": { - "x": 5.501437800680661, - "y": 6.388737383129529 + "x": 5.545040866867447, + "y": 6.3010505113272135 }, "nextControl": null, "isLocked": false, @@ -31,11 +31,11 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -20.607589471852908, - "rotateFast": false + "rotationDegrees": -20.607589471852908 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -45,11 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red D to Shoot.path b/src/main/deploy/pathplanner/paths/Red D to Shoot.path index fa7a588a..2fbba63d 100644 --- a/src/main/deploy/pathplanner/paths/Red D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red D to Shoot.path @@ -8,28 +8,29 @@ }, "prevControl": null, "nextControl": { - "x": 6.165308740873893, - "y": 6.946327738809323 + "x": 6.183013205060271, + "y": 6.7693906328214934 }, "isLocked": false, - "linkedName": "Red D" + "linkedName": "Red DE Shoot Pose" }, { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 4.174260459756039, + "y": 6.097216993541131 }, "prevControl": { - "x": 4.028997363200515, - "y": 4.618183567988207 + "x": 6.212190515392087, + "y": 6.767069075359839 }, "nextControl": null, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 12.244059995551996, - "rotateFast": false + "rotation": 5.776351579033 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red E Shoot to F.path b/src/main/deploy/pathplanner/paths/Red E Shoot to F.path index a59a7ee2..d9c68d43 100644 --- a/src/main/deploy/pathplanner/paths/Red E Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Red E Shoot to F.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 4.174260459756039, + "y": 6.097216993541131 }, "prevControl": null, "nextControl": { - "x": 3.014973686157626, - "y": 4.243399109341907 + "x": 4.433427085790184, + "y": 4.041046382874016 }, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": null }, { "anchor": { @@ -20,8 +20,8 @@ "y": 4.107340350097367 }, "prevControl": { - "x": 7.942820216124693, - "y": 6.3148433989167545 + "x": 4.9073880885857015, + "y": 4.01114439040634 }, "nextControl": null, "isLocked": false, @@ -30,6 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -76.9119463322888, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red E to Shoot.path b/src/main/deploy/pathplanner/paths/Red E to Shoot.path index bb761358..d4951993 100644 --- a/src/main/deploy/pathplanner/paths/Red E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red E to Shoot.path @@ -8,28 +8,29 @@ }, "prevControl": null, "nextControl": { - "x": 5.564270444017079, - "y": 6.29686807829717 + "x": 5.6023930807162206, + "y": 6.857217956437401 }, "isLocked": false, "linkedName": "Red E" }, { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 4.174260459756039, + "y": 6.097216993541131 }, "prevControl": { - "x": 3.2443344676098436, - "y": 4.197344272068541 + "x": 5.485813769246587, + "y": 6.9959112816291125 }, "nextControl": null, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 11.086538893198085, - "rotateFast": false + "rotation": 6.150276616687043 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red F Shoot to A.path b/src/main/deploy/pathplanner/paths/Red F Shoot to A.path new file mode 100644 index 00000000..186f634f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red F Shoot to A.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.20100142052136, + "y": 4.993335426485668 + }, + "prevControl": null, + "nextControl": { + "x": 2.4549505437092742, + "y": 5.970307148287061 + }, + "isLocked": false, + "linkedName": "Red F to Shoot" + }, + { + "anchor": { + "x": 2.1574662038212966, + "y": 6.075378178534135 + }, + "prevControl": { + "x": 3.430478072743293, + "y": 5.445921230130763 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red BCA C Shoot Pose" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 32.332853404618355 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red F Shoot to G.path b/src/main/deploy/pathplanner/paths/Red F Shoot to G.path new file mode 100644 index 00000000..c9f76e71 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red F Shoot to G.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.20100142052136, + "y": 4.993335426485668 + }, + "prevControl": null, + "nextControl": { + "x": 4.85129764577899, + "y": 3.905854764731189 + }, + "isLocked": false, + "linkedName": "Red F to Shoot" + }, + { + "anchor": { + "x": 7.454, + "y": 2.759 + }, + "prevControl": { + "x": 6.0387031217871945, + "y": 4.161316913636803 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red G" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.75099449320135 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red F to Close C Shoot.path b/src/main/deploy/pathplanner/paths/Red F to Close C Shoot.path new file mode 100644 index 00000000..b6b8ea1e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red F to Close C Shoot.path @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.285159462804046, + "y": 4.107340350097367 + }, + "prevControl": null, + "nextControl": { + "x": 4.741121743925669, + "y": 3.913082376500847 + }, + "isLocked": false, + "linkedName": "Red F" + }, + { + "anchor": { + "x": 4.590444617119941, + "y": 4.527401755529436 + }, + "prevControl": { + "x": 4.985219802098831, + "y": 3.937158592323137 + }, + "nextControl": { + "x": 4.1083373280769155, + "y": 5.248218418068103 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3521618769428017, + "y": 4.5938005780401445 + }, + "prevControl": { + "x": 3.225184679536026, + "y": 5.982512565843585 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red FC Shoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.25, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.204 + }, + "reversed": false, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red F to Shoot.path b/src/main/deploy/pathplanner/paths/Red F to Shoot.path index d113e392..4d447a5c 100644 --- a/src/main/deploy/pathplanner/paths/Red F to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red F to Shoot.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.974337717193953, - "y": 4.201266592348 + "x": 5.861319337035996, + "y": 4.176807197550407 }, "isLocked": false, "linkedName": "Red F" @@ -20,15 +20,20 @@ "y": 4.993335426485668 }, "prevControl": { - "x": 4.908506900347135, - "y": 3.754696069577325 + "x": 4.909695637843657, + "y": 3.9358125219226627 }, "nextControl": null, "isLocked": false, "linkedName": "Red F to Shoot" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + } + ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], diff --git a/src/main/deploy/pathplanner/paths/Red FC Shoot to C.path b/src/main/deploy/pathplanner/paths/Red FC Shoot to C.path new file mode 100644 index 00000000..184e2dcc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red FC Shoot to C.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.3521618769428017, + "y": 4.5938005780401445 + }, + "prevControl": null, + "nextControl": { + "x": 2.6307561793776664, + "y": 4.306955544666831 + }, + "isLocked": false, + "linkedName": "Red FC Shoot" + }, + { + "anchor": { + "x": 2.6088286579474946, + "y": 4.298674642736191 + }, + "prevControl": { + "x": 2.433393427330297, + "y": 4.477196748773984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red BCA C" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.204 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": -36.204 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G Shoot to F.path b/src/main/deploy/pathplanner/paths/Red G Shoot to F.path index ba05e8a0..30bac21f 100644 --- a/src/main/deploy/pathplanner/paths/Red G Shoot to F.path +++ b/src/main/deploy/pathplanner/paths/Red G Shoot to F.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 2.365670654890417, + "y": 3.567518045564365 }, "prevControl": null, "nextControl": { - "x": 2.995936404779492, - "y": 1.1590710824269248 + "x": 3.433056408523532, + "y": 2.156884339810345 }, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": "Red HG Shoot Pose" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 4.107340350097367 }, "prevControl": { - "x": 5.386156485668626, - "y": 4.755617469754285 + "x": 5.313276388271527, + "y": 4.534092740990542 }, "nextControl": null, "isLocked": false, @@ -31,11 +31,11 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 40.376097780771914, - "rotateFast": false + "rotationDegrees": 40.376097780771914 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -45,11 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": -40.84105469095527 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G Shoot to H.path b/src/main/deploy/pathplanner/paths/Red G Shoot to H.path index fa348b24..7ed9c5bf 100644 --- a/src/main/deploy/pathplanner/paths/Red G Shoot to H.path +++ b/src/main/deploy/pathplanner/paths/Red G Shoot to H.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 2.365670654890417, + "y": 3.567518045564365 }, "prevControl": null, "nextControl": { - "x": 2.7950423529565005, - "y": 2.7689947578926515 + "x": 3.287158947348484, + "y": 2.4072769107971963 }, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": "Red HG Shoot Pose" }, { "anchor": { @@ -21,7 +21,7 @@ }, "prevControl": { "x": 4.957734392898985, - "y": 1.2700678383977873 + "y": 1.2700678383977868 }, "nextControl": null, "isLocked": false, @@ -31,28 +31,27 @@ "rotationTargets": [ { "waypointRelativePos": 0.1, - "rotationDegrees": 0, - "rotateFast": true + "rotationDegrees": 0.0 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": "Utils", - "previewStartingState": { - "rotation": -48.142420767008986, - "velocity": 0 + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": -40.21562048161929 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G to F Reroute.path b/src/main/deploy/pathplanner/paths/Red G to F Reroute.path index 4da630da..d05cd529 100644 --- a/src/main/deploy/pathplanner/paths/Red G to F Reroute.path +++ b/src/main/deploy/pathplanner/paths/Red G to F Reroute.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 90.0, - "rotateFast": false + "rotation": 90.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red Reroutes", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G to F Shoot.path b/src/main/deploy/pathplanner/paths/Red G to F Shoot.path new file mode 100644 index 00000000..7d37be63 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red G to F Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.454, + "y": 2.759 + }, + "prevControl": null, + "nextControl": { + "x": 6.055144410250129, + "y": 4.402407737150295 + }, + "isLocked": false, + "linkedName": "Red G" + }, + { + "anchor": { + "x": 4.20100142052136, + "y": 4.993335426485668 + }, + "prevControl": { + "x": 4.800291192389595, + "y": 4.07603652250542 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red F to Shoot" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": -36.99723984606199 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G to Shoot.path b/src/main/deploy/pathplanner/paths/Red G to Shoot.path index dc12629b..2a5a2c30 100644 --- a/src/main/deploy/pathplanner/paths/Red G to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red G to Shoot.path @@ -9,27 +9,28 @@ "prevControl": null, "nextControl": { "x": 6.0473116670698905, - "y": 0.3802405704573891 + "y": 0.380240570457389 }, "isLocked": false, "linkedName": "Red G" }, { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 2.365670654890417, + "y": 3.567518045564365 }, "prevControl": { - "x": 3.9892515935562076, - "y": 1.6965412609997725 + "x": 4.48136818794819, + "y": 1.3348234139043176 }, "nextControl": null, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": "Red HG Shoot Pose" } ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.5, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -46.65527786297665, - "rotateFast": false + "rotation": -46.65527786297665 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red GF Shoot to H.path b/src/main/deploy/pathplanner/paths/Red GF Shoot to H.path new file mode 100644 index 00000000..9121a15d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red GF Shoot to H.path @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.20100142052136, + "y": 4.993335426485668 + }, + "prevControl": null, + "nextControl": { + "x": 4.70318182778396, + "y": 4.575524788967071 + }, + "isLocked": false, + "linkedName": "Red F to Shoot" + }, + { + "anchor": { + "x": 5.94073749066397, + "y": 3.992059035085332 + }, + "prevControl": { + "x": 5.380760588507756, + "y": 4.143605727037275 + }, + "nextControl": { + "x": 6.58680320894868, + "y": 3.817214104852317 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.416583266214966, + "y": 1.3565219153230341 + }, + "prevControl": { + "x": 6.6316650272078155, + "y": 3.0688869518701107 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red H" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1, + "rotationDegrees": -39.22308339213737 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -64.40320817809109 + }, + "reversed": false, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red H Shoot to G.path b/src/main/deploy/pathplanner/paths/Red H Shoot to G.path index 7b9c3214..10280d2e 100644 --- a/src/main/deploy/pathplanner/paths/Red H Shoot to G.path +++ b/src/main/deploy/pathplanner/paths/Red H Shoot to G.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 2.365670654890417, + "y": 3.567518045564365 }, "prevControl": null, "nextControl": { - "x": 4.981849233692769, - "y": 1.6229676225335365 + "x": 5.473965828084752, + "y": 1.2612497754380816 }, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": "Red HG Shoot Pose" }, { "anchor": { @@ -31,11 +31,11 @@ "rotationTargets": [ { "waypointRelativePos": 0.6, - "rotationDegrees": 38.202090103962774, - "rotateFast": true + "rotationDegrees": 38.202090103962774 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.5, @@ -45,11 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 37.583, - "rotateFast": false + "rotation": 37.583 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": -41.72613849556501 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red H to F Shoot.path b/src/main/deploy/pathplanner/paths/Red H to F Shoot.path new file mode 100644 index 00000000..ae956e14 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red H to F Shoot.path @@ -0,0 +1,73 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.416583266214966, + "y": 1.3565219153230341 + }, + "prevControl": null, + "nextControl": { + "x": 6.402977280955835, + "y": 3.7934574188681864 + }, + "isLocked": false, + "linkedName": "Red H" + }, + { + "anchor": { + "x": 6.120437889127267, + "y": 3.907016230141091 + }, + "prevControl": { + "x": 6.480328574548539, + "y": 3.7267985048182815 + }, + "nextControl": { + "x": 5.451547194750325, + "y": 4.2419678260377776 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.20100142052136, + "y": 4.993335426485668 + }, + "prevControl": { + "x": 4.7735140062087345, + "y": 4.110072874060266 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red F to Shoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.85, + "rotationDegrees": -53.447147122261576 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": -67.59068855738553 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red H to G Reroute.path b/src/main/deploy/pathplanner/paths/Red H to G Reroute.path index de660537..ff2d7881 100644 --- a/src/main/deploy/pathplanner/paths/Red H to G Reroute.path +++ b/src/main/deploy/pathplanner/paths/Red H to G Reroute.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 90.0, - "rotateFast": false + "rotation": 90.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red Reroutes", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red H to Shoot.path b/src/main/deploy/pathplanner/paths/Red H to Shoot.path index bcaccdc0..a7a9b9c3 100644 --- a/src/main/deploy/pathplanner/paths/Red H to Shoot.path +++ b/src/main/deploy/pathplanner/paths/Red H to Shoot.path @@ -8,28 +8,29 @@ }, "prevControl": null, "nextControl": { - "x": 3.9083369334829836, - "y": 1.321439451995713 + "x": 3.9047098620610936, + "y": 1.581054652064661 }, "isLocked": false, "linkedName": "Red H" }, { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 2.365670654890417, + "y": 3.567518045564365 }, "prevControl": { - "x": 2.820780570336069, - "y": 2.8416795295129056 + "x": 3.464785210820422, + "y": 2.154528716609517 }, "nextControl": null, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": "Red HG Shoot Pose" } ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.5, @@ -39,11 +40,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -40.100907546212255, - "rotateFast": false + "rotation": -42.377464368931555 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red To Shoot", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path b/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path index f15583bf..23fe29ba 100644 --- a/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path +++ b/src/main/deploy/pathplanner/paths/Red Midline Waypoints.path @@ -68,8 +68,8 @@ "y": 1.3565219153230341 }, "prevControl": { - "x": 7.640282691128509, - "y": 1.2449039629437462 + "x": 7.640282691128518, + "y": 1.2449039629437417 }, "nextControl": { "x": 7.192883841301399, @@ -96,19 +96,19 @@ }, { "anchor": { - "x": 1.8735540604984342, - "y": 3.92923589265982 + "x": 4.174260459756039, + "y": 6.097216993541131 }, "prevControl": { - "x": 2.0982152651384745, - "y": 3.8195666484311577 + "x": 4.398921664396079, + "y": 5.987547749312468 }, "nextControl": { - "x": 1.648892855858394, - "y": 4.0389051368884825 + "x": 3.9495992551159986, + "y": 6.206886237769793 }, "isLocked": false, - "linkedName": "Red Midline Shoot Pose" + "linkedName": null }, { "anchor": { @@ -142,6 +142,7 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -151,11 +152,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path b/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path index 44d0057f..e3c821e6 100644 --- a/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path +++ b/src/main/deploy/pathplanner/paths/Red Source Side Mobility.path @@ -30,20 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0.0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, - "folder": null, - "previewStartingState": null, + "folder": "Red Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Source to G.path b/src/main/deploy/pathplanner/paths/Red Source to G.path index 53f83999..9badcabe 100644 --- a/src/main/deploy/pathplanner/paths/Red Source to G.path +++ b/src/main/deploy/pathplanner/paths/Red Source to G.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.4870477832123745, - "y": 0.6431784943341966 + "x": 5.487047783212375, + "y": 0.6431784943341965 }, "isLocked": false, "linkedName": "Red HGF Starting Pose" @@ -21,7 +21,7 @@ }, "prevControl": { "x": 6.013284733122172, - "y": 0.9589206642800752 + "y": 0.9589206642800749 }, "nextControl": null, "isLocked": false, @@ -30,23 +30,23 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 4.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 45.820024158319185, - "rotateFast": false + "rotation": 45.820024158319185 }, "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -59.59000000000003, - "velocity": 0 + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Source to H.path b/src/main/deploy/pathplanner/paths/Red Source to H.path index 0954db67..2e1d1cd7 100644 --- a/src/main/deploy/pathplanner/paths/Red Source to H.path +++ b/src/main/deploy/pathplanner/paths/Red Source to H.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 3.0294717209717748, - "y": 1.268225315196365 + "y": 1.2682253151963656 }, "nextControl": null, "isLocked": false, @@ -31,11 +31,11 @@ "rotationTargets": [ { "waypointRelativePos": 0.35, - "rotationDegrees": 0.0, - "rotateFast": false + "rotationDegrees": 0.0 } ], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 2.5, @@ -45,14 +45,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -1.59, - "rotateFast": false + "rotation": -1.59 }, "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -59.59000000000003, - "velocity": 0 + "folder": "Red To Note", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path new file mode 100644 index 00000000..c0c123d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.0, + "y": 0.0 + }, + "prevControl": null, + "nextControl": { + "x": 0.9999999999999982, + "y": 0.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 0.0 + }, + "prevControl": { + "x": 1.0, + "y": 0.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Blue Util", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 1cee7665..5effc93b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 1.00076, + "robotLength": 0.87122, "holonomicMode": true, "pathFolders": [ "Blue To Note", @@ -22,10 +22,10 @@ "defaultMaxAngAccel": 720.0, "robotMass": 60.0, "robotMOI": 6.883, - "robotWheelbase": 0.495, - "robotTrackwidth": 0.673, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "robotWheelbase": 0.48895, + "robotTrackwidth": 0.67945, + "driveWheelRadius": 0.0508, + "driveGearing": 5.357, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 15b32df8..cb66e368 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -20,12 +20,16 @@ import com.stuypulse.robot.commands.auton.ADEF.FivePieceADEF; import com.stuypulse.robot.commands.auton.BCA.FourPieceBCA; import com.stuypulse.robot.commands.auton.BCA.RightAngleFourPieceBCA; +import com.stuypulse.robot.commands.auton.BF_Series.FivePieceBFGH; +import com.stuypulse.robot.commands.auton.BF_Series.SixPieceBDEFA; +import com.stuypulse.robot.commands.auton.BF_Series.SixPieceBFCAD; import com.stuypulse.robot.commands.auton.HGF.ThreePieceGH; import com.stuypulse.robot.commands.auton.HGF.ThreePieceHG; import com.stuypulse.robot.commands.auton.HGF.FourPieceHGF; import com.stuypulse.robot.commands.auton.HGF.ReroutableFourPieceHGF; import com.stuypulse.robot.commands.auton.SideAutons.OnePieceAmpSide; import com.stuypulse.robot.commands.auton.SideAutons.OnePieceSourceSide; +import com.stuypulse.robot.commands.auton.tests.StraightLine; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeSetAcquire; import com.stuypulse.robot.commands.intake.IntakeStop; @@ -294,10 +298,10 @@ public void configureAutons() { AutonConfig MOBILITY_RED = new AutonConfig("Mobility", Mobility::new, "Mobility"); // BCA - AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, - "Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center"); - AutonConfig BCA_RED = new AutonConfig("4 BCA", FourPieceBCA::new, - "Red Center to B", "Red B to Center", "Red Center to C", "Red C to Shoot Before A", "Red Center to A", "Red A to Center"); + //AutonConfig BCA_BLUE = new AutonConfig("4 BCA", FourPieceBCA::new, + //"Blue Center to B", "Blue B to Center", "Blue Center to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center"); + //AutonConfig BCA_RED = new AutonConfig("4 BCA", FourPieceBCA::new, + //"Red Center to B", "Red B to Center", "Red Center to C", "Red C to Shoot Before A", "Red Center to A", "Red A to Center"); // HGF AutonConfig HGF_BLUE = new AutonConfig("4 HGF", FourPieceHGF::new, @@ -318,10 +322,10 @@ public void configureAutons() { "Red Source to H", "Red H to Shoot", "Red H Shoot to G", "Red G to Shoot"); // Reroutable HGF - AutonConfig ReroutableHGF_BLUE = new AutonConfig("4 HGF Reroute", ReroutableFourPieceHGF::new, - "Blue Source to H", "Blue H to Shoot", "Blue H Shoot to G", "Blue G to Shoot", "Blue G Shoot to F", "Blue F to Shoot", "Blue H to G Reroute", "Blue G to F Reroute"); - AutonConfig ReroutableHGF_RED = new AutonConfig("4 HGF Reroute", ReroutableFourPieceHGF::new, - "Red Source to H", "Red H to Shoot", "Red H Shoot to G", "Red G to Shoot", "Red G Shoot to F", "Red F to Shoot", "Red H to G Reroute", "Red G to F Reroute"); + //AutonConfig ReroutableHGF_BLUE = new AutonConfig("4 HGF Reroute", ReroutableFourPieceHGF::new, + // "Blue Source to H", "Blue H to Shoot", "Blue H Shoot to G", "Blue G to Shoot", "Blue G Shoot to F", "Blue F to Shoot", "Blue H to G Reroute", "Blue G to F Reroute"); + //AutonConfig ReroutableHGF_RED = new AutonConfig("4 HGF Reroute", ReroutableFourPieceHGF::new, + //"Red Source to H", "Red H to Shoot", "Red H Shoot to G", "Red G to Shoot", "Red G Shoot to F", "Red F to Shoot", "Red H to G Reroute", "Red G to F Reroute"); // ADEF AutonConfig ADEF_BLUE = new AutonConfig("5 ADEF", FivePieceADEF::new, @@ -329,11 +333,29 @@ public void configureAutons() { AutonConfig ADEF_RED = new AutonConfig("5 ADEF", FivePieceADEF::new, "Red Amp to A", "Red A to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F", "Red F to Shoot"); + // BDEFA + AutonConfig BDEFA_BLUE = new AutonConfig("6 BDEFA", SixPieceBDEFA::new, + "Blue Center to B", "Blue B to D", "Blue D to Shoot", "Blue D Shoot to E", "Blue E to Shoot", "Blue E Shoot to F", "Blue F to Shoot", "Blue F Shoot to A", "Blue Center to A"); + AutonConfig BDEFA_RED = new AutonConfig("6 BDEFA", SixPieceBDEFA::new, + "Red Center to B", "Red B to D", "Red D to Shoot", "Red D Shoot to E", "Red E to Shoot", "Red E Shoot to F", "Red F to Shoot", "Red F Shoot to A", "Red Center to A"); + + // BFCAD + AutonConfig BFCAD_BLUE = new AutonConfig("6 BFCAD", SixPieceBFCAD::new, + "Blue Center to B", "Blue B to F", "Blue F to Close C Shoot", "Blue FC Shoot to C", "Blue C to Shoot Before A", "Blue Center to A", "Blue A to Center", "Blue A Shoot to D", "Blue D to Shoot"); + AutonConfig BFCAD_RED = new AutonConfig("6 BFCAD", SixPieceBFCAD::new, + "Red Center to B", "Red B to F", "Red F to Close C Shoot", "Red FC Shoot to C", "Red C to Shoot Before A", "Red Center to A", "Red A to Center", "Red A Shoot to D", "Red D to Shoot"); + + // BFGH + AutonConfig BFGH_BLUE = new AutonConfig("5 BFGH", FivePieceBFGH:: new, + "Blue Center to B", "Blue B to F", "Blue F to Shoot", "Blue F Shoot to G", "Blue G to F Shoot", "Blue GF Shoot to H", "Blue H to F Shoot"); + AutonConfig BFGH_RED = new AutonConfig("5 BFGH", FivePieceBFGH:: new, + "Red Center to B", "Red B to F", "Red F to Shoot", "Red F Shoot to G", "Red G to F Shoot", "Red GF Shoot to H", "Red H to F Shoot"); + // Reroute Test - AutonConfig Reroute_Test_Blue = new AutonConfig("Reroute Test", RerouteTest::new, - "Blue Center to B", "Blue B to Center", "B to A Reroute Test", "Blue A to Center"); - AutonConfig Reroute_Test_Red = new AutonConfig("Reroute Test", RerouteTest::new, - "Red Center to B", "Red B to Center", "B to A Reroute Test", "Red A to Center"); + //AutonConfig Reroute_Test_Blue = new AutonConfig("Reroute Test", RerouteTest::new, + //"Blue Center to B", "Blue B to Center", "B to A Reroute Test", "Blue A to Center"); + //AutonConfig Reroute_Test_Red = new AutonConfig("Reroute Test", RerouteTest::new, + //"Red Center to B", "Red B to Center", "B to A Reroute Test", "Red A to Center"); // New BCA AutonConfig New_BCA_Blue = new AutonConfig("New BCA", RightAngleFourPieceBCA::new, @@ -344,17 +366,32 @@ public void configureAutons() { AutonConfig One_Piece_Mobility_Amp_Side_Blue = new AutonConfig("One Piece Amp Side", OnePieceAmpSide::new, "Blue Amp Side Mobility"); + // Straight Line + AutonConfig Straight_Line = new AutonConfig("Straight Line Test", StraightLine::new, + "Straight Line"); + One_Piece_Mobility_Amp_Side_Blue.registerBlue(autonChooser); + Straight_Line.registerBlue(autonChooser); + MOBILITY_BLUE.registerBlue(autonChooser); MOBILITY_RED.registerRed(autonChooser); - BCA_BLUE.registerDefaultBlue(autonChooser); - BCA_RED.registerDefaultRed(autonChooser); + //BCA_BLUE.registerDefaultBlue(autonChooser); + //BCA_RED.registerDefaultRed(autonChooser); New_BCA_Blue.registerDefaultBlue(autonChooser); New_BCA_Red.registerDefaultRed(autonChooser); + BDEFA_BLUE.registerBlue(autonChooser); + BDEFA_RED.registerRed(autonChooser); + + BFCAD_BLUE.registerBlue(autonChooser); + BFCAD_RED.registerRed(autonChooser); + + BFGH_BLUE.registerBlue(autonChooser); + BFGH_RED.registerRed(autonChooser); + HGF_BLUE.registerBlue(autonChooser); HGF_RED.registerRed(autonChooser); @@ -364,14 +401,14 @@ public void configureAutons() { GH_BLUE.registerBlue(autonChooser); GH_RED.registerRed(autonChooser); - ReroutableHGF_BLUE.registerBlue(autonChooser); - ReroutableHGF_RED.registerRed(autonChooser); + //ReroutableHGF_BLUE.registerBlue(autonChooser); + //ReroutableHGF_RED.registerRed(autonChooser); ADEF_BLUE.registerBlue(autonChooser); ADEF_RED.registerRed(autonChooser); - Reroute_Test_Blue.registerBlue(autonChooser); - Reroute_Test_Red.registerRed(autonChooser); + //Reroute_Test_Blue.registerBlue(autonChooser); + //Reroute_Test_Red.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); } 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 deleted file mode 100644 index 9db1f028..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFAC.java +++ /dev/null @@ -1,53 +0,0 @@ -package com.stuypulse.robot.commands.auton.BF_Series; - -import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.arm.ArmToFeed; -import com.stuypulse.robot.commands.auton.ShootRoutine; -import com.stuypulse.robot.commands.intake.IntakeSetAcquire; -import com.stuypulse.robot.commands.shooter.ShooterFeederStop; -import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class FivePieceBFAC extends SequentialCommandGroup { - - public FivePieceBFAC(PathPlannerPath... paths) { - - addCommands( - // Preload Shot - ShootRoutine.fromSubwoofer(), - new ArmToFeed(), - - new IntakeSetAcquire(), - SwerveDrive.getInstance().followPathCommand(paths[0]), - new WaitCommand(1.0).until(() -> Shooter.getInstance().hasNote() || Intake.getInstance().hasNote()), - new WaitUntilCommand(() -> Shooter.getInstance().hasNote()).onlyIf(() -> Intake.getInstance().hasNote()).withTimeout(1.0), - ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), - new ArmToFeed(), - - new IntakeSetAcquire(), - SwerveDrive.getInstance().followPathCommand(paths[0]), - SwerveDrive.getInstance().followPathCommand(paths[1]), - ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), - new ArmToFeed(), - - new IntakeSetAcquire(), - SwerveDrive.getInstance().followPathCommand(paths[2]), - SwerveDrive.getInstance().followPathCommand(paths[3]), - ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), - new ArmToFeed(), - - new IntakeSetAcquire(), - SwerveDrive.getInstance().followPathCommand(paths[2]), - SwerveDrive.getInstance().followPathCommand(paths[3]), - ShootRoutine.fromAnywhere().onlyIf(() -> Shooter.getInstance().hasNote()), - new ArmToFeed() - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java deleted file mode 100644 index b81f4081..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFCA.java +++ /dev/null @@ -1,39 +0,0 @@ -// package com.stuypulse.robot.commands.auton.BF_Series; - -// import com.pathplanner.lib.path.PathPlannerPath; -// import com.stuypulse.robot.commands.arm.ArmToFeed; -// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -// import com.stuypulse.robot.commands.auton.ShootRoutine; -// import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - -// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -// public class FivePieceBFCA extends SequentialCommandGroup { - -// public FivePieceBFCA(PathPlannerPath... paths) { - -// addCommands( -// ShootRoutine.fromSubwoofer(), -// new ArmToFeed(), - -// new FollowPathAndIntake(paths[0]), -// ShootRoutine.fromAnywhere(), - -// new FollowPathAndIntake(paths[1]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), -// ShootRoutine.fromAnywhere(), - -// new FollowPathAndIntake(paths[3]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), -// ShootRoutine.fromAnywhere(), - -// new FollowPathAndIntake(paths[5]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), -// ShootRoutine.fromAnywhere() -// ); - -// } - -// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java deleted file mode 100644 index c52f0b1f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFED.java +++ /dev/null @@ -1,39 +0,0 @@ -// package com.stuypulse.robot.commands.auton.BF_Series; - -// import com.pathplanner.lib.path.PathPlannerPath; -// import com.stuypulse.robot.commands.arm.ArmToFeed; -// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -// import com.stuypulse.robot.commands.auton.ShootRoutine; -// import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; - -// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -// public class FivePieceBFED extends SequentialCommandGroup { - -// public FivePieceBFED(PathPlannerPath... paths) { - -// addCommands( -// ShootRoutine.fromSubwoofer(), -// new ArmToFeed(), - -// new FollowPathAndIntake(paths[0]), -// ShootRoutine.fromAnywhere(), - -// new FollowPathAndIntake(paths[1]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), -// ShootRoutine.fromAnywhere(), - -// new FollowPathAndIntake(paths[3]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), -// ShootRoutine.fromAnywhere(), - -// new FollowPathAndIntake(paths[5]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), -// ShootRoutine.fromAnywhere() -// ); - -// } - -// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java index 0b437ec8..a5489b8c 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/FivePieceBFGH.java @@ -1,39 +1,49 @@ -// package com.stuypulse.robot.commands.auton.BF_Series; +package com.stuypulse.robot.commands.auton.BF_Series; -// import com.pathplanner.lib.path.PathPlannerPath; -// import com.stuypulse.robot.commands.arm.ArmToFeed; -// import com.stuypulse.robot.commands.auton.FollowPathAndIntake; -// import com.stuypulse.robot.commands.auton.ShootRoutine; -// import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; -// import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; -// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.FollowPathThenShoot; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -// public class FivePieceBFGH extends SequentialCommandGroup { +public class FivePieceBFGH extends SequentialCommandGroup { -// public FivePieceBFGH(PathPlannerPath... paths) { + public FivePieceBFGH(PathPlannerPath... paths) { -// addCommands( -// ShootRoutine.fromSubwoofer(), -// new ArmToFeed(), + addCommands( + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), -// new FollowPathAndIntake(paths[0]), -// ShootRoutine.fromAnywhere(), + // Get B + Shoot B from there + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + ShootRoutine.fromAnywhere(), + new ArmToFeed(), -// new FollowPathAndIntake(paths[1]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[2]), -// ShootRoutine.fromAnywhere(), + // Get F + Shoot F + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new FollowPathThenShoot(paths[2], false), + new ArmToFeed(), -// new FollowPathAndIntake(paths[3]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[4]), -// ShootRoutine.fromAnywhere(), + // Get G + Shoot G + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[3]), + new FollowPathThenShoot(paths[4], false), + new ArmToFeed(), -// new FollowPathAndIntake(paths[5]), -// SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(paths[6]), -// ShootRoutine.fromAnywhere() -// ); + // Get H + Shoot H + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[5]), + new FollowPathThenShoot(paths[6], true), + new ArmToFeed() -// } + ); -// } + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/SixPieceBDEFA.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/SixPieceBDEFA.java new file mode 100644 index 00000000..7620cfcc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/SixPieceBDEFA.java @@ -0,0 +1,54 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.FollowPathThenShoot; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class SixPieceBDEFA extends SequentialCommandGroup { + + public SixPieceBDEFA(PathPlannerPath... paths) { + + addCommands( + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + // Get B + Shoot B from there + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + ShootRoutine.fromAnywhere(), + new ArmToFeed(), + + // Get D + Shoot D + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new FollowPathThenShoot(paths[2], false), + new ArmToFeed(), + + // Get E + Shoot E + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[3]), + new FollowPathThenShoot(paths[4], false), + new ArmToFeed(), + + // Get F + Shoot F + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[5]), + new FollowPathThenShoot(paths[6], true), + new ArmToFeed(), + + // Get A + Shoot A from there + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[7]), + new FollowPathThenShoot(paths[8], true), + new ArmToFeed() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/SixPieceBFCAD.java b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/SixPieceBFCAD.java new file mode 100644 index 00000000..7368e3b6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/BF_Series/SixPieceBFCAD.java @@ -0,0 +1,55 @@ +package com.stuypulse.robot.commands.auton.BF_Series; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.auton.FollowPathThenShoot; +import com.stuypulse.robot.commands.auton.ShootRoutine; +import com.stuypulse.robot.commands.intake.IntakeSetAcquire; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class SixPieceBFCAD extends SequentialCommandGroup { + + public SixPieceBFCAD(PathPlannerPath... paths) { + + addCommands( + // Preload + ShootRoutine.fromSubwoofer(), + new ArmToFeed(), + + // Get B + Shoot B from there + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[0]), + ShootRoutine.fromAnywhere(), + new ArmToFeed(), + + // Get F + Shoot F + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[1]), + new FollowPathThenShoot(paths[2], false), + new ArmToFeed(), + + // Get C + Shoot C + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[3]), + new FollowPathThenShoot(paths[4], false), + new ArmToFeed(), + + // Get A + Shoot A + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[5]), + new FollowPathThenShoot(paths[6], false), + new ArmToFeed(), + + // Get D to Shoot D + new IntakeSetAcquire(), + SwerveDrive.getInstance().followPathCommand(paths[7]), + new FollowPathThenShoot(paths[8], true), + new ArmToFeed() + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/CurvedLine.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/CurvedLine.java new file mode 100644 index 00000000..8bd08ccf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/CurvedLine.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.stuypulse.robot.commands.vision.VisionChangeWhiteList; +import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class CurvedLine extends SequentialCommandGroup { + + public CurvedLine() { + addCommands( + new VisionChangeWhiteList(7, 8), + SwerveDrive.getInstance().followPathCommand("Curved Line"), + new VisionReloadWhiteList() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/SharpCurvedLine.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/SharpCurvedLine.java new file mode 100644 index 00000000..c8adf1f3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/SharpCurvedLine.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.stuypulse.robot.commands.vision.VisionChangeWhiteList; +import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class SharpCurvedLine extends SequentialCommandGroup { + + public SharpCurvedLine() { + addCommands( + new VisionChangeWhiteList(7, 8), + SwerveDrive.getInstance().followPathCommand("Sharp Curved Line"), + new VisionReloadWhiteList() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/SimpleNote.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/SimpleNote.java new file mode 100644 index 00000000..52b51330 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/SimpleNote.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.stuypulse.robot.commands.vision.VisionChangeWhiteList; +import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class SimpleNote extends SequentialCommandGroup { + + public SimpleNote() { + addCommands( + new VisionChangeWhiteList(7, 8), + SwerveDrive.getInstance().followPathCommand("Simple Note"), + new VisionReloadWhiteList() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/Speed.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/Speed.java new file mode 100644 index 00000000..9f02be7a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/Speed.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.stuypulse.robot.commands.vision.VisionEnable; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class Speed extends SequentialCommandGroup { + + public Speed() { + addCommands( + SwerveDrive.getInstance().followPathCommand("SPEED A"), + SwerveDrive.getInstance().followPathCommand("SPEED B"), + SwerveDrive.getInstance().followPathCommand("SPEED A"), + SwerveDrive.getInstance().followPathCommand("SPEED B"), + SwerveDrive.getInstance().followPathCommand("SPEED A"), + SwerveDrive.getInstance().followPathCommand("SPEED B"), + new VisionEnable() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/Square.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/Square.java new file mode 100644 index 00000000..bb65f0b0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/Square.java @@ -0,0 +1,25 @@ +// package com.stuypulse.robot.commands.auton.tests; + +// import com.pathplanner.lib.path.PathPlannerPath; +// import com.stuypulse.robot.commands.swerve.SwerveDriveResetOdometry; +// import com.stuypulse.robot.commands.vision.VisionDisable; +// import com.stuypulse.robot.commands.vision.VisionEnable; +// import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// public class Square extends SequentialCommandGroup { + +// public Square() { +// addCommands( +// new SwerveDriveResetOdometry(PathPlannerPath.fromPathFile("SquareA").getPreviewStartingHolonomicPose()), +// new VisionDisable(), +// SwerveDrive.getInstance().followPathCommand("SquareA"), +// SwerveDrive.getInstance().followPathCommand("SquareB"), +// SwerveDrive.getInstance().followPathCommand("SquareC"), +// SwerveDrive.getInstance().followPathCommand("SquareD"), +// new VisionEnable() +// ); +// } + +// } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java new file mode 100644 index 00000000..548de18a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLine.java @@ -0,0 +1,20 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.vision.VisionChangeWhiteList; +import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class StraightLine extends SequentialCommandGroup { + + public StraightLine(PathPlannerPath... paths) { + addCommands( + new VisionChangeWhiteList(7, 8), + SwerveDrive.getInstance().followPathCommand(paths[0]), + new VisionReloadWhiteList() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLineTurning.java b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLineTurning.java new file mode 100644 index 00000000..b476c189 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/tests/StraightLineTurning.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.auton.tests; + +import com.stuypulse.robot.commands.vision.VisionChangeWhiteList; +import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class StraightLineTurning extends SequentialCommandGroup { + + public StraightLineTurning() { + addCommands( + new VisionChangeWhiteList(7, 8), + SwerveDrive.getInstance().followPathCommand("Straight Line Turning"), + new VisionReloadWhiteList() + ); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 419b7c21..13d3b51c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -151,9 +151,11 @@ public static AprilTag getTag(int id) { /*** SPEAKER ***/ + double SPEAKER_OPENING_X = Units.inchesToMeters(9.952119); + public static Pose2d getAllianceSpeakerPose() { return (Robot.isBlue() ? NamedTags.BLUE_SPEAKER : NamedTags.RED_SPEAKER) - .getLocation().toPose2d(); + .getLocation().toPose2d().transformBy(new Transform2d(SPEAKER_OPENING_X, 0, new Rotation2d())); } public static Pose2d getSpeakerPathFindPose() { @@ -161,8 +163,6 @@ public static Pose2d getSpeakerPathFindPose() { new Transform2d(0, Units.inchesToMeters(200), new Rotation2d())); } - double SPEAKER_OPENING_X = Units.inchesToMeters(13.6); - /*** AMP ***/ public static Pose2d getAllianceAmpPose() { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a325ede9..90c24f94 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -24,8 +24,8 @@ public interface Settings { boolean SAFE_MODE_ENABLED = false; - double WIDTH = Units.inchesToMeters(36); // intake side - double LENGTH = Units.inchesToMeters(32); + double WIDTH = Units.inchesToMeters(39.4); // intake side + double LENGTH = Units.inchesToMeters(34.3); double CENTER_TO_FRONT_OF_INTAKE = Units.inchesToMeters(13.0); @@ -198,22 +198,6 @@ public interface Motion { PIDConstants THETA = new PIDConstants(5.0, 0, 0.2); } - public interface Encoder { - public interface Drive { - double WHEEL_DIAMETER = Units.inchesToMeters(4); - double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; - double GEAR_RATIO = 1.0 / Swerve.Drive.L3; - - double POSITION_CONVERSION = WHEEL_CIRCUMFERENCE * GEAR_RATIO; - double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; - } - - public interface Turn { - double POSITION_CONVERSION = 1; - double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0; - } - } - public interface Turn { SmartNumber kP = new SmartNumber("Swerve/Turn/PID/kP", Robot.isReal() ? 9.0 : 9.0); SmartNumber kI = new SmartNumber("Swerve/Turn/PID/kI", Robot.isReal() ? 0.0 : 0.0); @@ -240,6 +224,11 @@ public interface Drive { double L2 = ((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)); // 6.74607175 double L3 = ((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 6.12244898 double L4 = ((50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0)); // 5.35714285714 + + double WHEEL_DIAMETER = 4; + double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; + + double GEAR_RATIO = Swerve.Drive.L4; } public interface FrontRight { diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 35a143cc..3f0e3ac2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -11,6 +11,7 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.util.ArmEncoderFeedforward; import com.stuypulse.robot.util.FilteredRelativeEncoder; +import com.stuypulse.robot.util.SpeakerAngleElinInterpolation; import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; @@ -99,7 +100,8 @@ private double getTargetDegrees() { case SUBWOOFER_SHOT: return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); case SPEAKER: - return getSpeakerAngle(); + // return getSpeakerAngle(); + return getSpeakerAngleElin(); case LOW_FERRY: return Settings.Arm.LOW_FERRY_ANGLE.get(); case LOW_FERRY_MANUAL: @@ -121,6 +123,19 @@ private double getTargetDegrees() { } } + private double getSpeakerAngleElin() { + try { + Pose2d speakerPose = Field.getAllianceSpeakerPose(); + double distanceToSpeaker = Units.metersToInches(SwerveDrive.getInstance().getPose().minus(speakerPose).getTranslation().getNorm()) - Units.metersToInches(Settings.WIDTH / 2); + SmartDashboard.putNumber("harry", distanceToSpeaker); + return SpeakerAngleElinInterpolation.getAngleInDegrees(distanceToSpeaker); + } + catch (Exception e) { + e.printStackTrace(); + return Settings.Arm.SUBWOOFER_SHOT_ANGLE.get(); + } + } + private double getSpeakerAngle() { try { Pose3d speakerPose = new Pose3d( 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 6e155411..bfa84f33 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -174,7 +174,7 @@ public void drive(Vector2D velocity, double rotation) { twistVel.dy / Settings.DT, twistVel.dtheta / Settings.DT )); - }/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + }/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); @@ -259,7 +259,7 @@ private void updateEstimatorWithVisionData(ArrayList outputs) { for (VisionData data : outputs) { Pose2d weighted = data.getPose().toPose2d().times(data.getArea()); - poseSum = new Pose2d( + poseSum = new Pose2d( poseSum.getTranslation().plus(weighted.getTranslation()), poseSum.getRotation().plus(weighted.getRotation()) ); @@ -298,7 +298,7 @@ public void periodic() { ArrayList outputs = AprilTagVision.getInstance().getOutputs(); if (Settings.Vision.IS_ACTIVE.get() && outputs.size() > 0) { updateEstimatorWithVisionData(outputs); - + } for (int i = 0; i < Modules.length; i++) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDriveConstants.java index 30672ef2..a704a43f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDriveConstants.java @@ -46,9 +46,9 @@ public class SwerveDriveConstants { .withPigeon2Configs(pigeonConfigs); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(1 / Settings.Swerve.Drive.L4) + .withDriveMotorGearRatio(Settings.Swerve.Drive.GEAR_RATIO) .withSteerMotorGearRatio(Settings.Swerve.Turn.GEAR_RATIO) - .withWheelRadius(Settings.Swerve.Encoder.Drive.WHEEL_DIAMETER / 2) + .withWheelRadius(Settings.Swerve.Drive.WHEEL_DIAMETER / 2) .withSlipCurrent(Settings.Swerve.SLIP_CURRENT) .withSteerMotorGains(steerGains) .withDriveMotorGains(driveGains) @@ -101,6 +101,6 @@ public class SwerveDriveConstants { Settings.Swerve.BackRight.MODULE_OFFSET.getY(), Settings.Swerve.BackRight.DRIVE_INVERTED) .withSteerMotorInverted(Settings.Swerve.Turn.INVERTED); - + public static final double UpdateOdometryFrequency = 50 /* hz */; } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java index 65639265..44a8c28d 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java @@ -2,12 +2,7 @@ import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -/** Interpolation based on ferry angle of 50 degrees */ public class ShooterLobFerryInterpolation { - - public static void main(String[] args) { - System.out.println(getRPM(450)); - } private static final InterpolatingDoubleTreeMap interpolatingDoubleTreeMap; diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java index 5ae6e472..178b2822 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java @@ -2,12 +2,7 @@ import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -/** Interpolation based on ferry angle of -50 degrees */ public class ShooterLowFerryInterpolation { - - public static void main(String[] args) { - System.out.println(getRPM(450)); - } private static final InterpolatingDoubleTreeMap interpolatingDoubleTreeMap; diff --git a/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java new file mode 100644 index 00000000..c8c92d5a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SpeakerAngleElinInterpolation.java @@ -0,0 +1,84 @@ +package com.stuypulse.robot.util; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.util.Units; + +/** Huge thanks to Elin for doing all the math */ +public class SpeakerAngleElinInterpolation { + + private static final InterpolatingDoubleTreeMap interpolatingDoubleTreeMap; + + // distance (angle), angle (radians) + private static final double[][] distanceAndAngle = { + {45.1854245,-0.57751}, + {47.5, -0.60548}, + {50, -0.63458}, + {52.5, -0.66255}, + {55, -0.68943}, + {57.5,-0.71524}, + {60, -0.74002}, + {62.5, -0.76382}, + {65, -0.78668}, + {67.5, -0.80862}, + {70, -0.82969}, + {72.5, -0.84993}, + {75, -0.86938}, + {77.5, -0.88806}, + {80, -0.90602}, + {82.5, -0.92328}, + {85, -0.93989}, + {87.5, -0.95587}, + {90, -0.97125}, + {92.5, -0.98606}, + {95, -1.00032}, + {97.5, -1.01407}, + {100, -1.02732}, + {102.5, -1.0401}, + {105, -1.05243}, + {107.5, -1.06434}, + {110, -1.07583}, + {112.5, -1.08693}, + {115, -1.09767}, + {117.5, -1.10804}, + {120, -1.11808}, + {122.5, -1.12779}, + {125, -1.13719}, + {127.5, -1.14629}, + {130, -1.15512}, + {132.5, -1.16366}, + {135, -1.17195}, + {137.5, -1.17999}, + {140, -1.18779}, + {142.5, -1.19536}, + {145, -1.20271}, + {147.5, -1.20986}, + {150, -1.21679}, + {152.5, -1.22354}, + {155, -1.2301}, + {157.5, -1.23647}, + {160, -1.24268}, + {162.5, -1.24872}, + {165, -1.2546}, + {167.5, -1.26033}, + {170, -1.26591}, + {172.5, -1.27134}, + {175, -1.27664}, + {177.5, -1.28181}, + {180, -1.28685}, + }; + + static { + interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap(); + for (double[] data: distanceAndAngle) { + interpolatingDoubleTreeMap.put(data[0], data[1]); + } + } + + public static double getAngleInRadians(double distanceInInches) { + return interpolatingDoubleTreeMap.get(distanceInInches); + } + + public static double getAngleInDegrees(double distanceInInches) { + return Units.radiansToDegrees(getAngleInRadians(distanceInInches)); + } +}