diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C1-3 S.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C1-3 S.auto deleted file mode 100644 index 41de0ed1..00000000 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C1-3 S.auto +++ /dev/null @@ -1,114 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.29235458470917, - "y": 4.778983272252259 - }, - "rotation": 151.53108100340572 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "FullPowerPreload" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S W3-1 Bayou" - } - }, - { - "type": "named", - "data": { - "name": "EnableLimelight" - } - }, - { - "type": "named", - "data": { - "name": "ShootInstantlyWhenReady" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "PrepareSWD" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "W1 C1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopAll" - } - }, - { - "type": "named", - "data": { - "name": "ToIndexer" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "C1toC5nonOBJ" - } - } - ] - } - }, - "folder": "Normal", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S.auto similarity index 95% rename from src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S.auto rename to src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S.auto index 0df240f6..1292d639 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S.auto +++ b/src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, { "type": "named", "data": { @@ -109,7 +115,7 @@ { "type": "named", "data": { - "name": "C1toC2nonOBJ" + "name": "C1toC5nonOBJ" } } ] diff --git a/src/main/deploy/pathplanner/paths/A C2.path b/src/main/deploy/pathplanner/paths/A C2.path index d9d4d7d3..3a371925 100644 --- a/src/main/deploy/pathplanner/paths/A C2.path +++ b/src/main/deploy/pathplanner/paths/A C2.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": { - "x": 7.901389778143498, - "y": 5.680088493435666 + "x": 8.163407972892946, + "y": 5.495910528372229 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A W1.path b/src/main/deploy/pathplanner/paths/A W1.path index c623be20..bb7324b1 100644 --- a/src/main/deploy/pathplanner/paths/A W1.path +++ b/src/main/deploy/pathplanner/paths/A W1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": { - "x": 1.844887968272587, - "y": 5.788662487005561 + "x": 1.8470169457631473, + "y": 5.675626833459793 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C1 C2.path b/src/main/deploy/pathplanner/paths/C1 C2.path index 89336822..d45976be 100644 --- a/src/main/deploy/pathplanner/paths/C1 C2.path +++ b/src/main/deploy/pathplanner/paths/C1 C2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": { - "x": 8.259914481755056, - "y": 6.378752018422289 + "x": 8.521932676504504, + "y": 6.194574053358852 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C1 L.path b/src/main/deploy/pathplanner/paths/C1 L.path index 90b99d1f..4f5cc84b 100644 --- a/src/main/deploy/pathplanner/paths/C1 L.path +++ b/src/main/deploy/pathplanner/paths/C1 L.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.3719141995954764, - "y": 7.156704660553778 + "x": 6.27438500662272, + "y": 7.495444556106504 }, "isLocked": false, "linkedName": "C1" }, { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.593798420959752, + "y": 5.550562440965166 }, "prevControl": { - "x": 5.919544888735168, - "y": 6.892024564190972 + "x": 5.036070551289278, + "y": 7.109381814149608 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -173.5955883034978, + "rotation": -179.47339920782028, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C2 A.path b/src/main/deploy/pathplanner/paths/C2 A.path index bb93ba8f..dbaa1b16 100644 --- a/src/main/deploy/pathplanner/paths/C2 A.path +++ b/src/main/deploy/pathplanner/paths/C2 A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": null, "nextControl": { - "x": 2.636652908270122, - "y": 7.479926566561713 + "x": 2.89867110301957, + "y": 7.295748601498276 }, "isLocked": false, "linkedName": "C2" diff --git a/src/main/deploy/pathplanner/paths/C2 C1.path b/src/main/deploy/pathplanner/paths/C2 C1.path index f870d25b..30ae0cbc 100644 --- a/src/main/deploy/pathplanner/paths/C2 C1.path +++ b/src/main/deploy/pathplanner/paths/C2 C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": null, "nextControl": { - "x": 8.218546246722951, - "y": 6.374155547863166 + "x": 8.4805644414724, + "y": 6.189977582799729 }, "isLocked": false, "linkedName": "C2" diff --git a/src/main/deploy/pathplanner/paths/C2 C3.path b/src/main/deploy/pathplanner/paths/C2 C3.path index f17a2b1b..53972631 100644 --- a/src/main/deploy/pathplanner/paths/C2 C3.path +++ b/src/main/deploy/pathplanner/paths/C2 C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": null, "nextControl": { - "x": 8.158409090241099, - "y": 5.444519377280635 + "x": 8.420427284990547, + "y": 5.260341412217198 }, "isLocked": false, "linkedName": "C2" diff --git a/src/main/deploy/pathplanner/paths/C2 L.path b/src/main/deploy/pathplanner/paths/C2 L.path index 93b72cad..212e9c98 100644 --- a/src/main/deploy/pathplanner/paths/C2 L.path +++ b/src/main/deploy/pathplanner/paths/C2 L.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": null, "nextControl": { - "x": 6.419028022826482, - "y": 7.2494001501627485 + "x": 6.68104621757593, + "y": 7.065222185099311 }, "isLocked": false, "linkedName": "C2" }, { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.593798420959752, + "y": 5.550562440965166 }, "prevControl": { - "x": 4.653217249696911, - "y": 6.87019132903514 + "x": 4.472107336965418, + "y": 6.505096160409611 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -173.51654383579043, + "rotation": -178.58557678859785, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/L C1.path b/src/main/deploy/pathplanner/paths/L C1.path index aa642858..b2f8c681 100644 --- a/src/main/deploy/pathplanner/paths/L C1.path +++ b/src/main/deploy/pathplanner/paths/L C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.593798420959752, + "y": 5.550562440965166 }, "prevControl": null, "nextControl": { - "x": 4.6022730343333045, - "y": 6.4856199589218875 + "x": 4.421163121601807, + "y": 6.120524790296359 }, "isLocked": false, "linkedName": "L" diff --git a/src/main/deploy/pathplanner/paths/L C2.path b/src/main/deploy/pathplanner/paths/L C2.path index f2734117..7caf8836 100644 --- a/src/main/deploy/pathplanner/paths/L C2.path +++ b/src/main/deploy/pathplanner/paths/L C2.path @@ -3,48 +3,54 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.593798420959752, + "y": 5.550562440965166 }, "prevControl": null, "nextControl": { - "x": 6.23248792596876, - "y": 7.146745641009012 + "x": 5.371143874497155, + "y": 7.349760502537864 }, "isLocked": false, "linkedName": "L" }, { "anchor": { - "x": 7.047595371786488, - "y": 5.77776349281702 + "x": 7.2067629494620125, + "y": 6.686898058800555 }, "prevControl": { - "x": 6.626048946393878, - "y": 5.801182738672167 + "x": 6.805634327255101, + "y": 7.081556864520259 }, "nextControl": { - "x": 7.295804781979105, - "y": 5.763974081139652 + "x": 7.658383515524795, + "y": 6.242561695416204 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": { - "x": 7.308445076016692, - "y": 5.83636849244583 + "x": 7.549120475348316, + "y": 6.3299721275573875 }, "nextControl": null, "isLocked": false, "linkedName": "C2" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 136.9677610942732, + "rotateFast": false + } + ], "constraintZones": [ { "name": "SpeedyIntake", @@ -67,13 +73,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0, + "rotation": 135.61605990839928, "rotateFast": true }, "reversed": false, "folder": "Stage", "previewStartingState": { - "rotation": -165.35696772976468, + "rotation": -178.90878377473737, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/L C3.path b/src/main/deploy/pathplanner/paths/L C3.path index f6767208..34595279 100644 --- a/src/main/deploy/pathplanner/paths/L C3.path +++ b/src/main/deploy/pathplanner/paths/L C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.593798420959752, + "y": 5.550562440965166 }, "prevControl": null, "nextControl": { - "x": 5.074494720418012, - "y": 4.031350919844697 + "x": 4.893384807686517, + "y": 3.6662557512191682 }, "isLocked": false, "linkedName": "L" diff --git a/src/main/deploy/pathplanner/paths/M C2.path b/src/main/deploy/pathplanner/paths/M C2.path index 89061506..5f25f0a3 100644 --- a/src/main/deploy/pathplanner/paths/M C2.path +++ b/src/main/deploy/pathplanner/paths/M C2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": { - "x": 6.908552137373031, - "y": 6.387944959540534 + "x": 7.170570332122479, + "y": 6.203766994477097 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S C2.path b/src/main/deploy/pathplanner/paths/S C2.path index 1bb90071..bd836639 100644 --- a/src/main/deploy/pathplanner/paths/S C2.path +++ b/src/main/deploy/pathplanner/paths/S C2.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": { - "x": 7.087814489178809, - "y": 6.424716724013514 + "x": 7.349832683928257, + "y": 6.240538758950077 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S W1.path b/src/main/deploy/pathplanner/paths/S W1.path index 2e8eee34..4e3c5bb6 100644 --- a/src/main/deploy/pathplanner/paths/S W1.path +++ b/src/main/deploy/pathplanner/paths/S W1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": { - "x": 2.2185338517641693, - "y": 6.762260696421776 + "x": 2.2206628292547297, + "y": 6.649225042876008 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S W3 New.path b/src/main/deploy/pathplanner/paths/S W3 New.path index e7c45d9a..79b75b87 100644 --- a/src/main/deploy/pathplanner/paths/S W3 New.path +++ b/src/main/deploy/pathplanner/paths/S W3 New.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.4553382440098215, - "y": 4.4758132069455625 + "x": 2.428325992410636, + "y": 4.23940595884741 }, "prevControl": { - "x": 2.293682792812724, - "y": 4.170464021351045 + "x": 2.2666705412135384, + "y": 3.934056773252893 }, "nextControl": { - "x": 2.57944294910613, - "y": 4.710233205460812 + "x": 2.5524306975069444, + "y": 4.473825957362659 }, "isLocked": false, "linkedName": "W3 Pickup" diff --git a/src/main/deploy/pathplanner/paths/S W3-1 2.path b/src/main/deploy/pathplanner/paths/S W3-1 2.path index 830c1515..8f4733a7 100644 --- a/src/main/deploy/pathplanner/paths/S W3-1 2.path +++ b/src/main/deploy/pathplanner/paths/S W3-1 2.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": { - "x": 1.2330601144964886, - "y": 6.050487412658289 + "x": 1.235189091987049, + "y": 5.9374517591125215 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path b/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path index eeb52424..930a3393 100644 --- a/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path +++ b/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path @@ -16,44 +16,44 @@ }, { "anchor": { - "x": 2.4553382440098215, - "y": 4.4758132069455625 + "x": 2.428325992410636, + "y": 4.23940595884741 }, "prevControl": { - "x": 2.2079885136348345, - "y": 4.2342834702264565 + "x": 2.180976262035649, + "y": 3.997876222128304 }, "nextControl": { - "x": 2.8460382415352363, - "y": 4.857320263352733 + "x": 2.819025989936051, + "y": 4.6209130152545805 }, "isLocked": false, "linkedName": "W3 Pickup" }, { "anchor": { - "x": 2.0113957958412367, - "y": 5.378253593386619 + "x": 2.049547453132173, + "y": 5.237341725792591 }, "prevControl": { - "x": 1.145914832282723, - "y": 5.291535853302232 + "x": 1.2243689644069953, + "y": 4.9622822295508655 }, "nextControl": { - "x": 3.1606075951005406, - "y": 5.4934000313860505 + "x": 3.2514408950734497, + "y": 5.637972873106349 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8872307611518506, + "y": 6.825297909690762 }, "prevControl": { - "x": 1.2422530556147342, - "y": 6.025972903009635 + "x": 1.237097830426863, + "y": 5.869232033393276 }, "nextControl": null, "isLocked": false, @@ -68,12 +68,12 @@ }, { "waypointRelativePos": 1.7999999999999998, - "rotationDegrees": 170.6683195762387, + "rotationDegrees": 169.22011509237518, "rotateFast": false }, { "waypointRelativePos": 2.55, - "rotationDegrees": -146.99446263627644, + "rotationDegrees": -162.7437926739913, "rotateFast": false } ], @@ -150,7 +150,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -145.24068548829183, + "rotation": -148.53585636913425, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/S W3-1.path b/src/main/deploy/pathplanner/paths/S W3-1.path index 5b531aea..a1693c23 100644 --- a/src/main/deploy/pathplanner/paths/S W3-1.path +++ b/src/main/deploy/pathplanner/paths/S W3-1.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": { - "x": 1.2330601144964886, - "y": 6.050487412658289 + "x": 1.235189091987049, + "y": 5.9374517591125215 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Shot Spots.path b/src/main/deploy/pathplanner/paths/Shot Spots.path index 87b1d1aa..5e89920d 100644 --- a/src/main/deploy/pathplanner/paths/Shot Spots.path +++ b/src/main/deploy/pathplanner/paths/Shot Spots.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.593798420959752, + "y": 5.550562440965166 }, "prevControl": null, "nextControl": { - "x": 4.774908333691249, - "y": 5.915657609590695 + "x": 4.593798420959755, + "y": 5.550562440965166 }, "isLocked": false, "linkedName": "L" diff --git a/src/main/deploy/pathplanner/paths/W1 A.path b/src/main/deploy/pathplanner/paths/W1 A.path index 5f072084..80b4376b 100644 --- a/src/main/deploy/pathplanner/paths/W1 A.path +++ b/src/main/deploy/pathplanner/paths/W1 A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 2.448666263038041, - "y": 7.238220481000093 + "x": 2.4507952405286013, + "y": 7.1251848274543255 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W1 C1 N.path b/src/main/deploy/pathplanner/paths/W1 C1 N.path index 30209f06..86045e6f 100644 --- a/src/main/deploy/pathplanner/paths/W1 C1 N.path +++ b/src/main/deploy/pathplanner/paths/W1 C1 N.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 3.862241274314576, - "y": 6.756811721910118 + "x": 3.8643702518051364, + "y": 6.6437760683643505 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W1 C1.path b/src/main/deploy/pathplanner/paths/W1 C1.path index ce1383d2..699ed049 100644 --- a/src/main/deploy/pathplanner/paths/W1 C1.path +++ b/src/main/deploy/pathplanner/paths/W1 C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 6.151283612757595, - "y": 7.317581130123067 + "x": 6.153412590248156, + "y": 7.204545476577299 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W1 C2 N.path b/src/main/deploy/pathplanner/paths/W1 C2 N.path index e66c3198..b9039e09 100644 --- a/src/main/deploy/pathplanner/paths/W1 C2 N.path +++ b/src/main/deploy/pathplanner/paths/W1 C2 N.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 3.944977744378782, - "y": 7.211862307263249 + "x": 3.9471067218693423, + "y": 7.098826653717481 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W1 C2.path b/src/main/deploy/pathplanner/paths/W1 C2.path index c3785d28..76e65b2a 100644 --- a/src/main/deploy/pathplanner/paths/W1 C2.path +++ b/src/main/deploy/pathplanner/paths/W1 C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 6.119108318843736, - "y": 6.913091720920284 + "x": 6.1212372963342965, + "y": 6.800056067374516 }, "isLocked": false, "linkedName": "W1" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": { - "x": 7.37509389912397, - "y": 6.164250058996572 + "x": 7.637112093873418, + "y": 5.980072093933135 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/W1 C3.path b/src/main/deploy/pathplanner/paths/W1 C3.path index 19fbbb55..4c838948 100644 --- a/src/main/deploy/pathplanner/paths/W1 C3.path +++ b/src/main/deploy/pathplanner/paths/W1 C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 3.539722256749478, - "y": 5.662085650412435 + "x": 3.5418512342400383, + "y": 5.5490499968666676 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W1 W2.path b/src/main/deploy/pathplanner/paths/W1 W2.path index f4184801..8c4ffe46 100644 --- a/src/main/deploy/pathplanner/paths/W1 W2.path +++ b/src/main/deploy/pathplanner/paths/W1 W2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 2.3512739869758232, - "y": 6.889078258418361 + "x": 2.3534029644663836, + "y": 6.776042604872593 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W1 W3.path b/src/main/deploy/pathplanner/paths/W1 W3.path index fa558a34..02f8da3b 100644 --- a/src/main/deploy/pathplanner/paths/W1 W3.path +++ b/src/main/deploy/pathplanner/paths/W1 W3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": null, "nextControl": { - "x": 2.117670775281163, - "y": 7.208241836977058 + "x": 2.1197997527717236, + "y": 7.09520618343129 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W2 C2.path b/src/main/deploy/pathplanner/paths/W2 C2.path index 2029c4d0..fb024144 100644 --- a/src/main/deploy/pathplanner/paths/W2 C2.path +++ b/src/main/deploy/pathplanner/paths/W2 C2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.03009095379893, - "y": 5.873140256918812 + "x": 8.292109148548377, + "y": 5.688962291855375 }, "prevControl": { - "x": 6.54083449264323, - "y": 6.034016726488099 + "x": 6.802852687392678, + "y": 5.849838761424662 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/W2 W1.path b/src/main/deploy/pathplanner/paths/W2 W1.path index 4cc5afe3..fcac1dd6 100644 --- a/src/main/deploy/pathplanner/paths/W2 W1.path +++ b/src/main/deploy/pathplanner/paths/W2 W1.path @@ -32,28 +32,28 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": { - "x": 2.547650694405532, - "y": 6.913091720920284 + "x": 2.5497796718960926, + "y": 6.800056067374516 }, "nextControl": { - "x": 3.3373232041458003, - "y": 7.071026222868336 + "x": 3.3394521816363607, + "y": 6.957990569322568 }, "isLocked": false, "linkedName": "W1" }, { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.593798420959752, + "y": 5.550562440965166 }, "prevControl": { - "x": 3.650803628594939, - "y": 6.356918783266457 + "x": 3.469693715863443, + "y": 5.9918236146409285 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/W3 W1.path b/src/main/deploy/pathplanner/paths/W3 W1.path index dd2e7de7..9bf122a3 100644 --- a/src/main/deploy/pathplanner/paths/W3 W1.path +++ b/src/main/deploy/pathplanner/paths/W3 W1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.8945149638302823, + "y": 6.8690031257613535 }, "prevControl": { - "x": 1.4904624658073509, - "y": 6.756811721910118 + "x": 1.4925914432979113, + "y": 6.6437760683643505 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b2ecba27..e43d8c7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -221,7 +221,7 @@ public RobotContainer() { || (OIConstants.OPERATOR_PRESENT && operator.getLeftBumper()), () -> OIConstants.OPERATOR_PRESENT - && (driver.getLeftBumper() || operator.getYButton()) + && (operator.getYButton()) ) ); @@ -305,7 +305,6 @@ private void configureTimedEvents() { new Trigger(() -> Robot.gameMode == GameMode.TELEOP && shooter.getAverageSpeed() > 1700 - && shooter.getAverageTargetSpeed() != 2500 && swerve.getPose().getX() > FieldConstants.CENTERLINE_X ^ Robot.isBlueAlliance() && limelight3g.getPose2d().getTranslation().getDistance(swerve.getPose().getTranslation()) < Units.inchesToMeters(4)) .onTrue(Commands.runOnce(() -> @@ -326,12 +325,11 @@ private void configureTimedEvents() { // The reason we are checking bumpers // is so this doesn't happen on pieceControl::moveNote new Trigger( - () -> piPico.hasNoteShooter() + () -> (piPico.hasNoteShooter() || piPico.hasNoteElevator()) && ( // All of these buttons command intake // Whether that be source or not - driver.getLeftTrigger() - || driver.getXButton() + driver.getXButton() || (OIConstants.OPERATOR_PRESENT &&(operator.getLeftBumper() || operator.getStartButton() @@ -368,17 +366,7 @@ private void configureTimedEvents() { ).onFalse( driver.setRumble(() -> 0) ); - - // Controller rumble if climb hooks are up and we are inside of stage area - // AKA driver is good to climb - new Trigger(climb::getHooksUp).and(() -> PoseCalculations.inStageTriangle(robotPose2d)) - .onTrue(driver.setRumble(() -> 0.5)) - .onFalse(driver.setRumble(() -> 0)); - new Trigger(climb::getToggleMode) - .onTrue(swerve.resetHDCCommand()) - .onFalse(swerve.resetDesiredHDCPoseCommand()) - .whileTrue(alignmentCmds.chainRotationalAlignment(driver::getLeftX, driver::getLeftY, robotRelativeSupplier)); } private void configureFieldCalibrationBindings(PatriBoxController controller) { @@ -475,11 +463,9 @@ private void configureCommonDriverBindings(PatriBoxController controller) { controller.povDown() .onTrue( climb.toBottomCommand() - .alongWith( - Commands.waitUntil(elevator::atDesiredPosition) - .andThen(shooterCmds.stowPivot()))); + .alongWith(shooterCmds.stowPivot())); - // POV left is uncommonly used but needed incase of emergency + // POV left and right are uncommonly used but needed incase of emergency controller.povLeft() .onTrue( Commands.sequence( @@ -488,6 +474,13 @@ private void configureCommonDriverBindings(PatriBoxController controller) { shooterCmds.raisePivot() )); + controller.povRight() + .onTrue( + Commands.sequence( + pieceControl.stopAllMotors(), + shooterCmds.stowPivot() + )); + } private void configureSoloDriverBindings(PatriBoxController controller) { @@ -545,11 +538,11 @@ private void configureDriverBindings(PatriBoxController controller) { shooterCmds.prepareSubwooferCommand() ).finallyDo( () -> - Commands.parallel( - shooterCmds.raisePivot(), - shooterCmds.stopShooter()) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf) - .schedule())); + Commands.parallel( + shooterCmds.raisePivot(), + shooterCmds.stopShooter()) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf) + .schedule())); controller.a() @@ -562,8 +555,7 @@ private void configureDriverBindings(PatriBoxController controller) { limelight3g.setLEDState(() -> false), swerve.resetHDCCommand(), swerve.resetDesiredHDCPoseCommand())); - - controller.x() + controller.leftBumper() .whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity)) .negate().and(() -> !OIConstants.OPERATOR_PRESENT || !operator.getLeftBumper()) .onTrue(pieceControl.stopIntakeAndIndexer()); @@ -595,7 +587,7 @@ private void configureOperatorBindings(PatriBoxController controller) { controller.leftBumper() .whileTrue(pieceControl.intakeUntilNote()) - .negate().and(driver.x().negate()) + .negate().and(driver.leftBumper().negate()) .onTrue(pieceControl.stopIntakeAndIndexer()); controller.rightBumper() @@ -624,7 +616,9 @@ private void configureOperatorBindings(PatriBoxController controller) { .onFalse(pieceControl.stopPanicEject()); controller.start().or(controller.back()) - .whileTrue(pieceControl.sourceShooterIntake()) + .whileTrue( + Commands.waitUntil(elevator::atDesiredPosition) + .andThen(pieceControl.sourceShooterIntake())) .onFalse(pieceControl.stopIntakeAndIndexer()); controller.rightStick().and(controller.leftStick()) diff --git a/src/main/java/frc/robot/commands/drive/AlignmentCmds.java b/src/main/java/frc/robot/commands/drive/AlignmentCmds.java index 4101fc0c..561460b2 100644 --- a/src/main/java/frc/robot/commands/drive/AlignmentCmds.java +++ b/src/main/java/frc/robot/commands/drive/AlignmentCmds.java @@ -193,10 +193,21 @@ public Command wingRotationalAlignment(DoubleSupplier driverX, DoubleSupplier dr climb::getHooksUp); } + public Command preparePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier, BooleanSupplier lowPass) { + return + passRotationalAlignment(driverX, driverY, shooterCmds) + .alongWith(shooterCmds.preparePassCommand(swerve::getPose, lowPass)); + } + public Command preparePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier) { + return preparePassCommand(driverX, driverY, robotRelativeSupplier, () -> PoseCalculations.inLowPassZone(swerve.getPose())); + } + + public Command prepareSlidePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier) { return passRotationalAlignment(driverX, driverY, shooterCmds) - .alongWith(shooterCmds.preparePassCommand(swerve::getPose)); + .alongWith(shooterCmds.prepareSlidePassCommand()); + } public Command preparePresetPose(DoubleSupplier driverX, DoubleSupplier driverY, boolean xButtonPressed) { diff --git a/src/main/java/frc/robot/commands/managers/HDCTuner.java b/src/main/java/frc/robot/commands/managers/HDCTuner.java index 12e6f65f..f5c3b942 100644 --- a/src/main/java/frc/robot/commands/managers/HDCTuner.java +++ b/src/main/java/frc/robot/commands/managers/HDCTuner.java @@ -12,9 +12,9 @@ public class HDCTuner extends SubsystemBase { - @AutoLogOutput (key = "Managers//HDCTuner/PIDControllerIndex") + @AutoLogOutput (key = "Managers/HDCTuner/PIDControllerIndex") public int PIDControllerIndex = 0; - @AutoLogOutput (key = "Managers//HDCTuner/PIDIndex") + @AutoLogOutput (key = "Managers/HDCTuner/PIDIndex") public int PIDIndex = 0; public PIDController XYController = AutoConstants.HDC.getXController(); diff --git a/src/main/java/frc/robot/commands/managers/ShooterCmds.java b/src/main/java/frc/robot/commands/managers/ShooterCmds.java index f4c61be4..5b3c7fd4 100644 --- a/src/main/java/frc/robot/commands/managers/ShooterCmds.java +++ b/src/main/java/frc/robot/commands/managers/ShooterCmds.java @@ -16,6 +16,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.ShooterConstants; +import frc.robot.util.calc.PoseCalculations; import frc.robot.util.calc.ShooterCalc; import frc.robot.util.custom.SpeedAngleTriplet; @@ -147,15 +148,27 @@ public Command prepareStillSpeakerCommand(Supplier robotPose) { return prepareSWDCommand(robotPose, () -> new ChassisSpeeds()); } - public Command preparePassCommand(Supplier robotPose) { + public Command preparePassCommand(Supplier robotPose, BooleanSupplier lowPass) { return Commands.run( () -> { - desiredTriplet = shooterCalc.calculatePassTriplet(robotPose.get()); + desiredTriplet = shooterCalc.calculatePassTriplet(robotPose.get(), lowPass.getAsBoolean()); pivot.setAngle(desiredTriplet.getAngle()); shooter.setSpeed(desiredTriplet.getSpeeds()); }, pivot, shooter); } + public Command preparePassCommand(Supplier robotPose) { + return preparePassCommand(robotPose, () -> PoseCalculations.inLowPassZone(robotPose.get())); + } + + public Command prepareSlidePassCommand() { + return Commands.run( + () -> { + pivot.setAngle(ShooterConstants.PIVOT_LOWER_LIMIT_DEGREES); + shooter.setSpeed(ShooterConstants.SLIDE_PASS_AVERAGE_RPM); + }, pivot, shooter); + } + public Command prepareSubwooferCommand() { return prepareStillSpeakerCommand(FieldConstants::GET_SUBWOOFER_POSITION); } diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 7181e097..5e285ef7 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -31,8 +31,6 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.util.Color; import frc.robot.Robot; -import frc.robot.util.Constants.FieldConstants; -import frc.robot.util.Constants.LoggingConstants.RobotType; import frc.robot.util.custom.PatrIDConstants; import frc.robot.util.custom.SpeedAngleTriplet; import frc.robot.util.rev.Neo; @@ -198,6 +196,8 @@ public static final class ShooterConstants { public static final double PIVOT_UPPER_LIMIT_DEGREES = 56; public static final double PIVOT_RAISE_ANGLE_DEGREES = 49; + public static final double SLIDE_PASS_AVERAGE_RPM = 2500; + public static final SpeedAngleTriplet SHOOTER_AMP_TRIPLET = SpeedAngleTriplet.of(712.0, 554.0, 55.4); public static final double SHOOTER_RPM_LOWER_LIMIT = -NeoMotorConstants.VORTEX_FREE_SPEED_RPM; @@ -240,14 +240,14 @@ public static final class ShooterConstants { put(8.0, SpeedAngleTriplet.of(3024.0, 3319.0, 42.4)); put(8.5, SpeedAngleTriplet.of(3128.0, 3445.0, 40.9)); put(9.0, SpeedAngleTriplet.of(3305.0, 3606.0, 39.6)); - put(10.0, SpeedAngleTriplet.of(3441.0, 3842.0, 37.6)); - put(11.0, SpeedAngleTriplet.of(3647.0, 3965.0, 36.2)); - put(12.0, SpeedAngleTriplet.of(3781.0, 4147.0, 34.1)); - put(12.5, SpeedAngleTriplet.of(3945.0, 4320.0, 34.0)); - put(13.0, SpeedAngleTriplet.of(4106.0, 4490.0, 33.7)); - put(14.0, SpeedAngleTriplet.of(4379.0, 4648.0, 31.5)); - put(15.0, SpeedAngleTriplet.of(4445.0, 4714.0, 30.4)); - put(16.0, SpeedAngleTriplet.of(4540.0, 4809.0, 29.7)); + put(10.0, SpeedAngleTriplet.of(3441.0, 3842.0, 36.6)); + put(11.0, SpeedAngleTriplet.of(3647.0, 3965.0, 35.2)); + put(12.0, SpeedAngleTriplet.of(3781.0, 4147.0, 33.1)); + put(12.5, SpeedAngleTriplet.of(3945.0, 4320.0, 33.0)); + put(13.0, SpeedAngleTriplet.of(4106.0, 4490.0, 32.7)); + put(14.0, SpeedAngleTriplet.of(4379.0, 4648.0, 30.5)); + put(15.0, SpeedAngleTriplet.of(4445.0, 4714.0, 29.4)); + put(16.0, SpeedAngleTriplet.of(4540.0, 4809.0, 28.7)); // BAYOU // put(6.0, SpeedAngleTriplet.of(3007.0, 2850.0, 50.1)); @@ -400,7 +400,7 @@ public static final class ClimbConstants { public static final class AutoConstants { - // The below values need to be tuned for each new robot. + // The below values need to be tuned for each new robot.. // They are currently set to the values suggested by Choreo public static final double MAX_SPEED_METERS_PER_SECOND = 5; public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 5; @@ -499,8 +499,9 @@ public static final class AutoConstants { "S C5-3 3-5 S", "S W3-1 S C2-5 S", "S W3-1 S C2-3 S", // HOME AUTO - "S W3-1 S C1-2 S", + // "S W3-1 S C1-2 S", "S W3-1 S C3-5 S", + "S W3-1 S C1-5 S", "S W3-1 S", "S C1-5 S Over W1" }; @@ -936,7 +937,7 @@ public static final class FieldConstants { public static final List CENTER_PASS_TARGET_POSITIONS = new ArrayList() {{ // I swear bulldogs and hawaiin kids just had to get in here somehow - Pose2d bluePose = new Pose2d(1.07, 6.99, Rotation2d.fromDegrees(0)); + Pose2d bluePose = new Pose2d(0.630, 6.405, Rotation2d.fromDegrees(0)); Pose2d redPose = GeometryUtil.flipFieldPose(bluePose).plus(new Transform2d(0, 0, Rotation2d.fromDegrees(180))); add(bluePose); diff --git a/src/main/java/frc/robot/util/calc/ShooterCalc.java b/src/main/java/frc/robot/util/calc/ShooterCalc.java index 6e3d8020..08213fd0 100644 --- a/src/main/java/frc/robot/util/calc/ShooterCalc.java +++ b/src/main/java/frc/robot/util/calc/ShooterCalc.java @@ -84,8 +84,7 @@ public SpeedAngleTriplet calculateSWDTripletAuto(Pose2d pose, ChassisSpeeds spee ); } - public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose) { - boolean lowPass = PoseCalculations.inLowPassZone(robotPose); + public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose, boolean lowPass) { Rotation2d pivotAngle = calculatePassPivotAngle(robotPose, lowPass); Pair shooterSpeeds = calculateShooterSpeedsForPassApex(robotPose, pivotAngle, lowPass); return SpeedAngleTriplet.of( @@ -95,6 +94,10 @@ public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose) { pivotAngle.getDegrees()); } + public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose) { + return calculatePassTriplet(robotPose, PoseCalculations.inLowPassZone(robotPose)); + } + public SpeedAngleTriplet calculateApexTriplet(Pose2d robotPose) { Rotation2d pivotAngle = calculatePivotAngle(robotPose); Pair shooterSpeeds = calculateShooterSpeedsForSpeakerApex(robotPose, pivotAngle); @@ -129,14 +132,8 @@ public Rotation2d calculatePivotAngle(Pose2d robotPose) { public Rotation2d calculatePassPivotAngle(Pose2d robotPose, boolean lowPass) { - boolean sourcePass = PoseCalculations.inSourcePassZone(robotPose); - // Calculate the robot's pose relative to the desired pass's apex position - robotPose = - robotPose.relativeTo( - sourcePass - ? FieldConstants.GET_SOURCE_PASS_TARGET_POSITION() - : FieldConstants.GET_CENTER_PASS_TARGET_POSITION()); + robotPose = robotPose.relativeTo(FieldConstants.GET_CENTER_PASS_TARGET_POSITION()); // Calculate the distance and height in meters from the robot to the pass target pose double rangeMeters = robotPose.getTranslation().getNorm(); @@ -209,12 +206,12 @@ public Rotation2d calculateRobotAngleToPose(Pose2d robotPose, ChassisSpeeds robo Rotation2d currentAngleToTarget = new Rotation2d(poseRelativeToTarget.getX(), poseRelativeToTarget.getY()); double velocityArcTan = Math.atan2( velocityTangent, - (target.equals(FieldConstants.GET_CENTER_PASS_TARGET_POSITION()) || target.equals(FieldConstants.GET_SOURCE_PASS_TARGET_POSITION()) + (target.equals(FieldConstants.GET_CENTER_PASS_TARGET_POSITION())) // Run this calculation with high pass triplet, as low pass requires little rotational precision in swd ? rpmToVelocity(calculatePassTriplet(robotPose).getSpeeds()) : rpmToVelocity(calculateSWDTriplet(robotPose, robotVelocity).getSpeeds()) // rpmToVelocity(calculateShooterSpeedsForApex(robotPose, calculatePivotAngle(robotPose))) - )); + ); // Calculate the desired rotation to the speaker, taking into account the tangent velocity // Add PI because the speaker opening is the opposite direction that the robot needs to be facing Rotation2d desiredRotation2d = Rotation2d.fromRadians(MathUtil.angleModulus( @@ -239,11 +236,9 @@ public Rotation2d calculateRobotAngleToPass(Pose2d robotPose) { public Rotation2d calculateRobotAngleToPass(Pose2d robotPose, ChassisSpeeds robotVelocity) { return calculateRobotAngleToPose( - robotPose, + robotPose, robotVelocity, - PoseCalculations.inSourcePassZone(robotPose) - ? FieldConstants.GET_SOURCE_PASS_TARGET_POSITION() - : FieldConstants.GET_CENTER_PASS_TARGET_POSITION()); + FieldConstants.GET_CENTER_PASS_TARGET_POSITION()); } public Rotation2d calculateRobotAngleToSpeaker(Pose2d pose) {