diff --git a/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto b/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto new file mode 100644 index 00000000..87a18024 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.4, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5mForwardRotate" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideFar+AutoLine4Score.auto b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/PresetAmpSideFar+AutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto diff --git a/src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4N3.auto b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4N3.auto rename to src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto new file mode 100644 index 00000000..6965221a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerPassAutoLine" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSide2Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSide2Score.auto new file mode 100644 index 00000000..fbb31c20 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSide2Score.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNoteReturn" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto rename to src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto new file mode 100644 index 00000000..2a639396 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNote2Return" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerLowNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score.auto diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto diff --git a/src/main/deploy/pathplanner/autos/PresetMid1Score.auto b/src/main/deploy/pathplanner/autos/PresetMid1Score.auto index 7822078a..281017e2 100644 --- a/src/main/deploy/pathplanner/autos/PresetMid1Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetMid1Score.auto @@ -16,12 +16,6 @@ "data": { "name": "SubwooferLaunch" } - }, - { - "type": "path", - "data": { - "pathName": "MidSpeakerPassAutoLine" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/PresetMidAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/PresetMidAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto b/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto index c83d529a..6fa131e5 100644 --- a/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "rotation": 120.0 }, @@ -16,12 +16,6 @@ "data": { "name": "SubwooferLaunch" } - }, - { - "type": "path", - "data": { - "pathName": "LowSpeakerCenterLine" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto b/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto new file mode 100644 index 00000000..0aef4b66 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.47 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowSpeakerPassAutoLine" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto similarity index 98% rename from src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto index a636b97e..53a9f7f4 100644 --- a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "rotation": 120.0 }, diff --git a/src/main/deploy/pathplanner/autos/TopSpeakerCenterLineN1N2AutoLine1.auto b/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/TopSpeakerCenterLineN1N2AutoLine1.auto rename to src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto diff --git a/src/main/deploy/pathplanner/autos/TuneRotationalPID.auto b/src/main/deploy/pathplanner/autos/TuneRotationalPID.auto new file mode 100644 index 00000000..dd789b04 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TuneRotationalPID.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.3, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TuneRotationPID" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TuneTranslationalPID.auto b/src/main/deploy/pathplanner/autos/TuneTranslationalPID.auto new file mode 100644 index 00000000..b86e9f35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TuneTranslationalPID.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.3, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TuneTranslationalPID" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto new file mode 100644 index 00000000..c19c5ecd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerToHighNoteLaunch2" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighNoteLaunch2ToFarHighNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteCenterLineLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNote" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine5Score.auto rename to src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto diff --git a/src/main/deploy/pathplanner/autos/VisionMidFar4Score2.auto b/src/main/deploy/pathplanner/autos/VisionMidFar4Score2.auto new file mode 100644 index 00000000..460bc18a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionMidFar4Score2.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MidNoteToCenterMidNodeLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchPosFirstNoteCenterLineLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionMidFar4Score3.auto b/src/main/deploy/pathplanner/autos/VisionMidFar4Score3.auto new file mode 100644 index 00000000..cff2b961 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionMidFar4Score3.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MidNoteToCenterMidNodeLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchPosSecondCenterLineNoteLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto index 49933a47..03d3a316 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.48 + "y": 4.47 }, "rotation": 120.0 }, @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "lowLaunchNoteFourCenterLine" + "pathName": "LowLaunchNoteFourCenterLine" } }, { diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide3ScoreUnderStage.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide3ScoreUnderStage.auto new file mode 100644 index 00000000..545a6232 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide3ScoreUnderStage.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.47 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LowSpeakerToMiddleLowNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MiddleLowNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowLaunchNoteFourCenterLineUnderStage" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto similarity index 98% rename from src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto index ec8d70f8..a7dbf162 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.48 + "y": 4.47 }, "rotation": 120.0 }, diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto index d0587928..555cc4d3 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, - "y": 4.55 + "x": 0.73, + "y": 4.47 }, "rotation": 120.0 }, diff --git a/src/main/deploy/pathplanner/autos/nothing.auto b/src/main/deploy/pathplanner/autos/nothing.auto new file mode 100644 index 00000000..9fc664f4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/nothing.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.5, + "y": 2.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/5mForwardRotate.path b/src/main/deploy/pathplanner/paths/5mForwardRotate.path new file mode 100644 index 00000000..79d1fb9a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/5mForwardRotate.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.4, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.4, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.4, + "y": 7.0 + }, + "prevControl": { + "x": 4.4, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path index 2d3093eb..8e484700 100644 --- a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.6561390233992404, - "y": 3.9793961091774825 + "x": 2.5847640884470957, + "y": 4.095403506122328 }, "isLocked": false, "linkedName": "Podium" @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, @@ -63,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 140.5993393365206, + "rotation": 163.23907497881294, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/HighCenterSteal.path b/src/main/deploy/pathplanner/paths/HighCenterSteal.path index 1174c7cb..95c0b7a9 100644 --- a/src/main/deploy/pathplanner/paths/HighCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/HighCenterSteal.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.7207761529808778, + "x": 1.7207761529808776, "y": 6.857329003229436 }, "isLocked": false, @@ -52,12 +52,12 @@ "y": 4.132382318818692 }, "prevControl": { - "x": 6.456562495789792, - "y": 4.119496578235857 + "x": 6.787637867361823, + "y": 4.062596022400948 }, "nextControl": { - "x": 5.062344539103492, - "y": 4.141627021992783 + "x": 4.8985812257338806, + "y": 4.177945498748141 }, "isLocked": false, "linkedName": null @@ -84,7 +84,7 @@ }, { "waypointRelativePos": 0.25, - "rotationDegrees": 14.22369918817612, + "rotationDegrees": -172.7651863860264, "rotateFast": false }, { @@ -97,9 +97,9 @@ { "name": "New Constraints Zone", "minWaypointRelativePos": 0.9, - "maxWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, "constraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path index 31be5309..bd42028b 100644 --- a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 4.9, - "y": 6.998077532170032 + "x": 5.41, + "y": 6.91 }, "prevControl": { - "x": 5.379663050445332, - "y": 7.098633898139333 + "x": 5.889663050445332, + "y": 7.010556365969301 }, "nextControl": null, "isLocked": false, @@ -47,7 +47,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.75, - "rotationDegrees": 180.0, + "rotationDegrees": -172.5750748946377, "rotateFast": false } ], @@ -79,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -168.16, + "rotation": -165.92, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path b/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path index 868873a3..f5bd17b9 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.8990839963661905, - "y": 5.546821904454621 + "x": 2.8898392931920998, + "y": 5.556066607628711 }, "prevControl": { - "x": 3.189191600351392, - "y": 5.318880215609106 + "x": 3.037754543977556, + "y": 5.352683137798709 }, "nextControl": { - "x": 2.640232307491642, - "y": 5.750205374284623 + "x": 2.6556051426511798, + "y": 5.878138564622476 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path b/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path new file mode 100644 index 00000000..bd228207 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": null, + "nextControl": { + "x": 1.4199314885116259, + "y": 6.600718066300997 + }, + "isLocked": false, + "linkedName": "TopSpeaker" + }, + { + "anchor": { + "x": 1.7804749123011758, + "y": 5.898120625070079 + }, + "prevControl": { + "x": 1.514854202566117, + "y": 6.248257015175384 + }, + "nextControl": { + "x": 1.9838583821311784, + "y": 5.630024233021438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7713929062062417, + "y": 4.07539893464585 + }, + "prevControl": { + "x": 2.7954992632163718, + "y": 4.0608860165816605 + }, + "nextControl": { + "x": 2.5385405725766406, + "y": 4.215584647385511 + }, + "isLocked": false, + "linkedName": "Podium" + }, + { + "anchor": { + "x": 2.0947948202202706, + "y": 5.18627848066507 + }, + "prevControl": { + "x": 2.3979195267252282, + "y": 4.703524318453471 + }, + "nextControl": { + "x": 1.8451878345198125, + "y": 5.583800717150984 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": { + "x": 1.3274844567707151, + "y": 6.6931650980419075 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopSpeaker" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.6, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path index 4bc34150..8c00ac0f 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -146.8214883406072, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path index b569cf4a..fdd3fbb0 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 6.63 + "y": 6.62 }, "prevControl": null, "nextControl": { "x": 1.0476290014881247, - "y": 6.886930039136037 + "y": 6.876930039136037 }, "isLocked": false, "linkedName": "TopSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path new file mode 100644 index 00000000..37880f99 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.91, + "y": 1.95 + }, + "prevControl": null, + "nextControl": { + "x": 5.616894662362428, + "y": 1.2460539206809202 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.3, + "y": 2.4481294676875076 + }, + "prevControl": { + "x": 8.504807866758592, + "y": 1.9936862730868328 + }, + "nextControl": { + "x": 8.073502500912618, + "y": 2.950699259968285 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.758300373743605, + "y": 2.858594288617149 + }, + "prevControl": { + "x": 5.2064298414327865, + "y": 0.24921078413750497 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 146.44882381880868, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 162.59131699825656, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 132.28885464347817, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 124.79, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path new file mode 100644 index 00000000..01aa2e05 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path @@ -0,0 +1,102 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.91, + "y": 1.95 + }, + "prevControl": null, + "nextControl": { + "x": 5.250408215102145, + "y": 1.2900322943503038 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.3, + "y": 2.4481294676875076 + }, + "prevControl": { + "x": 8.337533809215634, + "y": 1.7307971969497182 + }, + "nextControl": { + "x": 8.240937624732393, + "y": 3.5769077252440225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9896948365325304, + "y": 5.321383214195 + }, + "prevControl": { + "x": 5.059835262527669, + "y": 3.9580536303929756 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 146.44882381880868, + "rotateFast": false + }, + { + "waypointRelativePos": 1.65, + "rotationDegrees": 176.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 162.59131699825656, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 176.97498809243632, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 124.79, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path index 539e4dda..df90aa16 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.77, - "y": 4.08 + "x": 2.7713929062062417, + "y": 4.07539893464585 }, "prevControl": null, "nextControl": { - "x": 2.3966676963984557, - "y": 4.103137615644601 + "x": 2.398060602604697, + "y": 4.098536550290452 }, "isLocked": false, - "linkedName": null + "linkedName": "Podium" }, { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": { "x": 1.1980586123334407, - "y": 3.964467068033235 + "y": 3.974467068033235 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path index 30af0d1c..15be5d2c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": null, "nextControl": { "x": 1.689169744135214, - "y": 2.3204424556117518 + "y": 2.3304424556117516 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path index 4d18958b..648629dc 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path @@ -33,11 +33,11 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": { "x": 1.679419985918358, - "y": 3.9389023196098654 + "y": 3.948902319609865 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path index c270d58a..21e23e74 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": null, "nextControl": { "x": 1.2211813497261212, - "y": 3.05167432187596 + "y": 3.0616743218759597 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path index c89fea9a..23c01db3 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path @@ -36,7 +36,7 @@ "y": 4.47 }, "prevControl": { - "x": 2.0791600728094584, + "x": 2.079160072809459, "y": 3.3149177937310745 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path index a8833ea8..179770b8 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": null, "nextControl": { "x": 1.6986020361229917, - "y": 4.087669396600055 + "y": 4.097669396600055 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path index 0c370098..f82cbb29 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path @@ -8,51 +8,35 @@ }, "prevControl": null, "nextControl": { - "x": 1.031653955199802, - "y": 3.6786365664623215 + "x": 1.6002032004064015, + "y": 1.5832214521571912 }, "isLocked": false, "linkedName": "LowSpeaker" }, { "anchor": { - "x": 3.034636378061356, - "y": 1.5112125236126965 + "x": 6.97, + "y": 0.8 }, "prevControl": { - "x": 1.7459946013496772, - "y": 2.4864008951782903 + "x": 4.942164998549343, + "y": 0.8 }, "nextControl": { - "x": 4.116859540132383, - "y": 0.6922328333967845 + "x": 7.077491190809847, + "y": 0.8 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.970353683931332, - "y": 0.8080477216589235 - }, - "prevControl": { - "x": 6.689293606832576, - "y": 0.8080477216589235 - }, - "nextControl": { - "x": 7.07784487474118, - "y": 0.8080477216589235 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 9.011238164994147, + "x": 9.01, "y": 0.81 }, "prevControl": { - "x": 8.449118010796635, + "x": 8.447879845802488, "y": 0.81 }, "nextControl": null, @@ -62,18 +46,31 @@ ], "rotationTargets": [ { - "waypointRelativePos": 2.6, - "rotationDegrees": 180.0, - "rotateFast": false - }, - { - "waypointRelativePos": 3, + "waypointRelativePos": 1.35, "rotationDegrees": 180.0, "rotateFast": false } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/lowLaunchNoteFourCenterLine.path b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path similarity index 62% rename from src/main/deploy/pathplanner/paths/lowLaunchNoteFourCenterLine.path rename to src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path index fcdf0c5d..77155f74 100644 --- a/src/main/deploy/pathplanner/paths/lowLaunchNoteFourCenterLine.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path @@ -3,73 +3,73 @@ "waypoints": [ { "anchor": { - "x": 2.91, - "y": 1.95 + "x": 2.9, + "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 3.91, - "y": 1.9500000000000002 + "x": 3.9067566423421134, + "y": 5.445130169539619 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.288745946861262, - "y": 2.477580450656397 + "x": 5.059835262527669, + "y": 4.265902246090207 }, "prevControl": { - "x": 6.088506591427597, - "y": 1.1740773031095617 + "x": 4.106065843242836, + "y": 4.566441586006087 }, "nextControl": { - "x": 8.906117742547979, - "y": 2.843334329613654 + "x": 6.137894028707098, + "y": 3.926198473430942 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.5784758596544215, - "y": 1.78422771259957 + "x": 8.3, + "y": 4.104648209296419 }, "prevControl": { - "x": 7.314430903909075, - "y": 1.8818544021435546 + "x": 8.202270809898764, + "y": 4.4595947557966555 }, "nextControl": { - "x": 5.6724949485935, - "y": 1.6640465713363868 + "x": 8.397729190101238, + "y": 3.749701662796183 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.008448377257114, - "y": 2.2002393554336663 + "x": 5.589292620026682, + "y": 4.265902246090207 }, "prevControl": { - "x": 4.979142210536673, - "y": 1.8027171189477522 + "x": 6.35984666440237, + "y": 3.9957190898956214 }, "nextControl": { - "x": 2.6107545939221435, - "y": 2.7726282381327487 + "x": 4.757269334358488, + "y": 4.55763866482688 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.30385717914293, - "y": 4.0606698356253865 + "x": 4.44, + "y": 5.78 }, "prevControl": { - "x": 2.4797706738270624, - "y": 3.3423563989985126 + "x": 4.4337047232653015, + "y": 5.001384417183249 }, "nextControl": null, "isLocked": false, @@ -81,7 +81,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.6, + "waypointRelativePos": 0, "command": { "type": "parallel", "data": { @@ -89,7 +89,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "VisionLaunch" } } ] @@ -105,13 +105,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 140.0, + "rotation": 180.0, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 124.79, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path index 80f71ce7..884e7744 100644 --- a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 4.400159657462172, + "x": 4.45, "y": 4.8 }, "prevControl": { - "x": 3.9457164628614976, - "y": 4.910918393265359 + "x": 3.9750353786421866, + "y": 4.9548967669363915 }, "nextControl": { - "x": 4.876891015948452, - "y": 4.6836416147699715 + "x": 5.107679146631323, + "y": 4.5855158702064 }, "isLocked": false, "linkedName": null @@ -56,8 +56,8 @@ "y": 3.2543996516564464 }, "nextControl": { - "x": 8.94894077946899, - "y": 4.467680827130437 + "x": 9.077551692461096, + "y": 4.542485133666662 }, "isLocked": false, "linkedName": null @@ -81,6 +81,11 @@ "waypointRelativePos": 1.8, "rotationDegrees": 180.0, "rotateFast": false + }, + { + "waypointRelativePos": 2.9, + "rotationDegrees": -156.94, + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path index 042878f6..fd501f89 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 8.3, - "y": 0.8 + "x": 9.01, + "y": 0.81 }, "prevControl": null, "nextControl": { - "x": 5.795424362277297, - "y": 1.0847998838854824 + "x": 6.502808157044885, + "y": 1.2707133785696143 }, "isLocked": false, - "linkedName": null + "linkedName": "CenterLineN5" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 1.9539173222406319 }, "prevControl": { - "x": 4.517435320584927, - "y": 1.275372836459959 + "x": 4.649370441598027, + "y": 1.5685619942668465 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path index a493f90b..3348acba 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path @@ -33,11 +33,11 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": { "x": 1.4899471997783693, - "y": 3.0719258673640133 + "y": 3.081925867364013 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path b/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path index 47993557..42d480e4 100644 --- a/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path +++ b/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.7104749123011758, + "x": 1.7104749123011755, "y": 7.595188383710101 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/tunePID.path b/src/main/deploy/pathplanner/paths/TuneRotationPID.path similarity index 82% rename from src/main/deploy/pathplanner/paths/tunePID.path rename to src/main/deploy/pathplanner/paths/TuneRotationPID.path index ec9e6372..9d5868fb 100644 --- a/src/main/deploy/pathplanner/paths/tunePID.path +++ b/src/main/deploy/pathplanner/paths/TuneRotationPID.path @@ -4,24 +4,24 @@ { "anchor": { "x": 2.3, - "y": 2.0 + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 3.051535872234666, - "y": 2.0 + "x": 3.3, + "y": 7.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.5, - "y": 2.0 + "x": 6.3, + "y": 7.0 }, "prevControl": { - "x": 4.876760993243372, - "y": 2.0 + "x": 5.3, + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 180.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path b/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path new file mode 100644 index 00000000..7dbd0f72 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.3, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.3, + "y": 7.0 + }, + "prevControl": { + "x": 5.3, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path b/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path index 5e7923b3..70fd0573 100644 --- a/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path @@ -8,24 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.092311041764941, - "y": 6.406183098080483 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.159294604303494, - "y": 6.391523640190138 - }, - "prevControl": { - "x": 5.7178954724802065, - "y": 6.3466355928860745 - }, - "nextControl": { - "x": 7.024202619833812, - "y": 6.479480387532203 + "x": 4.957219057295258, + "y": 6.611415508545304 }, "isLocked": false, "linkedName": null @@ -36,24 +20,24 @@ "y": 7.461664066185276 }, "prevControl": { - "x": 8.607035964096708, - "y": 7.292808820292638 + "x": 8.602702708501509, + "y": 7.283357704346585 }, "nextControl": { - "x": 7.845520369587389, - "y": 7.630519312077913 + "x": 7.947748466925505, + "y": 7.593599187198374 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.2095867048876965, - "y": 6.831307376900469 + "x": 5.41, + "y": 6.787329003229436 }, "prevControl": { - "x": 5.543597372909033, - "y": 7.0218803294749454 + "x": 6.670713378569614, + "y": 7.227112739939765 }, "nextControl": null, "isLocked": false, @@ -62,8 +46,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.6, - "rotationDegrees": 145.0, + "waypointRelativePos": 0.8, + "rotationDegrees": -128.61667081319877, "rotateFast": false } ], @@ -71,7 +55,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 1.0999999999999999, + "waypointRelativePos": 0.55, "command": { "type": "parallel", "data": { @@ -95,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -156.19405648154228, + "rotation": -165.92, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path index 6c3f157c..4d963f3b 100644 --- a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.532094778475272, - "y": 6.303566892848072 + "x": 4.572375270876667, + "y": 6.35111108060054 }, "isLocked": false, "linkedName": null @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 4.004354294422875, - "y": 6.640734424325992 + "x": 5.478356181937588, + "y": 6.434313409167359 }, "prevControl": { - "x": 3.3446786893573788, - "y": 6.787329003229436 + "x": 6.837327548528969, + "y": 6.332621674252358 }, "nextControl": null, "isLocked": false, @@ -62,8 +62,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.5, - "rotationDegrees": 130.88664561136105, + "waypointRelativePos": 1.6, + "rotationDegrees": 149.98, + "rotateFast": false + }, + { + "waypointRelativePos": 2.0999999999999996, + "rotationDegrees": 151.4886678965681, "rotateFast": false } ], @@ -95,7 +100,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -162.8972710309476, + "rotation": -170.32327316993567, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path index 1ad4eea1..ead4c42a 100644 --- a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.9, - "y": 7.0 + "x": 5.41, + "y": 6.79 }, "prevControl": null, "nextControl": { - "x": 7.082840451395188, - "y": 6.538118219093581 + "x": 7.391052103040704, + "y": 6.450099183528485 }, "isLocked": false, "linkedName": null @@ -20,31 +20,37 @@ "y": 5.777939483806896 }, "prevControl": { - "x": 8.140830696075806, - "y": 6.508271034560088 + "x": 8.331400517956347, + "y": 6.093373983450684 }, "nextControl": { - "x": 8.323025974780654, - "y": 5.548709233381218 + "x": 8.211618708951704, + "y": 5.365361587866033 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.7680860947307475, - "y": 6.609962769475088 + "x": 5.41, + "y": 6.347545266519105 }, "prevControl": { - "x": 5.764941980334411, - "y": 6.831835645653273 + "x": 7.097499909285533, + "y": 6.450161471751516 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": -148.64916364014846, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { @@ -73,13 +79,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -163.097682967436, + "rotation": -170.61848983925177, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": -168.16, + "rotation": -165.92, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path index 5df50cec..95c41790 100644 --- a/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path @@ -3,29 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.9, - "y": 7.0 + "x": 5.41, + "y": 6.91 }, "prevControl": null, "nextControl": { - "x": 4.6833117089657605, - "y": 6.5360051440823606 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.147118924868479, - "y": 6.027546469507354 - }, - "prevControl": { - "x": 4.609354083573032, - "y": 6.434313409167359 - }, - "nextControl": { - "x": 3.596045138868069, - "y": 5.542601537826994 + "x": 5.001197430966291, + "y": 6.68743713487427 }, "isLocked": false, "linkedName": null @@ -36,8 +20,8 @@ "y": 5.55 }, "prevControl": { - "x": 3.2873615296780136, - "y": 5.574556013976893 + "x": 3.242062484124969, + "y": 5.702529119343954 }, "nextControl": null, "isLocked": false, @@ -49,7 +33,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0, + "waypointRelativePos": 0.5, "command": { "type": "parallel", "data": { @@ -79,7 +63,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -168.16, + "rotation": -165.92, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index c0df2eda..16cf5edb 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,7 +1,6 @@ package frc.team2412.robot; import com.ctre.phoenix6.SignalLogger; -import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; @@ -11,7 +10,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -20,9 +18,9 @@ import frc.team2412.robot.Subsystems.SubsystemConstants; import frc.team2412.robot.commands.diagnostic.IntakeDiagnosticCommand; import frc.team2412.robot.commands.diagnostic.LauncherDiagnosticCommand; -import frc.team2412.robot.util.AutoLogic; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.MatchDashboard; +import frc.team2412.robot.util.auto.AutoLogic; public class Robot extends TimedRobot { /** Singleton Stuff */ @@ -51,8 +49,6 @@ public static Robot getInstance() { public MatchDashboard dashboard; public AutoLogic autoLogic; - public SendableChooser autoChooser; - protected Robot(RobotType type) { // non public for singleton. Protected so test class can subclass instance = this; @@ -86,17 +82,14 @@ public void robotInit() { subsystems = new Subsystems(); controls = new Controls(subsystems); - autoLogic = new AutoLogic(); - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - SmartDashboard.putString("current bot", getTypeFromAddress().toString()); + AutoLogic.registerCommands(); if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { - autoChooser = AutoBuilder.buildAutoChooser(); - } else { - autoChooser = new SendableChooser<>(); + AutoLogic.initShuffleBoard(); } + SmartDashboard.putString("current bot", getTypeFromAddress().toString()); + Shuffleboard.startRecording(); if (RobotBase.isReal()) { @@ -145,7 +138,10 @@ public void autonomousInit() { // Checks if FMS is attatched and enables joystick warning if true DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached()); - autoChooser.getSelected().schedule(); + // System.out.println(AutoLogic.getSelected() != null); + if (AutoLogic.getSelectedAuto() != null) { + AutoLogic.getSelectedAuto().schedule(); + } } @Override diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index e57a70b3..139af72c 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -71,7 +71,7 @@ public class DrivebaseSubsystem extends SubsystemBase { Robot.getInstance().getRobotType() == RobotType.PRACTICE ? new PIDConstants(5, 0, 0.5) // practice : Robot.getInstance().getRobotType() == RobotType.BONK - ? new PIDConstants(5, 0, 0.1) // bonk + ? new PIDConstants(6, 0, 0.1) // bonk : Robot.getInstance().getRobotType() == RobotType.CRANE ? new PIDConstants(3.9, 0, 0.2) // crane : new PIDConstants(0.1, 0, 0.1); // bobot TODO: tune diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/AutoLogic.java deleted file mode 100644 index 8dcd81da..00000000 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ /dev/null @@ -1,105 +0,0 @@ -package frc.team2412.robot.util; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.team2412.robot.Controls; -import frc.team2412.robot.Robot; -import frc.team2412.robot.Subsystems; -import frc.team2412.robot.commands.intake.AllInCommand; -import frc.team2412.robot.commands.intake.AllInSensorOverrideCommand; -import frc.team2412.robot.commands.intake.FeederInCommand; -import frc.team2412.robot.commands.intake.IntakeStopCommand; -import frc.team2412.robot.commands.launcher.FullTargetCommand; -import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; -import frc.team2412.robot.commands.launcher.StopLauncherCommand; -import frc.team2412.robot.subsystems.LauncherSubsystem; - -public class AutoLogic { - public static Robot r = Robot.getInstance(); - public static final Subsystems s = r.subsystems; - public static final Controls controls = r.controls; - - // in place of launching command cause launcher doesnt exist - public static SequentialCommandGroup vibrateControllerCommand = - new SequentialCommandGroup( - new InstantCommand(() -> controls.vibrateDriveController(0.5)), - new WaitCommand(1.5), - new InstantCommand(() -> controls.vibrateDriveController(0.0))); - - /** - * Placeholder for vision detect note - * - * @return true - */ - public static boolean dummyLogic() { - return true; - } - - public AutoLogic() { - - registerCommands(); - } - - /** Registers commands in PathPlanner */ - public void registerCommands() { - // param: String commandName, Command command - - // Intake - NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null)); - NamedCommands.registerCommand( - "IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem)); - // Launcher - NamedCommands.registerCommand( - "VisionLaunch", - Commands.sequence( - new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls), - new FeederInCommand(s.intakeSubsystem))); - - NamedCommands.registerCommand( - "SubwooferLaunch", - new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE) - .andThen(new WaitCommand(1)) - .andThen(new FeederInCommand(s.intakeSubsystem).andThen(new WaitCommand(0.5)))); - NamedCommands.registerCommand("StopLaunch", new StopLauncherCommand(s.launcherSubsystem)); - NamedCommands.registerCommand( - "RetractPivot", - new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0)); // TODO: add retract angle - // Complex Autos - NamedCommands.registerCommand("AutoLogicTest", AutoPaths.testAuto); - NamedCommands.registerCommand( - "MidSpeakerCenterLineN5N4N3", AutoPaths.midSpeakerCenterLineN3N2N1); - NamedCommands.registerCommand( - "LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4N3); - NamedCommands.registerCommand("LowSpeakerCenterLineN5N4", AutoPaths.lowSpeakerCenterLineN5N4); - NamedCommands.registerCommand( - "TopSpeakerCenterLineN1N2AutoLine1", AutoPaths.TopSpeakerCenterLineN1N2AutoLine1); - NamedCommands.registerCommand( - "TopSpeakerCenterLineN1N2N3", AutoPaths.TopSpeakerCenterLineN1N2N3); - } - - // public Command getConditionalCommand(){} - - /** - * Takes a PathPlanner path and returns it as a command. - * - * @param pathName - * @return follow path command - */ - public static Command getAutoCommand(String pathName) { - // Load the path you want to follow using its name in the GUI - PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - - // Create a path following command using AutoBuilder. This will also trigger event markers. - return AutoBuilder.followPath(path); - } -} diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 451dba90..f5853231 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -3,14 +3,13 @@ import com.pathplanner.lib.path.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import frc.team2412.robot.util.auto.AutoLogic; import java.util.List; import java.util.Map; import java.util.Optional; @@ -31,14 +30,18 @@ public static void configureShuffleboardTab( tab.add("Auto display speed", DEFAULT_PLAYBACK_SPEED) .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("Min", 0.5, "Max", 2.5)) - .withPosition(columnIndex, rowIndex + 2) // Offset by height of Field2d display - .withSize(2, 1) + .withPosition(columnIndex + 1, rowIndex + 3) // Offset by height of Field2d display + .withSize(3, 1) .getEntry(); - StringSubscriber activeAutoSub = - NetworkTableInstance.getDefault() - .getTable("Shuffleboard/Match/Auto Chooser") - .getStringTopic("active") - .subscribe(""); + + tab.addDouble("Est. Time", AutoLogic::getEstimatedAutoDuration) + .withPosition(columnIndex, rowIndex + 3); + + // StringSubscriber activeAutoSub = + // NetworkTableInstance.getDefault() + // .getTable("Shuffleboard/Match/" + autoChooserName) + // .getStringTopic("active") + // .subscribe(""); var autonomousField = new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); var watchdog = @@ -46,7 +49,8 @@ public static void configureShuffleboardTab( addPeriodic.accept( () -> { watchdog.reset(); - autonomousField.update(activeAutoSub.get()); + + autonomousField.update(AutoLogic.getSelectedAutoName()); watchdog.addEpoch("AutonomousField.update()"); watchdog.disable(); if (watchdog.isExpired()) { @@ -56,7 +60,7 @@ public static void configureShuffleboardTab( UPDATE_RATE); tab.add("Selected auto", autonomousField.getField()) .withPosition(columnIndex, rowIndex) - .withSize(2, 2); + .withSize(4, 3); } // Display @@ -123,6 +127,7 @@ public Pose2d getUpdatedPose(String autoName) { lastFPGATime = fpgaTime; while (lastTrajectoryTimeOffset > trajectories.get(trajectoryIndex).getTotalTimeSeconds()) { lastTrajectoryTimeOffset -= trajectories.get(trajectoryIndex).getTotalTimeSeconds(); + trajectoryIndex++; if (trajectoryIndex >= trajectories.size()) { trajectoryIndex = 0; } @@ -139,11 +144,14 @@ public Pose2d getUpdatedPose(String autoName) { * * @param autoName The name of the selected PathPlanner autonomous routine. */ - public void update(String autoName) { + public void update(Optional autoName) { if (DriverStation.isEnabled()) { lastName = Optional.empty(); return; } - field.setRobotPose(getUpdatedPose(autoName)); + + if (autoName.isPresent()) { + field.setRobotPose(getUpdatedPose(autoName.get())); + } } } diff --git a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java new file mode 100644 index 00000000..e0c2ef47 --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java @@ -0,0 +1,174 @@ +package frc.team2412.robot.util; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.LinkedHashMap; +import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; + +public class DynamicSendableChooser implements Sendable, AutoCloseable { + + /** The key for the default value. */ + private static final String DEFAULT = "default"; + + /** The key for the selected option. */ + private static final String SELECTED = "selected"; + + /** The key for the active option. */ + private static final String ACTIVE = "active"; + + /** The key for the option array. */ + private static final String OPTIONS = "options"; + + /** The key for the instance number. */ + private static final String INSTANCE = ".instance"; + + /** A map linking strings to the objects they represent. */ + public final Map m_map = new LinkedHashMap<>(); + + private String m_defaultChoice = ""; + private final int m_instance; + private String m_previousVal; + private Consumer m_listener; + private static final AtomicInteger s_instances = new AtomicInteger(); + + /** Instantiates a {@link DynamicSendableChooser}. */ + @SuppressWarnings("this-escape") + public DynamicSendableChooser() { + m_instance = s_instances.getAndIncrement(); + SendableRegistry.add(this, "SendableChooser", m_instance); + } + + @Override + public void close() { + SendableRegistry.remove(this); + } + + /** + * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the + * object will appear as the given name. + * + * @param name the name of the option + * @param object the option + */ + public void addOption(String name, T object) { + m_map.put(name, object); + } + + /** + * Removes the given object from the list of options. + * + * @param object the option + */ + public void removeOption(String name) { + m_map.remove(name); + } + + /** Removes all objects from the list of options */ + public void clearOptions() { + m_map.clear(); + } + + /** + * Adds the given object to the list of options and marks it as the default. Functionally, this is + * very close to {@link #addOption(String, Object)} except that it will use this as the default + * option if none other is explicitly selected. + * + * @param name the name of the option + * @param object the option + */ + public void setDefaultOption(String name, T object) { + requireNonNullParam(name, "name", "setDefaultOption"); + + m_defaultChoice = name; + addOption(name, object); + } + + /** + * Returns the selected option. If there is none selected, it will return the default. If there is + * none selected and no default, then it will return {@code null}. + * + * @return the option selected + */ + public T getSelected() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_map.get(m_selected); + } else { + return m_map.get(m_defaultChoice); + } + } finally { + m_mutex.unlock(); + } + } + + /** + * Bind a listener that's called when the selected value changes. Only one listener can be bound. + * Calling this function will replace the previous listener. + * + * @param listener The function to call that accepts the new value + */ + public void onChange(Consumer listener) { + requireNonNullParam(listener, "listener", "onChange"); + m_mutex.lock(); + m_listener = listener; + m_mutex.unlock(); + } + + private String m_selected; + private final ReentrantLock m_mutex = new ReentrantLock(); + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("String Chooser"); + builder.publishConstInteger(INSTANCE, m_instance); + builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); + builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); + builder.addStringProperty( + ACTIVE, + () -> { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + }, + null); + builder.addStringProperty( + SELECTED, + null, + val -> { + T choice; + Consumer listener; + m_mutex.lock(); + try { + m_selected = val; + if (!m_selected.equals(m_previousVal) && m_listener != null) { + choice = m_map.get(val); + listener = m_listener; + } else { + choice = null; + listener = null; + } + m_previousVal = val; + } finally { + m_mutex.unlock(); + } + if (listener != null) { + listener.accept(choice); + } + }); + } +} diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index c0849f1f..d8290074 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -20,7 +20,6 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - tab.add("Auto Chooser", r.autoChooser).withPosition(4, 0).withSize(2, 1); - AutonomousField.configureShuffleboardTab(tab, 6, 0, "Auto Chooser", r::addPeriodic); + AutonomousField.configureShuffleboardTab(tab, 7, 0, "Available Auto Variants", r::addPeriodic); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java new file mode 100644 index 00000000..02fd9d60 --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -0,0 +1,336 @@ +package frc.team2412.robot.util.auto; + +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPlannerTrajectory; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.team2412.robot.Controls; +import frc.team2412.robot.Robot; +import frc.team2412.robot.Subsystems; +import frc.team2412.robot.commands.intake.AllInCommand; +import frc.team2412.robot.commands.intake.AllInSensorOverrideCommand; +import frc.team2412.robot.commands.intake.FeederInCommand; +import frc.team2412.robot.commands.intake.IntakeStopCommand; +import frc.team2412.robot.commands.launcher.FullTargetCommand; +import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; +import frc.team2412.robot.commands.launcher.StopLauncherCommand; +import frc.team2412.robot.subsystems.LauncherSubsystem; +import frc.team2412.robot.util.DynamicSendableChooser; +import frc.team2412.robot.util.PathPlannerAutos; +import frc.team2412.robot.util.PathPlannerAutos.Auto; +import java.util.List; +import java.util.Map; +import java.util.Optional; + +public class AutoLogic { + public static Robot r = Robot.getInstance(); + public static final Subsystems s = r.subsystems; + public static final Controls controls = r.controls; + + public static enum StartPosition { + AMP_SIDE_SUBWOOFER( + "Amp Side Subwoofer", new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), + MID_SIDE_SUBWOOFER( + "Mid Side Subwoofer", new Pose2d(1.33, 5.55, new Rotation2d(Units.degreesToRadians(180)))), + SOURCE_SIDE_SUBWOOFER( + "Source Side Subwoofer", + new Pose2d(0.73, 4.47, new Rotation2d(Units.degreesToRadians(120)))), + MISC("Misc", null); + + final String title; // for shuffleboard display + final Pose2d startPose; // for identifying path's starting positions for filtering + + StartPosition(String title, Pose2d startPose) { + this.title = title; + this.startPose = startPose; + } + }; + + static { + registerCommands(); + } + + // paths lists + + private static AutoPath defaultPath = new AutoPath("do nothing", "nothing"); + + private static List noPiecePaths = + List.of( + // presets + new AutoPath("Test Path Rotate", "5mForwardRotate180"), + new AutoPath("Test Path", "DiameterTest"), + new AutoPath("Tune Translational PID", "TuneTranslationalPID"), + new AutoPath("Tune Rotational PID", "TuneRotationalPID"), + new AutoPath("Stand Still", "PresetSourceSide1Score"), + new AutoPath("Stand Still", "PresetMid1Score"), + new AutoPath("Stand Still", "PresetAmpSide1Score"), + // new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), + new AutoPath("Pass Autoline", "PresetAmpSide1ScorePassAutoline"), + new AutoPath("Pass Autoline", "PresetSourceSide1ScorePassAutoline")); + + private static List onePiecePaths = + List.of( + // presets + new AutoPath("Autoline N1", "PresetAmpSide2Score"), + new AutoPath("Autoline N2", "PresetMidAutoline2Score"), + new AutoPath("Autoline N3", "PresetSourceSideAutoline2Score"), + new AutoPath("Centerline N5", "PresetSourceSideFar2Score"), + // vision + new AutoPath("Centerline N3 N1", "VisionMidFar2Score", true)); + + private static List twoPiecePaths = + List.of( + // presets + new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), + new AutoPath("Autoline N2", "PresetMidAutoline3Score"), + // vision + new AutoPath("Centerline N5 N4", "VisionSourceSide3Score", true), + new AutoPath( + "Autoline N1 Centerline STEAL(N1 N2 N3 N4) N5", "VisionAmpSideFarSteal", true)); + + private static List threePiecePaths = + List.of( + // presets + new AutoPath("Autoline N1 N2 N3", "PresetAmpSideAutoline4Score"), + new AutoPath("Autoline N2 N3 N1", "PresetMidAutoline4Score"), + new AutoPath("Autoline N3 N2 N1", "PresetSourceSideAutoline4Score"), + new AutoPath("Centerline N1 Autoline Autoline N1 N2", "PresetAmpSideAutolineFar4Score"), + // vision + new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score", true), + new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoline4Score", true), + new AutoPath("Autoline N3 N2 N1", "VisionMid4Score", true), + new AutoPath("Autoline N2 Centerline N3 N1", "VisionMidFar4Score2", true), + new AutoPath("Autoline N2 Centerline N3 N2", "VisionMidFar4Score3", true), + new AutoPath("Autoline N3 N2 N1", "VisionSourceSideAutoline4Score", true), + new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score", true)); + + private static List fourPiecePaths = + List.of( + // presets + // vision + new AutoPath("Centerline N1 Autoline N1 N2 N3", "VisionAmpSideAutoline5Score", true), + new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score", true)); + + private static List fivePiecePaths; + + // map (gulp) + private static Map> commandsMap = + Map.of( + 0, + noPiecePaths, + 1, + onePiecePaths, + 2, + twoPiecePaths, + 3, + threePiecePaths, + 4, + fourPiecePaths); + + // vars + + // in place of launching command cause launcher doesnt exist + public static SequentialCommandGroup vibrateControllerCommand = + new SequentialCommandGroup( + new InstantCommand(() -> controls.vibrateDriveController(0.5)), + new WaitCommand(1.5), + new InstantCommand(() -> controls.vibrateDriveController(0.0))); + + // shuffleboard + private static ShuffleboardTab tab = Shuffleboard.getTab("Match"); + + private static SendableChooser startPositionChooser = + new SendableChooser(); + private static DynamicSendableChooser availableAutos = + new DynamicSendableChooser(); + private static SendableChooser gameObjects = new SendableChooser(); + private static SendableChooser isVision = new SendableChooser(); + + private static GenericEntry autoDelayEntry; + + // methods + + public static Command registerAuto(String autoName, Command command, String... primaryPathNames) { + PathPlannerAutos.registerAuto(autoName, primaryPathNames); + return command.withName(autoName); + } + + /** Registers commands in PathPlanner */ + public static void registerCommands() { + // param: String commandName, Command command + + // Intake + NamedCommands.registerCommand( + "StopIntake", + (INTAKE_ENABLED ? new IntakeStopCommand(s.intakeSubsystem) : Commands.none())); + NamedCommands.registerCommand( + "Intake", (INTAKE_ENABLED ? new AllInCommand(s.intakeSubsystem, null) : Commands.none())); + NamedCommands.registerCommand( + "IntakeSensorOverride", + (INTAKE_ENABLED ? new AllInSensorOverrideCommand(s.intakeSubsystem) : Commands.none())); + // Launcher + NamedCommands.registerCommand( + "VisionLaunch", + (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED + ? Commands.sequence( + new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls), + new FeederInCommand(s.intakeSubsystem)) + : Commands.none())); + + NamedCommands.registerCommand( + "SubwooferLaunch", + (LAUNCHER_ENABLED && INTAKE_ENABLED + ? new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE) + .andThen(new WaitCommand(1)) + .andThen(new FeederInCommand(s.intakeSubsystem)) + .andThen(new WaitCommand(0.5)) + : Commands.none())); + NamedCommands.registerCommand( + "StopLaunch", + (LAUNCHER_ENABLED ? new StopLauncherCommand(s.launcherSubsystem) : Commands.none())); + NamedCommands.registerCommand( + "RetractPivot", + (LAUNCHER_ENABLED && INTAKE_ENABLED + ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0) + : Commands.none())); // TODO: add retract angle + + // Complex Autos + NamedCommands.registerCommand("AutoLogicTest", ComplexAutoPaths.testAuto); + + NamedCommands.registerCommand( + "MidSpeakerCenterLineN3N2N1", ComplexAutoPaths.midSpeakerCenterLineN3N2N1); + NamedCommands.registerCommand( + "LowSpeakerCenterLineN5N4N3", ComplexAutoPaths.lowSpeakerCenterLineN5N4N3); + NamedCommands.registerCommand( + "LowSpeakerCenterLineN5N4", ComplexAutoPaths.lowSpeakerCenterLineN5N4); + NamedCommands.registerCommand( + "TopSpeakerCenterLineN1N2AutoLineN1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLineN1); + NamedCommands.registerCommand( + "TopSpeakerCenterLineN1N2N3", ComplexAutoPaths.TopSpeakerCenterLineN1N2N3); + } + + // public Command getConditionalCommand(){} + + /** + * Takes a PathPlanner path and returns it as a command. + * + * @param pathName + * @return follow path command + */ + public static Command getAutoCommand(String pathName) { + // Load the path you want to follow using its name in the GUI + PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); + + // Create a path following command using AutoBuilder. This will also trigger event markers. + return AutoBuilder.followPath(path); + } + + public static void initShuffleBoard() { + + startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); + for (StartPosition startPosition : StartPosition.values()) { + startPositionChooser.addOption(startPosition.title, startPosition); + } + isVision.setDefaultOption("Presets", false); + isVision.addOption("Vision", true); + gameObjects.setDefaultOption("0", 0); + for (int i = 1; i < commandsMap.size(); i++) { + gameObjects.addOption(String.valueOf(i), i); + } + + tab.add("Starting Position", startPositionChooser).withPosition(5, 0).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(5, 1); + tab.add("Game Objects", gameObjects).withPosition(6, 1); + tab.add("Available Auto Variants", availableAutos).withPosition(5, 2).withSize(2, 1); + autoDelayEntry = tab.add("Auto Delay", 0).withPosition(5, 3).withSize(1, 1).getEntry(); + + isVision.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); + startPositionChooser.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); + gameObjects.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); + + filterAutos(gameObjects.getSelected()); + } + + /** Takes the auto filtering entries in shuffleboard to provide a list of suitable autos */ + public static void filterAutos(int numGameObjects) { + + // resets/clears all options + availableAutos.clearOptions(); + + // filter based off gameobejct count + List autoCommandsList = commandsMap.get(numGameObjects); + + // filter more then add to chooser + for (AutoPath auto : autoCommandsList) { + if (auto.getStartPose().equals(startPositionChooser.getSelected()) + && auto.isVision() == isVision.getSelected()) { + availableAutos.addOption(auto.getDisplayName(), auto); + } + } + } + + // get auto + + public static Optional getSelectedAutoName() { + if (availableAutos.getSelected() == null) { + return Optional.empty(); + } + return Optional.of(availableAutos.getSelected().getAutoName()); + } + + public static boolean chooserHasAutoSelected() { + return availableAutos.getSelected() != null; + } + + public static Command getSelectedAuto() { + return Commands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0)), + availableAutos.getSelected().getAutoCommand()); + } + + /** + * Takes all of the trajectories of an auto to find the total estimated duration of an auto + * + * @return auto duration in seconds + */ + public static double getEstimatedAutoDuration() { + if (getSelectedAutoName().isPresent()) { + + Auto auto = PathPlannerAutos.getAuto(getSelectedAutoName().get()); + double autoTime = 0; + + for (PathPlannerTrajectory trajectory : auto.trajectories) { + autoTime += trajectory.getTotalTimeSeconds(); + } + + // TODO: more accurate estimating by viewing named commands involved + + // rounds time to two decimals + autoTime *= 100; + autoTime = ((double) ((int) autoTime)) / 100; + + // add autoDelay to estimation as well + autoTime += autoDelayEntry.getDouble(0); + + return autoTime; + } + return 0; + } +} diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java new file mode 100644 index 00000000..a739ed25 --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -0,0 +1,85 @@ +package frc.team2412.robot.util.auto; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.util.auto.AutoLogic.StartPosition; + +public class AutoPath { + + private final Pose2d startPose2d; + private StartPosition startPosition; + private String pathPlannerAutoName; + private final String displayName; + private final Command autoCommand; + private final boolean vision; + + public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) { + this.displayName = displayName; + this.pathPlannerAutoName = pathPlannerAutoName; + startPose2d = PathPlannerAuto.getStaringPoseFromAutoFile(pathPlannerAutoName); + autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName); + this.vision = vision; + + // in the case that the auto for whatever reason's starting pose is slightly off, + // is still able to match with a startPosition if it is close enough + for (StartPosition pos : StartPosition.values()) { + if (!pos.equals(StartPosition.MISC) && matchesStartPosition(pos)) { + startPosition = pos; + break; + } + } + if (startPosition == null) { + startPosition = StartPosition.MISC; + } + // debug purposes + // System.out.println(startPosition + " " + displayName + " " + startPose2d.toString()); + + } + + public AutoPath(String displayName, String pathPlannerAutoName) { + this(displayName, pathPlannerAutoName, false); + } + + public StartPosition getStartPose() { + return startPosition; + } + + public Pose2d getStartPose2d() { + return startPose2d; + } + + public String getAutoName() { + return pathPlannerAutoName; + } + + public String getDisplayName() { + return displayName; + } + + public Command getAutoCommand() { + return autoCommand; + } + + public boolean isVision() { + return vision; + } + + /** + * Checks the x, y, and rotation of the auto's starting position and compares it with the expected + * starting position, returning true if it is considered close enough to be the same. + * + * @param expectedStartPosition + * @return if it is matching + */ + public boolean matchesStartPosition(StartPosition expectedStartPosition) { + return (MathUtil.isNear(expectedStartPosition.startPose.getX(), startPose2d.getX(), .05) + && MathUtil.isNear(expectedStartPosition.startPose.getY(), startPose2d.getY(), .05) + && MathUtil.isNear( + expectedStartPosition.startPose.getRotation().getDegrees(), + startPose2d.getRotation().getDegrees(), + 5)); + } +} diff --git a/src/main/java/frc/team2412/robot/util/AutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java similarity index 80% rename from src/main/java/frc/team2412/robot/util/AutoPaths.java rename to src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index e1dc7da2..4c48ffa4 100644 --- a/src/main/java/frc/team2412/robot/util/AutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -1,6 +1,7 @@ -package frc.team2412.robot.util; +package frc.team2412.robot.util.auto; -import static frc.team2412.robot.util.AutoLogic.*; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; +import static frc.team2412.robot.util.auto.AutoLogic.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -9,13 +10,9 @@ import frc.team2412.robot.commands.launcher.FullTargetCommand; import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; +import java.util.function.BooleanSupplier; -public class AutoPaths { - private static Command registerAuto( - String autoName, Command command, String... primaryPathNames) { - PathPlannerAutos.registerAuto(autoName, primaryPathNames); - return command.withName(autoName); - } +public class ComplexAutoPaths { // Test Auto @@ -27,15 +24,15 @@ private static Command registerAuto( new ConditionalCommand( AutoLogic.getAutoCommand("TestPathTrue"), AutoLogic.getAutoCommand("TestPathFalse"), - () -> false)), + checkForTargets())), "TestPath", "TestPathTrue"); // Complex Autos - public static Command TopSpeakerCenterLineN1N2AutoLine1 = + public static Command TopSpeakerCenterLineN1N2AutoLineN1 = registerAuto( - "TopSpeakerCenterLineN1N2AutoLine1", + "TopSpeakerCenterLineN1N2AutoLineN1", Commands.sequence( SubwooferLaunchCommand(), getAutoCommand("TopSpeakerQCenterLineN1"), @@ -52,8 +49,8 @@ private static Command registerAuto( VisionLaunchCommand(), getAutoCommand("LCenterLineN2LAutoLineN1")), Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), - () -> true)), - () -> true), + checkForTargets())), + checkForTargets()), VisionLaunchCommand()), "TopSpeakerQCenterLineN1", "QCenterLineN1LCenterLineN1", @@ -78,8 +75,8 @@ private static Command registerAuto( VisionLaunchCommand(), getAutoCommand("LCenterLineN2LCenterLineN3")), Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), - () -> true)), - () -> true), + checkForTargets())), + checkForTargets()), VisionLaunchCommand()), "TopSpeakerQCenterLineN1", "QCenterLineN1LCenterLineN1", @@ -102,11 +99,11 @@ private static Command registerAuto( VisionLaunchCommand(), getAutoCommand("N3LCenterLineN2LCenterLineN1")), getAutoCommand("QCenterLineN2LCenterLineN1"), - () -> true)), + checkForTargets())), Commands.sequence( getAutoCommand("QCenterLineN3QCenterLineN2"), getAutoCommand("QCenterLineN2LCenterLineN2")), - () -> true), + checkForTargets()), VisionLaunchCommand()), "MidSpeakerQCenterLineN3", "QCenterLineN3LCenterLineN3", @@ -126,7 +123,7 @@ private static Command registerAuto( Commands.waitSeconds(0.5), getAutoCommand("LCenterLineN5LCenterLineN4")), Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), - () -> false)), + checkForTargets())), "LowSpeakerQCenterLineN5", "QCenterLineN5LCenterLineN5", "LCenterLineN5LCenterLineN4"); @@ -146,12 +143,12 @@ private static Command registerAuto( VisionLaunchCommand(), getAutoCommand("LCenterLineN4LCenterLineN3")), Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), - () -> true)), + checkForTargets())), Commands.sequence( getAutoCommand("QCenterLineN5LCenterLineN4"), VisionLaunchCommand(), getAutoCommand("LCenterLineN4LCenterLineN3")), - () -> false), + checkForTargets()), VisionLaunchCommand()), "LowSpeakerQCenterLineN5", "QCenterLineN5QCenterLineN4", @@ -160,14 +157,22 @@ private static Command registerAuto( // new command getters - public static Command VisionLaunchCommand() { - return new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls); + public static final Command VisionLaunchCommand() { + return (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED + ? new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls) + : Commands.none()); + } + + public static final Command SubwooferLaunchCommand() { + return (LAUNCHER_ENABLED + ? new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE) + : Commands.none()); } - public static Command SubwooferLaunchCommand() { - return new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE); + public static BooleanSupplier checkForTargets() { + return (LIMELIGHT_ENABLED ? s.limelightSubsystem::hasTargets : () -> true); } }