diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index ec725ea2..3f82bc00 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.81, + "robotLength": 0.813, "holonomicMode": true, "pathFolders": [ "Collect Notes", diff --git a/src/main/deploy/pathplanner/autos/FleeAmp.auto b/src/main/deploy/pathplanner/autos/FleeAmp.auto index 565917d4..d1e40a9a 100644 --- a/src/main/deploy/pathplanner/autos/FleeAmp.auto +++ b/src/main/deploy/pathplanner/autos/FleeAmp.auto @@ -2,19 +2,25 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.1333456901148034, - "y": 6.915037974220092 + "x": 0.7523718013035084, + "y": 6.631709206478911 }, - "rotation": 0 + "rotation": -120.37688046831643 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "path", + "type": "named", + "data": { + "name": "initialShot" + } + }, + { + "type": "wait", "data": { - "pathName": "initalShotBlueAmp" + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/FleeMid.auto b/src/main/deploy/pathplanner/autos/FleeMid.auto index 4719e75b..359f79c4 100644 --- a/src/main/deploy/pathplanner/autos/FleeMid.auto +++ b/src/main/deploy/pathplanner/autos/FleeMid.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3552185662929883, - "y": 4.779511541005064 + "x": 1.3074487833435935, + "y": 5.52155524239874 }, "rotation": 179.80105712063937 }, @@ -12,9 +12,15 @@ "data": { "commands": [ { - "type": "path", + "type": "named", + "data": { + "name": "initialShot" + } + }, + { + "type": "wait", "data": { - "pathName": "initalShotBlueMiddle" + "waitTime": 1.25 } }, { diff --git a/src/main/deploy/pathplanner/autos/FleeSource.auto b/src/main/deploy/pathplanner/autos/FleeSource.auto index 0b2b62e2..b687d9d8 100644 --- a/src/main/deploy/pathplanner/autos/FleeSource.auto +++ b/src/main/deploy/pathplanner/autos/FleeSource.auto @@ -2,19 +2,25 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.28, - "y": 2.87 + "x": 0.7523718013035084, + "y": 4.450354048988052 }, - "rotation": 0 + "rotation": 120.69972255081439 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "path", + "type": "named", + "data": { + "name": "initialShot" + } + }, + { + "type": "wait", "data": { - "pathName": "initalShotBlueSource" + "waitTime": 1.25 } }, { diff --git a/src/main/deploy/pathplanner/autos/ShuffleShoot.auto b/src/main/deploy/pathplanner/autos/ShuffleShoot.auto index 3ab86d8f..140d3730 100644 --- a/src/main/deploy/pathplanner/autos/ShuffleShoot.auto +++ b/src/main/deploy/pathplanner/autos/ShuffleShoot.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7434431971001721, - "y": 2.17419608235891 + "x": 0.7328954159687686, + "y": 4.421139470985942 }, - "rotation": 0 + "rotation": 119.74488129694214 }, "command": { "type": "sequential", @@ -14,13 +14,13 @@ { "type": "named", "data": { - "name": "intakeOn" + "name": "initialShot" } }, { - "type": "named", + "type": "wait", "data": { - "name": "shooterOn" + "waitTime": 1.25 } }, { diff --git a/src/main/deploy/pathplanner/autos/RightSideFarBlue.auto b/src/main/deploy/pathplanner/autos/SourceSideFar3.auto similarity index 67% rename from src/main/deploy/pathplanner/autos/RightSideFarBlue.auto rename to src/main/deploy/pathplanner/autos/SourceSideFar3.auto index e2c680f5..59940191 100644 --- a/src/main/deploy/pathplanner/autos/RightSideFarBlue.auto +++ b/src/main/deploy/pathplanner/autos/SourceSideFar3.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3182397535966242, - "y": 3.096975563320497 + "x": 0.7621099939708783, + "y": 4.4600922416554205 }, - "rotation": 126.75928817029215 + "rotation": 121.21840276434641 }, "command": { "type": "sequential", @@ -14,13 +14,13 @@ { "type": "named", "data": { - "name": "shooterOn" + "name": "initialShot" } }, { - "type": "path", + "type": "wait", "data": { - "pathName": "initalShotBlueSource" + "waitTime": 1.25 } }, { @@ -35,6 +35,12 @@ "pathName": "BlueSourceShoot5" } }, + { + "type": "named", + "data": { + "name": "shootNote" + } + }, { "type": "path", "data": { @@ -47,6 +53,12 @@ "pathName": "BlueSourceShoot4" } }, + { + "type": "named", + "data": { + "name": "shootNote" + } + }, { "type": "path", "data": { @@ -58,6 +70,12 @@ "data": { "pathName": "BlueMiddleShoot3" } + }, + { + "type": "named", + "data": { + "name": "shootNote" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto b/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto index df9f0412..488b6645 100644 --- a/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto +++ b/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.087122174244349, - "y": 7.036539787365289 + "x": 0.7523718013035084, + "y": 6.641447399146282 }, - "rotation": 0 + "rotation": -120.96375653207352 }, "command": { "type": "sequential", @@ -14,7 +14,13 @@ { "type": "named", "data": { - "name": "intakeOn" + "name": "initialShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.25 } }, { diff --git a/src/main/deploy/pathplanner/autos/StartMLeft3.auto b/src/main/deploy/pathplanner/autos/StartMLeft3.auto index ab799d9a..d25457a2 100644 --- a/src/main/deploy/pathplanner/autos/StartMLeft3.auto +++ b/src/main/deploy/pathplanner/autos/StartMLeft3.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.4649253051475237, - "y": 4.114397967513275 + "x": 1.3171869760109631, + "y": 5.560508013068222 }, - "rotation": 0 + "rotation": 180.0 }, "command": { "type": "sequential", @@ -14,7 +14,13 @@ { "type": "named", "data": { - "name": "intakeOn" + "name": "initialShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.25 } }, { @@ -32,7 +38,7 @@ { "type": "named", "data": { - "name": "shooterOn" + "name": "shootNote" } }, { @@ -50,7 +56,7 @@ { "type": "named", "data": { - "name": "shooterOn" + "name": "shootNote" } }, { @@ -68,7 +74,7 @@ { "type": "named", "data": { - "name": "shooterOn" + "name": "shootNote" } } ] diff --git a/src/main/deploy/pathplanner/autos/StartMRight3.auto b/src/main/deploy/pathplanner/autos/StartMRight3.auto index cb62b6c6..6ad9dcaf 100644 --- a/src/main/deploy/pathplanner/autos/StartMRight3.auto +++ b/src/main/deploy/pathplanner/autos/StartMRight3.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.4649253051475237, - "y": 3.17842117869509 + "x": 1.3171869760109631, + "y": 5.531293435066112 }, - "rotation": 0 + "rotation": 180.0 }, "command": { "type": "sequential", @@ -14,7 +14,13 @@ { "type": "named", "data": { - "name": "intakeOn" + "name": "initialShot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.25 } }, { @@ -32,7 +38,7 @@ { "type": "named", "data": { - "name": "shooterOn" + "name": "shootNote" } }, { @@ -50,7 +56,7 @@ { "type": "named", "data": { - "name": "shooterOn" + "name": "shootNote" } }, { @@ -68,7 +74,7 @@ { "type": "named", "data": { - "name": "shooterOn" + "name": "shootNote" } } ] diff --git a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path index 876f6a2d..eb7d9501 100644 --- a/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2AutoPickup.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 7.5585241906825855, - "y": 5.937602754065366 + "x": 7.491201127123489, + "y": 5.940297527095648 }, "prevControl": { - "x": 7.231087866294267, - "y": 6.007767680720005 + "x": 7.16376480273517, + "y": 6.0104624537502875 }, "nextControl": null, "isLocked": false, - "linkedName": "CollectNote2Blue" + "linkedName": null } ], "rotationTargets": [ @@ -54,7 +54,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue2Pickup.path b/src/main/deploy/pathplanner/paths/Blue2Pickup.path index 544708b3..4447ffca 100644 --- a/src/main/deploy/pathplanner/paths/Blue2Pickup.path +++ b/src/main/deploy/pathplanner/paths/Blue2Pickup.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.652433485047149, - "y": 6.240174642511447 + "x": 6.975076915752882, + "y": 6.212966921782006 }, "prevControl": null, "nextControl": { - "x": 7.323030030675445, - "y": 6.0692661556263285 + "x": 7.267222695773978, + "y": 6.105846802440936 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.5585241906825855, - "y": 5.937602754065366 + "x": 7.958634375157244, + "y": 5.872130178424058 }, "prevControl": { - "x": 7.020593086330348, - "y": 6.042850144047325 + "x": 7.763870521809846, + "y": 5.950035719763017 }, "nextControl": null, "isLocked": false, @@ -33,11 +33,6 @@ "waypointRelativePos": 1, "rotationDegrees": 167.62, "rotateFast": true - }, - { - "waypointRelativePos": 0, - "rotationDegrees": 166.83, - "rotateFast": false } ], "constraintZones": [], @@ -54,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path index 5e329f7f..9b6a93ee 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoPickupAmpSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 7.6323720215007995, - "y": 6.5729839567787245 + "x": 8.665673221827673, + "y": 7.19589929449488 }, "isLocked": false, "linkedName": "SourceSideShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path index 77747fe6..93943adb 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageAmp.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 3.9896948365325304, - "y": 3.2543996516564464 + "x": 2.3981263620890236, + "y": 3.866062488945856 }, "isLocked": false, "linkedName": "SourceSideShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path index 8ebec517..9ec1f27d 100644 --- a/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path +++ b/src/main/deploy/pathplanner/paths/Blue3AutoUnderStageSource.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": null, "nextControl": { - "x": 4.448769166286213, - "y": 4.577929329594664 + "x": 2.8168686467859296, + "y": 4.002397186289035 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path b/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path index 5efe8dec..8e4ffe3f 100644 --- a/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path +++ b/src/main/deploy/pathplanner/paths/Blue3PickupAmpSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 7.6323720215007995, - "y": 6.5729839567787245 + "x": 8.665673221827673, + "y": 7.19589929449488 }, "isLocked": false, "linkedName": "SourceSideShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageAmp.path b/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageAmp.path index 70ebe924..c5f41e1d 100644 --- a/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageAmp.path +++ b/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageAmp.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 3.9896948365325304, - "y": 3.2543996516564464 + "x": 3.011632500133328, + "y": 2.9896251488825647 }, "isLocked": false, "linkedName": "SourceSideShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageSource.path b/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageSource.path index 8d286098..e814cb2b 100644 --- a/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageSource.path +++ b/src/main/deploy/pathplanner/paths/Blue3PickupUnderStageSource.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": null, "nextControl": { - "x": 4.448769166286213, - "y": 4.577929329594664 + "x": 2.748701298114341, + "y": 4.7814525996786275 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path index e52f05e7..052f3ad5 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": null, "nextControl": { - "x": 5.973352786882025, - "y": 0.92772431489892 + "x": 6.912961158238873, + "y": 0.24326353075685425 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path index dcef7059..1b0e05d2 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupUnderStage.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 2.934213868427737, - "y": 3.899415798831598 + "x": 3.196658160813356, + "y": 3.496011167585799 }, "isLocked": false, "linkedName": "SourceSideShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path index d34809e5..dcbd1494 100644 --- a/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue4PickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": null, "nextControl": { - "x": 5.973352786882025, - "y": 0.92772431489892 + "x": 6.912961158238873, + "y": 0.24326353075685425 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue4PickupUnderStage.path b/src/main/deploy/pathplanner/paths/Blue4PickupUnderStage.path index 9583d8a1..53f36809 100644 --- a/src/main/deploy/pathplanner/paths/Blue4PickupUnderStage.path +++ b/src/main/deploy/pathplanner/paths/Blue4PickupUnderStage.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 2.934213868427737, - "y": 3.899415798831598 + "x": 3.9675150687546097, + "y": 4.522331136547754 }, "isLocked": false, "linkedName": "SourceSideShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path index b484915e..ebe55b2c 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": null, "nextControl": { - "x": 5.957315089360964, - "y": 1.4130734890974712 + "x": 6.8969234607178125, + "y": 0.7286127049554054 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path index 8fa74dba..7a9e01f0 100644 --- a/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue5PickupSourceSide.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": null, "nextControl": { - "x": 6.310520318359945, - "y": 1.2209134727058073 + "x": 7.250128689716793, + "y": 0.5364526885637415 }, "isLocked": false, "linkedName": "SourceSideBlueShoot" @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": null, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path index c5b01612..a12c4218 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 7.952560925752258, - "y": 7.448538179873802 + "x": 7.637274017134038, + "y": 7.410764619868505 }, "isLocked": false, "linkedName": "CollectNote1Blue" }, { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": { - "x": 5.692854468473409, - "y": 6.73197085882371 + "x": 5.679897290992685, + "y": 7.099142454512667 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "to shooting pos", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path index f4355817..361bc518 100644 --- a/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path +++ b/src/main/deploy/pathplanner/paths/BlueAmpShoot2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.5585241906825855, - "y": 5.937602754065366 + "x": 7.958634375157244, + "y": 5.872130178424058 }, "prevControl": null, "nextControl": { - "x": 7.649614087916611, - "y": 5.896339809164482 + "x": 7.706285650918117, + "y": 6.147494346140698 }, "isLocked": false, "linkedName": "CollectNote2Blue" }, { "anchor": { - "x": 4.276544769305755, - "y": 6.378845190122813 + "x": 5.309845969632628, + "y": 7.001760527838968 }, "prevControl": { - "x": 6.504518234261694, - "y": 6.748633317086454 + "x": 6.7413602917360045, + "y": 6.534327279805212 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "to shooting pos", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path index b5977d0d..1133b470 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot4.path @@ -16,28 +16,12 @@ }, { "anchor": { - "x": 3.9896948365325304, - "y": 2.433470009797163 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": { - "x": 4.572111136500266, - "y": 1.4350420669953317 - }, - "nextControl": { - "x": 5.802874109263657, - "y": -1.4482422802850352 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 - }, - "prevControl": { - "x": -0.21765755624215544, - "y": -2.3095123797612356 + "x": 6.0499486123527415, + "y": 1.5775872121139263 }, "nextControl": null, "isLocked": false, @@ -46,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.0, + "waypointRelativePos": 0.5, "rotationDegrees": 146.98388319699862, "rotateFast": false } @@ -65,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "to shooting pos", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path index 1bc9186a..dc226559 100644 --- a/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path +++ b/src/main/deploy/pathplanner/paths/BlueSourceShoot5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.2728556716020805, - "y": 2.3496917302623217 + "x": 5.212464042958929, + "y": 1.665230946120256 }, "prevControl": { - "x": 5.3639947805212245, - "y": 1.6819600267165467 + "x": 6.303603151878073, + "y": 0.9974992425744809 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ "rotateFast": false }, "reversed": false, - "folder": "to shooting pos", + "folder": null, "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/CloseLeftPickup.path b/src/main/deploy/pathplanner/paths/CloseLeftPickup.path index 98eef62c..21d276fb 100644 --- a/src/main/deploy/pathplanner/paths/CloseLeftPickup.path +++ b/src/main/deploy/pathplanner/paths/CloseLeftPickup.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "feedShooter" + "name": "shootNote" } } ] @@ -61,7 +61,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": { "rotation": -169.0, "velocity": 0.0 diff --git a/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path b/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path index 9f761ff6..12ffa817 100644 --- a/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path +++ b/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "feedShooter" + "name": "shootNote" } } ] @@ -57,11 +57,11 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 179.37126181017194, "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": { "rotation": -169.0, "velocity": 0.0 diff --git a/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path b/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path index 133c8b95..f09d3daa 100644 --- a/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path +++ b/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path @@ -63,7 +63,7 @@ { "type": "named", "data": { - "name": "feedShooter" + "name": "shootNote" } } ] @@ -83,7 +83,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Collect Notes", + "folder": null, "previewStartingState": { "rotation": -178.1523897340052, "velocity": 0.0 diff --git a/src/main/deploy/pathplanner/paths/SuffleShootPath.path b/src/main/deploy/pathplanner/paths/SuffleShootPath.path index ddc2bb89..b91c8368 100644 --- a/src/main/deploy/pathplanner/paths/SuffleShootPath.path +++ b/src/main/deploy/pathplanner/paths/SuffleShootPath.path @@ -3,45 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.3576779647621067, - "y": 2.17419608235891 + "x": 2.6513193714406413, + "y": 4.0900409202953645 }, "prevControl": null, "nextControl": { - "x": 2.147408380327451, - "y": 1.7744559954678114 + "x": 2.5149846740974624, + "y": 4.021873571623774 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.9329136995566167, - "y": 4.094898451079563 + "x": 1.9696458847247476, + "y": 4.060826342293255 }, "prevControl": { - "x": 0.9596300064068812, - "y": 4.201559677726109 + "x": 1.748962420788093, + "y": 3.860671572676289 }, "nextControl": { - "x": 2.6446460493871116, - "y": 4.016900385344714 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6056470165196877, - "y": 4.094898451079563 - }, - "prevControl": { - "x": 2.6056470165196877, - "y": 3.997400868911001 - }, - "nextControl": { - "x": 2.6056470165196877, - "y": 4.603383628819343 + "x": 3.225872738815466, + "y": 5.200194884375534 }, "isLocked": false, "linkedName": null @@ -62,13 +46,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1, - "rotationDegrees": 144.11786275379578, - "rotateFast": false - }, - { - "waypointRelativePos": 2, - "rotationDegrees": -179.99999999999994, + "waypointRelativePos": 1.0, + "rotationDegrees": 152.349644753163, "rotateFast": false } ], @@ -76,7 +55,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.75, + "waypointRelativePos": 0.45, "command": { "type": "parallel", "data": { @@ -86,11 +65,39 @@ "data": { "name": "feedShooter" } - }, + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 1.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shootNote" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 2.0, + "command": { + "type": "parallel", + "data": { + "commands": [ { "type": "named", "data": { - "name": "intakeOn" + "name": "shootNote" } } ] diff --git a/src/main/deploy/pathplanner/paths/initalShotBlueMiddle.path b/src/main/deploy/pathplanner/paths/initalShotBlueMiddle.path index 2f2cdda6..8000f5bd 100644 --- a/src/main/deploy/pathplanner/paths/initalShotBlueMiddle.path +++ b/src/main/deploy/pathplanner/paths/initalShotBlueMiddle.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3552185662929883, - "y": 4.779511541005064 + "x": 1.3074487833435935, + "y": 5.599460783737701 }, "prevControl": null, "nextControl": { - "x": 1.6325596615157194, - "y": 4.982895010835067 + "x": 1.4243070953520323, + "y": 5.55076982040085 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.233465367831636, - "y": 5.45 + "x": 3.245349124150206, + "y": 5.346267774386083 }, "prevControl": { - "x": 2.0115924916534516, - "y": 5.255861233344088 + "x": 3.0018943074659585, + "y": 5.297576811049233 }, "nextControl": null, "isLocked": false, @@ -61,9 +61,9 @@ "rotateFast": false }, "reversed": false, - "folder": "Initial Shots", + "folder": null, "previewStartingState": { - "rotation": 146.915147071265, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/initalShotBlueSource.path b/src/main/deploy/pathplanner/paths/initalShotBlueSource.path index 295d98ec..6115f15d 100644 --- a/src/main/deploy/pathplanner/paths/initalShotBlueSource.path +++ b/src/main/deploy/pathplanner/paths/initalShotBlueSource.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.28, - "y": 2.87 + "x": 0.7621099939708783, + "y": 4.265328388308022 }, "prevControl": null, "nextControl": { - "x": 1.6775222364859144, - "y": 3.0641387666559114 + "x": 1.159632230456793, + "y": 4.459467154963934 }, "isLocked": false, "linkedName": null @@ -61,7 +61,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Initial Shots", + "folder": null, "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f203003..fb4981b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import static frc.robot.settings.Constants.DriveConstants.*; import com.ctre.phoenix6.hardware.Pigeon2; +import com.fasterxml.jackson.core.sym.Name; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; @@ -43,6 +44,8 @@ import frc.robot.commands.ManualShoot; import frc.robot.commands.RotateRobot; import frc.robot.commands.autoAimParallel; +import frc.robot.commands.NamedCommands.initialShot; +import frc.robot.commands.NamedCommands.shootNote; import frc.robot.commands.goToPose.GoToAmp; import frc.robot.commands.goToPose.GoToClimbSpot; import frc.robot.commands.climber_commands.AutoClimb; @@ -322,6 +325,10 @@ private void registerNamedCommands() { if(intakeExists) { NamedCommands.registerCommand("intakeOn", new InstantCommand(()-> intake.intakeYes(1))); } + if(indexerExists&&shooterExists) { + NamedCommands.registerCommand("initialShot", new initialShot(shooter, indexer, 0.75, 0.5)); + NamedCommands.registerCommand("shootNote", new shootNote(indexer, 0.5)); + } NamedCommands.registerCommand("wait x seconds", new WaitCommand(Preferences.getDouble("wait # of seconds", 0))); } public void teleopInit() { diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index eca9a393..ac37e2a2 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -6,6 +6,7 @@ import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PS4Controller; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.DriveConstants; @@ -28,6 +29,7 @@ public class IndexCommand extends Command { IntakeSubsystem intake; DrivetrainSubsystem drivetrain; AngleShooterSubsystem angleShooterSubsytem; + boolean auto; /** Creates a new IndexCommand. */ public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem) { @@ -38,7 +40,6 @@ public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIf this.intake = intake; this.drivetrain = drivetrain; this.angleShooterSubsytem = angleShooterSubsystem; - // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_IndexerSubsystem, shooter, intake); } @@ -52,10 +53,17 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if(DriverStation.isAutonomousEnabled()) { + auto = true; + } else { + auto = false; + } if (!m_Indexer.isNoteIn()) { intake.intakeYes(1); m_Indexer.on(); - shooter.turnOff(); + if(!auto) { + shooter.turnOff(); + } } else { intake.intakeOff(); if(revUpSupplier.getAsBoolean()) { diff --git a/src/main/java/frc/robot/commands/NamedCommands/initialShot.java b/src/main/java/frc/robot/commands/NamedCommands/initialShot.java new file mode 100644 index 00000000..2881c521 --- /dev/null +++ b/src/main/java/frc/robot/commands/NamedCommands/initialShot.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.NamedCommands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.settings.Constants.ShooterConstants; +import frc.robot.subsystems.IndexerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class initialShot extends Command { + ShooterSubsystem shooter; + IndexerSubsystem indexer; + Timer timer; + double revtime; + double shootTime; + /** Creates a new shootThing. */ + public initialShot(ShooterSubsystem shooter, IndexerSubsystem indexer, double revTime, double shootTime) { + this.indexer = indexer; + this.shooter = shooter; + this.revtime = revtime; + this.shootTime = shootTime; + timer = new Timer(); + addRequirements(shooter, indexer); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.start(); + shooter.shootThing(1); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if(timer.get()>revtime) { + indexer.on(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get()>shootTime; + } +} diff --git a/src/main/java/frc/robot/commands/NamedCommands/shootNote.java b/src/main/java/frc/robot/commands/NamedCommands/shootNote.java new file mode 100644 index 00000000..e7896d41 --- /dev/null +++ b/src/main/java/frc/robot/commands/NamedCommands/shootNote.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.NamedCommands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.IndexerSubsystem; + +public class shootNote extends Command { + IndexerSubsystem indexer; + Timer timer; + double shootTime; + /** Creates a new shootNote. */ + public shootNote(IndexerSubsystem indexer, double shootTime) { + this.indexer = indexer; + this.shootTime = shootTime; + timer = new Timer(); + addRequirements(indexer); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.start(); + indexer.on(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.get()>shootTime; + } +}