diff --git a/src/main/deploy/pathplanner/autos/BLS Auto.auto b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto similarity index 77% rename from src/main/deploy/pathplanner/autos/BLS Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto index c8d1190..649472d 100644 --- a/src/main/deploy/pathplanner/autos/BLS Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE AMP SIDE Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7275269251393154, - "y": 4.268366371490578 + "x": 0.4234789096358818, + "y": 6.981410202136645 }, - "rotation": 58.172553423326896 + "rotation": 2.145150039040434 }, "command": { "type": "sequential", @@ -14,13 +14,13 @@ { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootFlyWheel" + "name": "SHOOTFLYS" } }, { diff --git a/src/main/deploy/pathplanner/autos/BMS Auto.auto b/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/BMS Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto index c392660..27122dd 100644 --- a/src/main/deploy/pathplanner/autos/BMS Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE Middle Shoot Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 5.849566570928014, - "y": 4.315142989260338 + "x": 1.3674277229789629, + "y": 5.557362183607978 }, "rotation": 0 }, @@ -12,27 +12,27 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "BS Path" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTFLYS" } }, { - "type": "named", + "type": "wait", "data": { - "name": "shootFlyWheel" + "waitTime": 2.0 } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 2.0 + "pathName": "BS Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/BRS Auto.auto b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto similarity index 85% rename from src/main/deploy/pathplanner/autos/BRS Auto.auto rename to src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto index 0d0019e..af696be 100644 --- a/src/main/deploy/pathplanner/autos/BRS Auto.auto +++ b/src/main/deploy/pathplanner/autos/BLUE SOURCE SIDE Auto.auto @@ -5,7 +5,7 @@ "x": 0.79769185179396, "y": 4.572414386994017 }, - "rotation": -62.59242456218156 + "rotation": -125.24061873233006 }, "command": { "type": "sequential", @@ -14,13 +14,13 @@ { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootFlyWheel" + "name": "SHOOTFLYS" } }, { diff --git a/src/main/deploy/pathplanner/autos/BR Path.auto b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto similarity index 84% rename from src/main/deploy/pathplanner/autos/BR Path.auto rename to src/main/deploy/pathplanner/autos/BLUE TAXI.auto index f70a47e..a9f9f7a 100644 --- a/src/main/deploy/pathplanner/autos/BR Path.auto +++ b/src/main/deploy/pathplanner/autos/BLUE TAXI.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.107671952684762, - "y": 2.4888707299839377 + "x": 0.400090600751002, + "y": 1.9997004096572308 }, "rotation": 0 }, diff --git a/src/main/deploy/pathplanner/autos/RRS Auto.auto b/src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto similarity index 73% rename from src/main/deploy/pathplanner/autos/RRS Auto.auto rename to src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto index ae2fb20..bec19dc 100644 --- a/src/main/deploy/pathplanner/autos/RRS Auto.auto +++ b/src/main/deploy/pathplanner/autos/RED AMP SIDE Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 15.976704318081, - "y": 4.408696224799858 + "x": 16.147134266099517, + "y": 6.9085276198721415 }, - "rotation": -120.5792268724892 + "rotation": 0.0 }, "command": { "type": "sequential", @@ -14,13 +14,13 @@ { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootFlyWheel" + "name": "SHOOTFLYS" } }, { @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "RRS Path" + "pathName": "Straight Red Amp Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/RMS Auto.auto b/src/main/deploy/pathplanner/autos/RED Middle Shoot Auto.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/RMS Auto.auto rename to src/main/deploy/pathplanner/autos/RED Middle Shoot Auto.auto index 720e4f6..4986d8b 100644 --- a/src/main/deploy/pathplanner/autos/RMS Auto.auto +++ b/src/main/deploy/pathplanner/autos/RED Middle Shoot Auto.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 15.146419352667763, - "y": 5.578111669043852 + "x": 15.319384055583853, + "y": 5.551187918118234 }, "rotation": 0 }, @@ -12,27 +12,27 @@ "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "RS Path" + "name": "SHOOTCONVEYOR" } }, { "type": "named", "data": { - "name": "shootConveyor" + "name": "SHOOTFLYS" } }, { - "type": "named", + "type": "wait", "data": { - "name": "shootFlyWheel" + "waitTime": 2.0 } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 2.0 + "pathName": "RS Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/RL Path.auto b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto similarity index 68% rename from src/main/deploy/pathplanner/autos/RL Path.auto rename to src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto index 60d48cd..b5b6605 100644 --- a/src/main/deploy/pathplanner/autos/RL Path.auto +++ b/src/main/deploy/pathplanner/autos/RED SOURCE SIDE Auto.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 15.541771563143822, - "y": 2.430613727257548 + "x": 16.1521166347176, + "y": 4.234725089168489 }, - "rotation": -179.1864566650106 + "rotation": -1.7858847665219348 }, "command": { "type": "sequential", @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "RL Path" + "pathName": "RRS Path" } } ] diff --git a/src/main/deploy/pathplanner/autos/RED TAXI.auto b/src/main/deploy/pathplanner/autos/RED TAXI.auto new file mode 100644 index 0000000..1332884 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RED TAXI.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 16.112410901029318, + "y": 2.0413098429059118 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RR Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RLS Auto.auto b/src/main/deploy/pathplanner/autos/RLS Auto.auto deleted file mode 100644 index 2062588..0000000 --- a/src/main/deploy/pathplanner/autos/RLS Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 15.965010163634727, - "y": 6.6773621866284305 - }, - "rotation": -57.5288077091511 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shootConveyor" - } - }, - { - "type": "named", - "data": { - "name": "shootFlyWheel" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "RLS Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/bl Auto.auto b/src/main/deploy/pathplanner/autos/bl Auto.auto deleted file mode 100644 index ffaf58c..0000000 --- a/src/main/deploy/pathplanner/autos/bl Auto.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.38839644630856207, - "y": 7.3322348354098414 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "bl Path" - } - }, - { - "type": "named", - "data": { - "name": "Dump bed " - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "bl2 Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/rr Auto.auto b/src/main/deploy/pathplanner/autos/rr Auto.auto deleted file mode 100644 index 37da37c..0000000 --- a/src/main/deploy/pathplanner/autos/rr Auto.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 16.1521166347176, - "y": 7.390705607622041 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "rr Path" - } - }, - { - "type": "named", - "data": { - "name": "shootConveyor" - } - }, - { - "type": "named", - "data": { - "name": "shootFlyWheel" - } - }, - { - "type": "named", - "data": { - "name": "dumpBed" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "rr2 Path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BLS Path.path b/src/main/deploy/pathplanner/paths/BLS Path.path index efa2a7e..702e6fe 100644 --- a/src/main/deploy/pathplanner/paths/BLS Path.path +++ b/src/main/deploy/pathplanner/paths/BLS Path.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 0.7275269251393154, - "y": 6.712444649961881 + "x": 0.4234789096358818, + "y": 6.981410202136645 }, "prevControl": null, "nextControl": { - "x": 0.7860254744404522, - "y": 6.731944166395593 + "x": 0.48197745893701854, + "y": 7.000909718570356 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.4691504014837022, - "y": 6.27884429165533 + "x": 2.855863033663389, + "y": 7.659671159798161 }, "prevControl": { - "x": 2.4886499179174146, - "y": 6.298343808089042 + "x": 2.8753625500971016, + "y": 7.679170676231872 }, "nextControl": { - "x": 2.44965088504999, - "y": 6.259344775221619 + "x": 2.8363635172296764, + "y": 7.640171643364449 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.019361957963822, - "y": 6.27884429165533 + "x": 5.311635466575775, + "y": 7.659671159798161 }, "prevControl": { - "x": 3.2150069050731935, - "y": 6.244720137896334 + "x": 4.507280413685147, + "y": 7.625547006039165 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BR Path.path b/src/main/deploy/pathplanner/paths/BR Path.path index 921f65f..95c294e 100644 --- a/src/main/deploy/pathplanner/paths/BR Path.path +++ b/src/main/deploy/pathplanner/paths/BR Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.107671952684762, - "y": 2.4888707299839377 + "x": 0.4281021799968474, + "y": 1.99725279593672 }, "prevControl": null, "nextControl": { - "x": 1.1278060404789791, - "y": 2.4858374872999747 + "x": 0.4482362677910645, + "y": 1.994219553252757 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/BRS Path.path b/src/main/deploy/pathplanner/paths/BRS Path.path index 2f5e90b..061428c 100644 --- a/src/main/deploy/pathplanner/paths/BRS Path.path +++ b/src/main/deploy/pathplanner/paths/BRS Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.79769185179396, - "y": 4.303448834817898 + "x": 0.400090600751002, + "y": 4.209895599278378 }, "prevControl": null, "nextControl": { - "x": 0.8561904010950968, - "y": 4.3229483512516085 + "x": 0.45858915005213896, + "y": 4.229395115712089 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/rr2 Path.path b/src/main/deploy/pathplanner/paths/Blue (amp side w shoot).path similarity index 60% rename from src/main/deploy/pathplanner/paths/rr2 Path.path rename to src/main/deploy/pathplanner/paths/Blue (amp side w shoot).path index fcfdd2b..28b2afc 100644 --- a/src/main/deploy/pathplanner/paths/rr2 Path.path +++ b/src/main/deploy/pathplanner/paths/Blue (amp side w shoot).path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 15.052866117129224, - "y": 7.414093916500834 + "x": 0.5416846869729774, + "y": 6.6561862853455445 }, "prevControl": null, "nextControl": { - "x": 15.041171962687761, - "y": 7.4374822253796244 + "x": 1.5308679266007181, + "y": 6.582228659952817 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 14.175804533946229, - "y": 6.303149244469041 + "x": 4.0, + "y": 6.0 }, "prevControl": { - "x": 14.175804533947208, - "y": 6.291455090020513 + "x": 3.0, + "y": 6.0 }, "nextControl": { - "x": 14.17580453394525, - "y": 6.314843398917569 + "x": 5.0, + "y": 6.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 12.176104124289, - "y": 6.303149244469041 + "x": 4.923673991492127, + "y": 7.377273132924644 }, "prevControl": { - "x": 13.076554016356384, - "y": 6.273913858365985 + "x": 4.961836995746063, + "y": 6.688636566462322 }, "nextControl": null, "isLocked": false, @@ -55,11 +55,14 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 0, "rotateFast": false }, "reversed": false, "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": 59.931417178137586, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Amp Side w shoot.path b/src/main/deploy/pathplanner/paths/Blue Amp Side w shoot.path new file mode 100644 index 0000000..3e1fe36 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Amp Side w shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.5417903407235394, + "y": 4.4271562828839945 + }, + "prevControl": null, + "nextControl": { + "x": 1.5417903407235425, + "y": 4.4271562828839945 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8609165789760156, + "y": 0.6450161471751537 + }, + "prevControl": { + "x": 1.8609165789760156, + "y": 0.6450161471751537 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.5344550805402, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bl Path.path b/src/main/deploy/pathplanner/paths/Blue Center w shoot.path similarity index 68% rename from src/main/deploy/pathplanner/paths/bl Path.path rename to src/main/deploy/pathplanner/paths/Blue Center w shoot.path index f9e3f02..a368a14 100644 --- a/src/main/deploy/pathplanner/paths/bl Path.path +++ b/src/main/deploy/pathplanner/paths/Blue Center w shoot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.38839644630856207, - "y": 7.3322348354098414 + "x": 1.2337167531477924, + "y": 5.55593454044051 }, "prevControl": null, "nextControl": { - "x": 0.4351730640783217, - "y": 7.297152372082522 + "x": 2.233716753147796, + "y": 5.55593454044051 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.5461177361100726, - "y": 7.3322348354098414 + "x": 4.0, + "y": 6.0 }, "prevControl": { - "x": 0.9380217051031957, - "y": 5.8704655301091 + "x": 3.0, + "y": 6.0 }, "nextControl": null, "isLocked": false, @@ -44,6 +44,9 @@ }, "reversed": false, "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RLS Path.path b/src/main/deploy/pathplanner/paths/RLS Path.path index 0efd85c..20c218c 100644 --- a/src/main/deploy/pathplanner/paths/RLS Path.path +++ b/src/main/deploy/pathplanner/paths/RLS Path.path @@ -3,57 +3,41 @@ "waypoints": [ { "anchor": { - "x": 15.82468031032928, - "y": 6.770915422172725 + "x": 16.0936458625054, + "y": 6.922939429924444 }, "prevControl": null, "nextControl": { - "x": 15.836374464771719, - "y": 6.782609576615165 + "x": 16.152144411806535, + "y": 6.9424389463581555 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 14.24596946059989, - "y": 6.209596008935608 + "x": 13.731426665132533, + "y": 7.6947536231254805 }, "prevControl": { - "x": 14.23427530615745, - "y": 6.221290163378046 + "x": 13.750926181566244, + "y": 7.714253139559192 }, "nextControl": { - "x": 14.257663615042329, - "y": 6.19790185449317 + "x": 13.711927148698821, + "y": 7.675254106691769 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 11.474454857741623, - "y": 6.385008325572207 + "x": 11.205489305565505, + "y": 7.519341306488881 }, "prevControl": { - "x": 11.474454857741623, - "y": 6.396702480014649 - }, - "nextControl": { - "x": 11.474454857741623, - "y": 6.373314171129766 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.936823343732158, - "y": 7.460870534276682 - }, - "prevControl": { - "x": 10.12962709686103, - "y": 6.882009889375905 + "x": 11.231224680658903, + "y": 7.502458900427612 }, "nextControl": null, "isLocked": false, @@ -64,17 +48,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", + "name": "shoot", "waypointRelativePos": 0, "command": { "type": "parallel", @@ -92,7 +66,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/RR Path.path b/src/main/deploy/pathplanner/paths/RR Path.path index b566cec..85eb5dc 100644 --- a/src/main/deploy/pathplanner/paths/RR Path.path +++ b/src/main/deploy/pathplanner/paths/RR Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 16.128728325832714, - "y": 7.414093916506922 + "x": 16.112410901029318, + "y": 2.0413098429059118 }, "prevControl": null, "nextControl": { - "x": 16.128728325832714, - "y": 7.414093916506922 + "x": 16.132544988823533, + "y": 2.038276600221949 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 15.052866117128234, - "y": 7.414093916506922 + "x": 13.615844906108418, + "y": 0.6167986575686938 }, "prevControl": { - "x": 15.25166674264972, - "y": 5.496252587946771 + "x": 13.654843938975842, + "y": 0.5875493829181261 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.0, + "rotation": -1.080924186660749, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/RRS Path.path b/src/main/deploy/pathplanner/paths/RRS Path.path index 8926bf5..55cf11a 100644 --- a/src/main/deploy/pathplanner/paths/RRS Path.path +++ b/src/main/deploy/pathplanner/paths/RRS Path.path @@ -3,57 +3,41 @@ "waypoints": [ { "anchor": { - "x": 15.836374464771723, - "y": 4.326837143702777 + "x": 16.058563399178077, + "y": 4.198201444835939 }, "prevControl": null, "nextControl": { - "x": 15.848068619214162, - "y": 4.3385312981452175 + "x": 16.11706194847921, + "y": 4.217700961269649 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.87175651844181, - "y": 2.2569718073909093 + "x": 14.35496392119545, + "y": 2.1968539711727315 }, "prevControl": { - "x": 13.86006236399937, - "y": 2.268665961833347 + "x": 14.374463437629162, + "y": 2.2163534876064426 }, "nextControl": { - "x": 13.88345067288425, - "y": 2.2452776529484715 + "x": 14.335464404761737, + "y": 2.17735445473902 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 11.299042541105024, - "y": 1.6839582397113522 + "x": 11.240571768892824, + "y": 0.7601200387585969 }, "prevControl": { - "x": 11.299042541105024, - "y": 1.6956523941537935 - }, - "nextControl": { - "x": 11.299042541105024, - "y": 1.6722640852689108 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 9.170706432580957, - "y": 0.8068966565283563 - }, - "prevControl": { - "x": 9.170706432580957, - "y": 0.8010495793071356 + "x": 11.265813024784427, + "y": 0.7625494581674139 }, "nextControl": null, "isLocked": false, @@ -64,17 +48,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", + "name": "shoot", "waypointRelativePos": 0, "command": { "type": "parallel", @@ -92,7 +66,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/RS Path.path b/src/main/deploy/pathplanner/paths/RS Path.path index 0d86dd1..50324dd 100644 --- a/src/main/deploy/pathplanner/paths/RS Path.path +++ b/src/main/deploy/pathplanner/paths/RS Path.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 15.146419352667763, - "y": 5.578111669043852 + "x": 15.319384055583853, + "y": 5.551187918118234 }, "prevControl": null, "nextControl": { - "x": 15.158113507110203, - "y": 5.589805823486293 + "x": 15.377882604884993, + "y": 5.570687434551946 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 14.24596946059989, - "y": 6.209596008935608 + "x": 13.615844906108418, + "y": 4.86096084893422 }, "prevControl": { - "x": 14.23427530615745, - "y": 6.221290163378046 + "x": 13.63534442254213, + "y": 4.880460365367932 }, "nextControl": { - "x": 14.257663615042329, - "y": 6.19790185449317 + "x": 13.596345389674706, + "y": 4.841461332500509 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 11.801891182129943, - "y": 6.326537553360007 + "x": 9.180768844543056, + "y": 7.328155479208991 }, "prevControl": { - "x": 11.801891182129943, - "y": 6.338231707802448 + "x": 8.376413791652427, + "y": 7.294031325449995 }, "nextControl": null, "isLocked": false, @@ -48,17 +48,7 @@ "constraintZones": [], "eventMarkers": [ { - "name": "shootConveyor", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - }, - { - "name": "shootFlyWheel", + "name": "shoot", "waypointRelativePos": 0, "command": { "type": "parallel", @@ -76,7 +66,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Straight Red Amp Path.path b/src/main/deploy/pathplanner/paths/Straight Red Amp Path.path new file mode 100644 index 0000000..b2f8724 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Red Amp Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 16.147134266099517, + "y": 6.9085276198721415 + }, + "prevControl": null, + "nextControl": { + "x": 13.950280294926786, + "y": 7.7034418857570115 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.204212830960874, + "y": 7.732347859061915 + }, + "prevControl": { + "x": 13.747938481792456, + "y": 7.862424738933985 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 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/RL Path.path b/src/main/deploy/pathplanner/paths/Straight Red Source Path.path similarity index 63% rename from src/main/deploy/pathplanner/paths/RL Path.path rename to src/main/deploy/pathplanner/paths/Straight Red Source Path.path index 7d07639..6f288f7 100644 --- a/src/main/deploy/pathplanner/paths/RL Path.path +++ b/src/main/deploy/pathplanner/paths/Straight Red Source Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 15.541771563143822, - "y": 2.430613727257548 + "x": 16.233852186014232, + "y": 4.176913142558679 }, "prevControl": null, "nextControl": { - "x": 15.5446962437617, - "y": 2.4068975552230634 + "x": 14.716288587506751, + "y": 3.757776529637567 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 13.700871867301933, - "y": 1.1992202606733011 + "x": 11.117494911046158, + "y": 1.8355293048614274 }, "prevControl": { - "x": 13.716393538454641, - "y": 1.1975158911190236 + "x": 13.73663367045045, + "y": 1.503110611855028 }, "nextControl": null, "isLocked": false, @@ -39,11 +39,14 @@ }, "goalEndState": { "velocity": 0, - "rotation": -92.40840760476861, + "rotation": 0, "rotateFast": false }, "reversed": false, "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bl2 Path.path b/src/main/deploy/pathplanner/paths/bl2 Path.path deleted file mode 100644 index 1739aed..0000000 --- a/src/main/deploy/pathplanner/paths/bl2 Path.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.52342385444866, - "y": 7.390316728376925 - }, - "prevControl": null, - "nextControl": { - "x": 1.2771521839339972, - "y": 6.525338178881486 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.4106518521846283, - "y": 6.337342840960248 - }, - "prevControl": { - "x": 2.4301513686183416, - "y": 6.3470925991771034 - }, - "nextControl": { - "x": 2.391152335750915, - "y": 6.327593082743392 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5026247724725117, - "y": 6.337342840960248 - }, - "prevControl": { - "x": 2.4106518521846274, - "y": 6.3470925991771034 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.95, - "rotationDegrees": -90.60054724312275, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6dca6d3..766580a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -44,8 +44,8 @@ public static final class DumpConstants { public static final class ElevatorConstants { // upy downy chain lift thing - public static final int IN = 10; - public static final int OUT = 11; + public static final int IN = 11; + public static final int OUT = 10; } } @@ -77,7 +77,6 @@ public static class MechanismConstants { public static final boolean INTAKE_MOTOR_FRONT_INVERTED = false; public static final double INTAKE_MOTOR_SPEED_FRONT = 0.1; public static final double INTAKE_MOTOR_SPEED_SUSHI = 1; - // Conveyor Motors public static final int CONVEYOR_MOTOR_1 = 1; public static final int CONVEYOR_MOTOR_2 = 2; @@ -90,9 +89,10 @@ public static class MechanismConstants { public static final int FLYWHEEL_MOTOR_2 = 4; public static final boolean FLYWHEEL_MOTOR_1_INVERTED = true; public static final boolean FLYWHEEL_MOTOR_2_INVERTED = false; - public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.51; + public static final double FLYWHEEL_MOTOR_FULL_SPEED = 0.60; public static final double FLYWHEEL_MOTOR_REDUCED_SPEED = 0.20; public static final double FLYWHEEL_MOTOR_SPEED_SLOW = 0.15; + public static final double FLYWHEEL_MOTOR_SPEED_100 = 1; } public class VisionConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42e3a98..707c89f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,8 @@ import frc.robot.commands.ElevatorControl; import frc.robot.commands.FlyWCommand; import frc.robot.commands.IntakeCommand; +import frc.robot.commands.auto.SHOOTCONVEYOR; +import frc.robot.commands.auto.SHOOTFLYS; import frc.robot.commands.autoCommands.dumpBed; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.subsystems.Conveyor; @@ -72,6 +74,9 @@ public class RobotContainer { private final PhotonCamera intakeCamera = vision.getIntakeCamera(); private final PhotonCamera conveyorCamera = vision.getConveyorCamera(); + private final SHOOTCONVEYOR shootconveyorcmd = new SHOOTCONVEYOR(conveyor); + private final SHOOTFLYS shootflyscmd = new SHOOTFLYS(FlyWheel); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -87,6 +92,8 @@ public RobotContainer() { "shootFlyWheel", new InstantCommand(() -> FlyWheel.enableflywheelfull())); NamedCommands.registerCommand("dump", new InstantCommand(() -> dump.open())); NamedCommands.registerCommand("Dump bed", new dumpBed(dump)); + NamedCommands.registerCommand("SHOOTCONVEYOR", shootconveyorcmd); + NamedCommands.registerCommand("SHOOTFLYS", shootflyscmd); // Configure the trigger bindings configureBindings(); diff --git a/src/main/java/frc/robot/commands/ConveyorCommand.java b/src/main/java/frc/robot/commands/ConveyorCommand.java index 7c60288..1697fe9 100644 --- a/src/main/java/frc/robot/commands/ConveyorCommand.java +++ b/src/main/java/frc/robot/commands/ConveyorCommand.java @@ -28,9 +28,7 @@ public void execute() { || ConveyorJoystick.getRawButton(3) || ConveyorJoystick.getRawButton(10)) { m_ConveyorSubsystem.intakeConveyor(); - } else if (ConveyorJoystick.getRawButton(6) - || ConveyorJoystick.getRawButton(4) - || ConveyorJoystick.getRawButton(9)) { + } else if (ConveyorJoystick.getRawButton(6) || ConveyorJoystick.getRawButton(4)) { m_ConveyorSubsystem.reverseConveyor(); System.out.println("Conveyor Moving in Reverse"); } else { diff --git a/src/main/java/frc/robot/commands/FlyWCommand.java b/src/main/java/frc/robot/commands/FlyWCommand.java index 8ea2c73..6aa9750 100644 --- a/src/main/java/frc/robot/commands/FlyWCommand.java +++ b/src/main/java/frc/robot/commands/FlyWCommand.java @@ -36,6 +36,8 @@ public void execute() { } else if (FlyWJoystick.getRawButton(11) || FlyWJoystick.getRawButton(7)) { m_FlyWSubsystem.reverseflywheel(); System.out.println("Flywheels Moving in Reverse"); + } else if (FlyWJoystick.getRawButton(9)) { + m_FlyWSubsystem.fastflywheel(); } else { m_FlyWSubsystem.disableflywheel(); } diff --git a/src/main/java/frc/robot/commands/autoCommands/shootConveyor.java b/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java similarity index 58% rename from src/main/java/frc/robot/commands/autoCommands/shootConveyor.java rename to src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java index 0765334..1e93ba1 100644 --- a/src/main/java/frc/robot/commands/autoCommands/shootConveyor.java +++ b/src/main/java/frc/robot/commands/auto/SHOOTCONVEYOR.java @@ -2,23 +2,35 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands.autoCommands; +package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Conveyor; -public class shootConveyor extends Command { - /** Creates a new shootConveyor. */ - public shootConveyor() { +public class SHOOTCONVEYOR extends Command { + /** Creates a new SHOOT. */ + private final Conveyor m_conveyor; + + private boolean m_finished = false; + + public SHOOTCONVEYOR(Conveyor conveyor) { + m_conveyor = conveyor; + addRequirements(m_conveyor); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_finished = false; + m_conveyor.intakeConveyor(); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_finished = true; + } // Called once the command ends or is interrupted. @Override @@ -27,6 +39,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_finished; } } diff --git a/src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java similarity index 58% rename from src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java rename to src/main/java/frc/robot/commands/auto/SHOOTFLYS.java index 2bc10ad..c793b2a 100644 --- a/src/main/java/frc/robot/commands/autoCommands/shootFlyWheel.java +++ b/src/main/java/frc/robot/commands/auto/SHOOTFLYS.java @@ -2,23 +2,35 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands.autoCommands; +package frc.robot.commands.auto; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.FlyWheel; -public class shootFlyWheel extends Command { - /** Creates a new shootFlyWheel. */ - public shootFlyWheel() { +public class SHOOTFLYS extends Command { + /** Creates a new SHOOTFLYS. */ + private final FlyWheel m_flywheel; + + private boolean m_finished = false; + + public SHOOTFLYS(FlyWheel flywheel) { + m_flywheel = flywheel; + addRequirements(m_flywheel); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_finished = false; + m_flywheel.enableflywheelfull(); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + m_finished = true; + } // Called once the command ends or is interrupted. @Override @@ -27,6 +39,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_finished; } } diff --git a/src/main/java/frc/robot/subsystems/Conveyor.java b/src/main/java/frc/robot/subsystems/Conveyor.java index 30099db..0cd28c3 100644 --- a/src/main/java/frc/robot/subsystems/Conveyor.java +++ b/src/main/java/frc/robot/subsystems/Conveyor.java @@ -6,7 +6,6 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.MechanismConstants; @@ -46,8 +45,8 @@ public void periodic() { // This method will be called once per scheduler run } - public Command shootCommand() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); + public void SHOOTCONVEYOR() { + conveyorMotor1.set(MechanismConstants.CONVEYOR_MOTOR_SPEED); + conveyorMotor2.set(MechanismConstants.CONVEYOR_MOTOR_SPEED); } } diff --git a/src/main/java/frc/robot/subsystems/FlyWheel.java b/src/main/java/frc/robot/subsystems/FlyWheel.java index 8f9a90d..a0070a9 100644 --- a/src/main/java/frc/robot/subsystems/FlyWheel.java +++ b/src/main/java/frc/robot/subsystems/FlyWheel.java @@ -57,8 +57,13 @@ public void periodic() { // This method will be called once per scheduler run } - public Object shootCommand() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shootCommand'"); + public void fastflywheel() { + flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); + flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_SPEED_100); + } + + public void SHOOTFLYS() { + flywheelMotor1.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); + flywheelMotor2.set(MechanismConstants.FLYWHEEL_MOTOR_FULL_SPEED); } }