diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..576286f --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,109 @@ +{ + "FMS": { + "window": { + "visible": false + } + }, + "System Joysticks": { + "window": { + "enabled": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ], + "zeroDisconnectedJoysticks": false +} diff --git a/src/main/deploy/pathplanner/autos/Center L4 Algae.auto b/src/main/deploy/pathplanner/autos/Center L4 Algae.auto new file mode 100644 index 0000000..dd87fb7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center L4 Algae.auto @@ -0,0 +1,113 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Wrist Out" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToCoralH" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.45 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralHToAlgaeRemove" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "L3 Alage Remove" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "path", + "data": { + "pathName": "HAlgaeToRemove" + } + }, + { + "type": "path", + "data": { + "pathName": "RemoveAlgaeH" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center L4.auto b/src/main/deploy/pathplanner/autos/Center L4.auto new file mode 100644 index 0000000..76d7f43 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center L4.auto @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Wrist Out" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToCoralH" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.45 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "path", + "data": { + "pathName": "CoralHToAlgaeRemove" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + }, + "resetOdom": true, + "folder": " Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NOSide 3xL4.auto b/src/main/deploy/pathplanner/autos/NOSide 3xL4.auto new file mode 100644 index 0000000..58a7fe3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NOSide 3xL4.auto @@ -0,0 +1,209 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToCoralI" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralIToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToCoralK" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.4 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralKToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToCoralL" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Go To L2" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + } + ] + } + }, + "resetOdom": true, + "folder": " Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Side 2xL4 L2.auto b/src/main/deploy/pathplanner/autos/Side 2xL4 L2.auto new file mode 100644 index 0000000..9693b1d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Side 2xL4 L2.auto @@ -0,0 +1,215 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Wrist Out" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToCoralI" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralIToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToCoralL" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralLToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TopSourceToCenterL2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Go To L2" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + } + ] + } + }, + "resetOdom": true, + "folder": " Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Side 2xL4.auto b/src/main/deploy/pathplanner/autos/Side 2xL4.auto new file mode 100644 index 0000000..0faba5c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Side 2xL4.auto @@ -0,0 +1,164 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToCoralI" + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralIToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToCoralK" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.7 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralKToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": " Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Side 3xL4.auto b/src/main/deploy/pathplanner/autos/Side 3xL4.auto new file mode 100644 index 0000000..ed9e25f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Side 3xL4.auto @@ -0,0 +1,214 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToCoralI" + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.15 + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.35 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralIToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.8 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToCoralK" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.1 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.35 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "CoralKToSource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "Go To Source" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceToCoralL" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.1 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "named", + "data": { + "name": "Reverse Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.35 + } + } + ] + } + }, + "resetOdom": true, + "folder": " Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestBlue.auto b/src/main/deploy/pathplanner/autos/Side L4.auto similarity index 64% rename from src/main/deploy/pathplanner/autos/TestBlue.auto rename to src/main/deploy/pathplanner/autos/Side L4.auto index b7f38e9..70b6f55 100644 --- a/src/main/deploy/pathplanner/autos/TestBlue.auto +++ b/src/main/deploy/pathplanner/autos/Side L4.auto @@ -11,13 +11,26 @@ { "type": "path", "data": { - "pathName": "New Path" + "pathName": "ToCoralI" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "Go To L4" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "named", + "data": { + "name": "Go To L4" + } + } + ] } } ] @@ -29,6 +42,12 @@ "name": "Reverse Intake" } }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, { "type": "parallel", "data": { @@ -36,7 +55,7 @@ { "type": "path", "data": { - "pathName": "Source" + "pathName": "CoralIToSource" } }, { @@ -46,7 +65,7 @@ { "type": "wait", "data": { - "waitTime": 0.8 + "waitTime": 0.4 } }, { @@ -65,6 +84,6 @@ } }, "resetOdom": true, - "folder": "Blue Right", + "folder": " Right", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Tests.auto b/src/main/deploy/pathplanner/autos/Tests.auto deleted file mode 100644 index e8c593b..0000000 --- a/src/main/deploy/pathplanner/autos/Tests.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SimplePath" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index bab0da9..640a0d1 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralHToAlgaeRemove.path b/src/main/deploy/pathplanner/paths/CoralHToAlgaeRemove.path new file mode 100644 index 0000000..4e054da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralHToAlgaeRemove.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.78, + "y": 4.13 + }, + "prevControl": null, + "nextControl": { + "x": 6.097470632875189, + "y": 3.984296756850397 + }, + "isLocked": false, + "linkedName": "CoralH" + }, + { + "anchor": { + "x": 6.28155737704918, + "y": 4.025 + }, + "prevControl": { + "x": 5.816784891305164, + "y": 4.207795207870129 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HAlageReady" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.7, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralIToSource.path b/src/main/deploy/pathplanner/paths/CoralIToSource.path new file mode 100644 index 0000000..f76cf89 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralIToSource.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.301, + "y": 5.05 + }, + "prevControl": null, + "nextControl": { + "x": 4.721805898271478, + "y": 6.379917760037189 + }, + "isLocked": false, + "linkedName": "CoralI" + }, + { + "anchor": { + "x": 1.4265368852459013, + "y": 7.33360655737705 + }, + "prevControl": { + "x": 2.2549035474062675, + "y": 6.228515820668262 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 1000.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 126.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralKToSource.path b/src/main/deploy/pathplanner/paths/CoralKToSource.path new file mode 100644 index 0000000..30647f9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralKToSource.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.00389344262295, + "y": 5.2597336065573765 + }, + "prevControl": null, + "nextControl": { + "x": 2.925, + "y": 6.038934426229508 + }, + "isLocked": false, + "linkedName": "CoralK" + }, + { + "anchor": { + "x": 1.4265368852459013, + "y": 7.33360655737705 + }, + "prevControl": { + "x": 2.4407855173609163, + "y": 6.0433411970712365 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 126.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralLToSource.path b/src/main/deploy/pathplanner/paths/CoralLToSource.path new file mode 100644 index 0000000..df7405c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralLToSource.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.668237704918033, + "y": 5.067930327868853 + }, + "prevControl": null, + "nextControl": { + "x": 3.4806958709673737, + "y": 5.233242337470162 + }, + "isLocked": false, + "linkedName": "CoralL" + }, + { + "anchor": { + "x": 1.4265368852459013, + "y": 7.33360655737705 + }, + "prevControl": { + "x": 1.84583176853025, + "y": 6.762411134151682 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.2, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 126.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HAlgaeToRemove.path b/src/main/deploy/pathplanner/paths/HAlgaeToRemove.path new file mode 100644 index 0000000..07fae7c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HAlgaeToRemove.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.28155737704918, + "y": 4.025 + }, + "prevControl": null, + "nextControl": { + "x": 6.042937382540423, + "y": 3.950431251716013 + }, + "isLocked": false, + "linkedName": "HAlageReady" + }, + { + "anchor": { + "x": 5.730122950819672, + "y": 3.97 + }, + "prevControl": { + "x": 6.070321154826206, + "y": 3.97 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "AlageRemove" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.7, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source.path b/src/main/deploy/pathplanner/paths/RemoveAlgaeH.path similarity index 63% rename from src/main/deploy/pathplanner/paths/Source.path rename to src/main/deploy/pathplanner/paths/RemoveAlgaeH.path index 39ad90d..83412b2 100644 --- a/src/main/deploy/pathplanner/paths/Source.path +++ b/src/main/deploy/pathplanner/paths/RemoveAlgaeH.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.21625, - "y": 3.0304999999999995 + "x": 5.730122950819672, + "y": 3.97 }, "prevControl": null, "nextControl": { - "x": 4.797000000000001, - "y": 2.075 + "x": 6.001738466702153, + "y": 3.939984378773848 }, "isLocked": false, - "linkedName": "first" + "linkedName": "AlageRemove" }, { "anchor": { - "x": 1.1992500000000001, - "y": 0.9244999999999993 + "x": 6.245594262295082, + "y": 4.025 }, "prevControl": { - "x": 1.8915000000000002, - "y": 1.499749999999999 + "x": 5.995838872934667, + "y": 4.036056468035822 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 0.5, + "maxVelocity": 0.2, + "maxAcceleration": 0.1, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -126.40877456777238 + "rotation": 146.68936917543914 }, "reversed": false, - "folder": "Blue Right", + "folder": "Right", "idealStartingState": { "velocity": 0, - "rotation": 121.26373169437744 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SimplePath.path b/src/main/deploy/pathplanner/paths/SimplePath.path deleted file mode 100644 index 1c1dd5c..0000000 --- a/src/main/deploy/pathplanner/paths/SimplePath.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.7931352459016394, - "y": 2.274795081967213 - }, - "prevControl": null, - "nextControl": { - "x": 2.9992128622624037, - "y": 1.917858180127682 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.292790014151457, - "y": 4.494829275372837 - }, - "prevControl": { - "x": 5.292790014151457, - "y": 3.7217006253977063 - }, - "nextControl": { - "x": 5.292790014151457, - "y": 5.182086031031642 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 12.215381640288863, - "y": 2.923206785184939 - }, - "prevControl": { - "x": 12.12956096836855, - "y": 2.4364935687758864 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3, - "maxAcceleration": 3, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 58.29857033049431 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -59.86347243867993 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceToCoralK.path b/src/main/deploy/pathplanner/paths/SourceToCoralK.path new file mode 100644 index 0000000..2ab9124 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceToCoralK.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4265368852459013, + "y": 7.33360655737705 + }, + "prevControl": null, + "nextControl": { + "x": 2.8770491803278686, + "y": 6.590368852459017 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 4.00389344262295, + "y": 5.2597336065573765 + }, + "prevControl": { + "x": 3.9306689931289513, + "y": 5.502597209740501 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CoralK" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8443496801705674, + "rotationDegrees": -59.99999999999999 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 850.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 126.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceToCoralL.path b/src/main/deploy/pathplanner/paths/SourceToCoralL.path new file mode 100644 index 0000000..8786f6f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceToCoralL.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4265368852459013, + "y": 7.33360655737705 + }, + "prevControl": null, + "nextControl": { + "x": 2.481464308562815, + "y": 5.960600832544747 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 3.668237704918033, + "y": 5.067930327868853 + }, + "prevControl": { + "x": 2.9373484743997063, + "y": 5.917061157261267 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CoralL" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8891919378698225, + "rotationDegrees": -59.99999999999999 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 850.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 126.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/ToCoralH.path similarity index 65% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/ToCoralH.path index a5df396..9d6185a 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/ToCoralH.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.079713114754098, - "y": 1.8792008196721315 + "x": 7.863934426229507, + "y": 4.13 }, "prevControl": null, "nextControl": { - "x": 6.880942622950819, - "y": 2.059016393442623 + "x": 6.863934426229508, + "y": 4.13 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.21625, - "y": 3.0304999999999995 + "x": 5.78, + "y": 4.13 }, "prevControl": { - "x": 5.78686475409836, - "y": 2.046069672131147 + "x": 6.223707048407509, + "y": 4.13 }, "nextControl": null, "isLocked": false, - "linkedName": "first" + "linkedName": "CoralH" } ], "rotationTargets": [], @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.7, "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 121.26373169437744 + "rotation": 180.0 }, "reversed": false, - "folder": "Blue Right", + "folder": "Right", "idealStartingState": { "velocity": 0, - "rotation": -178.15238973400534 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToCoralI.path b/src/main/deploy/pathplanner/paths/ToCoralI.path new file mode 100644 index 0000000..6a614b0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ToCoralI.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.204610655737704, + "y": 5.523463114754098 + }, + "prevControl": null, + "nextControl": { + "x": 5.449850400727928, + "y": 5.466020022053859 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.301, + "y": 5.05 + }, + "prevControl": { + "x": 5.694159836065573, + "y": 5.367622950819673 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CoralI" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.551597941321499, + "rotationDegrees": -119.99999999999999 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 2.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TopSourceToCenterL2.path b/src/main/deploy/pathplanner/paths/TopSourceToCenterL2.path new file mode 100644 index 0000000..e188c60 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TopSourceToCenterL2.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4265368852459013, + "y": 7.33360655737705 + }, + "prevControl": null, + "nextControl": { + "x": 2.4766808736767345, + "y": 6.037863162879677 + }, + "isLocked": false, + "linkedName": "Source" + }, + { + "anchor": { + "x": 3.2, + "y": 4.18 + }, + "prevControl": { + "x": 2.56006691261574, + "y": 4.463603877314815 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.698294193786982, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.2, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Right", + "idealStartingState": { + "velocity": 0, + "rotation": 126.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index fe265aa..3213646 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,27 +1,27 @@ { "robotWidth": 0.8636, - "robotLength": 0.9144, + "robotLength": 0.914, "holonomicMode": true, "pathFolders": [ - "Blue Right" + "Right" ], "autoFolders": [ - "Blue Right" + " Right" ], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 1.2, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 75.0, - "robotMOI": 6.883, + "robotMass": 65.771, + "robotMOI": 12.0, "robotTrackwidth": 0.5588, - "driveWheelRadius": 0.05504, - "driveGearing": 7.363636, - "maxDriveSpeed": 4.7, - "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 120.0, - "wheelCOF": 1.7, + "driveWheelRadius": 0.0508, + "driveGearing": 6.39, + "maxDriveSpeed": 5.639, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 70.0, + "wheelCOF": 2.0, "flModuleX": 0.267, "flModuleY": 0.2667, "frModuleX": 0.267, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d1c8103..50156a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,64 +4,89 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.AlignToTarget; +import frc.robot.commands.ArmCommands; +import frc.robot.commands.LEDDefaultCommand; +import frc.robot.constants.AlignmentConstants; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.Climb; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Vision; -import frc.robot.constants.AlignmentConstants; -import frc.robot.commands.AlignToTarget; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Wrist; import frc.robot.subsystems.Elevator; -import frc.robot.commands.ArmCommands; -import com.pathplanner.lib.auto.NamedCommands; +import frc.robot.subsystems.Intake; import frc.robot.subsystems.LEDSubsystem; -import frc.robot.commands.LEDDefaultCommand; +import frc.robot.subsystems.Vision; +import frc.robot.subsystems.Wrist; public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + /* Constants */ + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)/2; // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.5).in(RadiansPerSecond); // 1/2 of a rotation per second - /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + + private double CrawlMaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond)/4; // kSpeedAt12Volts desired top speed + private double CrawlMaxAngularRate = RotationsPerSecond.of(0.5).in(RadiansPerSecond)/2; // 1/2 of a rotation per second + + /* Drive Request Objcets */ + private final SwerveRequest.FieldCentric fieldCentricDrive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1)// Accounts for 10% deadband from the movement joystick + .withRotationalDeadband(MaxAngularRate * 0.1) // Accounts for 10% deadband from the rotational joystick + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors making it respond from raw inputs private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() + private final SwerveRequest.RobotCentric robotCentricDrive = new SwerveRequest.RobotCentric() .withDriveRequestType(DriveRequestType.OpenLoopVoltage); private final Telemetry logger = new Telemetry(MaxSpeed); - private final CommandXboxController joystick = new CommandXboxController(0); // Driver controller + /* Controllers */ + private final CommandXboxController driverController = new CommandXboxController(0); // Driver controller private final CommandXboxController operatorController = new CommandXboxController(1); // Operator controller - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + /* Vision */ private final Vision vision; + private final LEDSubsystem ledSubsystem = new LEDSubsystem(0); /* Path follower */ private final SendableChooser autoChooser; + private final SendableChooser autoLocationChooser; + /* Subsystems */ + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private final Intake intake = new Intake(); private final Wrist wrist = new Wrist(); - private final Elevator elevator = new Elevator(); + private final Elevator elevator = new Elevator(wrist); + private final Climb climb = new Climb(); + double targetPosition = 0; + /* Commands */ private final ArmCommands armCommands; - private final LEDSubsystem ledSubsystem = new LEDSubsystem(0); - public RobotContainer() { // Create vision subsystem after drivetrain vision = new Vision(drivetrain); + SmartDashboard.putNumber("Max Speed:", MaxSpeed); + SmartDashboard.putNumber("Max Angular Rate:", MaxAngularRate); // Initialize arm commands armCommands = new ArmCommands(elevator, wrist, intake); @@ -75,79 +100,182 @@ public RobotContainer() { NamedCommands.registerCommand("Start Intake", intake.startIntakeCommand()); NamedCommands.registerCommand("Stop Intake", intake.stopIntakeCommand()); NamedCommands.registerCommand("Reverse Intake", intake.reverseIntakeCommand()); - + NamedCommands.registerCommand("Stall Intake", intake.stallIntakeCommand()); + NamedCommands.registerCommand("Wrist Out", wrist.goForwardCommand()); + NamedCommands.registerCommand("L3 Alage Remove", armCommands.goToAlgaeL3()); + NamedCommands.registerCommand("L2 Alage Remove", armCommands.goToAlgaeL2()); + NamedCommands.registerCommand("Intake Hold", intake.startIntakeCommand()); + autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Mode", autoChooser); + autoLocationChooser = new SendableChooser<>(); + autoLocationChooser.addOption("Left", new String("Left")); + autoLocationChooser.addOption("Center", new String("Center")); + autoLocationChooser.addOption("Right", new String("Right")); + + SmartDashboard.putData("Auto Location", autoLocationChooser); // Set default command for LEDs ledSubsystem.setDefaultCommand(new LEDDefaultCommand( ledSubsystem, drivetrain, - joystick, + driverController, operatorController )); configureBindings(); + + AlignmentConstants.printPositions(); } private void configureBindings() { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> - drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) - ) - ); + drivetrain.applyRequest(() -> { - joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); + double distanceToReef = new Translation2d( + drivetrain.getState().Pose.getX(), + drivetrain.getState().Pose.getY() + ).getDistance( + new Translation2d( + AlignmentConstants.findClosestTarget(drivetrain.getState().Pose).getX(), + AlignmentConstants.findClosestTarget(drivetrain.getState().Pose).getY() + ) + ); - - // X button for left side targets with LED feedback - joystick.x().whileTrue( + if (driverController.rightTrigger().getAsBoolean()) { + return fieldCentricDrive.withVelocityX(-driverController.getLeftY() * MaxSpeed / 2) + .withVelocityY(-driverController.getLeftX() * MaxSpeed / 2) + .withRotationalRate(-driverController.getRightX() * CrawlMaxAngularRate); + + } + else if(driverController.leftTrigger().getAsBoolean() && (Math.abs(driverController.getLeftY())>0.3 || Math.abs(driverController.getLeftX())>0.3)){ + return fieldCentricDrive.withVelocityX(-driverController.getLeftY() * MaxSpeed * 1.7) + .withVelocityY(-driverController.getLeftX() * MaxSpeed * 1.7) + .withRotationalRate(-driverController.getRightX() * MaxAngularRate); + } + else { + return fieldCentricDrive.withVelocityX(-driverController.getLeftY() * MaxSpeed) + .withVelocityY(-driverController.getLeftX() * MaxSpeed) + .withRotationalRate(-driverController.getRightX() * MaxAngularRate * 3/4); + } + }) + ); + + // Left Align Button + driverController.x().whileTrue( new AlignToTarget(drivetrain, () -> { var currentPose = drivetrain.getState().Pose; return AlignmentConstants.findClosestLeftTarget(currentPose); }) ); - // B button for right side targets with LED feedback - joystick.b().whileTrue( + // Right Align Button + driverController.b().whileTrue( new AlignToTarget(drivetrain, () -> { var currentPose = drivetrain.getState().Pose; return AlignmentConstants.findClosestRightTarget(currentPose); }) ); - joystick.pov(0).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(0.5).withVelocityY(0)) + // Robot Centric Drive If Requested + driverController.pov(0).whileTrue(drivetrain.applyRequest(() -> + robotCentricDrive.withVelocityX(0.5).withVelocityY(0)) ); - joystick.pov(180).whileTrue(drivetrain.applyRequest(() -> - forwardStraight.withVelocityX(-0.5).withVelocityY(0)) + driverController.pov(180).whileTrue(drivetrain.applyRequest(() -> + robotCentricDrive.withVelocityX(-0.5).withVelocityY(0)) ); - // Combined elevator and wrist controls for all positions - operatorController.a().onTrue(armCommands.goToL2()); // L2 position on A - operatorController.b().onTrue(armCommands.goToL3()); // L3 position on B - operatorController.y().onTrue(armCommands.goToL4()); // L4 position on Y - operatorController.x().onTrue(armCommands.goToSource()); // Source position + intake on X - joystick.leftBumper().onTrue(armCommands.goToRest()); // Rest position on driver left bumper + /* Operator Commands */ + + // Makes L4 Only Available When 1 Meter Close to Reef - // Stop intake - operatorController.leftBumper().onTrue(intake.reverseIntakeCommand()); + // not using currently since robot doesn't have limelights on it + // operatorController.y().onTrue( + // new InstantCommand(() -> { + // if (distance < 1.25) { + // armCommands.goToL4(); + // } + // }) + // ); + + operatorController.y().onTrue(armCommands.goToL4()); + // L4 position on Y + operatorController.x().onTrue(armCommands.goToSource()); // Source position on X + operatorController.b().onTrue(armCommands.goToL3()); // L4 position on Y + operatorController.a().onTrue(armCommands.goToL2()); // Reverse intake - toggle style operatorController.rightBumper().onTrue( intake.stopIntakeCommand().alongWith(armCommands.goToRest()) ); + operatorController.rightTrigger().onTrue(armCommands.goToAlgaeL2()); // Remove algae from L2 on Right Trigger + operatorController.leftTrigger().onTrue(armCommands.goToAlgaeL3()); // Remove algae from L3 on Right Trigger + + // Manual Control On Intake + operatorController.leftBumper().onTrue(intake.reverseIntakeCommand()); // Release Coral on Left Bumper + operatorController.rightBumper().onTrue(intake.stallIntakeCommand()); // Keep Coral In on Right Bumper + operatorController.start().onTrue(intake.stopIntakeCommand()); + + // Rest position on driver left bumper + driverController.leftBumper().onTrue(armCommands.goToRest()); // Home Position + + // Climb Control + driverController.a().whileTrue(climb.runClimb()); + driverController.y().whileTrue(climb.reverseClimb()); + + // Telementry Update drivetrain.registerTelemetry(logger::telemeterize); + + + + + // Manual Controls for Elevator + // while (operatorController.getLeftY() > 0.3){ + // double currentElevatorPosition = elevator.getPosition(); + // targetPosition = currentElevatorPosition + 0.2; + + // elevator.runOnce(() -> elevator.setTargetLocation(targetPosition)); + // elevator.elevatorMoveToDesired(); + // } + + // while (operatorController.a().getAsBoolean()){ + // double currentElevatorPosition = elevator.getPosition(); + // targetPosition = currentElevatorPosition - 1; + + + // elevator.setTargetLocation(targetPosition); + // elevator.elevatorMoveToDesired(); + // } + + + + + + // Manual Control on Elevator + // if(operatorController.getLeftY()>0.1){ + // elevator.setUp(); + // } + + // if(operatorController.getLeftY()<-0.1){ + // elevator.setDown(); + // } + + // if(operatorController.start().getAsBoolean()){ + // elevator.zeroEncoder(); + // } } public Command getAutonomousCommand() { - /* Run the path selected from the auto chooser */ - return autoChooser.getSelected(); + try{ + if(autoLocationChooser.getSelected().equals("Right")){ + return new PathPlannerAuto(autoChooser.getSelected().getName(), true); + }else{ + return new PathPlannerAuto(autoChooser.getSelected().getName(), false); + } + }catch(Exception e){ + DriverStation.reportError("PathPlanner ERROR: " + e.getMessage(), e.getStackTrace()); + return null; + } } } diff --git a/src/main/java/frc/robot/commands/AlignToTarget.java b/src/main/java/frc/robot/commands/AlignToTarget.java index deb7846..6d25537 100644 --- a/src/main/java/frc/robot/commands/AlignToTarget.java +++ b/src/main/java/frc/robot/commands/AlignToTarget.java @@ -3,6 +3,9 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.AlignmentConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -19,11 +22,12 @@ public class AlignToTarget extends Command { private final PIDController rotationController; private final SwerveRequest.FieldCentric fieldCentric = new SwerveRequest.FieldCentric(); private static final double ACTIVATION_DISTANCE_METERS = 1; - private static final double SLOW_DOWN_DISTANCE_METERS = 0.127; // 5 inches in meters - private static final double MAX_SPEED_FAR = 6.0; // Increased from 4.0 to 6.0 m/s - private static final double MAX_SPEED_NEAR = 3.0; // Increased from 2.0 to 3.0 m/s + private static final double SLOW_DOWN_DISTANCE_METERS = 0.3048; // 12 inches in meters + private static final double MAX_SPEED_FAR = 3.0; // Increased from 4.0 to 6.0 m/s + private static final double MAX_SPEED_NEAR = 0.25; // Increased from 2.0 to 3.0 m/s private static final double MAX_ROTATION_SPEED_FAR = 8.0; // Increased from 4.0 to 8.0 rad/s private static final double MAX_ROTATION_SPEED_NEAR = 4.0; // Increased from 2.0 to 4.0 rad/s + private boolean isRedAlliance; public AlignToTarget(CommandSwerveDrivetrain drivetrain, Supplier targetPoseSupplier) { this.drivetrain = drivetrain; @@ -57,6 +61,11 @@ public AlignToTarget(CommandSwerveDrivetrain drivetrain, Supplier target @Override public void initialize() { targetPose = targetPoseSupplier.get(); + isRedAlliance = DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == Alliance.Red; + + // If on red alliance, invert X and Y coordinates + xController.setSetpoint(targetPose.getX()); yController.setSetpoint(targetPose.getY()); rotationController.setSetpoint(targetPose.getRotation().getRadians()); @@ -71,8 +80,10 @@ public boolean isFinished() { currentPose.getX(), currentPose.getY() ).getDistance( + new Translation2d( - targetPose.getX(), + + targetPose.getX(), targetPose.getY() ) ); @@ -105,12 +116,19 @@ public void execute() { // Only run alignment if we're within activation distance if (distance <= ACTIVATION_DISTANCE_METERS) { - double xSpeed = xController.calculate(currentPose.getX()); - double ySpeed = yController.calculate(currentPose.getY()); + double xSpeed = xController.calculate(currentPose.getX(), targetPose.getX()); + double ySpeed = yController.calculate(currentPose.getY(), targetPose.getY()); double rotationSpeed = rotationController.calculate( - currentPose.getRotation().getRadians() + currentPose.getRotation().getRadians(), + targetPose.getRotation().getRadians() ); + // Invert X and Y speeds if on red alliance since we're using field-oriented control + if (isRedAlliance) { + xSpeed = -xSpeed; + ySpeed = -ySpeed; + } + // Calculate speed limits based on distance double speedScale; double rotationScale; diff --git a/src/main/java/frc/robot/commands/ArmCommands.java b/src/main/java/frc/robot/commands/ArmCommands.java index 20212d4..3883331 100644 --- a/src/main/java/frc/robot/commands/ArmCommands.java +++ b/src/main/java/frc/robot/commands/ArmCommands.java @@ -16,29 +16,38 @@ public ArmCommands(Elevator elevator, Wrist wrist, Intake intake) { this.intake = intake; } + // Rest Position Command + public Command goToRest() { + return elevator.goToRestCommand().alongWith(wrist.goToRestCommand()).andThen(intake.stopIntakeCommand()); + } + + // Source Position with Intake Command + public Command goToSource() { + return elevator.goToSourceCommand().alongWith(wrist.goToSourceCommand()).andThen(intake.startIntakeCommand()); + } + // L2 Position Command public Command goToL2() { - return elevator.goToL2Command().alongWith(wrist.goToL3Command()); + return elevator.goToL2Command().alongWith(wrist.goToL3Command()).andThen(intake.stallIntakeCommand()); } // L3 Position Command public Command goToL3() { - return elevator.goToL3Command().alongWith(wrist.goToL3Command()); + return elevator.goToL3Command().alongWith(wrist.goToL3Command()).andThen(intake.stallIntakeCommand()); } // L4 Position Command public Command goToL4() { - return elevator.goToL4Command().alongWith(wrist.goToL4Command()); + return elevator.goToL4Command().alongWith(wrist.goToL4Command()).andThen(intake.stallIntakeCommand()); } - // Source Position with Intake Command - public Command goToSource() { - return elevator.goToSourceCommand().alongWith(wrist.goToSourceCommand()).andThen(intake.startIntakeCommand()); + // Alage Position Commands + public Command goToAlgaeL3() { + return elevator.goToAlgaeL3Command().alongWith(wrist.goToAlage()).andThen(intake.slowReverseIntakeCommand()); } - // Rest Position Command - public Command goToRest() { - return elevator.goToRestCommand().alongWith(wrist.goToRestCommand()); + public Command goToAlgaeL2() { + return elevator.goToAlgaeL2Command().alongWith(wrist.goToAlage()).andThen(intake.slowReverseIntakeCommand()); } // Stop All Command diff --git a/src/main/java/frc/robot/constants/AlignmentConstants.java b/src/main/java/frc/robot/constants/AlignmentConstants.java index a3f57f3..7676c7d 100644 --- a/src/main/java/frc/robot/constants/AlignmentConstants.java +++ b/src/main/java/frc/robot/constants/AlignmentConstants.java @@ -6,58 +6,110 @@ public final class AlignmentConstants { // PID Constants for alignment - Increased for faster response - public static final double XY_P = 3.0; // Increased from 1.0 to 3.0 - public static final double XY_I = 0.0; - public static final double XY_D = 0.1; // Added small derivative gain + public static final double XY_P = 3.0; + public static final double XY_I = 0.01; + public static final double XY_D = 0.1; - public static final double ROTATION_P = 3.0; // Increased from 1.0 to 3.0 - public static final double ROTATION_I = 0.0; - public static final double ROTATION_D = 0.1; // Added small derivative gain + public static final double ROTATION_P = 3.0; + public static final double ROTATION_I = 0.01; + public static final double ROTATION_D = 0.1; // Tolerance for considering alignment complete - public static final double POSITION_TOLERANCE_METERS = 0.05; // 5 cm - public static final double ROTATION_TOLERANCE_DEGREES = 2.0; // 2 degrees - - // Left Side Positions - public static final Pose2d[] LEFT_POSITIONS = { - new Pose2d(3.2, 4.18, Rotation2d.fromDegrees(0)), - new Pose2d(3.7, 2.99, Rotation2d.fromDegrees(60)), - new Pose2d(5.03, 2.8, Rotation2d.fromDegrees(120)), - new Pose2d(5.83, 3.87, Rotation2d.fromDegrees(180)), - new Pose2d(5.3, 5.07, Rotation2d.fromDegrees(240)), - new Pose2d(3.9, 5.2, Rotation2d.fromDegrees(300)), - new Pose2d(13.87, 5.133, Rotation2d.fromDegrees(-120)), // RC - new Pose2d(12.504, 5.316, Rotation2d.fromDegrees(-60)),// RE - new Pose2d(11.673, 4.196, Rotation2d.fromDegrees(0)),//RG - new Pose2d(12.237, 2.924, Rotation2d.fromDegrees(60)),//RI - new Pose2d(13.581, 2.753, Rotation2d.fromDegrees(120)),//RK - new Pose2d(14.426, 3.868, Rotation2d.fromDegrees(180)),//RA - // new Pose2d(1.166, 7.057, Rotation2d.fromDegrees(0)), - // new Pose2d(16.44, 7.009, Rotation2d.fromDegrees(0)), - // new Pose2d(16.44, 1.010, Rotation2d.fromDegrees(0)) - }; - - // Right Side Positions - public static final Pose2d[] RIGHT_POSITIONS = { - new Pose2d(3.15, 3.8, Rotation2d.fromDegrees(0)),//A - new Pose2d(4.0, 2.8, Rotation2d.fromDegrees(60)),//C - new Pose2d(5.31, 3.02, Rotation2d.fromDegrees(120)),//E - new Pose2d(5.8, 4.21, Rotation2d.fromDegrees(180)),//G - new Pose2d(4.96, 5.28, Rotation2d.fromDegrees(240)),//I - new Pose2d(3.62, 5.03, Rotation2d.fromDegrees(300)),//K - new Pose2d(1.214, 1.006, Rotation2d.fromDegrees(225)), - new Pose2d(13.574, 5.307, Rotation2d.fromDegrees(-120)),//RD - new Pose2d(12.251, 5.140, Rotation2d.fromDegrees(-60)),//RF - new Pose2d(11.682, 3.867, Rotation2d.fromDegrees(0)),//RH - new Pose2d(12.502, 2.772, Rotation2d.fromDegrees(60)),//RJ - new Pose2d(13.884, 2.897, Rotation2d.fromDegrees(120)),//Rl - new Pose2d(14.431, 4.203, Rotation2d.fromDegrees(180)),//RB - // new Pose2d(13.809, 2.985, Rotation2d.fromDegrees(0)), - // new Pose2d(1.198, 7.013, Rotation2d.fromDegrees(135)), - // new Pose2d(16.382, 7.009, Rotation2d.fromDegrees(0)), - // new Pose2d(16.470, 1.096, Rotation2d.fromDegrees(0)) - }; + public static final double POSITION_TOLERANCE_METERS = 0.015; + public static final double ROTATION_TOLERANCE_DEGREES = 1.0; + + // new Pose2d(13.8927, 4.0260, Rotation2d.fromDegrees(180)), // Tag 7 + // Offsets for alignment |Tag 7 Pose - Recorded Pose| + public static final double TAG_LEFT_OFFSET_X = 14.352-13.8927; + public static final double TAG_LEFT_OFFSET_Y = 4.16-4.0260+0.04 + ; + public static final double TAG_RIGHT_OFFSET_X = 14.355-13.8927; + public static final double TAG_RIGHT_OFFSET_Y = 4.0260-3.812-0.0254; + + // Holds All Target Positions + public static final Pose2d[] LEFT_POSITIONS = calculateLeftTargetsWithOffset(); + public static final Pose2d[] RIGHT_POSITIONS = calculateRightTargetsWithOffset(); + + private static Pose2d[] calculateRightTargetsWithOffset() { + Pose2d[] baseTargets = { + new Pose2d(13.4724, 3.3073, Rotation2d.fromDegrees(120)), // Tag 6 + new Pose2d(13.8927, 4.0260, Rotation2d.fromDegrees(180)), // Tag 7 + new Pose2d(13.4724, 4.7460, Rotation2d.fromDegrees(240)), // Tag 8 + new Pose2d(12.6414, 4.7460, Rotation2d.fromDegrees(300)), // Tag 9 + new Pose2d(12.2287, 4.0260, Rotation2d.fromDegrees(0)), // Tag 10 + new Pose2d(12.6414, 3.3073, Rotation2d.fromDegrees(60)), // Tag 11 + new Pose2d(3.6576, 4.0260, Rotation2d.fromDegrees(0)), // Tag 17 + new Pose2d(4.9067, 3.3073, Rotation2d.fromDegrees(120)), // Tag 22 + new Pose2d(5.3209, 4.0260, Rotation2d.fromDegrees(180)), // Tag 21 + new Pose2d(4.9067, 4.7460, Rotation2d.fromDegrees(240)), // Tag 20 + new Pose2d(4.0740, 4.7460, Rotation2d.fromDegrees(300)), // Tag 19 + new Pose2d(4.0740, 3.3073, Rotation2d.fromDegrees(60)), // Tag 18 + }; + + Pose2d[] adjustedTargets = new Pose2d[baseTargets.length]; + + for (int i = 0; i < baseTargets.length; i++) { + Pose2d tagPose = baseTargets[i]; + double theta = tagPose.getRotation().getRadians(); + + double offsetX = TAG_LEFT_OFFSET_X; + double offsetY = TAG_LEFT_OFFSET_Y; + + // rotate the offset around theta (rotational matrix) + double rotatedOffsetX = Math.cos(theta) * offsetX - Math.sin(theta) * offsetY; + double rotatedOffsetY = Math.sin(theta) * offsetX + Math.cos(theta) * offsetY; + + // apply the rotated offset to the tag's position + double adjustedX = tagPose.getX() - rotatedOffsetX; + double adjustedY = tagPose.getY() - rotatedOffsetY; + + adjustedTargets[i] = new Pose2d(adjustedX, adjustedY, tagPose.getRotation()); + } + + return adjustedTargets; + } + + + private static Pose2d[] calculateLeftTargetsWithOffset() { + Pose2d[] baseTargets = { + new Pose2d(13.4724, 3.3073, Rotation2d.fromDegrees(120)), // Tag 6 + new Pose2d(13.8927, 4.0260, Rotation2d.fromDegrees(180)), // Tag 7 + new Pose2d(13.4724, 4.7460, Rotation2d.fromDegrees(240)), // Tag 8 + new Pose2d(12.6414, 4.7460, Rotation2d.fromDegrees(300)), // Tag 9 + new Pose2d(12.2287, 4.0260, Rotation2d.fromDegrees(0)), // Tag 10 + new Pose2d(12.6414, 3.3073, Rotation2d.fromDegrees(60)), // Tag 11 + new Pose2d(3.6576, 4.0260, Rotation2d.fromDegrees(0)), // Tag 17 + new Pose2d(4.9067, 3.3073, Rotation2d.fromDegrees(120)), // Tag 22 + new Pose2d(5.3209, 4.0260, Rotation2d.fromDegrees(180)), // Tag 21 + new Pose2d(4.9067, 4.7460, Rotation2d.fromDegrees(240)), // Tag 20 + new Pose2d(4.0740, 4.7460, Rotation2d.fromDegrees(300)), // Tag 19 + new Pose2d(4.0740, 3.3073, Rotation2d.fromDegrees(60)), // Tag 18 + }; + + Pose2d[] adjustedTargets = new Pose2d[baseTargets.length]; + + for (int i = 0; i < baseTargets.length; i++) { + Pose2d tagPose = baseTargets[i]; + double theta = tagPose.getRotation().getRadians(); + + double offsetX = -TAG_RIGHT_OFFSET_X; + double offsetY = TAG_RIGHT_OFFSET_Y; + + // rotate the offset around theta (rotational matrix) + double rotatedOffsetX = Math.cos(theta) * offsetX - Math.sin(theta) * offsetY; + double rotatedOffsetY = Math.sin(theta) * offsetX + Math.cos(theta) * offsetY; + + // apply the rotated offset to the tag's position + double adjustedX = tagPose.getX() + rotatedOffsetX; + double adjustedY = tagPose.getY() + rotatedOffsetY; + + adjustedTargets[i] = new Pose2d(adjustedX, adjustedY, tagPose.getRotation()); + } + + return adjustedTargets; + } + // Method to find closest target public static Pose2d findClosestLeftTarget(Pose2d currentPose) { Pose2d closest = LEFT_POSITIONS[0]; @@ -103,4 +155,83 @@ public static Pose2d findClosestTarget(Pose2d currentPose) { return leftDistance < rightDistance ? leftTarget : rightTarget; } -} \ No newline at end of file + + // Method to print out the positions +public static void printPositions() { + System.out.println("Left Positions:"); + for (Pose2d position : LEFT_POSITIONS) { + System.out.println("X: " + position.getX() + ", Y: " + position.getY() + ", Rotation: " + position.getRotation().getDegrees() + " degrees"); + } + + System.out.println("\nRight Positions:"); + for (Pose2d position : RIGHT_POSITIONS) { + System.out.println("X: " + position.getX() + ", Y: " + position.getY() + ", Rotation: " + position.getRotation().getDegrees() + " degrees"); + } +} + +} + + + + +// OLD TARGETS: + + +// // Left Side Positions +// public static final Pose2d[] LEFT_POSITIONS = { +// new Pose2d(3.2, 4.18, Rotation2d.fromDegrees(0)), +// new Pose2d(3.7, 2.99, Rotation2d.fromDegrees(60)), +// new Pose2d(5.03, 2.8, Rotation2d.fromDegrees(120)), +// new Pose2d(5.83, 3.87, Rotation2d.fromDegrees(180)), +// new Pose2d(5.3, 5.07, Rotation2d.fromDegrees(240)), +// new Pose2d(3.9, 5.2, Rotation2d.fromDegrees(300)), +// new Pose2d(13.89, 5.06, Rotation2d.fromDegrees(-120)), // RC +// new Pose2d(12.504, 5.22, Rotation2d.fromDegrees(-60)),// RE +// new Pose2d(11.673, 4.196, Rotation2d.fromDegrees(0)),//RG +// new Pose2d(13.58,2.78, Rotation2d.fromDegrees(120)),//RK +// new Pose2d(12.27,2.95, Rotation2d.fromDegrees(60)),//RI +// new Pose2d(14.37, 3.89, Rotation2d.fromDegrees(180)),//RA +// new Pose2d(16.39, 1.03, Rotation2d.fromDegrees(-60)),//RA +// new Pose2d(16.817, 0.98, Rotation2d.fromDegrees(-57)),//LeftSourceLeft(BLUE) +// }; + + +// // Right Side Positions +// public static final Pose2d[] RIGHT_POSITIONS = { +// new Pose2d(3.15, 3.8, Rotation2d.fromDegrees(0)),//A +// new Pose2d(4.0, 2.8, Rotation2d.fromDegrees(60)),//C +// new Pose2d(5.31, 3.02, Rotation2d.fromDegrees(120)),//E +// new Pose2d(5.8, 4.21, Rotation2d.fromDegrees(180)),//G +// new Pose2d(4.96, 5.28, Rotation2d.fromDegrees(240)),//I +// new Pose2d(3.62, 5.03, Rotation2d.fromDegrees(300)),//K +// new Pose2d(1.214, 1.006, Rotation2d.fromDegrees(225)), +// new Pose2d(13.574, 5.307, Rotation2d.fromDegrees(-120)),//RD +// new Pose2d(12.23, 5.02, Rotation2d.fromDegrees(-60)),//RF +// new Pose2d(11.682, 3.867, Rotation2d.fromDegrees(0)),//RH +// new Pose2d(13.89,2.97,Rotation2d.fromDegrees(120)),//RL +// new Pose2d(12.52,2.79,Rotation2d.fromDegrees(60)),//RJ +// new Pose2d(14.35, 4.2, Rotation2d.fromDegrees(180)),//RB +// new Pose2d(16.11, 0.63, Rotation2d.fromDegrees(-54)),//LeftSourceLeft(BLUE) +// }; + + // // Left Side Positions + // public static final Pose2d[] LEFT_POSITIONS = { + // new Pose2d(14.344, 3.867, Rotation2d.fromDegrees(180)), + // new Pose2d(13.5, 2.771, Rotation2d.fromDegrees(120)), + // new Pose2d(12.220, 3.143, Rotation2d.fromDegrees(60)), + // new Pose2d(11.762, 4.187, Rotation2d.fromDegrees(0)), + // new Pose2d(12.583, 5.227, Rotation2d.fromDegrees(-60)), + // new Pose2d(13.825, 5.065, Rotation2d.fromDegrees(-120)), + // }; + + // // Right Side Positions + // public static final Pose2d[] RIGHT_POSITIONS = { + // new Pose2d(14.358, 4.146, Rotation2d.fromDegrees(180)), + // new Pose2d(13.568, 3.264, Rotation2d.fromDegrees(120)), + // new Pose2d(12.564, 2.845, Rotation2d.fromDegrees(60)), + // new Pose2d(11.746, 3.891, Rotation2d.fromDegrees(0)), + // new Pose2d(12.292, 5.076, Rotation2d.fromDegrees(-60)), + // new Pose2d(13.537, 5.237, Rotation2d.fromDegrees(-120)), + // }; + + diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 63f725d..c9debbb 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -1,14 +1,16 @@ package frc.robot.constants; public final class ElevatorConstants { - // Positions in meters - public static final double Source_Distance = 0.14; // ~10 inches - public static final double L2_Distance = 0.16; // ~14 inches - public static final double L3_Distance = 0.35; // ~23 inches - public static final double L4_Distance = 0.78; // ~31 inches - public static final double resetPos = 0.0; // Home position + + public static final double Source_Distance = 7.55; + public static final double L2_Distance = 9; + public static final double L3_Distance = 22.1; + public static final double L4_Distance = 44.1; + public static final double AlgaeL3_Distance = 19; + public static final double AlgaeL2_Distance = 9; + public static final double resetPos = 0; // Motor configuration - public static final int LEADER_MOTOR_ID = 14; - public static final int FOLLOWER_MOTOR_ID = 15; + public static final int ELEVATOR_MOTOR_ID = 15; + public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 21; } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/WristConstants.java b/src/main/java/frc/robot/constants/WristConstants.java index 71e9836..b752b28 100644 --- a/src/main/java/frc/robot/constants/WristConstants.java +++ b/src/main/java/frc/robot/constants/WristConstants.java @@ -1,25 +1,15 @@ package frc.robot.constants; public final class WristConstants { - public static final double L4_Angle = 7.4; - public static final double L3_Angle = 7; - public static final double Source_Angle = 4.2; - public static final double Rest_Angle = 0; + public static final double Ratio = -1/6; + public static final double L4_Angle = 0.585; // 0.5; + public static final double L3_Angle = 0.56; // 0.45; + public static final double Source_Angle = 0.340; // 0.28; + public static final double Rest_Angle = 0.25; // 0.188; + public static final double Forward_Angle = 0.65; + public static final double Alage_Angle = 0.623; + public static final double Safe_Angle = 0.47; // Motor configuration public static final int WRIST_MOTOR_ID = 19; // Updated to correct CAN ID - public static final double WRIST_GEAR_RATIO = 100.0; // Adjust based on your gear reduction - - // PID Constants - public static final double kP = 0.1; - public static final double kI = 0; - public static final double kD = 0; - - // Motion constraints - public static final double MAX_VELOCITY = 100; // deg/s - public static final double MAX_ACCELERATION = 200; // deg/s^2 - - // Soft limits - public static final double FORWARD_SOFT_LIMIT = 30; // degrees - public static final double REVERSE_SOFT_LIMIT = -5; // degrees } \ No newline at end of file diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 9a22610..694390b 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -24,14 +24,14 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(80).withKI(0.001).withKD(0.55) - .withKS(0.1).withKV(1.66).withKA(0) + .withKP(60).withKI(0.00).withKD(0.57) + .withKS(0.07).withKV(1.5).withKA(0.5) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKS(0).withKV(0.19); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -51,19 +51,21 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(50.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withMotorOutput(new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true) - ); + ) + .withMotorOutput(new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; @@ -74,7 +76,7 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(2); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java new file mode 100644 index 0000000..a9935b0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climb.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climb extends SubsystemBase { + private final SparkMax climbMotor; + private final SparkClosedLoopController closedLoopController; + private double targetPosition = 0; + private static final int MOTOR_ID = 10; + private static final double CLIMB_SPEED = 1; // 30% speed + private static final double ADDITIONAL_ROTATIONS = 330; + + public Climb() { + climbMotor = new SparkMax(MOTOR_ID, MotorType.kBrushless); + closedLoopController = climbMotor.getClosedLoopController(); + + SparkMaxConfig config = new SparkMaxConfig(); + config.inverted(false); + config.idleMode(IdleMode.kBrake) + .smartCurrentLimit(40) + .voltageCompensation(12); + + config.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .p(5) + .d(0) + .i(0) + .outputRange(-0, 1) + .maxMotion + .maxVelocity(5000) + .maxAcceleration(2000) + .allowedClosedLoopError(0.1); + + climbMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public Command runClimb() { + return this.startEnd( + // When command starts + () -> climbMotor.set(CLIMB_SPEED), + // When command ends + () -> { + climbMotor.set(0); + + } + ); + } + + public Command reverseClimb() { + return this.startEnd( + // When command starts + () -> climbMotor.set(-CLIMB_SPEED), + // When command ends + () -> { + climbMotor.set(0); + + } + ); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("Climb Position", climbMotor.getEncoder().getPosition()); + SmartDashboard.putNumber("Climb Target", targetPosition); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..d7826fd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class Climber extends SubsystemBase { + private final SparkMax climberMotor; + private static final int CLIMBER_MOTOR_ID = 17; // Adjust this ID as needed + private static final double CLIMBER_SPEED = 0.5; // 70% speed for intake + private static final double REVERSE_SPEED = -0.5; // 70% speed for outtake + + public Climber() { + climberMotor = new SparkMax(CLIMBER_MOTOR_ID, MotorType.kBrushed); + + // Configure the motor + SparkMaxConfig config = new SparkMaxConfig(); + + // Apply Configuration + climberMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public Command stopIntakeCommand() { + return this.runOnce( + () -> climberMotor.set(0) + ); + } + + public Command startClimberCommand(){ + return this.runOnce( + () -> climberMotor.set(CLIMBER_SPEED) + ); + } + + public Command reverseClimberCommand() { + return this.runOnce( + () -> climberMotor.set(REVERSE_SPEED) + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 0241673..13fe164 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -59,7 +59,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + Volts.of(2), // Reduce dynamic step voltage to 4 V to prevent brownout null, // Use default timeout (10 s) // Log state with SignalLogger class state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) @@ -211,9 +211,9 @@ private void configureAutoBuilder() { ), new PPHolonomicDriveController( // PID constants for translation - new PIDConstants(10, 0, 0), + new PIDConstants(5, 0.0, 0), // PID constants for rotation - new PIDConstants(7, 0, 0) + new PIDConstants(5, 0.0, 0.01) ), config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 9217309..5a6919c 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,90 +1,104 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.StrictFollower; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLimitSwitch; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.LimitSwitchConfig.Type; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.ElevatorConstants; +import frc.robot.constants.WristConstants; + public class Elevator extends SubsystemBase { - private final TalonFX leaderMotor; - private final TalonFX followerMotor; - private final MotionMagicVoltage motionMagicRequest; - private double targetPosition = 0; - private static final double METERS_PER_ROTATION = 0.1595; // π * 0.0508m (circumference of 2-inch sprocket) - - public Elevator() { - leaderMotor = new TalonFX(ElevatorConstants.LEADER_MOTOR_ID); - followerMotor = new TalonFX(ElevatorConstants.FOLLOWER_MOTOR_ID); - motionMagicRequest = new MotionMagicVoltage(0); - - configureMotors(); - } - - private void configureMotors() { - TalonFXConfiguration config = new TalonFXConfiguration(); - - // Configure gear ratio and mechanical conversion - FeedbackConfigs feedback = config.Feedback; - feedback.SensorToMechanismRatio = 5.0; // 5:1 gear reduction - - // Configure Motion Magic - MotionMagicConfigs mm = config.MotionMagic; - mm.withMotionMagicCruiseVelocity(39.27) - .withMotionMagicAcceleration(59.54) - .withMotionMagicJerk(100.08); - - // Configure PID values - Slot0Configs slot0 = config.Slot0; - slot0.kS = 0.25; - slot0.kV = 0.02; - slot0.kA = 0.01; - slot0.kP = 10; - slot0.kI = 0; - slot0.kD = 1.0; - - // Set to brake mode - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - // Configure leader motor - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - StatusCode status = StatusCode.StatusCodeNotInitialized; - for (int i = 0; i < 5; ++i) { - status = leaderMotor.getConfigurator().apply(config); - if (status.isOK()) break; - } - if (!status.isOK()) { - System.out.println("Could not configure leader motor. Error: " + status.toString()); - } + private final SparkMax elevatorMotor; + private final SparkMax elevatorFollowerMotor; + private final SparkClosedLoopController closedLoopController; + private double targetLocation = 0; + private final Wrist wrist; + + public Elevator(Wrist wrist) { + this.wrist = wrist; + elevatorMotor = new SparkMax(ElevatorConstants.ELEVATOR_MOTOR_ID, MotorType.kBrushless); + elevatorFollowerMotor = new SparkMax(ElevatorConstants.ELEVATOR_FOLLOWER_MOTOR_ID, MotorType.kBrushless); + + closedLoopController = elevatorMotor.getClosedLoopController(); + + // Configure the main elevator motor + SparkMaxConfig config = new SparkMaxConfig(); + config.inverted(true); // Main motor is inverted + config.idleMode(IdleMode.kBrake) + .smartCurrentLimit(40) + .voltageCompensation(12); + + config.limitSwitch + .reverseLimitSwitchEnabled(true) + .reverseLimitSwitchType(Type.kNormallyOpen) + .forwardLimitSwitchEnabled(true) + .forwardLimitSwitchType(Type.kNormallyOpen); + + config.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .p(0.6) + // .d(0.001) + .i(0.002) + + .maxMotion + .maxVelocity(6000) + .maxAcceleration(5000) + .allowedClosedLoopError(0.5); + + elevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + // Configure the follower motor + SparkMaxConfig followerConfig = new SparkMaxConfig(); + followerConfig.inverted(false); + followerConfig.follow(15, true); // Changed to false since main motor is inverted + followerConfig.idleMode(IdleMode.kCoast) + .smartCurrentLimit(40) + .voltageCompensation(12); + elevatorFollowerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + followerConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .p(0.6) + .i(0.002) + // .d(0.001) + + .outputRange(-1, 1) + .maxMotion + .maxVelocity(6000) + .maxAcceleration(5000) + .allowedClosedLoopError(0.5); + + // Set follower motor to follow the main motor in opposite direction + elevatorFollowerMotor.isFollower(); // true inverts the following direction + + zeroEncoder(); + } - // Configure follower motor - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - status = StatusCode.StatusCodeNotInitialized; - for (int i = 0; i < 5; ++i) { - status = followerMotor.getConfigurator().apply(config); - if (status.isOK()) break; - } - if (!status.isOK()) { - System.out.println("Could not configure follower motor. Error: " + status.toString()); - } + public Command setUp() { + return this.run(() -> elevatorMotor.set(0.2)); + } + + public Command setDown() { + return this.run(() -> elevatorMotor.set(-0.2)); + } - // Set follower to follow leader - followerMotor.setControl(new StrictFollower(ElevatorConstants.LEADER_MOTOR_ID)); + public void setTargetLocation(double targetLocation) { + this.targetLocation = targetLocation; } - public void setTargetPosition(double positionMeters) { - targetPosition = positionMeters; - leaderMotor.setControl(motionMagicRequest.withPosition(positionMeters / METERS_PER_ROTATION).withSlot(0)); + public void zeroEncoder() { + elevatorMotor.getEncoder().setPosition(0); } // Command wrappers for preset positions @@ -92,6 +106,13 @@ public Command goToL4Command() { return this.runOnce(() -> setTargetPosition(ElevatorConstants.L4_Distance)); } + public Command goToAlgaeL3Command() { + return this.runOnce(() -> setTargetLocation(ElevatorConstants.AlgaeL3_Distance)); + } + public Command goToAlgaeL2Command() { + return this.runOnce(() -> setTargetLocation(ElevatorConstants.AlgaeL2_Distance)); + } + public Command goToL3Command() { return this.runOnce(() -> setTargetPosition(ElevatorConstants.L3_Distance)); } @@ -108,22 +129,60 @@ public Command goToRestCommand() { return this.runOnce(() -> setTargetPosition(ElevatorConstants.resetPos)); } + public void elevatorMoveToDesired(){ + closedLoopController.setReference(this.targetLocation, ControlType.kMAXMotionPositionControl); + } + + public void elevatorMoveToL2(){ + closedLoopController.setReference(ElevatorConstants.L2_Distance, ControlType.kMAXMotionPositionControl); + } + + /* Motion Planning Logic: */ @Override public void periodic() { - updateDashboard(); + if (getPosition() <= ElevatorConstants.L2_Distance && targetLocation < ElevatorConstants.L2_Distance) { + wrist.resume(); + elevatorMoveToDesired(); + } + else if (getPosition() <= ElevatorConstants.L2_Distance && targetLocation >= ElevatorConstants.L2_Distance) { + if (wrist.isAtTarget()||wrist.getCurrentAngle()>WristConstants.Safe_Angle) { + elevatorMoveToDesired(); + } else { + wrist.resume(); + elevatorMoveToL2(); + } + } + else if(getPosition() > ElevatorConstants.L2_Distance && targetLocation <= ElevatorConstants.L2_Distance){ + elevatorMoveToDesired(); + wrist.holdLastTarget(); + + if(getPosition() <= ElevatorConstants.L2_Distance){ + wrist.resume(); + + } + } + + else if(getPosition() > ElevatorConstants.L2_Distance && targetLocation > ElevatorConstants.L2_Distance){ + wrist.resume(); + elevatorMoveToDesired(); + } } + private void updateDashboard() { SmartDashboard.putNumber("Elevator Position (m)", getPositionMeters()); SmartDashboard.putNumber("Elevator Target (m)", targetPosition); SmartDashboard.putNumber("Elevator Velocity (m/s)", getVelocityMetersPerSecond()); } - public double getPositionMeters() { - return leaderMotor.getRotorPosition().getValueAsDouble() * METERS_PER_ROTATION; + public void printDashboard() { + SmartDashboard.putNumber("Elevator/ Position", elevatorMotor.getEncoder().getPosition()); + SmartDashboard.putNumber("Elevator/ Target", targetLocation); + SmartDashboard.putNumber("Elevator/Follower Current", elevatorFollowerMotor.getOutputCurrent()); + SmartDashboard.putNumber("Elevator/Follower Temperature", elevatorFollowerMotor.getMotorTemperature()); } - public double getVelocityMetersPerSecond() { - return leaderMotor.getRotorVelocity().getValueAsDouble() * METERS_PER_ROTATION; + public double getPosition() { + return elevatorMotor.getEncoder().getPosition(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 71d1d16..81dfa85 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,37 +1,45 @@ package frc.robot.subsystems; - -import frc.robot.Robot; -import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { private final SparkMax intakeMotor; private static final int INTAKE_MOTOR_ID = 17; // Adjust this ID as needed - private static final double INTAKE_SPEED = 0.9; // 70% speed for intake - private static final double REVERSE_SPEED = -0.9; // 70% speed for outtake + private static final double INTAKE_SPEED = 0.85; // 70% speed for intake + private static final double REVERSE_SPEED = -1; // 70% speed for outtake + private static final double STALL_SPEED = 0.1; + private static final double SLOW_REVERSE_SPEED = -0.65; + public static SparkLimitSwitch limitSwitch; + public SparkMaxConfig config; public Intake() { intakeMotor = new SparkMax(INTAKE_MOTOR_ID, MotorType.kBrushed); // Configure the motor - SparkMaxConfig config = new SparkMaxConfig(); + limitSwitch = intakeMotor.getForwardLimitSwitch(); + config = new SparkMaxConfig(); + config.limitSwitch.forwardLimitSwitchEnabled(false); + + intakeMotor.configure(config, null, null); + } public Command startIntakeCommand() { - return this.startEnd( - // When the command starts, run the intake - () -> intakeMotor.set(INTAKE_SPEED), - // When the command ends, stop the intake - () -> intakeMotor.set(0) + return this.runOnce( + () -> intakeMotor.set(INTAKE_SPEED) ); } + public Command stallIntakeCommand() { + return this.runOnce( + () -> intakeMotor.set(STALL_SPEED)); + + } public Command reverseIntakeCommand() { return this.runOnce( @@ -39,6 +47,12 @@ public Command reverseIntakeCommand() { ); } + public Command slowReverseIntakeCommand() { + return this.runOnce( + () -> intakeMotor.set(SLOW_REVERSE_SPEED) + ); + } + public Command stopIntakeCommand() { return this.runOnce( () -> intakeMotor.set(0) @@ -47,6 +61,7 @@ public Command stopIntakeCommand() { @Override public void periodic() { + SmartDashboard.putNumber("Intake/ Speed", intakeMotor.get()); // This method will be called once per scheduler run } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index f7ad45e..bd63779 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -17,6 +17,8 @@ public Vision(CommandSwerveDrivetrain drivetrain) { @Override public void periodic() { updatePoseEstimates(); + + SmartDashboard.putString("Left Limelight", LimelightHelpers.getLimelightNTString(getSubsystem(), getName())); } private void updatePoseEstimates() { diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index 7ddd941..c2956a3 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -4,13 +4,13 @@ import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkLimitSwitch; -import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.AbsoluteEncoderConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkAbsoluteEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -20,37 +20,65 @@ public class Wrist extends SubsystemBase { private final SparkMax wristMotor; private final SparkClosedLoopController closedLoopController; + private final SparkAbsoluteEncoder absoluteEncoder; private double targetLocation = 0; private double desiredLocation = 0; + private boolean isInSafePosition = false; + + private double kP = 0.5; + private double kI = 0.005; + private double kD = 1.2; + + // private double kP = 0.28; + // private double kI = 0.265; + // private double kD = 0.27; + + SparkMaxConfig config; + AbsoluteEncoderConfig absoluteEncoderConfig; + public Wrist() { wristMotor = new SparkMax(WristConstants.WRIST_MOTOR_ID, MotorType.kBrushless); closedLoopController = wristMotor.getClosedLoopController(); - - SparkMaxConfig config = new SparkMaxConfig(); - config.inverted(false); - config.idleMode(IdleMode.kCoast) - .smartCurrentLimit(50) + absoluteEncoder = wristMotor.getAbsoluteEncoder(); + absoluteEncoderConfig = new AbsoluteEncoderConfig(); + absoluteEncoderConfig.inverted(true); + + + this.config = new SparkMaxConfig(); + config.inverted(false) + .idleMode + (IdleMode.kBrake) + .smartCurrentLimit(40) .voltageCompensation(12); - + config.softLimit.forwardSoftLimitEnabled(true); + config.softLimit.reverseSoftLimitEnabled(true); + config.softLimit.forwardSoftLimit( 0.669); + config.softLimit.reverseSoftLimit(0.223); + config.apply(absoluteEncoderConfig); config.closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .p(0.1) - .d(0.08) - .i(0.00006) - .iZone(0.5) - .outputRange(-0.5, 0.5) + + .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) + .p(kP) + .i(kI) + // .d(kD) + .iZone(0.07) + .outputRange(-0.2,0.2) .maxMotion - .maxVelocity(4200) - .maxAcceleration(4000) - .allowedClosedLoopError(.25); + .maxVelocity(700) + .maxAcceleration(500) + .allowedClosedLoopError(.025); + wristMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - zeroEncoder(); + + + // closedLoopController.setReference(absoluteEncoder.getPosition(), ControlType.kMAXMotionPositionControl); + this.desiredLocation = 0.25; } public void setTargetLocation(double targetLocation) { - this.desiredLocation = targetLocation; + this.desiredLocation = Math.min(Math.max(targetLocation, 0.223), 0.669); } public void resume() { @@ -58,11 +86,10 @@ public void resume() { closedLoopController.setReference(this.targetLocation, ControlType.kMAXMotionPositionControl); } - private void zeroEncoder() { - wristMotor.getEncoder().setPosition(0); + public void holdLastTarget(){ + closedLoopController.setReference(this.targetLocation, ControlType.kMAXMotionPositionControl); } - // Command wrappers for the preset positions public Command goToL4Command() { return this.runOnce(() -> setTargetLocation(WristConstants.L4_Angle)); } @@ -79,13 +106,39 @@ public Command goToRestCommand() { return this.runOnce(() -> setTargetLocation(WristConstants.Rest_Angle)); } + public Command goForwardCommand() { + return this.runOnce(() -> setTargetLocation(WristConstants.Forward_Angle)); + } + + public Command goToAlage() { + return this.runOnce(() -> setTargetLocation(WristConstants.Alage_Angle)); + } + + public boolean isAtTarget() { + return Math.abs(absoluteEncoder.getPosition() - desiredLocation) < 0.025; + } + @Override public void periodic() { - resume(); - SmartDashboard.putNumber("Wrist Encoder Reading", wristMotor.getEncoder().getPosition()); - SmartDashboard.putNumber("Wrist Target Location", targetLocation); + SmartDashboard.putNumber("Wrist/P Gain", kP); + SmartDashboard.putNumber("Wrist/I Gain", kI); + SmartDashboard.putNumber("Wrist/D Gain", kD); + SmartDashboard.putNumber("Wrist Error", Math.abs(targetLocation - absoluteEncoder.getPosition())); + SmartDashboard.putNumber("Wrist/Absolute Position", absoluteEncoder.getPosition()); + SmartDashboard.putNumber("Wrist/Absolute Velocity", absoluteEncoder.getVelocity()); + SmartDashboard.putNumber("Wrist/Target Location", targetLocation); + SmartDashboard.putNumber("Wrist/Desired Location", desiredLocation); + SmartDashboard.putBoolean("Wrist/Is Safe Position", isInSafePosition); + SmartDashboard.putNumber("Wrist I Accumlated", wristMotor.getClosedLoopController().getIAccum()); + SmartDashboard.putNumber("Wrist/Motor Output", wristMotor.getAppliedOutput()); + SmartDashboard.putNumber("Wrist/Motor Current", wristMotor.getOutputCurrent()); } public double getCurrentAngle() { - return wristMotor.getEncoder().getPosition(); - }} \ No newline at end of file + return absoluteEncoder.getPosition(); + } + + public boolean isAtSafeAngle() { + return isInSafePosition; + } +}