diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 60d622f9..a9e9a1f1 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -11,8 +11,8 @@ ], "autoFolders": [], "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxAccel": 2.5, "defaultMaxAngVel": 45.0, - "defaultMaxAngAccel": 45.0, + "defaultMaxAngAccel": 60.0, "maxModuleSpeed": 3.0 } \ No newline at end of file diff --git a/advantageScopeAssets/Robot_Tucker/config.json b/advantageScopeAssets/Robot_Tucker/config.json index 9ef3b708..0101d264 100644 --- a/advantageScopeAssets/Robot_Tucker/config.json +++ b/advantageScopeAssets/Robot_Tucker/config.json @@ -19,17 +19,50 @@ "cameras": [ { "name": "FrontCam", - "position": [ - 0.0508, - -0.1524, - 0.2604 - ], - "resolution": [ - 1280, - 960 - ], - "fov": 100 - } + "position": [ + 0.0508, + -0.1524, + 0.589701 + ], + "rotations": [ + { + "axis": "y", + "degrees": -33 + }, + { + "axis": "z", + "degrees": 15 + } + ], + "resolution": [ + 1280, + 960 + ], + "fov": 80 + }, + { + "name": "BackCam", + "position": [ + -0.254, + -0.155575, + 0.589701 + ], + "rotations" : [ + { + "axis": "z", + "degrees": 180 + }, + { + "axis": "y", + "degrees": -10 + } + ], + "resolution": [ + 1280, + 960 + ], + "fov": 80 + } ], "components": [ { @@ -132,6 +165,10 @@ 0, 0 ] + }, + { + "zeroedRotations": [], + "zeroedPosition": [] } ] } \ No newline at end of file diff --git a/advantageScopeAssets/Robot_Tucker/model_0.glb b/advantageScopeAssets/Robot_Tucker/model_0.glb index 7363ba79..2ece3957 100644 Binary files a/advantageScopeAssets/Robot_Tucker/model_0.glb and b/advantageScopeAssets/Robot_Tucker/model_0.glb differ diff --git a/advantageScopeAssets/Robot_Tucker/model_1.glb b/advantageScopeAssets/Robot_Tucker/model_1.glb index 8fd3d282..b95c3ee7 100644 Binary files a/advantageScopeAssets/Robot_Tucker/model_1.glb and b/advantageScopeAssets/Robot_Tucker/model_1.glb differ diff --git a/advantageScopeAssets/Robot_Tucker/model_2.glb b/advantageScopeAssets/Robot_Tucker/model_2.glb index 8af20e82..f3b901c2 100644 Binary files a/advantageScopeAssets/Robot_Tucker/model_2.glb and b/advantageScopeAssets/Robot_Tucker/model_2.glb differ diff --git a/advantageScopeAssets/Robot_Tucker/model_3.glb b/advantageScopeAssets/Robot_Tucker/model_3.glb index e460b05f..c97a06a9 100644 Binary files a/advantageScopeAssets/Robot_Tucker/model_3.glb and b/advantageScopeAssets/Robot_Tucker/model_3.glb differ diff --git a/advantageScopeAssets/Robot_Tucker/model_4.glb b/advantageScopeAssets/Robot_Tucker/model_4.glb index ca440acb..deefced9 100644 Binary files a/advantageScopeAssets/Robot_Tucker/model_4.glb and b/advantageScopeAssets/Robot_Tucker/model_4.glb differ diff --git a/advantageScopeAssets/Robot_Tucker/model_5.glb b/advantageScopeAssets/Robot_Tucker/model_5.glb new file mode 100644 index 00000000..6285ef85 Binary files /dev/null and b/advantageScopeAssets/Robot_Tucker/model_5.glb differ diff --git a/build.gradle b/build.gradle index 359e375d..d4dad29c 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" } repositories { diff --git a/glass.json b/glass.json new file mode 100644 index 00000000..68d1f72f --- /dev/null +++ b/glass.json @@ -0,0 +1,35 @@ +{ + "NetworkTables": { + "transitory": { + "Robot": { + "choreoChooser": { + "open": true + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/Robot/choreoChooser": "String Chooser", + "/Robot/field2d": "Field2d", + "/Robot/pathPlannerStorage/autoChooser": "String Chooser" + }, + "windows": { + "/Robot/pathPlannerStorage/autoChooser": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables Info": { + "Clients": { + "open": true + }, + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "4738" + } +} diff --git a/src/main/deploy/pathplanner/autos/S C1-3 S.auto b/src/main/deploy/pathplanner/autos/S C1-3 S.auto index e314e705..b3842c50 100644 --- a/src/main/deploy/pathplanner/autos/S C1-3 S.auto +++ b/src/main/deploy/pathplanner/autos/S C1-3 S.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3070140425995143, - "y": 6.301761136557157 + "x": 1.3127322708546125, + "y": 5.531086239477443 }, - "rotation": -149.77686787793002 + "rotation": 179.27393174384346 }, "command": { "type": "sequential", @@ -18,33 +18,33 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "C1 L" + "name": "C1toC3" } }, { "type": "path", "data": { - "pathName": "L C2" + "pathName": "M W3" } }, { "type": "path", "data": { - "pathName": "C2 L" + "pathName": "W3 W2" } }, { "type": "path", "data": { - "pathName": "L C3" + "pathName": "W2 W1" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "C3 M" + "name": "StopAll" } } ] diff --git a/src/main/deploy/pathplanner/autos/S C1-5 S.auto b/src/main/deploy/pathplanner/autos/S C1-5 S.auto index 4d541346..158b6607 100644 --- a/src/main/deploy/pathplanner/autos/S C1-5 S.auto +++ b/src/main/deploy/pathplanner/autos/S C1-5 S.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3070140425995143, + "x": 1.205481291141754, "y": 6.301761136557157 }, "rotation": -147.41876198989902 diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S.auto b/src/main/deploy/pathplanner/autos/S W3-1 S.auto index e7157657..5be273ce 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S.auto +++ b/src/main/deploy/pathplanner/autos/S W3-1 S.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.2514459967329792, - "y": 4.481558795144466 + "x": 0.8059318609740422, + "y": 4.440615856320681 }, "rotation": 136.71493019552656 }, diff --git a/src/main/deploy/pathplanner/paths/A C1.path b/src/main/deploy/pathplanner/paths/A C1.path index 4fe9bf26..c8ed8e88 100644 --- a/src/main/deploy/pathplanner/paths/A C1.path +++ b/src/main/deploy/pathplanner/paths/A C1.path @@ -89,9 +89,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A C2.path b/src/main/deploy/pathplanner/paths/A C2.path index 3b7dc8eb..71f94441 100644 --- a/src/main/deploy/pathplanner/paths/A C2.path +++ b/src/main/deploy/pathplanner/paths/A C2.path @@ -89,9 +89,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A W1.path b/src/main/deploy/pathplanner/paths/A W1.path index 81d9c01b..d45008cc 100644 --- a/src/main/deploy/pathplanner/paths/A W1.path +++ b/src/main/deploy/pathplanner/paths/A W1.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1 C2.path b/src/main/deploy/pathplanner/paths/C1 C2.path index 8852b656..fb187316 100644 --- a/src/main/deploy/pathplanner/paths/C1 C2.path +++ b/src/main/deploy/pathplanner/paths/C1 C2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 8.001277427009363, - "y": 7.134919234844645 + "x": 7.287760958500641, + "y": 7.357800247515389 }, "isLocked": false, "linkedName": "C1" @@ -20,8 +20,8 @@ "y": 5.773167022257897 }, "prevControl": { - "x": 6.714263440877643, - "y": 5.432519782289461 + "x": 6.763763314760672, + "y": 5.269087417607459 }, "nextControl": null, "isLocked": false, @@ -47,13 +47,30 @@ ] } } + }, + { + "name": "StopIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopIntake" + } + } + ] + } + } } ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1 L.path b/src/main/deploy/pathplanner/paths/C1 L.path index 5a7ed33d..29f81ac9 100644 --- a/src/main/deploy/pathplanner/paths/C1 L.path +++ b/src/main/deploy/pathplanner/paths/C1 L.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.1237047894028604, - "y": 6.205235254815414 + "x": 5.11, + "y": 6.23 }, "prevControl": { - "x": 6.762614197120893, - "y": 6.609724664018198 + "x": 5.8754953792102445, + "y": 6.559163487867849 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ }, { "name": "Shoot", - "waypointRelativePos": 1.0, + "waypointRelativePos": 0.75, "command": { "type": "parallel", "data": { @@ -74,13 +74,13 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, - "rotation": -178.44131579378922, + "rotation": -177.32156198722237, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C2 C1.path b/src/main/deploy/pathplanner/paths/C2 C1.path index a3e40600..dcd36c53 100644 --- a/src/main/deploy/pathplanner/paths/C2 C1.path +++ b/src/main/deploy/pathplanner/paths/C2 C1.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C2 C3.path b/src/main/deploy/pathplanner/paths/C2 C3.path index af631933..18ba4deb 100644 --- a/src/main/deploy/pathplanner/paths/C2 C3.path +++ b/src/main/deploy/pathplanner/paths/C2 C3.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C2 L.path b/src/main/deploy/pathplanner/paths/C2 L.path index 661baa7e..e465e514 100644 --- a/src/main/deploy/pathplanner/paths/C2 L.path +++ b/src/main/deploy/pathplanner/paths/C2 L.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.1237047894028604, - "y": 6.205235254815414 + "x": 5.11, + "y": 6.23 }, "prevControl": { - "x": 6.721245962088789, - "y": 6.09032349083735 + "x": 5.781084701631889, + "y": 6.225403529440879 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ }, { "name": "Shoot", - "waypointRelativePos": 1.0, + "waypointRelativePos": 0.65, "command": { "type": "parallel", "data": { @@ -74,13 +74,13 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, - "rotation": -173.9932219126334, + "rotation": -176.87039607585024, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C3 C4.path b/src/main/deploy/pathplanner/paths/C3 C4.path index 5ce3d5e2..18d7251e 100644 --- a/src/main/deploy/pathplanner/paths/C3 C4.path +++ b/src/main/deploy/pathplanner/paths/C3 C4.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C3 L.path b/src/main/deploy/pathplanner/paths/C3 L.path index 7589e6e9..ad598091 100644 --- a/src/main/deploy/pathplanner/paths/C3 L.path +++ b/src/main/deploy/pathplanner/paths/C3 L.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 6.1237047894028604, - "y": 6.205235254815414 + "x": 5.11, + "y": 6.23 }, "prevControl": { - "x": 6.83615772606685, - "y": 6.163867019783312 + "x": 5.82245293666399, + "y": 6.188631764967898 }, "nextControl": null, "isLocked": false, @@ -74,9 +74,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C3 M.path b/src/main/deploy/pathplanner/paths/C3 M.path index 83c77cae..47011ea8 100644 --- a/src/main/deploy/pathplanner/paths/C3 M.path +++ b/src/main/deploy/pathplanner/paths/C3 M.path @@ -20,8 +20,8 @@ "y": 4.806186572471635 }, "prevControl": { - "x": 5.079428633796431, - "y": 4.589892129330836 + "x": 5.079435279774727, + "y": 4.589911127587042 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ }, { "name": "Shoot", - "waypointRelativePos": 1.0, + "waypointRelativePos": 0.6, "command": { "type": "parallel", "data": { @@ -74,13 +74,13 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, - "rotation": 170.0690749948686, + "rotation": 165.70500276495054, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C4 C3.path b/src/main/deploy/pathplanner/paths/C4 C3.path index 05839d89..b51f895f 100644 --- a/src/main/deploy/pathplanner/paths/C4 C3.path +++ b/src/main/deploy/pathplanner/paths/C4 C3.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C4 C5.path b/src/main/deploy/pathplanner/paths/C4 C5.path index f49ec37c..b1d94ef7 100644 --- a/src/main/deploy/pathplanner/paths/C4 C5.path +++ b/src/main/deploy/pathplanner/paths/C4 C5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.293238893308695, - "y": 0.7722070539325854 + "x": 8.281967377058862, + "y": 0.7612043754757987 }, "prevControl": { - "x": 6.528191172750587, - "y": 0.5542999240055222 + "x": 6.516919656500754, + "y": 0.5432972455487355 }, "nextControl": null, "isLocked": false, @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C4 M.path b/src/main/deploy/pathplanner/paths/C4 M.path index bdc654ae..26356964 100644 --- a/src/main/deploy/pathplanner/paths/C4 M.path +++ b/src/main/deploy/pathplanner/paths/C4 M.path @@ -74,9 +74,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C4 R.path b/src/main/deploy/pathplanner/paths/C4 R.path index 639ae616..2354db6a 100644 --- a/src/main/deploy/pathplanner/paths/C4 R.path +++ b/src/main/deploy/pathplanner/paths/C4 R.path @@ -74,9 +74,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C5 C4.path b/src/main/deploy/pathplanner/paths/C5 C4.path index 890a85f3..e820e2aa 100644 --- a/src/main/deploy/pathplanner/paths/C5 C4.path +++ b/src/main/deploy/pathplanner/paths/C5 C4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.293238893308695, - "y": 0.7722070539325854 + "x": 8.281967377058862, + "y": 0.7612043754757987 }, "prevControl": null, "nextControl": { - "x": 7.551843971534794, - "y": 1.4284762698796707 + "x": 7.540572455284961, + "y": 1.4174735914228842 }, "isLocked": false, "linkedName": "C5" @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C5 R.path b/src/main/deploy/pathplanner/paths/C5 R.path index 2ba62343..e442e1bc 100644 --- a/src/main/deploy/pathplanner/paths/C5 R.path +++ b/src/main/deploy/pathplanner/paths/C5 R.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.293238893308695, - "y": 0.7722070539325854 + "x": 8.281967377058862, + "y": 0.7612043754757987 }, "prevControl": null, "nextControl": { - "x": 7.121138900732449, - "y": 0.8779258767924034 + "x": 7.109867384482616, + "y": 0.8669231983356167 }, "isLocked": false, "linkedName": "C5" @@ -74,9 +74,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/L C1.path b/src/main/deploy/pathplanner/paths/L C1.path index afdcb078..cc946d9d 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": 6.1237047894028604, - "y": 6.205235254815414 + "x": 5.11, + "y": 6.23 }, "prevControl": null, "nextControl": { - "x": 6.951069490044915, - "y": 6.775197604146607 + "x": 5.937364700642055, + "y": 6.799962349331193 }, "isLocked": false, "linkedName": "L" @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/L C2.path b/src/main/deploy/pathplanner/paths/L C2.path index 5bc5dd43..4203c906 100644 --- a/src/main/deploy/pathplanner/paths/L C2.path +++ b/src/main/deploy/pathplanner/paths/L C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.1237047894028604, - "y": 6.205235254815414 + "x": 5.11, + "y": 6.23 }, "prevControl": null, "nextControl": { - "x": 8.66555300859762, - "y": 5.75937761058053 + "x": 6.590063520037455, + "y": 6.390876469569289 }, "isLocked": false, "linkedName": "L" @@ -20,8 +20,8 @@ "y": 5.773167022257897 }, "prevControl": { - "x": 7.240647135269635, - "y": 5.993797609095778 + "x": 7.476982329851185, + "y": 5.945917707438252 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.15, + "waypointRelativePos": 0.8, "command": { "type": "parallel", "data": { @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/L C3.path b/src/main/deploy/pathplanner/paths/L C3.path index d100aac4..0e85dd3b 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": 6.1237047894028604, - "y": 6.205235254815414 + "x": 5.11, + "y": 6.23 }, "prevControl": null, "nextControl": { - "x": 7.28661184086086, - "y": 6.214428195933659 + "x": 7.293323515583201, + "y": 6.606910585848048 }, "isLocked": false, "linkedName": "L" @@ -20,8 +20,8 @@ "y": 4.099779112962734 }, "prevControl": { - "x": 7.486332030789803, - "y": 4.469830434322791 + "x": 7.429102428193659, + "y": 4.173595267683257 }, "nextControl": null, "isLocked": false, @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/M C2.path b/src/main/deploy/pathplanner/paths/M C2.path index e8fa9593..dd2d2bb7 100644 --- a/src/main/deploy/pathplanner/paths/M C2.path +++ b/src/main/deploy/pathplanner/paths/M C2.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/M C3.path b/src/main/deploy/pathplanner/paths/M C3.path index cb7e08d9..f02eda6a 100644 --- a/src/main/deploy/pathplanner/paths/M C3.path +++ b/src/main/deploy/pathplanner/paths/M C3.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/M C4.path b/src/main/deploy/pathplanner/paths/M C4.path index 2f88d01b..ef17f125 100644 --- a/src/main/deploy/pathplanner/paths/M C4.path +++ b/src/main/deploy/pathplanner/paths/M C4.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/M W3.path b/src/main/deploy/pathplanner/paths/M W3.path new file mode 100644 index 00000000..30c6eede --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M W3.path @@ -0,0 +1,104 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.461157776932109, + "y": 4.806186572471635 + }, + "prevControl": null, + "nextControl": { + "x": 2.9975302503796497, + "y": 4.868811439750539 + }, + "isLocked": false, + "linkedName": "M" + }, + { + "anchor": { + "x": 2.5685447337679963, + "y": 4.3334957369796125 + }, + "prevControl": { + "x": 0.7705402644847841, + "y": 4.621368107984443 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "W3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "StopIntake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopIntake" + } + } + ] + } + } + }, + { + "name": "Intake", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "PrepareShooterW3", + "waypointRelativePos": 0.4, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShooterW3" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 45.0, + "maxAngularAcceleration": 60.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 169.00174959554687, + "rotateFast": false + }, + "reversed": false, + "folder": "Stage", + "previewStartingState": { + "rotation": 171.92085261432547, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P A.path b/src/main/deploy/pathplanner/paths/P A.path index 59a88dea..acaeb6d9 100644 --- a/src/main/deploy/pathplanner/paths/P A.path +++ b/src/main/deploy/pathplanner/paths/P A.path @@ -39,9 +39,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/R C3.path b/src/main/deploy/pathplanner/paths/R C3.path index 193712cd..48a43ff0 100644 --- a/src/main/deploy/pathplanner/paths/R C3.path +++ b/src/main/deploy/pathplanner/paths/R C3.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/R C4.path b/src/main/deploy/pathplanner/paths/R C4.path index b91a2b7d..c14fb875 100644 --- a/src/main/deploy/pathplanner/paths/R C4.path +++ b/src/main/deploy/pathplanner/paths/R C4.path @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/R C5.path b/src/main/deploy/pathplanner/paths/R C5.path index e4988879..d99f28e0 100644 --- a/src/main/deploy/pathplanner/paths/R C5.path +++ b/src/main/deploy/pathplanner/paths/R C5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.293238893308695, - "y": 0.7722070539325854 + "x": 8.281967377058862, + "y": 0.7612043754757987 }, "prevControl": { - "x": 7.493305272324831, - "y": 1.1287782575565153 + "x": 7.482033756074998, + "y": 1.1177755790997286 }, "nextControl": null, "isLocked": false, @@ -33,9 +33,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S C1.path b/src/main/deploy/pathplanner/paths/S C1.path index 3ec83766..389a3112 100644 --- a/src/main/deploy/pathplanner/paths/S C1.path +++ b/src/main/deploy/pathplanner/paths/S C1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.3070140425995143, - "y": 6.301761136557157 + "x": 1.3127322708546125, + "y": 5.531086239477443 }, "prevControl": null, "nextControl": { - "x": 2.307014042599504, - "y": 6.301761136557157 + "x": 1.8030224638276828, + "y": 5.552536435420015 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.225362448483157, - "y": 6.301761136557157 + "x": 3.7243471575409, + "y": 5.531086239477443 }, "prevControl": { - "x": 3.2253775429367515, - "y": 6.296266713999426 + "x": 1.415579801452321, + "y": 5.53499278322717 }, "nextControl": { - "x": 5.061920090245556, - "y": 6.30635760711611 + "x": 4.629851857688037, + "y": 5.5295540826244025 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 7.43617949033381 }, "prevControl": { - "x": 7.306444581645385, - "y": 7.440439611256061 + "x": 7.282015370301738, + "y": 7.280809365650086 }, "nextControl": null, "isLocked": false, @@ -47,25 +47,54 @@ "rotationTargets": [ { "waypointRelativePos": 1.15, + "rotationDegrees": -175.57940289068864, + "rotateFast": false + }, + { + "waypointRelativePos": 0.75, "rotationDegrees": 180.0, - "rotateFast": true + "rotateFast": false } ], "constraintZones": [], "eventMarkers": [ { - "name": "Start", + "name": "End", + "waypointRelativePos": 1.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "PrepareSWD", "waypointRelativePos": 0, "command": { "type": "parallel", "data": { - "commands": [] + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] } } }, { - "name": "End", - "waypointRelativePos": 1.75, + "name": "Shoot", + "waypointRelativePos": 0.45, "command": { "type": "parallel", "data": { @@ -73,7 +102,41 @@ { "type": "named", "data": { - "name": "Intake" + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "PrepareShotL", + "waypointRelativePos": 1.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShooterL" + } + } + ] + } + } + }, + { + "name": "ShootW2", + "waypointRelativePos": 0.8, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" } } ] @@ -83,19 +146,19 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { - "velocity": 0, - "rotation": 179.6530934882869, + "velocity": 0.0, + "rotation": -168.24048681001508, "rotateFast": false }, "reversed": false, "folder": "Preloads", "previewStartingState": { - "rotation": -158.8242605828954, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S C2.path b/src/main/deploy/pathplanner/paths/S C2.path index 4fc0b6d8..1bb0c270 100644 --- a/src/main/deploy/pathplanner/paths/S C2.path +++ b/src/main/deploy/pathplanner/paths/S C2.path @@ -95,9 +95,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S C3.path b/src/main/deploy/pathplanner/paths/S C3.path index 9ee0c2b7..a3b31427 100644 --- a/src/main/deploy/pathplanner/paths/S C3.path +++ b/src/main/deploy/pathplanner/paths/S C3.path @@ -106,9 +106,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S C4.path b/src/main/deploy/pathplanner/paths/S C4.path new file mode 100644 index 00000000..8821ea65 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S C4.path @@ -0,0 +1,137 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2671762861357376, + "y": 2.9511308095369406 + }, + "prevControl": null, + "nextControl": { + "x": 2.4031274310765443, + "y": 2.9862633191742836 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.511078009317217, + "y": 3.396142598276637 + }, + "prevControl": { + "x": 2.8660365081744184, + "y": 1.9754249381987647 + }, + "nextControl": { + "x": 6.314546837367568, + "y": 4.953683858865578 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.293238893308695, + "y": 2.4407258668940637 + }, + "prevControl": { + "x": 6.431215882529434, + "y": 3.9045804351167517 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "C4" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "End", + "waypointRelativePos": 1.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "PrepareSWD", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + } + }, + { + "name": "Shoot", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "PrepareShotL", + "waypointRelativePos": 1.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShooterL" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 45.0, + "maxAngularAcceleration": 60.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 125.21759296819283, + "rotateFast": false + }, + "reversed": false, + "folder": "Preloads", + "previewStartingState": { + "rotation": 136.00508600525427, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S C5.path b/src/main/deploy/pathplanner/paths/S C5.path new file mode 100644 index 00000000..9f649e53 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S C5.path @@ -0,0 +1,143 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2671762861357376, + "y": 2.9511308095369406 + }, + "prevControl": null, + "nextControl": { + "x": 2.4031274310765443, + "y": 2.9862633191742836 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.576764134983334, + "y": 1.5341195874973776 + }, + "prevControl": { + "x": 3.5532352481451897, + "y": 2.371441885499374 + }, + "nextControl": { + "x": 6.255992654638661, + "y": 1.2530595103986197 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.281967377058862, + "y": 0.7612043754757987 + }, + "prevControl": { + "x": 7.6144496939493145, + "y": 1.3467462027648742 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "C5" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 142.43140797117252, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "End", + "waypointRelativePos": 1.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "PrepareSWD", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + } + }, + { + "name": "Shoot", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "PrepareShotL", + "waypointRelativePos": 1.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareShooterL" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 45.0, + "maxAngularAcceleration": 60.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 142.85981591729995, + "rotateFast": false + }, + "reversed": false, + "folder": "Preloads", + "previewStartingState": { + "rotation": 144.65989307844237, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/W1 C1.path b/src/main/deploy/pathplanner/paths/W1 C1.path index 8ae19a23..e67898c0 100644 --- a/src/main/deploy/pathplanner/paths/W1 C1.path +++ b/src/main/deploy/pathplanner/paths/W1 C1.path @@ -61,9 +61,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/W1 C2.path b/src/main/deploy/pathplanner/paths/W1 C2.path index ffb1414e..b27f0b67 100644 --- a/src/main/deploy/pathplanner/paths/W1 C2.path +++ b/src/main/deploy/pathplanner/paths/W1 C2.path @@ -68,9 +68,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/W1 W2.path b/src/main/deploy/pathplanner/paths/W1 W2.path index 3633afa5..7f5f6443 100644 --- a/src/main/deploy/pathplanner/paths/W1 W2.path +++ b/src/main/deploy/pathplanner/paths/W1 W2.path @@ -84,9 +84,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/W2 W1.path b/src/main/deploy/pathplanner/paths/W2 W1.path index 4d2c36a5..9f0464bd 100644 --- a/src/main/deploy/pathplanner/paths/W2 W1.path +++ b/src/main/deploy/pathplanner/paths/W2 W1.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/W3 W2.path b/src/main/deploy/pathplanner/paths/W3 W2.path index 0b644f4c..e64d95d5 100644 --- a/src/main/deploy/pathplanner/paths/W3 W2.path +++ b/src/main/deploy/pathplanner/paths/W3 W2.path @@ -51,9 +51,9 @@ ], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 45.0, - "maxAngularAcceleration": 45.0 + "maxAngularAcceleration": 60.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d5b19cce..c821f22b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,11 +16,10 @@ import frc.robot.util.Neo; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.DriveConstants; -import frc.robot.util.Constants.FieldConstants; -import frc.robot.util.Constants.FieldConstants.GameMode; import frc.robot.util.Constants.NeoMotorConstants; import monologue.Monologue; + /** * The VM is configured to automatically run this class, and to call the * functions corresponding to @@ -32,13 +31,21 @@ */ public class Robot extends TimedRobot { - private Command autonomousCommand; - - private RobotContainer robotContainer; + private static Optional alliance = Optional.empty(); + public static GameMode gameMode = GameMode.DISABLED; + public static enum GameMode { + DISABLED, + AUTONOMOUS, + TELEOP, + TEST + }; public static double currentTimestamp = 0; public static double previousTimestamp = 0; - private boolean hasStartedURCL = false; + + private Command autonomousCommand; + + private RobotContainer robotContainer; @Override public void robotInit() { @@ -49,7 +56,8 @@ public void robotInit() { DataLogManager.logNetworkTables(true); DriverStation.startDataLog(DataLogManager.getLog(), true); DriverStation.silenceJoystickConnectionWarning(true); -} + URCL.start(NeoMotorConstants.CAN_ID_MAP); + } /** * This function is called every 20 ms, no matter the mode. Used for items like * diagnostics @@ -66,13 +74,12 @@ public void robotPeriodic() { Robot.previousTimestamp = Robot.currentTimestamp; Robot.currentTimestamp = Timer.getFPGATimestamp(); - } @Override public void disabledInit() { + Robot.gameMode = GameMode.DISABLED; robotContainer.onDisabled(); - FieldConstants.GAME_MODE = GameMode.DISABLED; } @Override @@ -80,23 +87,26 @@ public void disabledPeriodic() { // Now while this may not necessarily be a constant... // it needs to be updated. DriverStation.refreshData(); - FieldConstants.ALLIANCE = DriverStation.getAlliance(); + Robot.alliance = DriverStation.getAlliance(); } @Override public void disabledExit() { robotContainer.onEnabled(); - if (!hasStartedURCL) { - URCL.start(NeoMotorConstants.CAN_ID_MAP); - } } @Override public void autonomousInit() { + // Update "constants" DriveConstants.MAX_SPEED_METERS_PER_SECOND = AutoConstants.MAX_SPEED_METERS_PER_SECOND; + Robot.gameMode = GameMode.AUTONOMOUS; + // We only need to update alliance becuase + // sim GUI starts the bot in a "disconnected" + // state which won't update the alliance before + // we enable... DriverStation.refreshData(); - FieldConstants.ALLIANCE = DriverStation.getAlliance(); - FieldConstants.GAME_MODE = GameMode.AUTONOMOUS; + Robot.alliance = DriverStation.getAlliance(); + autonomousCommand = robotContainer.getAutonomousCommand(); if (autonomousCommand != null) { @@ -118,10 +128,9 @@ public void autonomousExit() { @Override public void teleopInit() { - FieldConstants.GAME_MODE = GameMode.TELEOP; + Robot.gameMode = GameMode.TELEOP; DriveConstants.MAX_SPEED_METERS_PER_SECOND = DriveConstants.MAX_TELEOP_SPEED_METERS_PER_SECOND; robotContainer.onEnabled(); - } @Override @@ -135,7 +144,7 @@ public void teleopExit() { @Override public void testInit() { // Cancels all running commands at the start of test mode. - FieldConstants.GAME_MODE = GameMode.TEST; + Robot.gameMode = GameMode.TEST; CommandScheduler.getInstance().cancelAll(); robotContainer.onTest(); } @@ -157,7 +166,7 @@ public void simulationInit() { @Override public void simulationPeriodic() { REVPhysicsSim.getInstance().run(); - FieldConstants.ALLIANCE = DriverStation.getAlliance(); + Robot.alliance = DriverStation.getAlliance(); for (Neo neo : NeoMotorConstants.MOTOR_LIST) { neo.tick(); @@ -165,10 +174,10 @@ public void simulationPeriodic() { } public static boolean isRedAlliance() { - return FieldConstants.ALLIANCE.equals(Optional.of(Alliance.Red)); + return alliance.equals(Optional.of(Alliance.Red)); } public static boolean isBlueAlliance() { - return FieldConstants.ALLIANCE.equals(Optional.of(Alliance.Blue)); + return alliance.equals(Optional.of(Alliance.Blue)); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69d13672..408e1209 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,14 +5,17 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.event.EventLoop; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot.GameMode; +import frc.robot.commands.AlignmentCalc; import frc.robot.commands.Drive; import frc.robot.commands.PieceControl; import frc.robot.commands.ShooterCalc; @@ -33,7 +36,6 @@ import frc.robot.util.Constants.NTConstants; import frc.robot.util.Constants.NeoMotorConstants; import frc.robot.util.Constants.OIConstants; -import frc.robot.util.Constants.FieldConstants.GameMode; import monologue.Annotations.Log; import frc.robot.util.PIDNotConstants; import frc.robot.util.PIDTunerCommands; @@ -63,6 +65,7 @@ public class RobotContainer implements Logged { private PathPlannerStorage pathPlannerStorage; private CalibrationControl calibrationControl; private PIDTunerCommands PIDTuner; + private AlignmentCalc alignmentCalc; public static HDCTuner HDCTuner; @@ -77,6 +80,9 @@ public class RobotContainer implements Logged { @Log private boolean freshCode = true; + + @Log + public static Field2d field2d = new Field2d(); public RobotContainer() { @@ -109,6 +115,8 @@ public RobotContainer() { swerve.getTurningPidNotConstants() }); + alignmentCalc = new AlignmentCalc(swerve, climb, shooterCalc); + pieceControl = new PieceControl( intake, triggerWheel, @@ -152,28 +160,29 @@ public RobotContainer() { () -> -driver.getRightX(), () -> !driver.getYButton(), () -> (!driver.getYButton() - && Robot.isRedAlliance()))); - - configureButtonBindings(); - initializeArrays(); - + && Robot.isRedAlliance()))); + pathPlannerStorage = new PathPlannerStorage(driver.y().negate()); + initializeArrays(); prepareNamedCommands(); // choreoPathStorage = new ChoreoStorage(driver.y()); // setupChoreoChooser(); pathPlannerStorage.configureAutoChooser(); + + configureButtonBindings(); } - + private void configureButtonBindings() { if (FieldConstants.IS_SIMULATION) { configureSimulationBindings(driver); } configureDriverBindings(driver); - operator.a() - .onTrue(Commands.runOnce(() -> freshCode = true)) - .onFalse(Commands.runOnce(() -> freshCode = false)); - // configureOperatorBindings(operator); + configureOperatorBindings(operator); configureTestBindings(); + + new Trigger(Robot::isRedAlliance) + .onTrue(pathPlannerStorage.updatePathViewerCommand()) + .onFalse(pathPlannerStorage.updatePathViewerCommand()); } private void configureTestBindings() { @@ -185,7 +194,6 @@ private void configureTestBindings() { } private void configureOperatorBindings(PatriBoxController controller) { - controller.povUp() .onTrue(elevator.toTopCommand()); @@ -213,7 +221,8 @@ private void configureDriverBindings(PatriBoxController controller) { controller.back().onTrue( Commands.runOnce(() -> swerve.resetOdometry( new Pose2d( - swerve.getPose().getTranslation(), + 1.31, + 5.53, Rotation2d.fromDegrees( Robot.isRedAlliance() ? 0 @@ -223,6 +232,7 @@ private void configureDriverBindings(PatriBoxController controller) { // Upon hitting start button // reset the orientation of the robot // to be facing TOWARDS the driver station + // TODO: for testing reset odometry to speaker controller.start().onTrue( Commands.runOnce(() -> swerve.resetOdometry( new Pose2d( @@ -241,37 +251,40 @@ private void configureDriverBindings(PatriBoxController controller) { controller.a().whileTrue( Commands.sequence( swerve.resetHDC(), - swerve.setAlignmentSpeed(), - swerve.ampAlignmentCommand(() -> driver.getLeftX()))); + Commands.either( + alignmentCalc.trapAlignmentCommand(() -> controller.getLeftY()), + alignmentCalc.ampAlignmentCommand(() -> controller.getLeftX()), + climb::hooksUp))); controller.rightTrigger() .onTrue(pieceControl.noteToTarget(swerve::getPose, swerve::getRobotRelativeVelocity)); controller.rightStick() - // TODO: AIM AT CHAIN IF HOOKS UP .toggleOnTrue( - Commands.parallel( - Commands.sequence( - swerve.resetHDC(), - swerve.getDriveCommand( - () -> { - return new ChassisSpeeds( - -controller.getLeftY(), - -controller.getLeftX(), - swerve.getAlignmentSpeeds(shooterCalc.calculateSWDRobotAngleToSpeaker(swerve.getPose(), swerve.getFieldRelativeVelocity()))); - }, - () -> true - ) - ), - shooterCalc.prepareSWDCommand(swerve::getPose, swerve::getRobotRelativeVelocity) - ) - ); + Commands.sequence( + swerve.resetHDC(), + Commands.either( + alignmentCalc.sourceRotationalAlignment(controller::getLeftX, controller::getLeftY), + alignmentCalc.wingRotationalAlignment(controller::getLeftX, controller::getLeftY), + alignmentCalc::onOppositeSide))); + + controller.b() + .onTrue(pieceControl.stopAllMotors()); + + controller.x() + .toggleOnTrue(shooterCalc.prepareSWDCommand(swerve::getPose, swerve::getRobotRelativeVelocity)); + + controller.leftBumper() + .onTrue(pieceControl.toggleIn()); + + controller.rightBumper() + .onTrue(pieceControl.toggleOut()); } private void configureSimulationBindings(PatriBoxController controller) { - controller.a().onTrue(shooterCalc.getNoteTrajectoryCommand(swerve::getPose, swerve::getRobotRelativeVelocity)); - controller.a().onFalse(shooterCalc.getNoteTrajectoryCommand(swerve::getPose, swerve::getRobotRelativeVelocity)); + controller.rightTrigger().onTrue(shooterCalc.getNoteTrajectoryCommand(swerve::getPose, swerve::getRobotRelativeVelocity)); + controller.rightTrigger().onFalse(shooterCalc.getNoteTrajectoryCommand(swerve::getPose, swerve::getRobotRelativeVelocity)); controller.rightTrigger() .onTrue(pieceControl.shootWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity)); } @@ -307,7 +320,7 @@ private void configurePIDTunerBindings(PatriBoxController controller) { } private void configureCalibrationBindings(PatriBoxController controller) { - controller.leftBumper(testButtonBindingLoop).onTrue(pieceControl.stopAllMotors().alongWith(shooterCalc.stopAllMotors())); + controller.leftBumper(testButtonBindingLoop).onTrue(pieceControl.stopAllMotors()); controller.rightBumper(testButtonBindingLoop).onTrue(calibrationControl.updateMotorsCommand()); controller.rightTrigger(0.5, testButtonBindingLoop).onTrue(pieceControl.shootWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity)); @@ -383,13 +396,16 @@ private void setupChoreoChooser() { } public void onDisabled() { - swerve.stopMotors(); - pieceControl.stopAllMotors().ignoringDisable(true).schedule();; + swerve.stopDriving(); + pieceControl.stopAllMotors().schedule(); + pathPlannerStorage.updatePathViewerCommand().schedule(); } public void onEnabled() { - if (FieldConstants.GAME_MODE == GameMode.TELEOP) + if (Robot.gameMode == GameMode.TELEOP) { new LPI(ledStrip, swerve::getPose, operator, swerve::setDesiredPose).schedule(); + pathPlannerStorage.updatePathViewerCommand().schedule(); + } this.freshCode = false; } @@ -401,6 +417,7 @@ public void prepareNamedCommands() { // TODO: prepare to shoot while driving (w1 - c1) NamedCommands.registerCommand("Intake", pieceControl.intakeAuto()); NamedCommands.registerCommand("StopIntake", pieceControl.stopIntakeAndIndexer()); + NamedCommands.registerCommand("StopAll", pieceControl.stopAllMotors()); NamedCommands.registerCommand("PrepareShooter", shooterCalc.prepareFireCommandAuto(swerve::getPose)); NamedCommands.registerCommand("Shoot", pieceControl.noteToShoot()); NamedCommands.registerCommand("ShootWhenReady", pieceControl.shootWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity)); @@ -408,6 +425,9 @@ public void prepareNamedCommands() { NamedCommands.registerCommand("PrepareShooterL", shooterCalc.prepareFireCommand(() -> FieldConstants.L_POSE)); NamedCommands.registerCommand("PrepareShooterM", shooterCalc.prepareFireCommand(() -> FieldConstants.M_POSE)); NamedCommands.registerCommand("PrepareShooterR", shooterCalc.prepareFireCommand(() -> FieldConstants.R_POSE)); + NamedCommands.registerCommand("PrepareShooterW3", shooterCalc.prepareFireCommand(() -> FieldConstants.W3_POSE)); + NamedCommands.registerCommand("PrepareShooter", shooterCalc.prepareFireCommand(pathPlannerStorage::getNextShotTranslation)); + NamedCommands.registerCommand("PrepareSWD", shooterCalc.prepareSWDCommand(swerve::getPose, swerve::getRobotRelativeVelocity)); for (int i = 1; i <= FieldConstants.CENTER_NOTE_COUNT; i++) { for (int j = 1; j <= FieldConstants.CENTER_NOTE_COUNT; j++) { if (i == j) { diff --git a/src/main/java/frc/robot/commands/AlignmentCalc.java b/src/main/java/frc/robot/commands/AlignmentCalc.java new file mode 100644 index 00000000..fca0e251 --- /dev/null +++ b/src/main/java/frc/robot/commands/AlignmentCalc.java @@ -0,0 +1,172 @@ +package frc.robot.commands; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Robot; +import frc.robot.subsystems.Climb; +import frc.robot.subsystems.Swerve; +import frc.robot.util.Constants.AutoConstants; +import frc.robot.util.Constants.FieldConstants; +import frc.robot.util.PoseCalculations; + +public class AlignmentCalc { + + private Climb climb; + private Swerve swerve; + private ShooterCalc shooterCalc; + + public AlignmentCalc(Swerve swerve, Climb climb, ShooterCalc shooterCalc) { + + this.climb = climb; + this.swerve = swerve; + this.shooterCalc = shooterCalc; + + } + + public Command getAutoAlignmentCommand(Supplier autoSpeeds, Supplier controllerSpeeds) { + return swerve.getDriveCommand(() -> { + ChassisSpeeds controllerSpeedsGet = controllerSpeeds.get(); + ChassisSpeeds autoSpeedsGet = autoSpeeds.get(); + return new ChassisSpeeds( + (controllerSpeedsGet.vxMetersPerSecond + autoSpeedsGet.vxMetersPerSecond), + -(controllerSpeedsGet.vyMetersPerSecond + autoSpeedsGet.vyMetersPerSecond), + controllerSpeedsGet.omegaRadiansPerSecond + autoSpeedsGet.omegaRadiansPerSecond); + }, () -> false); + } + + public double getAlignmentSpeeds(Rotation2d desiredAngle) { + return MathUtil.applyDeadband(AutoConstants.HDC.getThetaController().calculate( + swerve.getPose().getRotation().getRadians(), + desiredAngle.getRadians()), 0.02); + } + + public ChassisSpeeds getAmpAlignmentSpeeds() { + Pose2d ampPose = FieldConstants.GET_AMP_POSITION(); + Pose2d desiredPose = new Pose2d( + ampPose.getX(), + swerve.getPose().getY(), + ampPose.getRotation() + ); + swerve.setDesiredPose(desiredPose); + return + AutoConstants.HDC.calculate( + swerve.getPose(), + desiredPose, + 0, + desiredPose.getRotation() + ); + } + + public Command ampAlignmentCommand(DoubleSupplier driverX) { + return + getAutoAlignmentCommand( + () -> getAmpAlignmentSpeeds(), + () -> + ChassisSpeeds.fromFieldRelativeSpeeds( + 0, + driverX.getAsDouble() * (Robot.isRedAlliance() ? 1 : -1), + 0, + swerve.getPose().getRotation() + ) + ); + } + + public ChassisSpeeds getChainRotationalSpeeds(double driverX, double driverY) { + Pose2d closestChain = PoseCalculations.getClosestChain(swerve.getPose()); + return new ChassisSpeeds( + driverY * (Robot.isRedAlliance() ? -1 : 1), + driverX * (Robot.isRedAlliance() ? -1 : 1), + getAlignmentSpeeds(closestChain.getRotation()) + ); + } + + public Command chainRotationalAlignment(DoubleSupplier driverX, DoubleSupplier driverY) { + return swerve.getDriveCommand(() -> getChainRotationalSpeeds(driverX.getAsDouble(), driverY.getAsDouble()), () -> true); + } + + public ChassisSpeeds getSourceRotationalSpeeds(double driverX, double driverY) { + Pose2d source = FieldConstants.GET_SOURCE_POSITION(); + return new ChassisSpeeds( + driverY * (Robot.isRedAlliance() ? -1 : 1), + driverX * (Robot.isRedAlliance() ? -1 : 1), + getAlignmentSpeeds(source.getRotation()) + ); + } + + public Command sourceRotationalAlignment(DoubleSupplier driverX, DoubleSupplier driverY) { + return swerve.getDriveCommand(() -> getSourceRotationalSpeeds(driverX.getAsDouble(), driverY.getAsDouble()), () -> true); + } + + public ChassisSpeeds getSpeakerRotationalSpeeds(double driverX, double driverY, ShooterCalc shooterCalc) { + return new ChassisSpeeds( + driverY * (Robot.isRedAlliance() ? -1 : 1), + driverX * (Robot.isRedAlliance() ? -1 : 1), + getAlignmentSpeeds(shooterCalc.calculateSWDRobotAngleToSpeaker(swerve.getPose(), swerve.getFieldRelativeVelocity()))); + } + + public Command speakerRotationalAlignment(DoubleSupplier driverX, DoubleSupplier driverY, ShooterCalc shooterCalc) { + return swerve.getDriveCommand( + () -> + getSpeakerRotationalSpeeds( + driverX.getAsDouble(), + driverY.getAsDouble(), + shooterCalc), + () -> true); + } + + public ChassisSpeeds getTrapAlignmentSpeeds() { + Pose2d closestTrap = PoseCalculations.getClosestChain(swerve.getPose()); + Pose2d stage = FieldConstants.GET_STAGE_POSITION(); + double distance = swerve.getPose().relativeTo(stage).getTranslation().getNorm(); + double x = stage.getX() + distance * closestTrap.getRotation().getCos(); + double y = stage.getY() + distance * closestTrap.getRotation().getSin(); + Pose2d desiredPose = new Pose2d( + x, + y, + closestTrap.getRotation() + ); + swerve.setDesiredPose(desiredPose); + return + AutoConstants.HDC.calculate( + swerve.getPose(), + desiredPose, + 0, + desiredPose.getRotation() + ); + } + + public Command trapAlignmentCommand(DoubleSupplier driverY) { + return + getAutoAlignmentCommand( + () -> getTrapAlignmentSpeeds(), + () -> + ChassisSpeeds.fromFieldRelativeSpeeds( + -driverY.getAsDouble() * swerve.getPose().getRotation().getCos(), + -driverY.getAsDouble() * swerve.getPose().getRotation().getSin(), + 0, + swerve.getPose().getRotation() + ) + ); + } + + public Command wingRotationalAlignment(DoubleSupplier driverX, DoubleSupplier driverY) { + return + Commands.either( + chainRotationalAlignment(driverX, driverY), + speakerRotationalAlignment(driverX, driverY, shooterCalc), + climb::hooksUp); + } + + public boolean onOppositeSide() { + return Robot.isRedAlliance() + ? swerve.getPose().getX() < FieldConstants.CENTERLINE_X + : swerve.getPose().getX() > FieldConstants.CENTERLINE_X; + } +} diff --git a/src/main/java/frc/robot/commands/PieceControl.java b/src/main/java/frc/robot/commands/PieceControl.java index e8fe79df..ab0369e1 100644 --- a/src/main/java/frc/robot/commands/PieceControl.java +++ b/src/main/java/frc/robot/commands/PieceControl.java @@ -1,6 +1,5 @@ package frc.robot.commands; -import java.util.function.BooleanSupplier; import java.util.function.Supplier; import edu.wpi.first.math.geometry.Pose2d; @@ -24,7 +23,7 @@ public class PieceControl { private ShooterCalc shooterCalc; - private boolean shooterMode; + private boolean shooterMode = true; public PieceControl( Intake intake, @@ -44,7 +43,8 @@ public Command stopAllMotors() { intake.stopCommand(), indexer.stopCommand(), elevator.stopCommand(), - trapper.stopCommand()); + trapper.stopCommand(), + shooterCalc.stopAllMotors()).ignoringDisable(true); } public Command shootWhenReady(Supplier poseSupplier, Supplier speedSupplier) { @@ -64,8 +64,7 @@ public Command noteToShoot() { intake.inCommand(), trapper.intake(), indexer.toShooter(), - Commands.waitUntil(intake.possessionTrigger()), - indexCommand()); + Commands.waitSeconds(.75)); } @@ -85,7 +84,7 @@ public Command noteToTrap() { public Command toggleIn() { return Commands.either( - noteToShoot(), + noteToTrap(), stopIntakeAndIndexer(), intake::isStopped ); @@ -105,7 +104,7 @@ public Command ejectNote() { // rotation and speed before sending note from trapper into indexer and then into // shooter before stopping trapper and indexer return Commands.sequence( - intake.stopCommand(), + intake.outCommand(), trapper.outtake(), indexer.toElevator(), Commands.waitSeconds(.75), @@ -130,9 +129,10 @@ public Command dropPieceCommand() { } public Command indexCommand() { - return elevator.indexCommand() - .andThen(elevator.toBottomCommand()) - .alongWith(intake.stopCommand()); + return Commands.none(); + // return elevator.indexCommand() + // .andThen(elevator.toBottomCommand() + // .alongWith(intake.stopCommand())); } public Command intakeAuto() { diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java index 0bdda17b..99c09ed0 100644 --- a/src/main/java/frc/robot/commands/ShooterCalc.java +++ b/src/main/java/frc/robot/commands/ShooterCalc.java @@ -3,23 +3,24 @@ import java.util.function.BooleanSupplier; import java.util.function.Supplier; +import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Robot; +import frc.robot.Robot.GameMode; import frc.robot.subsystems.shooter.*; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.NTConstants; import frc.robot.util.Constants.ShooterConstants; import monologue.Logged; import monologue.Annotations.Log; -import frc.robot.util.Constants; import frc.robot.util.SpeedAngleTriplet; public class ShooterCalc implements Logged { @@ -52,23 +53,34 @@ public ShooterCalc(Shooter shooter, Pivot pivot) { * @param shootAtSpeaker A BooleanSupplier that returns true if the robot should * shoot at the * speaker, and false otherwise. - * @param robotPose The `robotPose` parameter represents the supplier of the current pose + * @param shootingPose The `robotPose` parameter represents the supplier of the current pose * (position and orientation) * of the robot. It is of type `Supplier`. * @return The method is returning a Command object. */ - public Command prepareFireCommand(Supplier robotPose) { - return Commands.runOnce(() -> { - desiredTriplet = calculateTriplet(robotPose.get()); + public Command prepareFireCommand(Supplier shootingPose) { + return Commands.runEnd(() -> { + Translation2d desiredPose = shootingPose.get(); + if (Robot.isRedAlliance() && Robot.gameMode == GameMode.AUTONOMOUS) { + desiredPose = GeometryUtil.flipFieldPosition(desiredPose); + } + desiredTriplet = calculateTriplet(desiredPose); pivot.setAngle(desiredTriplet.getAngle()); shooter.setSpeed(desiredTriplet.getSpeeds()); - }, pivot, shooter); + }, + () -> { + if (Robot.gameMode == GameMode.TELEOP) { + pivot.setAngle(0); + shooter.stop(); + } + }, + pivot, shooter); } public Command prepareFireCommandAuto(Supplier robotPose) { return Commands.run(() -> { - desiredTriplet = calculateTriplet(robotPose.get()); + desiredTriplet = calculateTriplet(robotPose.get().getTranslation()); pivot.setAngle(desiredTriplet.getAngle()); shooter.setSpeed(desiredTriplet.getSpeeds()); @@ -138,12 +150,10 @@ public BooleanSupplier readyToShootSupplier() { // Gets a SpeedAngleTriplet by interpolating values from a map of already // known required speeds and angles for certain poses - public SpeedAngleTriplet calculateTriplet(Pose2d robotPose) { + public SpeedAngleTriplet calculateTriplet(Translation2d robotPose) { // Get our position relative to the desired field element - robotPose = robotPose.relativeTo(FieldConstants.GET_SPEAKER_POSITION()); - // Use the distance as our key for interpolation - double distanceFeet = Units.metersToFeet(robotPose.getTranslation().getNorm()); + double distanceFeet = Units.metersToFeet(robotPose.getDistance(FieldConstants.GET_SPEAKER_TRANSLATION())); this.distance = distanceFeet; @@ -216,7 +226,7 @@ private Translation2d getVelocityVectorToSpeaker(Pose2d robotPose, ChassisSpeeds double velocityNormalToSpeaker = totalSpeed * Math.cos(angleDifference); - return new Translation2d(velocityTangentToSpeaker, -velocityNormalToSpeaker); + return new Translation2d(velocityTangentToSpeaker, velocityNormalToSpeaker); } /** @@ -321,7 +331,7 @@ public Command stopAllMotors() { */ private SpeedAngleTriplet calculateSWDTriplet(Pose2d pose, ChassisSpeeds speeds) { Pose2d currentPose = pose; - SpeedAngleTriplet currentTriplet = calculateTriplet(pose); + SpeedAngleTriplet currentTriplet = calculateTriplet(pose.getTranslation()); double normalVelocity = getVelocityVectorToSpeaker(currentPose, speeds).getY(); double originalv0 = rpmToVelocity(currentTriplet.getSpeeds()); diff --git a/src/main/java/frc/robot/commands/autonomous/PathPlannerStorage.java b/src/main/java/frc/robot/commands/autonomous/PathPlannerStorage.java index 66476968..8fc73ea1 100644 --- a/src/main/java/frc/robot/commands/autonomous/PathPlannerStorage.java +++ b/src/main/java/frc/robot/commands/autonomous/PathPlannerStorage.java @@ -1,18 +1,29 @@ package frc.robot.commands.autonomous; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.util.PatriSendableChooser; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.Robot.GameMode; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.FieldConstants; import monologue.Logged; import monologue.Annotations.Log; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; import java.util.function.BooleanSupplier; +import java.util.function.Consumer; /** * This file represents all of the auto paths that we will have @@ -25,8 +36,11 @@ public class PathPlannerStorage implements Logged { private final BooleanSupplier hasPieceSupplier; @Log.NT - private SendableChooser autoChooser = new SendableChooser<>(); + private PatriSendableChooser autoChooser = new PatriSendableChooser<>(); + public static final ArrayList AUTO_STARTING_POSITIONS = new ArrayList(); + + public static final HashMap> AUTO_PATHS = new HashMap>(); /** * Creates a new AutoPathStorage object. * @param hasPieceSupplier A supplier that returns whether or not the robot has a piece. @@ -40,7 +54,7 @@ public void configureAutoChooser() { /** * Warning * - * This method will load all autos in the deploy directory. Since the deploy + * AutoBuilder::buildAutoChooser will load all autos in the deploy directory. Since the deploy * process does not automatically clear the deploy directory, old auto files * that have since been deleted from the project could remain on the RIO, * therefore being added to the auto chooser. @@ -48,18 +62,58 @@ public void configureAutoChooser() { * To remove old options, the deploy directory will need to be cleared manually * via SSH, WinSCP, reimaging the RIO, etc. */ - + // Good news! // This auto caches our paths so we don't need to manually load them + for (String autoName : AutoConstants.AUTO_NAMES) { - autoChooser.addOption(autoName, AutoBuilder.buildAuto(autoName)); + // Load the auto and add it to the auto chooser + Command auto = AutoBuilder.buildAuto(autoName); + autoChooser.addOption(autoName, auto); + // Load the auto and add it to the list of starting positions + // for LPI + Pose2d startingPosition = PathPlannerAuto.getStaringPoseFromAutoFile(autoName); + PathPlannerStorage.AUTO_STARTING_POSITIONS.add(startingPosition); + // Load the auto and add it to the list of paths + // for trajectory visualization + List paths = PathPlannerAuto.getPathGroupFromAutoFile(autoName); + PathPlannerStorage.AUTO_PATHS.put(autoName, paths); } + + bindListener(getUpdatePathViewerCommand()); } public Command getSelectedAuto() { return autoChooser.getSelected(); } + public String getSelectedAutoName() { + return autoChooser.getSelectedName(); + } + + public void bindListener(Consumer consumer) { + autoChooser.onChange(consumer); + } + + public Command updatePathViewerCommand() { + return Commands.either( + Commands.runOnce(() -> { + RobotContainer.field2d.getObject("path").setPoses(new ArrayList<>()); + }), + Commands.runOnce(() -> { + RobotContainer.field2d.getObject("path") + .setPoses(getAutoPoses(getSelectedAutoName())); + }), + () -> Robot.gameMode == GameMode.TELEOP + ).ignoringDisable(true); + } + + private Consumer getUpdatePathViewerCommand() { + return (command) -> { + updatePathViewerCommand().schedule(); + }; + } + /** * Given a starting note and an ending note, this method will generate a command * that uses the booleanSupplier hasPieceSupplier to determine whether or not the @@ -114,4 +168,58 @@ public Command generateCenterLogic(int startingNote, int endingNote) { return commandGroup; } + + public Pose2d getPathEndPose(PathPlannerPath path) { + List poses = path.getPathPoses(); + Pose2d lastPose = poses.get(poses.size()-1); + return lastPose; + } + + public List getAutoPoses(String name) { + List paths = PathPlannerStorage.AUTO_PATHS.get(name); + List autoPoses = new ArrayList<>(); + if (paths == null) return autoPoses; + // Flip for red alliance + // and add all the poses to the list + for (PathPlannerPath path : paths) { + List pathPoses = + Robot.isRedAlliance() + ? path.flipPath().getPathPoses() + : path.getPathPoses(); + autoPoses.addAll(pathPoses); + } + + // We can only have up to 85 poses in the list, + // so we need to compress it if it's too large + compressList(autoPoses, 84); + + return autoPoses; + } + + private void compressList(List poses, int maxSize) { + int index = 0; + while (poses.size() > maxSize && index < poses.size()) { + if (index % 2 != 0) { + poses.remove(index); + } else { + index++; + } + } + } + + @Log + Translation2d activePathEndPose = new Translation2d(); + public Translation2d getNextShotTranslation() { + double[] activeTraj = NetworkTableInstance.getDefault().getTable("PathPlanner").getEntry("activePath").getDoubleArray(new double[0]); + + if (activeTraj.length == 0) return new Translation2d(0, 0); + + int activeTrajLength = activeTraj.length; + double endX = activeTraj[activeTrajLength - 3]; + double endY = activeTraj[activeTrajLength - 2]; + Translation2d endPose = new Translation2d(endX, endY); + + this.activePathEndPose = endPose; + return endPose; + } } diff --git a/src/main/java/frc/robot/commands/leds/LPI.java b/src/main/java/frc/robot/commands/leds/LPI.java index 6e689451..2ecc2b1c 100644 --- a/src/main/java/frc/robot/commands/leds/LPI.java +++ b/src/main/java/frc/robot/commands/leds/LPI.java @@ -16,12 +16,11 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; +import frc.robot.Robot.GameMode; +import frc.robot.commands.autonomous.PathPlannerStorage; import frc.robot.subsystems.LedStrip; import frc.robot.util.PatriBoxController; -import frc.robot.util.Constants.AutoConstants; -import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.LEDConstants; -import frc.robot.util.Constants.FieldConstants.GameMode; import monologue.Logged; import java.util.function.Consumer; import monologue.Annotations.Log; @@ -67,9 +66,9 @@ public void execute() { double closestDistance = 9999; - for (int i = 0; i < AutoConstants.AUTO_STARTING_POSITIONS.size(); i++) { + for (int i = 0; i < PathPlannerStorage.AUTO_STARTING_POSITIONS.size(); i++) { - Pose2d startingPosition = AutoConstants.AUTO_STARTING_POSITIONS.get(i); + Pose2d startingPosition = PathPlannerStorage.AUTO_STARTING_POSITIONS.get(i); if (Robot.isRedAlliance()) { startingPosition = GeometryUtil.flipFieldPose(startingPosition); } @@ -153,6 +152,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return FieldConstants.GAME_MODE.equals(GameMode.DISABLED); + return Robot.gameMode.equals(GameMode.DISABLED); } } diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java index b1c720ee..caa6a945 100644 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ b/src/main/java/frc/robot/subsystems/Climb.java @@ -15,8 +15,8 @@ import frc.robot.util.PIDNotConstants; import frc.robot.util.PoseCalculations; import frc.robot.util.Constants.ClimbConstants; +import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.NTConstants; -import frc.robot.util.Constants.ShooterConstants; import frc.robot.util.Neo.TelemetryPreference; import monologue.Logged; import monologue.Annotations.Log; @@ -28,14 +28,15 @@ public class Climb extends SubsystemBase implements Logged { private final PIDNotConstants climbPID; @Log - public double posLeft = 0, posRight = 0, targetPosRight = 0, targetPosLeft = 0; + private double posLeft = 0, posRight = 0, targetPosRight = 0, targetPosLeft = 0; @Log - public boolean atDesiredPos = false; + private boolean atDesiredPos = false, hooksUp = false; public Climb() { leftMotor = new Neo(ClimbConstants.LEFT_CLIMB_CAN_ID, false); - rightMotor = new Neo(ClimbConstants.RIGHT_CLIMB_CAN_ID, true); + // invert right motor in real life, not in sim + rightMotor = new Neo(ClimbConstants.RIGHT_CLIMB_CAN_ID, !FieldConstants.IS_SIMULATION); configureMotors(); climbPID = new PIDNotConstants(leftMotor.getPID(), leftMotor.getPIDController()); @@ -67,6 +68,7 @@ public void periodic() { posRight = rightMotor.getPosition(); atDesiredPos = atDesiredPosition().getAsBoolean(); + hooksUp = hooksUp(); RobotContainer.components3d[NTConstants.LEFT_CLIMB_INDEX] = new Pose3d( 0, 0, leftMotor.getPosition(), @@ -148,4 +150,8 @@ public BooleanSupplier atDesiredPosition() { rightMotor.getPosition() - rightMotor.getTargetPosition()), ClimbConstants.CLIMB_DEADBAND) == 0); } + + public boolean hooksUp() { + return (leftMotor.getTargetPosition() > 0 || rightMotor.getTargetPosition() > 0); + } } diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index d9e9f99a..9dba29f8 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -1,14 +1,11 @@ package frc.robot.subsystems; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.Constants.IntakeConstants; import frc.robot.util.Neo.TelemetryPreference; import frc.robot.util.Neo; -import frc.robot.util.Neo.TelemetryPreference; public class Indexer extends SubsystemBase { private final Neo triggerWheel; @@ -31,9 +28,9 @@ public double getDesiredPercent() { public void setDesiredPercent(double percent) { desiredPercent = MathUtil.clamp( - percent, - IntakeConstants.INDEXER_PERCENT_LOWER_LIMIT, - IntakeConstants.INDEXER_PERCENT_UPPER_LIMIT); + percent, + IntakeConstants.INDEXER_PERCENT_LOWER_LIMIT, + IntakeConstants.INDEXER_PERCENT_UPPER_LIMIT); triggerWheel.set(desiredPercent); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index de533591..1ad42d57 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,10 +2,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.commands.PieceControl; +import frc.robot.Robot; import frc.robot.util.Neo; import frc.robot.util.Constants.IntakeConstants; import frc.robot.util.Neo.TelemetryPreference; @@ -17,6 +16,7 @@ public class Intake extends SubsystemBase implements Logged { @Log private double desiredSpeed = 0; + private double startedIntakingTimestamp = 0; @Log private boolean notePossession = true; @@ -25,23 +25,38 @@ public Intake() { configMotors(); } + @Override + public void periodic() { + if (desiredSpeed == IntakeConstants.INTAKE_PERCENT + && Robot.currentTimestamp - startedIntakingTimestamp > 0.1 + && intakeMotor.getAppliedOutput() < ((Math.abs(IntakeConstants.INTAKE_PERCENT) - 0.2) * Math.signum(IntakeConstants.INTAKE_PERCENT))) + { + notePossession = true; + } + } + public void configMotors() { intakeMotor.setSmartCurrentLimit(IntakeConstants.INTAKE_CURRENT_LIMIT_AMPS); // See https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces intakeMotor.setTelemetryPreference(TelemetryPreference.NO_ENCODER); - } - + } + public void setPercent(double desiredPercent) { desiredSpeed = MathUtil.clamp( desiredPercent, IntakeConstants.INTAKE_PERCENT_LOWER_LIMIT, IntakeConstants.INTAKE_PERCENT_UPPER_LIMIT); - intakeMotor.setTargetPercent(desiredSpeed); + intakeMotor.set(desiredSpeed); } public Command setPercentCommand(double desiredPercent) { - return runOnce(() -> setPercent(desiredPercent)); + return runOnce(() -> { + setPercent(desiredPercent); + if(desiredPercent > 0) { + startedIntakingTimestamp = Robot.currentTimestamp; + } + }); } public Command inCommand() { @@ -53,7 +68,7 @@ public Command outCommand() { } public boolean isStopped() { - return desiredSpeed != 0; + return desiredSpeed == 0; } public Command stopCommand() { @@ -71,5 +86,4 @@ public void setPossession(boolean possession) { public Trigger possessionTrigger() { return new Trigger(this::getPossession); } - } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 49c9b9d9..4950bb33 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -12,11 +12,9 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.LimelightHelpers; -import frc.robot.util.Constants; import frc.robot.util.Constants.CameraConstants; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.LimelightHelpers.LimelightTarget_Fiducial; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4b173954..e0975e1a 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -4,13 +4,10 @@ package frc.robot.subsystems; -import java.lang.reflect.Field; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.ejml.sparse.csc.factory.FillReductionFactory_DSCC; - import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; @@ -28,17 +25,18 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.RobotContainer; import frc.robot.commands.Drive; import frc.robot.commands.DriveHDC; +import frc.robot.commands.ShooterCalc; import frc.robot.util.MAXSwerveModule; import frc.robot.util.PIDNotConstants; -import frc.robot.util.PatriBoxController; +import frc.robot.util.PoseCalculations; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.DriveConstants; import frc.robot.util.Constants.FieldConstants; @@ -97,9 +95,6 @@ public class Swerve extends SubsystemBase implements Logged { @Log Pose2d robotPose2d = new Pose2d(); - @Log.NT - Field2d field2d = new Field2d(); - // The gyro sensor private final Pigeon2 gyro = new Pigeon2(DriveConstants.PIGEON_CAN_ID); @@ -180,10 +175,19 @@ public void logPositions() { speeds.omegaRadiansPerSecond * .02))); } - field2d.setRobotPose(currentPose); + RobotContainer.field2d.setRobotPose(currentPose); SmartDashboard.putNumber("Swerve/RobotRotation", currentPose.getRotation().getRadians()); - robotPose2d = currentPose; + if (! (Double.isNaN(currentPose.getX()) + || Double.isNaN(currentPose.getY()) + || Double.isNaN(currentPose.getRotation().getDegrees()))) + { + robotPose2d = currentPose; + } else { + // Something in our pose was NaN... + resetOdometry(robotPose2d); + resetHDC(); + } robotPose3d = new Pose3d( new Translation3d( @@ -236,7 +240,7 @@ public void drive(double xSpeed, double ySpeed, double rotSpeed, boolean fieldRe setModuleStates(swerveModuleStates); } - public void stopMotors() { + public void stopDriving() { drive(0, 0, 0, false); } @@ -258,11 +262,12 @@ public ChassisSpeeds getFieldRelativeVelocity() { * Sets the wheels into an X formation to prevent movement. */ public void setWheelsX() { - SwerveModuleState[] desiredStates = new SwerveModuleState[4]; - desiredStates[0] = new SwerveModuleState(0, Rotation2d.fromDegrees(-45)); - desiredStates[1] = new SwerveModuleState(0, Rotation2d.fromDegrees(45)); - desiredStates[2] = new SwerveModuleState(0, Rotation2d.fromDegrees(45)); - desiredStates[3] = new SwerveModuleState(0, Rotation2d.fromDegrees(-45)); + SwerveModuleState[] desiredStates = new SwerveModuleState[] { + new SwerveModuleState(0, Rotation2d.fromDegrees(45)), + new SwerveModuleState(0, Rotation2d.fromDegrees(-45)), + new SwerveModuleState(0, Rotation2d.fromDegrees(-45)), + new SwerveModuleState(0, Rotation2d.fromDegrees(45)) + }; setModuleStates(desiredStates); } @@ -382,17 +387,6 @@ public void setCoastMode() { } } - public Command getAutoAlignmentCommand(Supplier autoSpeeds, Supplier controllerSpeeds) { - return new Drive(this, () -> { - ChassisSpeeds controllerSpeedsGet = controllerSpeeds.get(); - ChassisSpeeds autoSpeedsGet = autoSpeeds.get(); - return new ChassisSpeeds( - (controllerSpeedsGet.vxMetersPerSecond + autoSpeedsGet.vxMetersPerSecond), - -(controllerSpeedsGet.vyMetersPerSecond + autoSpeedsGet.vyMetersPerSecond), - controllerSpeedsGet.omegaRadiansPerSecond + autoSpeedsGet.omegaRadiansPerSecond); - }, () -> false, () -> false); - } - public Command getDriveCommand(Supplier speeds, BooleanSupplier fieldRelative) { return new Drive(this, speeds, fieldRelative, () -> false); } @@ -401,47 +395,12 @@ public DriveHDC getDriveHDCCommand(Supplier speeds, BooleanSuppli return new DriveHDC(this, speeds, fieldRelative, () -> false); } - public double getAlignmentSpeeds(Rotation2d desiredAngle) { - return MathUtil.applyDeadband(AutoConstants.HDC.getThetaController().calculate( - getPose().getRotation().getRadians(), - desiredAngle.getRadians()), 0.02); - } - - public Command ampAlignmentCommand(DoubleSupplier driverX) { - - return - getAutoAlignmentCommand( - () -> { - Pose2d ampPose = FieldConstants.GET_AMP_POSITION(); - Pose2d desiredPose = new Pose2d( - ampPose.getX(), - getPose().getY(), - ampPose.getRotation() - ); - setDesiredPose(desiredPose); - return - AutoConstants.HDC.calculate( - getPose(), - desiredPose, - 0, - desiredPose.getRotation() - ); - }, - () -> - ChassisSpeeds.fromFieldRelativeSpeeds( - 0, - driverX.getAsDouble() * (Robot.isRedAlliance() ? 1 : -1), - 0, - getPose().getRotation() - ) - ); - } - public Command resetHDC() { return Commands.sequence( - runOnce(() -> AutoConstants.HDC.getThetaController().reset(getPose().getRotation().getRadians())), - runOnce(() -> AutoConstants.HDC.getXController().reset()), - runOnce(() -> AutoConstants.HDC.getYController().reset()) + Commands.runOnce(() -> AutoConstants.HDC.getThetaController().reset(getPose().getRotation().getRadians())), + Commands.runOnce(() -> AutoConstants.HDC.getXController().reset()), + Commands.runOnce(() -> AutoConstants.HDC.getYController().reset()) ); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 0076f32e..7e7d0ab5 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.elevator; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -21,10 +19,10 @@ public class Elevator extends SubsystemBase implements Logged { private final Neo elevator; private final PIDNotConstants elevatorPID; @Log - public double pos = 0, desiredPos = 0; + private double pos = 0, desiredPos = 0; @Log - public boolean atDesiredPos = false; + private boolean atDesiredPos = false; /** Creates a new Elevator. */ public Elevator() { diff --git a/src/main/java/frc/robot/subsystems/elevator/Trapper.java b/src/main/java/frc/robot/subsystems/elevator/Trapper.java index 2b7c2e90..bb946aa9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Trapper.java +++ b/src/main/java/frc/robot/subsystems/elevator/Trapper.java @@ -82,11 +82,11 @@ public boolean getHasGamePiece() { } public Command outtake() { - return runOnce(() -> trapper.set(TrapConstants.TRAPPER_OUTTAKE_PERCENT)); + return runOnce(() -> setSpeed(TrapConstants.TRAPPER_OUTTAKE_PERCENT)); } public Command stopCommand() { - return runOnce(() -> trapper.set(TrapConstants.TRAPPER_STOP_PERCENT)); + return runOnce(() -> setSpeed(TrapConstants.TRAPPER_STOP_PERCENT)); } public void updateIntakingTimestamp() { @@ -95,7 +95,7 @@ public void updateIntakingTimestamp() { public Command intake() { return runOnce(() -> updateIntakingTimestamp()) - .andThen(runOnce(() -> trapper.set(TrapConstants.TRAPPER_INTAKE_PERCENT))); + .andThen(runOnce(() -> setSpeed(TrapConstants.TRAPPER_INTAKE_PERCENT))); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Pivot.java b/src/main/java/frc/robot/subsystems/shooter/Pivot.java index e57962a7..10a071b9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Pivot.java +++ b/src/main/java/frc/robot/subsystems/shooter/Pivot.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.shooter; -import java.util.function.BooleanSupplier; - import com.revrobotics.AbsoluteEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkAbsoluteEncoder.Type; @@ -29,10 +27,10 @@ public class Pivot extends SubsystemBase implements Logged { private SparkPIDController pivotPIDController; @Log - public double realAngle = 0, desiredAngle = 0; + private double realAngle = 0, desiredAngle = 0; @Log - public boolean atDesiredAngle = false; + private boolean atDesiredAngle = false; public Pivot() { this.pivot = new Neo(ShooterConstants.SHOOTER_PIVOT_CAN_ID); diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 4e46c3ec..f1dfb679 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -1,15 +1,14 @@ package frc.robot.util; import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.Map; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.util.GeometryUtil; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; -import java.util.Optional; import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; @@ -27,9 +26,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Robot; -import frc.robot.commands.Drive; import monologue.Logged; /** @@ -138,7 +135,7 @@ public static final class ShooterConstants { public static final double SHOOTER_BACK_SPEED = -0.5; public static final double PIVOT_DEADBAND = 1; - public static final double SHOOTER_RPM_DEADBAND = 50; + public static final double SHOOTER_RPM_DEADBAND = 150; // These are in % public static final double SHOOTER_MIN_OUTPUT = -1; @@ -226,8 +223,8 @@ public static final class TrapConstants { public static final double ELEVATOR_TOP_LIMIT = 0.48; public static final double ELEVATOR_BOTTOM_LIMIT = 0; - public static final double TRAPPER_LOWER_PERCENT_LIMIT = 1; - public static final double TRAPPER_UPPER_PERCENT_LIMIT = -1; + public static final double TRAPPER_LOWER_PERCENT_LIMIT = -1; + public static final double TRAPPER_UPPER_PERCENT_LIMIT = 1; public static final double TRAPPER_HAS_PIECE_UPPER_LIMIT = 0; public static final double TRAPPER_HAS_PIECE_LOWER_LIMIT = -0.25; @@ -267,7 +264,7 @@ public static final class AutoConstants { // 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 = 3; - public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3; + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2.5; public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = Math.PI/4.0; public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = Math.PI; @@ -318,13 +315,13 @@ public static final class AutoConstants { public static final HolonomicPathFollowerConfig HPFC = new HolonomicPathFollowerConfig( new PIDConstants( - AutoConstants.XY_CORRECTION_P, + AutoConstants.XY_CORRECTION_P*3, 0, - AutoConstants.XY_CORRECTION_D), + AutoConstants.XY_CORRECTION_D*2), new PIDConstants( - AutoConstants.ROTATION_CORRECTION_P, + AutoConstants.ROTATION_CORRECTION_P*3, 0, - AutoConstants.ROTATION_CORRECTION_D), + AutoConstants.ROTATION_CORRECTION_D*2), MAX_SPEED_METERS_PER_SECOND, Math.hypot(DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH)/2.0, new ReplanningConfig()); @@ -351,15 +348,6 @@ public static final class AutoConstants { "S W3-1 S", "S W3-1 S C1-3 S" }; - - public static final ArrayList AUTO_STARTING_POSITIONS = new ArrayList() { - { - for (int i = 0; i < AUTO_NAMES.length; i++) { - Pose2d startingPosition = PathPlannerAuto.getStaringPoseFromAutoFile(AUTO_NAMES[i]); - add(startingPosition); - } - } - }; } public static final class ModuleConstants { @@ -497,23 +485,23 @@ public static final class NeoMotorConstants { public static ArrayList MOTOR_LIST = new ArrayList<>(); public static final HashMap CAN_ID_MAP = new HashMap() {{ - /* 1 */ put(DriveConstants.FRONT_RIGHT_DRIVING_CAN_ID, "Front Right Drive"); - /* 2 */ put(DriveConstants.FRONT_RIGHT_TURNING_CAN_ID, "Front Right Turn"); - /* 3 */ put(DriveConstants.FRONT_LEFT_DRIVING_CAN_ID, "Front Left Drive"); - /* 4 */ put(DriveConstants.FRONT_LEFT_TURNING_CAN_ID, "Front Left Turn"); - /* 5 */ put(DriveConstants.REAR_LEFT_DRIVING_CAN_ID, "Rear Left Drive"); - /* 6 */ put(DriveConstants.REAR_LEFT_TURNING_CAN_ID, "Rear Left Turn"); - /* 7 */ put(DriveConstants.REAR_RIGHT_DRIVING_CAN_ID, "Rear Right Drive"); - /* 8 */ put(DriveConstants.REAR_RIGHT_TURNING_CAN_ID, "Rear Right Turn"); + /* 1 */ put(DriveConstants.FRONT_RIGHT_DRIVING_CAN_ID, "FrontRightDrive"); + /* 2 */ put(DriveConstants.FRONT_RIGHT_TURNING_CAN_ID, "FrontRightTurn"); + /* 3 */ put(DriveConstants.FRONT_LEFT_DRIVING_CAN_ID, "FrontLeftDrive"); + /* 4 */ put(DriveConstants.FRONT_LEFT_TURNING_CAN_ID, "FrontLeftTurn"); + /* 5 */ put(DriveConstants.REAR_LEFT_DRIVING_CAN_ID, "RearLeftDrive"); + /* 6 */ put(DriveConstants.REAR_LEFT_TURNING_CAN_ID, "RearLeftTurn"); + /* 7 */ put(DriveConstants.REAR_RIGHT_DRIVING_CAN_ID, "RearRightDrive"); + /* 8 */ put(DriveConstants.REAR_RIGHT_TURNING_CAN_ID, "RearRightTurn"); /* 9 */ put(IntakeConstants.INTAKE_CAN_ID, "Intake"); - /* 10 */ put(IntakeConstants.TRIGGER_WHEEL_CAN_ID, "Trigger Wheel"); - /* 11 */ put(ShooterConstants.LEFT_SHOOTER_CAN_ID, "Left Shooter"); - /* 12 */ put(ShooterConstants.RIGHT_SHOOTER_CAN_ID, "Right Shooter"); - /* 13 */ put(ShooterConstants.SHOOTER_PIVOT_CAN_ID, "Shooter Pivot"); + /* 10 */ put(IntakeConstants.TRIGGER_WHEEL_CAN_ID, "TriggerWheel"); + /* 11 */ put(ShooterConstants.LEFT_SHOOTER_CAN_ID, "LeftShooter"); + /* 12 */ put(ShooterConstants.RIGHT_SHOOTER_CAN_ID, "RightShooter"); + /* 13 */ put(ShooterConstants.SHOOTER_PIVOT_CAN_ID, "ShooterPivot"); /* 14 */ put(TrapConstants.ELEVATOR_CAN_ID, "Elevator"); /* 15 */ put(TrapConstants.TRAP_CAN_ID, "Trap"); - /* 16 */ put(ClimbConstants.LEFT_CLIMB_CAN_ID, "Left Climb"); - /* 17 */ put(ClimbConstants.RIGHT_CLIMB_CAN_ID, "Right Climb"); + /* 16 */ put(ClimbConstants.LEFT_CLIMB_CAN_ID, "LeftClimb"); + /* 17 */ put(ClimbConstants.RIGHT_CLIMB_CAN_ID, "RightClimb"); }}; } @@ -556,17 +544,6 @@ public static final class FieldConstants { public static final double CHAIN_HEIGHT_METERS = Units.feetToMeters(4); public static final double SPEAKER_HEIGHT_METERS = 2.082813; - public static Optional ALLIANCE = Optional.empty(); - - public static enum GameMode { - DISABLED, - AUTONOMOUS, - TELEOP, - TEST - }; - - public static GameMode GAME_MODE; - // Field: // https://cad.onshape.com/documents/dcbe49ce579f6342435bc298/w/b93673f5b2ec9c9bdcfec487/e/6ecb2d6b7590f4d1c820d5e3 // Chain Positions: Blue alliance left @@ -579,13 +556,23 @@ public static enum GameMode { // All points are in meters and radians // All relative to the blue origin // Blue Stage - new Pose2d(4.37, 3.201, Rotation2d.fromDegrees(60)), - new Pose2d(5.875, 4.168, Rotation2d.fromDegrees(180)), - new Pose2d(4.353, 4.938, Rotation2d.fromDegrees(-60)), + new Pose2d(4.37, 3.201, Rotation2d.fromDegrees(-120)), + new Pose2d(5.875, 4.168, Rotation2d.fromDegrees(0)), + new Pose2d(4.353, 4.938, Rotation2d.fromDegrees(120)), // Red Stage - new Pose2d(12.07, 3.237, Rotation2d.fromDegrees(120)), - new Pose2d(10.638, 4.204, Rotation2d.fromDegrees(0)), - new Pose2d(12.2, 5, Rotation2d.fromDegrees(-120)) + new Pose2d(12.07, 3.237, Rotation2d.fromDegrees(-60)), + new Pose2d(10.638, 4.204, Rotation2d.fromDegrees(180)), + new Pose2d(12.2, 5, Rotation2d.fromDegrees(60)) + }; + + public static final Pose2d[] STAGE_POSITIONS = new Pose2d[] { + new Pose2d(4.897, 4.064, new Rotation2d()), + new Pose2d(11.655, 4.064, new Rotation2d()) + }; + + public static final Pose2d[] SOURCE_POSITIONS = new Pose2d[] { + new Pose2d(15.452, 0.971, Rotation2d.fromDegrees(120)), + new Pose2d(1.079, 0.971, Rotation2d.fromDegrees(60)) }; public static final Pose3d[] CHAIN_POSE3DS = new Pose3d[] { @@ -626,16 +613,28 @@ public static enum GameMode { public static Pose2d GET_SPEAKER_POSITION() { return SPEAKER_POSITIONS[Robot.isRedAlliance() ? 1 : 0]; - } + } + + public static Pose2d GET_SOURCE_POSITION() { + return SOURCE_POSITIONS[Robot.isRedAlliance() ? 1 : 0]; + } + + public static Translation2d GET_SPEAKER_TRANSLATION() { + return SPEAKER_POSITIONS[Robot.isRedAlliance() ? 1 : 0].getTranslation(); + } public static Pose2d GET_AMP_POSITION() { return AMP_POSITIONS[Robot.isRedAlliance() ? 1 : 0]; } - // TODO: make real constants - public static final Pose2d L_POSE = new Pose2d(); - public static final Pose2d R_POSE = new Pose2d(); - public static final Pose2d M_POSE = new Pose2d(); + public static Pose2d GET_STAGE_POSITION() { + return STAGE_POSITIONS[Robot.isRedAlliance() ? 1 : 0]; + } + + public static Pose2d[] GET_CHAIN_POSITIONS() { + int startIndex = Robot.isRedAlliance() ? 3 : 0; + return Arrays.copyOfRange(CHAIN_POSITIONS, startIndex, startIndex + 3); + } public static final double CHAIN_LENGTH_METERS = Units.inchesToMeters(100); @@ -668,6 +667,11 @@ public static Pose2d GET_AMP_POSITION() { System.arraycopy(SPIKE_TRANSLATIONS_RED, 0, NOTE_TRANSLATIONS, SPIKE_TRANSLATIONS_BLUE.length, SPIKE_TRANSLATIONS_RED.length); System.arraycopy(CENTERLINE_TRANSLATIONS, 0, NOTE_TRANSLATIONS, SPIKE_TRANSLATIONS_BLUE.length + SPIKE_TRANSLATIONS_RED.length, CENTERLINE_TRANSLATIONS.length); } + + public static final Translation2d L_POSE = new Translation2d(5.11,6.23); + public static final Translation2d R_POSE = GET_SPEAKER_TRANSLATION(); + public static final Translation2d M_POSE = new Translation2d(4.46,4.81); + public static final Translation2d W3_POSE = SPIKE_TRANSLATIONS_BLUE[0].toTranslation2d(); } public static final class CameraConstants { diff --git a/src/main/java/frc/robot/util/HDCTuner.java b/src/main/java/frc/robot/util/HDCTuner.java index 595c4cae..096c75b8 100644 --- a/src/main/java/frc/robot/util/HDCTuner.java +++ b/src/main/java/frc/robot/util/HDCTuner.java @@ -3,7 +3,6 @@ import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/frc/robot/util/PatriBoxController.java b/src/main/java/frc/robot/util/PatriBoxController.java index c0db448e..4838367a 100644 --- a/src/main/java/frc/robot/util/PatriBoxController.java +++ b/src/main/java/frc/robot/util/PatriBoxController.java @@ -1,6 +1,5 @@ package frc.robot.util; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/util/PatriSendableChooser.java b/src/main/java/frc/robot/util/PatriSendableChooser.java new file mode 100644 index 00000000..b6e31a9d --- /dev/null +++ b/src/main/java/frc/robot/util/PatriSendableChooser.java @@ -0,0 +1,198 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.util; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import java.util.LinkedHashMap; +import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; + +/** + * The {@link PatriSendableChooser} class is a useful tool for presenting a + * selection of options to the + * {@link SmartDashboard}. + * + *

+ * For instance, you may wish to be able to select between multiple autonomous + * modes. You can do + * this by putting every possible Command you want to run as an autonomous into + * a {@link + * PatriSendableChooser} and then put it into the {@link SmartDashboard} to have + * a list of options appear + * on the laptop. Once autonomous starts, simply ask the + * {@link PatriSendableChooser} what the selected + * value is. + * + * @param The type of the values to be stored + */ +public class PatriSendableChooser implements Sendable, AutoCloseable { + /** The key for the default value. */ + private static final String DEFAULT = "default"; + + /** The key for the selected option. */ + private static final String SELECTED = "selected"; + + /** The key for the active option. */ + private static final String ACTIVE = "active"; + + /** The key for the option array. */ + private static final String OPTIONS = "options"; + + /** The key for the instance number. */ + private static final String INSTANCE = ".instance"; + + /** A map linking strings to the objects they represent. */ + public final Map m_map = new LinkedHashMap<>(); + + private String m_defaultChoice = ""; + private final int m_instance; + private String m_previousVal; + private Consumer m_listener; + private static final AtomicInteger s_instances = new AtomicInteger(); + + /** Instantiates a {@link PatriSendableChooser}. */ + @SuppressWarnings("this-escape") + public PatriSendableChooser() { + m_instance = s_instances.getAndIncrement(); + SendableRegistry.add(this, "SendableChooser", m_instance); + } + + @Override + public void close() { + SendableRegistry.remove(this); + } + + /** + * Adds the given object to the list of options. On the {@link SmartDashboard} + * on the desktop, the + * object will appear as the given name. + * + * @param name the name of the option + * @param object the option + */ + public void addOption(String name, V object) { + m_map.put(name, object); + } + + /** + * Adds the given object to the list of options and marks it as the default. + * Functionally, this is + * very close to {@link #addOption(String, Object)} except that it will use this + * as the default + * option if none other is explicitly selected. + * + * @param name the name of the option + * @param object the option + */ + public void setDefaultOption(String name, V object) { + requireNonNullParam(name, "name", "setDefaultOption"); + + m_defaultChoice = name; + addOption(name, object); + } + + /** + * Returns the selected option. If there is none selected, it will return the + * default. If there is + * none selected and no default, then it will return {@code null}. + * + * @return the option selected + */ + public V getSelected() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_map.get(m_selected); + } else { + return m_map.get(m_defaultChoice); + } + } finally { + m_mutex.unlock(); + } + } + + public String getSelectedName() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + } + + /** + * Bind a listener that's called when the selected value changes. Only one + * listener can be bound. + * Calling this function will replace the previous listener. + * + * @param listener The function to call that accepts the new value + */ + public void onChange(Consumer listener) { + requireNonNullParam(listener, "listener", "onChange"); + m_mutex.lock(); + m_listener = listener; + m_mutex.unlock(); + } + + private String m_selected; + private final ReentrantLock m_mutex = new ReentrantLock(); + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("String Chooser"); + builder.publishConstInteger(INSTANCE, m_instance); + builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); + builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); + builder.addStringProperty( + ACTIVE, + () -> { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + }, + null); + builder.addStringProperty( + SELECTED, + null, + val -> { + V choice; + Consumer listener; + m_mutex.lock(); + try { + m_selected = val; + if (!m_selected.equals(m_previousVal) && m_listener != null) { + choice = m_map.get(val); + listener = m_listener; + } else { + choice = null; + listener = null; + } + m_previousVal = val; + } finally { + m_mutex.unlock(); + } + if (listener != null) { + listener.accept(choice); + } + }); + } +} diff --git a/src/main/java/frc/robot/util/PoseCalculations.java b/src/main/java/frc/robot/util/PoseCalculations.java index 99dbc20a..0ada2ed5 100644 --- a/src/main/java/frc/robot/util/PoseCalculations.java +++ b/src/main/java/frc/robot/util/PoseCalculations.java @@ -3,7 +3,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; -import frc.robot.Robot; import frc.robot.util.Constants.ClimbConstants; import frc.robot.util.Constants.FieldConstants; import monologue.Logged; @@ -11,25 +10,7 @@ public class PoseCalculations implements Logged { public static Pair getChainIntercepts(Pose2d position) { - Pose2d closestChainPose; - - double minDifference = 10000; - int closestChainIndex = 0; - - int startingIndex = Robot.isRedAlliance() ? 3 : 0; - int endingIndex = Robot.isRedAlliance() ? 6 : 3; - - for (int i = startingIndex; i < endingIndex; i++) { - Pose2d currentChainPose = FieldConstants.CHAIN_POSITIONS[i]; - double currentChainDistance = position.getTranslation().getDistance(currentChainPose.getTranslation()); - - if (currentChainDistance < minDifference) { - minDifference = currentChainDistance; - closestChainIndex = i; - } - } - - closestChainPose = FieldConstants.CHAIN_POSITIONS[closestChainIndex]; + Pose2d closestChainPose = getClosestChain(position); Pose2d relativePosition = position.relativeTo(closestChainPose); @@ -43,6 +24,19 @@ public static Pair getChainIntercepts(Pose2d position) { return Pair.of(leftIntercept - 0.6, rightIntercept - 0.6); } + public static Pose2d getClosestChain(Pose2d position) { + Pose2d[] chainPoses = FieldConstants.GET_CHAIN_POSITIONS(); + Pose2d closestChain = chainPoses[0]; + double minDistance = Double.POSITIVE_INFINITY; + for (Pose2d pose : chainPoses) { + if (position.relativeTo(pose).getTranslation().getNorm() < minDistance) { + minDistance = position.relativeTo(pose).getTranslation().getNorm(); + closestChain = pose; + } + } + return closestChain; + } + /** * Given an intercept of the chain, return the height of the chain at that * location.