diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index cf941033..633c3b0d 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,7 +10,8 @@ "Top side paths" ], "autoFolders": [ - "Point Blank" + "Hide", + "Good" ], "defaultMaxVel": 5.0, "defaultMaxAccel": 4.0, diff --git a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto rename to src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto index 8ff90a92..66ca1fc2 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EF.auto @@ -121,6 +121,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto index 7efbcaff..0d51980d 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto @@ -103,6 +103,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto index 8af55fba..f1dd5869 100644 --- a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto @@ -18,7 +18,32 @@ { "type": "wait", "data": { - "waitTime": 4.0 + "waitTime": 6.0 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Enable-Vision" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Stop-Flywheel" } } ] @@ -39,6 +64,12 @@ "data": { "name": "Smart-Collect" } + }, + { + "type": "named", + "data": { + "name": "Stop-Flywheel" + } } ] } @@ -65,6 +96,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto index aaef91c9..68e4f7e8 100644 --- a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto @@ -24,20 +24,26 @@ ] } }, + { + "type": "named", + "data": { + "name": "Enable-Vision" + } + }, { "type": "deadline", "data": { "commands": [ { - "type": "path", + "type": "wait", "data": { - "pathName": "ST-C1" + "waitTime": 1.0 } }, { "type": "named", "data": { - "name": "Smart-Collect" + "name": "Stop-Flywheel" } } ] @@ -48,26 +54,38 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Point-At-Speaker" + "pathName": "ST-C1" } }, { "type": "named", "data": { - "name": "preAim" + "name": "Smart-Collect" } }, { "type": "named", "data": { - "name": "Smart-Collect" + "name": "Stop-Flywheel" } } ] } }, + { + "type": "named", + "data": { + "name": "Point-At-Speaker" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "deadline", "data": { @@ -90,6 +108,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-B-[F5-F4]-EC.auto b/src/main/deploy/pathplanner/autos/3-B-[F5-F4]-EC.auto deleted file mode 100644 index b9991797..00000000 --- a/src/main/deploy/pathplanner/autos/3-B-[F5-F4]-EC.auto +++ /dev/null @@ -1,338 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "B-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "BS-F5" - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F5-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "BS-F4" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F4-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - } - ] - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4-SB-[C3-C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/4-SB-[C3-C2-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3-F1]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/4-SB-[C3-C2-C1-F1]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/4-SB-[C3-C2-C1-F1]-EF.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C1-C2-C3]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[C2-C3-F3]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SF-[C2-C3-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C2-C3-F3]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[C2-C3]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[C2-C3]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/3-ST-[F1-F2]-EF.auto b/src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto similarity index 56% rename from src/main/deploy/pathplanner/autos/3-ST-[F1-F2]-EF.auto rename to src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto index 7a6f0633..51f07127 100644 --- a/src/main/deploy/pathplanner/autos/3-ST-[F1-F2]-EF.auto +++ b/src/main/deploy/pathplanner/autos/4-SB-[C2-C3-F3]-EF.auto @@ -18,29 +18,16 @@ { "type": "wait", "data": { - "waitTime": 6.0 + "waitTime": 3.0 } } ] } }, { - "type": "deadline", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "ST-F1" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Flywheel" - } - } - ] + "pathName": "SB-F4" } }, { @@ -50,64 +37,64 @@ } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "F1-TS" + "commands": [ + { + "type": "path", + "data": { + "pathName": "F4-BS" + } + } + ] } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "race", + "type": "deadline", "data": { "commands": [ { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] + "name": "Point-At-Speaker-left" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 2.0 + "name": "preAim" } } ] } }, { - "type": "deadline", + "type": "wait", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] + "waitTime": 2.0 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop-Drive" } } ] @@ -120,7 +107,7 @@ { "type": "path", "data": { - "pathName": "TS-F2" + "pathName": "BS-C3" } }, { @@ -141,62 +128,36 @@ { "type": "path", "data": { - "pathName": "F2-TS" + "pathName": "C3-SB" } }, { - "type": "sequential", + "type": "race", "data": { "commands": [ { - "type": "race", + "type": "deadline", "data": { "commands": [ { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] + "name": "Point-At-Speaker" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 2.0 + "name": "preAim" } } ] } }, { - "type": "deadline", + "type": "wait", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] + "waitTime": 2.0 } } ] @@ -207,15 +168,15 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "TS-F2" + "name": "Smart-Shoot" } }, { "type": "named", "data": { - "name": "Stop-Flywheel" + "name": "Stop-Drive" } } ] @@ -224,6 +185,6 @@ ] } }, - "folder": null, + "folder": "Good", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Point-Blank.auto b/src/main/deploy/pathplanner/autos/Point-Blank.auto index 3a8351e9..26809fee 100644 --- a/src/main/deploy/pathplanner/autos/Point-Blank.auto +++ b/src/main/deploy/pathplanner/autos/Point-Blank.auto @@ -14,6 +14,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Smart-Shoot.auto b/src/main/deploy/pathplanner/autos/Smart-Shoot.auto index b94aaf61..f0bdd08b 100644 --- a/src/main/deploy/pathplanner/autos/Smart-Shoot.auto +++ b/src/main/deploy/pathplanner/autos/Smart-Shoot.auto @@ -33,6 +33,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/nuh-uh.auto b/src/main/deploy/pathplanner/autos/nuh-uh.auto index 8bb33787..109510cc 100644 --- a/src/main/deploy/pathplanner/autos/nuh-uh.auto +++ b/src/main/deploy/pathplanner/autos/nuh-uh.auto @@ -39,6 +39,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/womp-womp.auto b/src/main/deploy/pathplanner/autos/womp-womp.auto index cb9b6edd..fb67bf21 100644 --- a/src/main/deploy/pathplanner/autos/womp-womp.auto +++ b/src/main/deploy/pathplanner/autos/womp-womp.auto @@ -51,6 +51,6 @@ ] } }, - "folder": null, + "folder": "Hide", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BB-SB.path b/src/main/deploy/pathplanner/paths/BB-SB.path index 708b60f0..06a4cac8 100644 --- a/src/main/deploy/pathplanner/paths/BB-SB.path +++ b/src/main/deploy/pathplanner/paths/BB-SB.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": { - "x": 1.4696783237842028, - "y": 2.8971303760630094 + "x": 1.1141331061329973, + "y": 2.9196157097460684 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BF-SB.path b/src/main/deploy/pathplanner/paths/BF-SB.path index 1d3f05de..d7e09cb5 100644 --- a/src/main/deploy/pathplanner/paths/BF-SB.path +++ b/src/main/deploy/pathplanner/paths/BF-SB.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": { - "x": 1.6275494087571425, - "y": 2.354448521468532 + "x": 1.2720041911059372, + "y": 2.376933855151591 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BS-C3.path b/src/main/deploy/pathplanner/paths/BS-C3.path new file mode 100644 index 00000000..6065fa75 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BS-C3.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9112722961537205, + "y": 3.6537810022838304 + }, + "prevControl": null, + "nextControl": { + "x": 1.8175856037874687, + "y": 3.876286896653679 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.75, + "y": 4.098792791023527 + }, + "prevControl": { + "x": 1.735609747966998, + "y": 3.9816844255657124 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": true + }, + "reversed": false, + "folder": "Return Paths", + "previewStartingState": { + "rotation": 1.0416266760099266, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F3-SFS.path b/src/main/deploy/pathplanner/paths/C3-SB.path similarity index 68% rename from src/main/deploy/pathplanner/paths/F3-SFS.path rename to src/main/deploy/pathplanner/paths/C3-SB.path index 7d72ce09..20bb67c2 100644 --- a/src/main/deploy/pathplanner/paths/F3-SFS.path +++ b/src/main/deploy/pathplanner/paths/C3-SB.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.279501243687172, - "y": 2.4313569347859425 + "x": 2.5292958694025494, + "y": 4.123137615644601 }, "prevControl": null, "nextControl": { - "x": 6.74488051678806, - "y": 3.457518987110047 + "x": 2.2977299021645106, + "y": 4.11050362756931 }, "isLocked": false, - "linkedName": "F4-R" + "linkedName": "C3" }, { "anchor": { - "x": 4.3135235820021185, - "y": 4.973650307660976 + "x": 2.1103565174320065, + "y": 4.133925300660873 }, "prevControl": { - "x": 5.774186683508502, - "y": 3.9382435521627808 + "x": 2.3679949214391995, + "y": 4.157346973752436 }, "nextControl": null, "isLocked": false, - "linkedName": "SFS" + "linkedName": null } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -5.194428907734747, + "rotation": -34.777831366363976, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/F4-BS.path b/src/main/deploy/pathplanner/paths/F4-BS.path index 7dff70f7..e70e96d2 100644 --- a/src/main/deploy/pathplanner/paths/F4-BS.path +++ b/src/main/deploy/pathplanner/paths/F4-BS.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "BS" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Linked Point.path b/src/main/deploy/pathplanner/paths/Linked Point.path index c0bb5451..2d779509 100644 --- a/src/main/deploy/pathplanner/paths/Linked Point.path +++ b/src/main/deploy/pathplanner/paths/Linked Point.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": { - "x": 0.455637834356241, - "y": 6.906859967572057 + "x": 1.0476544030047628, + "y": 7.084464938166614 }, "nextControl": { - "x": 0.9028367310599924, - "y": 6.614460688943504 + "x": 1.4948532997085144, + "y": 6.7920656595380615 }, "isLocked": false, "linkedName": "SubwooferTop" @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": { - "x": 0.9664642404366074, - "y": 3.6864858009336725 + "x": 0.6109190227854017, + "y": 3.7089711346167316 }, "nextControl": { - "x": 1.935734339259029, - "y": 4.405642948910615 + "x": 1.5801891216078232, + "y": 4.428128282593675 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-BF.path b/src/main/deploy/pathplanner/paths/SB-BF.path index 72214ebd..7e506e15 100644 --- a/src/main/deploy/pathplanner/paths/SB-BF.path +++ b/src/main/deploy/pathplanner/paths/SB-BF.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 1.0408986583738933, - "y": 2.9860391252314047 + "x": 0.6853534407226877, + "y": 3.0085244589144637 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-C2.path b/src/main/deploy/pathplanner/paths/SB-C2.path index 01bd4df3..63ebe89f 100644 --- a/src/main/deploy/pathplanner/paths/SB-C2.path +++ b/src/main/deploy/pathplanner/paths/SB-C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 2.900385031351464, - "y": 3.7259535721709405 + "x": 2.544839813700259, + "y": 3.7484389058539995 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-C3.path b/src/main/deploy/pathplanner/paths/SB-C3.path index 2077d729..8fdce66f 100644 --- a/src/main/deploy/pathplanner/paths/SB-C3.path +++ b/src/main/deploy/pathplanner/paths/SB-C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 1.854549532589, - "y": 3.9474882553368715 + "x": 1.4990043149377943, + "y": 3.9699735890199306 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-F3.path b/src/main/deploy/pathplanner/paths/SB-F3.path index 00b46eca..87f19370 100644 --- a/src/main/deploy/pathplanner/paths/SB-F3.path +++ b/src/main/deploy/pathplanner/paths/SB-F3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 1.573608059768328, - "y": 2.9463781897786516 + "x": 1.2180628421171225, + "y": 2.9688635234617107 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SB-F4.path b/src/main/deploy/pathplanner/paths/SB-F4.path index 3f47249c..3b35cc08 100644 --- a/src/main/deploy/pathplanner/paths/SB-F4.path +++ b/src/main/deploy/pathplanner/paths/SB-F4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 4.1436198255133565, - "y": 1.8512344381172867 + "x": 3.866981999299233, + "y": 1.7332038087756627 }, "isLocked": false, "linkedName": "SubwooferBottom" @@ -20,8 +20,8 @@ "y": 2.318745636064738 }, "prevControl": { - "x": 5.658739990803804, - "y": 0.9251560871167382 + "x": 5.31912573097614, + "y": 0.9485777602083012 }, "nextControl": null, "isLocked": false, @@ -38,7 +38,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.0, + "velocity": 1.0, "rotation": 1.301952672578864, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/SB-F5.path b/src/main/deploy/pathplanner/paths/SB-F5.path index e7a9e8a6..e9dd9392 100644 --- a/src/main/deploy/pathplanner/paths/SB-F5.path +++ b/src/main/deploy/pathplanner/paths/SB-F5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3182397535966242, - "y": 3.9474882553368715 + "x": 0.9626945359454185, + "y": 3.9699735890199306 }, "prevControl": null, "nextControl": { - "x": 3.1963933156757256, - "y": 1.0421451276309737 + "x": 2.8408480980245203, + "y": 1.0646304613140327 }, "isLocked": false, "linkedName": "SubwooferBottom" diff --git a/src/main/deploy/pathplanner/paths/SF-C3.path b/src/main/deploy/pathplanner/paths/SF-C3.path index 7b7b0332..d7a96cd6 100644 --- a/src/main/deploy/pathplanner/paths/SF-C3.path +++ b/src/main/deploy/pathplanner/paths/SF-C3.path @@ -20,7 +20,7 @@ "y": 4.123137615644601 }, "prevControl": { - "x": 1.872921944042086, + "x": 1.8729219440420857, "y": 4.28954227277824 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/ST-C1.path b/src/main/deploy/pathplanner/paths/ST-C1.path index 80949365..1b28b510 100644 --- a/src/main/deploy/pathplanner/paths/ST-C1.path +++ b/src/main/deploy/pathplanner/paths/ST-C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": null, "nextControl": { - "x": 1.5770914424711733, - "y": 6.915037974220092 + "x": 1.738340399757724, + "y": 7.084464938160646 }, "isLocked": false, "linkedName": "SubwooferTop" @@ -20,8 +20,8 @@ "y": 7.007485005961003 }, "prevControl": { - "x": 2.0485713043498155, - "y": 6.887303864697819 + "x": 2.1527519978116896, + "y": 7.005529395674177 }, "nextControl": null, "isLocked": false, @@ -40,7 +40,7 @@ "goalEndState": { "velocity": 0, "rotation": 0, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": "Top side paths", diff --git a/src/main/deploy/pathplanner/paths/ST-F1.path b/src/main/deploy/pathplanner/paths/ST-F1.path index bd2c1e91..bac4341c 100644 --- a/src/main/deploy/pathplanner/paths/ST-F1.path +++ b/src/main/deploy/pathplanner/paths/ST-F1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": null, "nextControl": { - "x": 1.2252593735956716, - "y": 7.489009593403803 + "x": 1.8172759422441933, + "y": 7.66661456399836 }, "isLocked": false, "linkedName": "SubwooferTop" diff --git a/src/main/deploy/pathplanner/paths/ST-TL.path b/src/main/deploy/pathplanner/paths/ST-TL.path index d06e0488..3dd8f5d8 100644 --- a/src/main/deploy/pathplanner/paths/ST-TL.path +++ b/src/main/deploy/pathplanner/paths/ST-TL.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": null, "nextControl": { - "x": 1.7238989114161292, - "y": 6.487803446366367 + "x": 2.315915480064651, + "y": 6.665408416960925 }, "isLocked": false, "linkedName": "SubwooferTop" diff --git a/src/main/deploy/pathplanner/paths/TF-T.path b/src/main/deploy/pathplanner/paths/TF-T.path index 12441cd9..7f2bc8d3 100644 --- a/src/main/deploy/pathplanner/paths/TF-T.path +++ b/src/main/deploy/pathplanner/paths/TF-T.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.7121783474336195, - "y": 6.739121939782341 + "x": 1.3041949160821413, + "y": 6.916726910376898 }, "prevControl": { - "x": 0.9884527461362631, - "y": 6.90685996756609 + "x": 1.5804693147847848, + "y": 7.0844649381606475 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8573a413..9d6d02c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -296,27 +296,17 @@ protected void configureButtonBindings() { .alongWith(new InstantCommand(() -> flywheel.resetBias()))); /* Other */ - new Trigger( - () -> ((limelights.getStopMe().hasTarget() || limelights.getChamps().hasTarget()) - && DriverStation.isEnabled())) + new Trigger(() -> ((limelights.getStopMe().hasTarget()) && DriverStation.isEnabled())) .whileTrue(leds.enableState(LED_STATES.HAS_VISION)); - new Trigger(() -> indexer.getEntryBeamBreakState() || indexer.getExitBeamBreakState() - || collector.getEntryBeamBreakState()) + new Trigger(() -> indexer.hasNote() && DriverStation.isEnabled()) .whileTrue(leds.enableState(LED_STATES.HAS_PIECE)) - .whileTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); - new Trigger(() -> drivetrain.isInField() && triggerInit) - .whileFalse(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() - && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) - .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> DriverStation.isDisabled() - && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) + .onTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); + + new Trigger(() -> DriverStation.isDisabled() && triggerInit + && !(limelights.getStopMe().hasTarget() || drivetrain.isInField() || drivetrain.isStable())) .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() - && (limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) + new Trigger(() -> drivetrain.isStable() && DriverStation.isDisabled() && drivetrain.isInField() + && limelights.getStopMe().hasTarget() && triggerInit) .whileTrue(leds.enableState(LED_STATES.GOOD_POSE)); triggerInit = true; diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index da096228..ba536cb7 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -10,11 +10,13 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.RobotMap.CAN; import frc.robot.Constants.RobotMap.DIO; import frc.robot.Constants; import frc.robot.Constants.CollectorConstants; import frc.thunder.hardware.ThunderBird; +import frc.thunder.shuffleboard.LightningShuffleboard; public class Collector extends SubsystemBase { @@ -27,6 +29,7 @@ public class Collector extends SubsystemBase { 0, false, false, false); private boolean hasPiece; + private double targetPower; private DoubleLogEntry collectorPowerLog; private BooleanLogEntry hasPieceLog; @@ -80,6 +83,7 @@ public void setPower(double power) { // Convert from -1,1 to RPS power = power * 100; motor.setControl(velocityVoltage.withVelocity(power)); + targetPower = power; } /** @@ -104,6 +108,11 @@ public void periodic() { public void updateLogging() { collectorPowerLog.append(motor.get()); hasPieceLog.append(hasPiece()); + + if(!DriverStation.isFMSAttached()) { + LightningShuffleboard.setDouble("Collector", "Collector power", getPower()); + LightningShuffleboard.setDouble("Collector", "Collector target power", targetPower); + } } /** diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java index 8c6b24f3..29e131b8 100644 --- a/src/main/java/frc/robot/subsystems/LEDs.java +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -145,6 +145,7 @@ public void periodic() { setStrandSingleHSV(1, 7, LEDsConstants.RED_HUE, 255, 255); } } + leds.setData(ledBuffer); } @@ -156,9 +157,7 @@ public Command enableState(LED_STATES state) { return new StartEndCommand( () -> { ledStates.put(state, true); - if (state.getPriority() > this.state.getPriority()) { - this.state = state; - } + reloadStates(); }, () -> { ledStates.put(state, false); @@ -168,7 +167,6 @@ public Command enableState(LED_STATES state) { public void setState(LED_STATES state, boolean value) { ledStates.put(state, value); - System.out.println("Setting " + state + " to " + value); } public void reloadStates() {