diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 75c81966..365b17ba 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -8,16 +8,16 @@ "Starting from Mid Field Notes" ], "autoFolders": [ + "0.", "1. A", "1. C", - "2. A+A", "2. A+C", "3. A+A+C", - "0." + "Hajel" ], - "defaultMaxVel": 1.5, - "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 30.0, - "defaultMaxAngAccel": 30.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 2.0, + "defaultMaxAngVel": 50.0, + "defaultMaxAngAccel": 100.0, "maxModuleSpeed": 4.0 } \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index f4b6d226..43fec4e1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,7 +1,12 @@ { - "System Joysticks": { + "Keyboard 0 Settings": { "window": { - "visible": false + "visible": true + } + }, + "Keyboard 1 Settings": { + "window": { + "visible": true } }, "keyboardJoysticks": [ @@ -23,24 +28,23 @@ } ], "axisCount": 3, - "buttonCount": 4, + "buttonCount": 14, "buttonKeys": [ - 90, - 88, - 67, - 86 - ], - "povConfig": [ - { - "key0": 328, - "key135": 323, - "key180": 322, - "key225": 321, - "key270": 324, - "key315": 327, - "key45": 329, - "key90": 326 - } + 321, + 322, + 323, + 324, + -1, + 326, + 327, + 328, + 329, + 320, + -1, + -1, + -1, + -1, + -1 ], "povCount": 1 }, @@ -56,12 +60,21 @@ } ], "axisCount": 2, - "buttonCount": 4, + "buttonCount": 13, "buttonKeys": [ - 77, - 44, - 46, - 47 + -1, + -1, + -1, + -1, + -1, + -1, + -1, + -1, + 325, + -1, + -1, + -1, + -1 ], "povCount": 0 }, @@ -96,8 +109,10 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" } ] } diff --git a/simgui.json b/simgui.json index aff4524f..5a131bdc 100644 --- a/simgui.json +++ b/simgui.json @@ -1,4 +1,11 @@ { + "HALProvider": { + "Other Devices": { + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -18,7 +25,9 @@ "/Shuffleboard/GameTab/Field": "Field2d", "/Shuffleboard/GameTab/Filed": "Field2d", "/Shuffleboard/GameTab/Increase Intake Offset": "Command", + "/Shuffleboard/GameTab/Intake Reject Note": "Command", "/Shuffleboard/GameTab/IntakeRejectNote": "Command", + "/Shuffleboard/GameTab/RESET TELEOP DRIVE": "Command", "/Shuffleboard/GameTab/Reset Arm": "Command", "/Shuffleboard/GameTab/Reset Intake": "Command", "/Shuffleboard/GameTab/Retract Reaction Bar": "Command", @@ -29,6 +38,11 @@ "/SmartDashboard/Pigeon 2 (v6) [30]": "Gyro" }, "windows": { + "/Shuffleboard/GameTab/Autonomous Chooser": { + "window": { + "visible": true + } + }, "/Shuffleboard/GameTab/Field": { "bottom": 1476, "height": 8.210550308227539, @@ -55,5 +69,8 @@ }, "NetworkTables Info": { "visible": true + }, + "NetworkTables View": { + "visible": false } } diff --git a/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto index 7fd82e82..ef587e51 100644 --- a/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S1-Leave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, - "rotation": -121.0 + "rotation": -120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto index 142e3255..44803b72 100644 --- a/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S1-noLeave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, - "rotation": -121.0 + "rotation": -120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto b/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto index a06ca138..fdd5e5e2 100644 --- a/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto +++ b/src/main/deploy/pathplanner/autos/0N-S1-noLeave.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7518994781213513, - "y": 6.686887667641241 + "x": 0.72, + "y": 6.67 }, "rotation": 60.0 }, diff --git a/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto index 67effdcf..4ba02f01 100644 --- a/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S2-noLeave (underhand).auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.35, + "x": 1.33, "y": 5.54 }, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto index a1518376..38adcdcf 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-Leave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": 121.0 + "rotation": 120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto b/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto index e9383068..f5b048a9 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-Leave.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, "rotation": -60.0 diff --git a/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto b/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto index 8b0fe6a2..88dbd6a0 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-noLeave (underhand).auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": 121.0 + "rotation": 120.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto b/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto index 248e2c44..ec3d7fd4 100644 --- a/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto +++ b/src/main/deploy/pathplanner/autos/0N-S3-noLeave.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7167669684840067, - "y": 4.391563704668065 + "x": 0.72, + "y": 4.44 }, "rotation": -60.0 }, diff --git a/src/main/deploy/pathplanner/autos/1N-S1-A1.auto b/src/main/deploy/pathplanner/autos/1N-S1-A1.auto index c29ce8ae..44767520 100644 --- a/src/main/deploy/pathplanner/autos/1N-S1-A1.auto +++ b/src/main/deploy/pathplanner/autos/1N-S1-A1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, - "rotation": 59.0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/1N-S3-C4.auto b/src/main/deploy/pathplanner/autos/1N-S3-C4.auto index 67840707..56cf944b 100644 --- a/src/main/deploy/pathplanner/autos/1N-S3-C4.auto +++ b/src/main/deploy/pathplanner/autos/1N-S3-C4.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/1N-S3-C5.auto b/src/main/deploy/pathplanner/autos/1N-S3-C5.auto index 6e804570..e65a98ea 100644 --- a/src/main/deploy/pathplanner/autos/1N-S3-C5.auto +++ b/src/main/deploy/pathplanner/autos/1N-S3-C5.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto b/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto index e3a73dc0..855d5698 100644 --- a/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto +++ b/src/main/deploy/pathplanner/autos/1N-Speaker2-A2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3535296301361848, + "x": 1.33, "y": 5.545221859674489 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto b/src/main/deploy/pathplanner/autos/2N-S1-A1.auto deleted file mode 100644 index bdafd5ce..00000000 --- a/src/main/deploy/pathplanner/autos/2N-S1-A1.auto +++ /dev/null @@ -1,82 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.85, - "y": 6.62 - }, - "rotation": 59.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S1)" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S1-A1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (A1-A2) then Intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A1-A2" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A2)" - } - } - ] - } - }, - "folder": "2. A+A", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2N-S1-C1.auto b/src/main/deploy/pathplanner/autos/2N-S1-C1.auto index ea7ad11f..ce9e55e4 100644 --- a/src/main/deploy/pathplanner/autos/2N-S1-C1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S1-C1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, - "rotation": 59.0 + "rotation": 60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/2N-S1-C2.auto b/src/main/deploy/pathplanner/autos/2N-S1-C2.auto index 64e5e9a0..0b6acb16 100644 --- a/src/main/deploy/pathplanner/autos/2N-S1-C2.auto +++ b/src/main/deploy/pathplanner/autos/2N-S1-C2.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, "rotation": 59.0 }, diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A1.auto b/src/main/deploy/pathplanner/autos/2N-S2-A1.auto index 388c10e0..ace2e107 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-A1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-A1.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S2-A3.auto b/src/main/deploy/pathplanner/autos/2N-S2-A3.auto index 79962fd4..3f12da71 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-A3.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-A3.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S2-C1.auto b/src/main/deploy/pathplanner/autos/2N-S2-C1.auto index 5e319608..b51c94dc 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-C1.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-C1.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S2-C2.auto b/src/main/deploy/pathplanner/autos/2N-S2-C2.auto index fa410321..8986e511 100644 --- a/src/main/deploy/pathplanner/autos/2N-S2-C2.auto +++ b/src/main/deploy/pathplanner/autos/2N-S2-C2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 diff --git a/src/main/deploy/pathplanner/autos/2N-S3-A3.auto b/src/main/deploy/pathplanner/autos/2N-S3-A3.auto deleted file mode 100644 index 01056d2a..00000000 --- a/src/main/deploy/pathplanner/autos/2N-S3-A3.auto +++ /dev/null @@ -1,82 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8, - "y": 4.44 - }, - "rotation": -59.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S3)" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S3-A3" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (A3-A2) then Intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "path", - "data": { - "pathName": "A3-A2" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Speaker (A2)" - } - } - ] - } - }, - "folder": "2. A+A", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2N-S3-C4.auto b/src/main/deploy/pathplanner/autos/2N-S3-C4.auto index abcf083d..4f76687e 100644 --- a/src/main/deploy/pathplanner/autos/2N-S3-C4.auto +++ b/src/main/deploy/pathplanner/autos/2N-S3-C4.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/2N-S3-C5.auto b/src/main/deploy/pathplanner/autos/2N-S3-C5.auto index fd3436b5..fa4fdf33 100644 --- a/src/main/deploy/pathplanner/autos/2N-S3-C5.auto +++ b/src/main/deploy/pathplanner/autos/2N-S3-C5.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, + "x": 0.72, "y": 4.44 }, - "rotation": -59.0 + "rotation": -60.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/3N-S1-C1.auto b/src/main/deploy/pathplanner/autos/3N-S1-C1.auto index f3af664f..d0133c37 100644 --- a/src/main/deploy/pathplanner/autos/3N-S1-C1.auto +++ b/src/main/deploy/pathplanner/autos/3N-S1-C1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 0.72, + "y": 6.67 }, - "rotation": 59.0 + "rotation": 60.0 }, "command": { "type": "sequential", @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A1-A2) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S1-C2.auto b/src/main/deploy/pathplanner/autos/3N-S1-C2.auto index cebd19df..0247fc0d 100644 --- a/src/main/deploy/pathplanner/autos/3N-S1-C2.auto +++ b/src/main/deploy/pathplanner/autos/3N-S1-C2.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A1-A2) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S2-C1.auto b/src/main/deploy/pathplanner/autos/3N-S2-C1.auto index a6ffe5a6..063f1cbe 100644 --- a/src/main/deploy/pathplanner/autos/3N-S2-C1.auto +++ b/src/main/deploy/pathplanner/autos/3N-S2-C1.auto @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A2-A1) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S2-C2.auto b/src/main/deploy/pathplanner/autos/3N-S2-C2.auto index c161a855..0640d011 100644 --- a/src/main/deploy/pathplanner/autos/3N-S2-C2.auto +++ b/src/main/deploy/pathplanner/autos/3N-S2-C2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0.0 @@ -43,7 +43,7 @@ { "type": "named", "data": { - "name": "Speaker (A2-A1) then Intake" + "name": null } }, { diff --git a/src/main/deploy/pathplanner/autos/3N-S3-allA.auto b/src/main/deploy/pathplanner/autos/Copy of Copy of Hajel 3.auto similarity index 70% rename from src/main/deploy/pathplanner/autos/3N-S3-allA.auto rename to src/main/deploy/pathplanner/autos/Copy of Copy of Hajel 3.auto index a7c3fcd8..020b5077 100644 --- a/src/main/deploy/pathplanner/autos/3N-S3-allA.auto +++ b/src/main/deploy/pathplanner/autos/Copy of Copy of Hajel 3.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, - "y": 4.44 + "x": 1.33, + "y": 5.54 }, - "rotation": -59.0 + "rotation": 0 }, "command": { "type": "sequential", @@ -18,7 +18,7 @@ { "type": "named", "data": { - "name": "Speaker (S3) then Intake" + "name": "Speaker (S2) then Intake" } }, { @@ -28,13 +28,13 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 2.75 } }, { "type": "path", "data": { - "pathName": "S3-A3" + "pathName": "S2-A2" } } ] @@ -43,14 +43,26 @@ ] } }, + { + "type": "path", + "data": { + "pathName": "A2-S2" + } + }, + { + "type": "named", + "data": { + "name": "Speaker (S2)" + } + }, { "type": "parallel", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Speaker (A3-A2) then Intake" + "pathName": "S2-A3" } }, { @@ -60,13 +72,13 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 0.65 } }, { - "type": "path", + "type": "named", "data": { - "pathName": "A3-A2" + "name": "Intake" } } ] @@ -75,14 +87,20 @@ ] } }, + { + "type": "named", + "data": { + "name": "Speaker (A3)" + } + }, { "type": "parallel", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Speaker (A2-A1) then Intake" + "pathName": "A3-A1" } }, { @@ -92,13 +110,13 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 1.2 } }, { - "type": "path", + "type": "named", "data": { - "pathName": "A2-A1" + "name": "Intake" } } ] @@ -116,6 +134,6 @@ ] } }, - "folder": null, + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3N-S1-allA.auto b/src/main/deploy/pathplanner/autos/Copy of Hajel middle bottom 2.auto similarity index 68% rename from src/main/deploy/pathplanner/autos/3N-S1-allA.auto rename to src/main/deploy/pathplanner/autos/Copy of Hajel middle bottom 2.auto index 20c75cc7..d4af2fbf 100644 --- a/src/main/deploy/pathplanner/autos/3N-S1-allA.auto +++ b/src/main/deploy/pathplanner/autos/Copy of Hajel middle bottom 2.auto @@ -2,40 +2,15 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.85, - "y": 6.62 + "x": 1.33, + "y": 5.54 }, - "rotation": 59.0 + "rotation": 0 }, "command": { "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Speaker (S1)" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S1-A1" - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - }, { "type": "parallel", "data": { @@ -43,7 +18,7 @@ { "type": "named", "data": { - "name": "Speaker (A1-A2) then Intake" + "name": "Speaker (S2) then Intake" } }, { @@ -53,13 +28,13 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 2.75 } }, { "type": "path", "data": { - "pathName": "A1-A2" + "pathName": "S2-A2" } } ] @@ -68,14 +43,26 @@ ] } }, + { + "type": "path", + "data": { + "pathName": "A2-S2" + } + }, + { + "type": "named", + "data": { + "name": "Speaker (S2)" + } + }, { "type": "parallel", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Speaker (A2-A3) then Intake" + "pathName": "S2-A3" } }, { @@ -85,13 +72,13 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 0.65 } }, { - "type": "path", + "type": "named", "data": { - "pathName": "A2-A3" + "name": "Intake" } } ] @@ -109,6 +96,6 @@ ] } }, - "folder": null, + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel all 3.auto b/src/main/deploy/pathplanner/autos/Hajel all 3.auto index eb381c6d..43dc08f3 100644 --- a/src/main/deploy/pathplanner/autos/Hajel all 3.auto +++ b/src/main/deploy/pathplanner/autos/Hajel all 3.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.35, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -152,6 +152,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel center.auto b/src/main/deploy/pathplanner/autos/Hajel center.auto index d6a84759..0032aecf 100644 --- a/src/main/deploy/pathplanner/autos/Hajel center.auto +++ b/src/main/deploy/pathplanner/autos/Hajel center.auto @@ -2,17 +2,36 @@ "version": 1.0, "startingPose": { "position": { - "x": 2.0400914981573175, - "y": 5.632912378520905 + "x": 0.72, + "y": 4.44 }, - "rotation": 0 + "rotation": -60.0 }, "command": { "type": "sequential", "data": { - "commands": [] + "commands": [ + { + "type": "named", + "data": { + "name": "Speaker (S3)" + } + }, + { + "type": "path", + "data": { + "pathName": "go to center" + } + }, + { + "type": "path", + "data": { + "pathName": "center" + } + } + ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel middle 1.auto b/src/main/deploy/pathplanner/autos/Hajel middle 1.auto index 04f3d20b..8e28c562 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle 1.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle 1.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -58,6 +58,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto b/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto index 6cc51b2d..82f8449a 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle bottom 2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -60,15 +60,28 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Intake" + "pathName": "S2-A3" } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "S2-A3" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.65 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } ] @@ -89,6 +102,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto b/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto index aa4d5093..cb1259b8 100644 --- a/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto +++ b/src/main/deploy/pathplanner/autos/Hajel middle top 2.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.37, + "x": 1.33, "y": 5.54 }, "rotation": 0 @@ -60,15 +60,28 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Intake" + "pathName": "S2-A1" } }, { - "type": "path", + "type": "sequential", "data": { - "pathName": "S2-A1" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.65 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } ] @@ -89,6 +102,6 @@ ] } }, - "folder": "Hajel Path", + "folder": "Hajel", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto deleted file mode 100644 index 07d4e332..00000000 --- a/src/main/deploy/pathplanner/autos/Test Auto.auto +++ /dev/null @@ -1,18 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2, - "y": 2 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1-A2.path b/src/main/deploy/pathplanner/paths/A1-A2.path index 6b715bc5..0aff71f7 100644 --- a/src/main/deploy/pathplanner/paths/A1-A2.path +++ b/src/main/deploy/pathplanner/paths/A1-A2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 1.6067905459634015, - "y": 6.710382806138379 + "x": 1.7312740565740923, + "y": 6.717535493054309 }, "isLocked": false, "linkedName": "A1" @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A1-C1.path b/src/main/deploy/pathplanner/paths/A1-C1.path index 10a2c01c..546379a3 100644 --- a/src/main/deploy/pathplanner/paths/A1-C1.path +++ b/src/main/deploy/pathplanner/paths/A1-C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 + "x": 2.7607658350837374, + "y": 6.75715268691593 }, "isLocked": false, "linkedName": "A1" @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-C2.path b/src/main/deploy/pathplanner/paths/A1-C2.path index 4a34aadb..49d23665 100644 --- a/src/main/deploy/pathplanner/paths/A1-C2.path +++ b/src/main/deploy/pathplanner/paths/A1-C2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 4.0362823244730475, - "y": 6.75 + "x": 4.160765835083739, + "y": 6.75715268691593 }, "isLocked": false, "linkedName": "A1" @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-C3.path b/src/main/deploy/pathplanner/paths/A1-C3.path index 87fb46f6..77f5eabc 100644 --- a/src/main/deploy/pathplanner/paths/A1-C3.path +++ b/src/main/deploy/pathplanner/paths/A1-C3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 + "x": 2.7607658350837374, + "y": 6.75715268691593 }, "isLocked": false, "linkedName": "A1" }, { "anchor": { - "x": 7.671662009990687, - "y": 4.118900460023142 + "x": 8.063050741486098, + "y": 4.499325921728767 }, "prevControl": { - "x": 6.2771340927297254, - "y": 7.380473222484906 + "x": 6.668522824225136, + "y": 7.760898684190531 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-S1.path b/src/main/deploy/pathplanner/paths/A1-S1.path index a84dfc92..aa7aa117 100644 --- a/src/main/deploy/pathplanner/paths/A1-S1.path +++ b/src/main/deploy/pathplanner/paths/A1-S1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": null, "nextControl": { - "x": 2.5220300737463233, - "y": 6.735747749273277 + "x": 2.646513584357014, + "y": 6.742900436189207 }, "isLocked": false, "linkedName": "A1" }, { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": { - "x": 0.7679149380520984, - "y": 6.690000000006141 + "x": 0.7379149380520984, + "y": 6.670000000006141 }, "nextControl": null, "isLocked": false, @@ -32,20 +32,20 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 60.0, "rotateFast": false }, "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-S2.path b/src/main/deploy/pathplanner/paths/A1-S2.path index e4161c81..9ee6c62b 100644 --- a/src/main/deploy/pathplanner/paths/A1-S2.path +++ b/src/main/deploy/pathplanner/paths/A1-S2.path @@ -12,15 +12,15 @@ "y": 6.75 }, "isLocked": false, - "linkedName": "A1" + "linkedName": null }, { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": { - "x": 1.2699999999999998, + "x": 1.23, "y": 5.54412553269551 }, "nextControl": null, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A1-Speaker1.path b/src/main/deploy/pathplanner/paths/A1-Speaker1.path deleted file mode 100644 index db281f42..00000000 --- a/src/main/deploy/pathplanner/paths/A1-Speaker1.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.5362823244730466, - "y": 6.75 - }, - "prevControl": null, - "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 - }, - "isLocked": false, - "linkedName": "A1" - }, - { - "anchor": { - "x": 0.7713800042984719, - "y": 6.640452511674254 - }, - "prevControl": { - "x": 0.6713800042984719, - "y": 6.640452511674254 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Speaker 1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 60.75, - "rotateFast": false - }, - "reversed": false, - "folder": "Notes Near Starting Zone", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1-Speaker2.path b/src/main/deploy/pathplanner/paths/A1-Speaker2.path deleted file mode 100644 index b6eed265..00000000 --- a/src/main/deploy/pathplanner/paths/A1-Speaker2.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.5362823244730466, - "y": 6.75 - }, - "prevControl": null, - "nextControl": { - "x": 2.6362823244730467, - "y": 6.75 - }, - "isLocked": false, - "linkedName": "A1" - }, - { - "anchor": { - "x": 1.3535296301361848, - "y": 5.545221859674489 - }, - "prevControl": { - "x": 1.253529630136185, - "y": 5.545221859674489 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Speaker 2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Notes Near Starting Zone", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A2-A1.path b/src/main/deploy/pathplanner/paths/A2-A1.path index be9b85a4..1244b1a0 100644 --- a/src/main/deploy/pathplanner/paths/A2-A1.path +++ b/src/main/deploy/pathplanner/paths/A2-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 1.7101812358942634, - "y": 6.424116753996703 + "x": 1.8346647465049541, + "y": 6.431269440912633 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-A3.path b/src/main/deploy/pathplanner/paths/A2-A3.path index 6ebe8d7e..b9dce19d 100644 --- a/src/main/deploy/pathplanner/paths/A2-A3.path +++ b/src/main/deploy/pathplanner/paths/A2-A3.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C1.path b/src/main/deploy/pathplanner/paths/A2-C1.path index f71bdb3f..a6d0760f 100644 --- a/src/main/deploy/pathplanner/paths/A2-C1.path +++ b/src/main/deploy/pathplanner/paths/A2-C1.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C2.path b/src/main/deploy/pathplanner/paths/A2-C2.path index 16b96f07..5fd2c034 100644 --- a/src/main/deploy/pathplanner/paths/A2-C2.path +++ b/src/main/deploy/pathplanner/paths/A2-C2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A2-C3.path b/src/main/deploy/pathplanner/paths/A2-C3.path index 08842494..8e920439 100644 --- a/src/main/deploy/pathplanner/paths/A2-C3.path +++ b/src/main/deploy/pathplanner/paths/A2-C3.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.950422248707747, - "y": 8.23929007943486 + "x": 6.277134092729725, + "y": 7.271936851566012 }, "isLocked": false, "linkedName": "A2" }, { "anchor": { - "x": 7.671662009990687, - "y": 4.118900460023142 + "x": 8.063050741486098, + "y": 4.499325921728767 }, "prevControl": { - "x": 7.571662009990687, - "y": 4.118900460023142 + "x": 7.244094488188977, + "y": 5.4564193743772105 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -60.101098161385586, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/A2-S1.path b/src/main/deploy/pathplanner/paths/A2-S1.path index 3c47c9d1..7fadf210 100644 --- a/src/main/deploy/pathplanner/paths/A2-S1.path +++ b/src/main/deploy/pathplanner/paths/A2-S1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": { - "x": 0.6499999999999999, - "y": 6.69 + "x": 0.6199999999999999, + "y": 6.67 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 60.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/A2-S2.path b/src/main/deploy/pathplanner/paths/A2-S2.path index 8355e8a0..2f1508f7 100644 --- a/src/main/deploy/pathplanner/paths/A2-S2.path +++ b/src/main/deploy/pathplanner/paths/A2-S2.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": { - "x": 1.27, + "x": 1.23, "y": 5.54412553269551 }, "nextControl": null, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-Speaker3.path b/src/main/deploy/pathplanner/paths/A3-A1.path similarity index 61% rename from src/main/deploy/pathplanner/paths/A3-Speaker3.path rename to src/main/deploy/pathplanner/paths/A3-A1.path index 01c43233..8086268a 100644 --- a/src/main/deploy/pathplanner/paths/A3-Speaker3.path +++ b/src/main/deploy/pathplanner/paths/A3-A1.path @@ -8,44 +8,44 @@ }, "prevControl": null, "nextControl": { - "x": 2.6, - "y": 4.3 + "x": 1.548236363234494, + "y": 4.801442983770418 }, "isLocked": false, "linkedName": "A3" }, { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 0.7, - "y": 4.440124264863915 + "x": 2.000623297127067, + "y": 6.366136931275289 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": "A1" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": -58.49573328079582, - "rotateFast": false + "rotation": 0.0, + "rotateFast": true }, "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": -35.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A3-A2.path b/src/main/deploy/pathplanner/paths/A3-A2.path index 28afbc2f..9a611791 100644 --- a/src/main/deploy/pathplanner/paths/A3-A2.path +++ b/src/main/deploy/pathplanner/paths/A3-A2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/A3-C5.path b/src/main/deploy/pathplanner/paths/A3-C5.path index ace413ba..220297ca 100644 --- a/src/main/deploy/pathplanner/paths/A3-C5.path +++ b/src/main/deploy/pathplanner/paths/A3-C5.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.872088097956508, - "y": 0.9251560871167382 + "x": 7.826244114026691, + "y": 0.779488482053888 }, "prevControl": { - "x": 7.772088097956508, - "y": 0.9251560871167378 + "x": 7.726244114026691, + "y": 0.7794884820538875 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": -37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A3-S2.path b/src/main/deploy/pathplanner/paths/A3-S2.path index eb4b1619..51521bd0 100644 --- a/src/main/deploy/pathplanner/paths/A3-S2.path +++ b/src/main/deploy/pathplanner/paths/A3-S2.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": { - "x": 1.2699999999999998, + "x": 1.23, "y": 5.54412553269551 }, "nextControl": null, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": -37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A3-S3.path b/src/main/deploy/pathplanner/paths/A3-S3.path index 298ee4e1..e9a696ac 100644 --- a/src/main/deploy/pathplanner/paths/A3-S3.path +++ b/src/main/deploy/pathplanner/paths/A3-S3.path @@ -16,36 +16,36 @@ }, { "anchor": { - "x": 1.25, - "y": 4.118900460023142 + "x": 0.72, + "y": 4.44 }, "prevControl": { - "x": 1.1499999999999997, - "y": 4.118900460023142 + "x": 0.6199999999999997, + "y": 4.44 }, "nextControl": null, "isLocked": false, - "linkedName": "S3" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": -55.0, + "rotation": -60.0, "rotateFast": false }, "reversed": false, "folder": "Notes Near Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": -37.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/C1-A1.path b/src/main/deploy/pathplanner/paths/C1-A1.path index b07be142..d775afb2 100644 --- a/src/main/deploy/pathplanner/paths/C1-A1.path +++ b/src/main/deploy/pathplanner/paths/C1-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.6347630997742675, - "y": 6.767364817766693 + "x": 2.759246610384958, + "y": 6.774517504682623 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-A2.path b/src/main/deploy/pathplanner/paths/C1-A2.path index 1c778d45..55d5b4c0 100644 --- a/src/main/deploy/pathplanner/paths/C1-A2.path +++ b/src/main/deploy/pathplanner/paths/C1-A2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C1-S2.path b/src/main/deploy/pathplanner/paths/C1-S2.path index eb86f035..1fd8d50f 100644 --- a/src/main/deploy/pathplanner/paths/C1-S2.path +++ b/src/main/deploy/pathplanner/paths/C1-S2.path @@ -25,17 +25,17 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 2" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C2-A1.path b/src/main/deploy/pathplanner/paths/C2-A1.path index 7b8c98aa..05f2554c 100644 --- a/src/main/deploy/pathplanner/paths/C2-A1.path +++ b/src/main/deploy/pathplanner/paths/C2-A1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.6347630997742675, - "y": 6.732635182233307 + "x": 2.759246610384958, + "y": 6.739787869149237 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting from Mid Field Notes", "previewStartingState": { - "rotation": 0, + "rotation": -18.565352613122048, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/C2-A2.path b/src/main/deploy/pathplanner/paths/C2-A2.path index 48a448c5..15328d68 100644 --- a/src/main/deploy/pathplanner/paths/C2-A2.path +++ b/src/main/deploy/pathplanner/paths/C2-A2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/C4-S3.path b/src/main/deploy/pathplanner/paths/C4-S3.path index 235d9441..00120eb0 100644 --- a/src/main/deploy/pathplanner/paths/C4-S3.path +++ b/src/main/deploy/pathplanner/paths/C4-S3.path @@ -16,30 +16,30 @@ }, { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": { - "x": 2.7228859892812496, - "y": 2.6633911139970734 + "x": 2.6396529577652563, + "y": 2.663266849133159 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": -59.0, + "rotation": -60.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C5-S3.path b/src/main/deploy/pathplanner/paths/C5-S3.path index f77b35a3..fa20dc2a 100644 --- a/src/main/deploy/pathplanner/paths/C5-S3.path +++ b/src/main/deploy/pathplanner/paths/C5-S3.path @@ -3,49 +3,49 @@ "waypoints": [ { "anchor": { - "x": 7.872088097956508, - "y": 0.9251560871167382 + "x": 7.826244114026691, + "y": 0.779488482053888 }, "prevControl": null, "nextControl": { - "x": 7.782137282098908, - "y": 0.9688464833904297 + "x": 7.736293298169091, + "y": 0.8231788783275794 }, "isLocked": false, "linkedName": "C5" }, { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.72, + "y": 4.44 }, "prevControl": { - "x": 2.1958983447210816, - "y": 2.7336561332717624 + "x": 2.1158983447210815, + "y": 2.733531868407848 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": "S3" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": -59.0, + "rotation": -60.0, "rotateFast": false }, "reversed": false, "folder": "Starting from Mid Field Notes", "previewStartingState": { - "rotation": -20.0, + "rotation": -2.726310993906272, "velocity": 0.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-A1.path b/src/main/deploy/pathplanner/paths/S1-A1.path index a1288074..77243f67 100644 --- a/src/main/deploy/pathplanner/paths/S1-A1.path +++ b/src/main/deploy/pathplanner/paths/S1-A1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": null, "nextControl": { - "x": 0.8500000000000002, - "y": 6.69 + "x": 0.8200000000000002, + "y": 6.67 }, "isLocked": false, "linkedName": "S1" }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.155193024633961, - "y": 6.412405917450922 + "x": 2.2796765352446515, + "y": 6.419558604366852 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": 59.0, + "rotation": 60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-A2.path b/src/main/deploy/pathplanner/paths/S1-A2.path index 87c26895..348e5954 100644 --- a/src/main/deploy/pathplanner/paths/S1-A2.path +++ b/src/main/deploy/pathplanner/paths/S1-A2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": null, "nextControl": { - "x": 0.8500000000000001, - "y": 6.69 + "x": 0.8200000000000001, + "y": 6.67 }, "isLocked": false, "linkedName": "S1" @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-C1.path b/src/main/deploy/pathplanner/paths/S1-C1.path index 813f1686..464373bc 100644 --- a/src/main/deploy/pathplanner/paths/S1-C1.path +++ b/src/main/deploy/pathplanner/paths/S1-C1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.75, - "y": 6.69 + "x": 0.72, + "y": 6.67 }, "prevControl": null, "nextControl": { - "x": 0.8500000000000001, - "y": 6.69 + "x": 0.8200000000000001, + "y": 6.67 }, "isLocked": false, "linkedName": "S1" @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": 0, + "rotation": 60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S1-Leave.path b/src/main/deploy/pathplanner/paths/S1-Leave.path index 75969f47..2ab76907 100644 --- a/src/main/deploy/pathplanner/paths/S1-Leave.path +++ b/src/main/deploy/pathplanner/paths/S1-Leave.path @@ -12,7 +12,7 @@ "y": 7.187749496967226 }, "isLocked": false, - "linkedName": "Speaker 1" + "linkedName": null }, { "anchor": { @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A1.path b/src/main/deploy/pathplanner/paths/S2-A1.path index 3d620310..719ee9a0 100644 --- a/src/main/deploy/pathplanner/paths/S2-A1.path +++ b/src/main/deploy/pathplanner/paths/S2-A1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, - "y": 5.54412553269551 + "x": 2.3914165945307624, + "y": 5.703177397795595 }, "isLocked": false, "linkedName": "S2" }, { "anchor": { - "x": 2.5362823244730466, - "y": 6.75 + "x": 2.6607658350837373, + "y": 6.75715268691593 }, "prevControl": { - "x": 2.4362823244730465, - "y": 6.75 + "x": 1.6855724787889659, + "y": 6.2536022981555925 }, "nextControl": null, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A2.path b/src/main/deploy/pathplanner/paths/S2-A2.path index 53b97235..224864d8 100644 --- a/src/main/deploy/pathplanner/paths/S2-A2.path +++ b/src/main/deploy/pathplanner/paths/S2-A2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, + "x": 1.4300000000000002, "y": 5.54412553269551 }, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-A3.path b/src/main/deploy/pathplanner/paths/S2-A3.path index ba30eeb5..ed9d4a22 100644 --- a/src/main/deploy/pathplanner/paths/S2-A3.path +++ b/src/main/deploy/pathplanner/paths/S2-A3.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, + "x": 1.4300000000000002, "y": 5.54412553269551 }, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S2-C1.path b/src/main/deploy/pathplanner/paths/S2-C1.path index 3896dbbd..283d8796 100644 --- a/src/main/deploy/pathplanner/paths/S2-C1.path +++ b/src/main/deploy/pathplanner/paths/S2-C1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.37, + "x": 1.33, "y": 5.54412553269551 }, "prevControl": null, "nextControl": { - "x": 1.4700000000000002, + "x": 1.4300000000000002, "y": 5.54412553269551 }, "isLocked": false, @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/S3-A3.path b/src/main/deploy/pathplanner/paths/S3-A3.path index 71ae9221..a9ecf3a6 100644 --- a/src/main/deploy/pathplanner/paths/S3-A3.path +++ b/src/main/deploy/pathplanner/paths/S3-A3.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 1.4535688381373444, - "y": 4.731177964495728 + "x": 1.370335806621351, + "y": 4.731053699631814 }, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S3-C4.path b/src/main/deploy/pathplanner/paths/S3-C4.path index 786025a6..6e44ef9e 100644 --- a/src/main/deploy/pathplanner/paths/S3-C4.path +++ b/src/main/deploy/pathplanner/paths/S3-C4.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.72, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 2.378724925254083, - "y": 2.365588982247864 + "x": 2.298724925254083, + "y": 2.36546471738395 }, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S3-C5.path b/src/main/deploy/pathplanner/paths/S3-C5.path index 76f149a8..346d1d34 100644 --- a/src/main/deploy/pathplanner/paths/S3-C5.path +++ b/src/main/deploy/pathplanner/paths/S3-C5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 2.535512604548745, - "y": 2.241800998348939 + "x": 2.452279573032752, + "y": 2.2416767334850247 }, "isLocked": true, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { - "x": 7.872088097956508, - "y": 0.9251560871167382 + "x": 7.826244114026691, + "y": 0.779488482053888 }, "prevControl": { - "x": 7.300080185895035, - "y": 1.10373384732504 + "x": 7.254236201965218, + "y": 0.9580662422621897 }, "nextControl": null, "isLocked": false, @@ -32,20 +32,20 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, - "rotation": -20.0, + "rotation": -2.3532968661083107, "rotateFast": false }, "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S3-Leave.path b/src/main/deploy/pathplanner/paths/S3-Leave.path index c76a8461..09304e66 100644 --- a/src/main/deploy/pathplanner/paths/S3-Leave.path +++ b/src/main/deploy/pathplanner/paths/S3-Leave.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.8, - "y": 4.440124264863915 + "x": 0.7167669684840067, + "y": 4.44 }, "prevControl": null, "nextControl": { - "x": 0.7284778050297884, - "y": 0.9719994332998654 + "x": 0.6452447735137951, + "y": 0.971875168435951 }, "isLocked": false, - "linkedName": "Speaker 3" + "linkedName": null }, { "anchor": { @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -45,7 +45,7 @@ "reversed": false, "folder": "Starting Zone", "previewStartingState": { - "rotation": -59.0, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/A2-Speaker2.path b/src/main/deploy/pathplanner/paths/center.path similarity index 57% rename from src/main/deploy/pathplanner/paths/A2-Speaker2.path rename to src/main/deploy/pathplanner/paths/center.path index 340ac675..63189897 100644 --- a/src/main/deploy/pathplanner/paths/A2-Speaker2.path +++ b/src/main/deploy/pathplanner/paths/center.path @@ -3,39 +3,39 @@ "waypoints": [ { "anchor": { - "x": 2.45, - "y": 5.55 + "x": 8.340521559787769, + "y": 1.428722058585343 }, "prevControl": null, "nextControl": { - "x": 2.5500000000000003, - "y": 5.55 + "x": 8.340521559787769, + "y": 2.4287220585853433 }, "isLocked": false, - "linkedName": "A2" + "linkedName": null }, { "anchor": { - "x": 1.3535296301361848, - "y": 5.545221859674489 + "x": 8.340521559787769, + "y": 7.412959533479694 }, "prevControl": { - "x": 1.2535296301361847, - "y": 5.545221859674489 + "x": 8.340521559787769, + "y": 6.412959533479694 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 2" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -43,7 +43,7 @@ "rotateFast": false }, "reversed": false, - "folder": "Notes Near Starting Zone", + "folder": null, "previewStartingState": { "rotation": 0, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/A2-Speaker2(Amp).path b/src/main/deploy/pathplanner/paths/go to center.path similarity index 57% rename from src/main/deploy/pathplanner/paths/A2-Speaker2(Amp).path rename to src/main/deploy/pathplanner/paths/go to center.path index b34d46d1..61cff81a 100644 --- a/src/main/deploy/pathplanner/paths/A2-Speaker2(Amp).path +++ b/src/main/deploy/pathplanner/paths/go to center.path @@ -3,39 +3,39 @@ "waypoints": [ { "anchor": { - "x": 2.64, - "y": 6.11 + "x": 0.7167669684840067, + "y": 4.414985377759629 }, "prevControl": null, "nextControl": { - "x": 2.6586524036008736, - "y": 6.011754960227449 + "x": 3.503946066380006, + "y": 1.0656861256661172 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.3535296301361848, - "y": 5.545221859674489 + "x": 8.34, + "y": 1.43 }, "prevControl": { - "x": 1.2535296301361847, - "y": 5.545221859674489 + "x": 7.34, + "y": 1.4299999999999997 }, "nextControl": null, "isLocked": false, - "linkedName": "Speaker 2" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 30.0, - "maxAngularAcceleration": 30.0 + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0 }, "goalEndState": { "velocity": 0, @@ -43,9 +43,9 @@ "rotateFast": false }, "reversed": false, - "folder": "Notes Near Starting Zone", + "folder": null, "previewStartingState": { - "rotation": -63.75999999999999, + "rotation": -60.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/swerve/hajel_kraken/modules/backleft.json b/src/main/deploy/swerve/hajel_kraken/modules/backleft.json index 031ff679..beefc08e 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/backleft.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/backleft.json @@ -1,9 +1,9 @@ { "location": { - "front": -13.779, - "left": 13.779 + "front": -13, + "left": 13 }, - "absoluteEncoderOffset": 301.816404, + "absoluteEncoderOffset": 122.871, "drive": { "type": "talonfx", "id": 3, @@ -24,7 +24,7 @@ "canbus": "CAN_Network" }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderInverted": false diff --git a/src/main/deploy/swerve/hajel_kraken/modules/backright.json b/src/main/deploy/swerve/hajel_kraken/modules/backright.json index b720d798..5fe94c84 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/backright.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/backright.json @@ -1,9 +1,9 @@ { "location": { - "front": -13.779, - "left": -13.779 + "front": -13, + "left": -13 }, - "absoluteEncoderOffset": 228.691404, + "absoluteEncoderOffset": 229.746, "drive": { "type": "talonfx", "id": 1, diff --git a/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json b/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json index 1589d98f..5a466fff 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/frontleft.json @@ -1,9 +1,9 @@ { "location": { - "front": 13.779, - "left": 13.779 + "front": 13, + "left": 13 }, - "absoluteEncoderOffset": 281.33789, + "absoluteEncoderOffset": 103.008, "drive": { "type": "talonfx", "id": 5, @@ -24,7 +24,7 @@ "canbus": "CAN_Network" }, "inverted": { - "drive": false, + "drive": true, "angle": true }, "absoluteEncoderInverted": false diff --git a/src/main/deploy/swerve/hajel_kraken/modules/frontright.json b/src/main/deploy/swerve/hajel_kraken/modules/frontright.json index 3c71b12a..1e3ca78e 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/frontright.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/frontright.json @@ -1,9 +1,9 @@ { "location": { - "front": 13.779, - "left": -13.779 + "front": 13, + "left": -13 }, - "absoluteEncoderOffset": 7.1191404, + "absoluteEncoderOffset": 6.502, "drive": { "type": "talonfx", "id": 7, diff --git a/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json b/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json index 75d55289..cc3e36dd 100644 --- a/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json +++ b/src/main/deploy/swerve/hajel_kraken/modules/pidfproperties.json @@ -9,7 +9,7 @@ "angle": { "p": 50, "i": 0, - "d": 0.32, + "d": 0, "f": 0, "iz": 0 } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f01e898b..6154906a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -83,20 +83,22 @@ public static final class ClimbConstants { public static final int TOP_RIGHT_LIMIT_ID = 4; public static final int TOP_LEFT_LIMIT_ID = 5; + public static final int CLIMB_ARM_ANGLE = 255; + public static final int CLIMB_ARM_ARNGLE_FOR_SERVO = 330; } - + // hi this is a comment public static final class IntakeConstants { public static final int INTAKE_LOWER_INTAKE_ID = 55; public static final int INTAKE_SPIN_MOTOR_ID = 20; public static final int INTAKE_ENCODER_CHANNEL_ID = 9; public static final int INTAKE_NOTESENSOR_CHANNEL_ID = 8; - public static final double TOLERANCE = 4; // in degrees--- TO-DO: Tune Tolerance, intake is not lowering enough. - public static final double MAX_ARM_ANGLE = 85; //need to check max arm angle + public static final double TOLERANCE = 2; // in degrees--- TO-DO: Tune Tolerance, intake is not lowering enough. + public static final double MAX_ARM_ANGLE = 95; //need to check max arm angle public static final double ROTATION_TO_DEGREES = 360; public static final double ENCODER_RAW_TO_ROTATION = 8132.; - public static double ENCODER_OFFSET = 1; //need to check the encoder value - public static final double UP_POSITION = 79; + public static double ENCODER_OFFSET = 55.5; //need to check the encoder value + public static final double UP_POSITION = 95; public static final double SOURCE_INTAKE_ANGLE = 160; } @@ -116,13 +118,13 @@ public static final class LEDConstants public static final int ledPortNumber = 9; } public static final class DriveConstants { - public static final double kP = 0.04; - public static final double kI = 0.055; - public static final double kD = 0.000; + public static final double kP = 0.02; + public static final double kI = 0; + public static final double kD = 0; public static final double THETA_TOLERANCE = 0.1; public static final double STEADY_STATE_TOLERANCE = 0.1; } - + // ur mom public static final class FaceSpeakerConstants { public static final double kP = 0.03; diff --git a/src/main/java/frc/robot/GameRobotContainer.java b/src/main/java/frc/robot/GameRobotContainer.java index 40fffab8..cc3004bc 100644 --- a/src/main/java/frc/robot/GameRobotContainer.java +++ b/src/main/java/frc/robot/GameRobotContainer.java @@ -3,6 +3,7 @@ import java.io.File; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -13,6 +14,8 @@ import frc.robot.commands.Arm.*; import frc.robot.commands.ClimberCommands.ClimbParts.*; import frc.robot.commands.IntakeCommands.*; +import frc.robot.commands.ReactionArmCommands.Extend; +import frc.robot.commands.ReactionArmCommands.Retract; import frc.robot.commands.driveCommands.*; import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.subsystems.*; @@ -46,8 +49,6 @@ public String getDriveTrainName(){ public GameRobotContainer() { - // LimelightHelpers.setStreamMode_PiPSecondary("limelight"); - // Shuffleboard.getTab("GameTab").addCamera("Vision", "limelight", "http://limelight.local:5800").withSize(4,3).withPosition(5, 0); m_drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), getDriveTrainName())); m_IntakeSubsystem = new IntakeSubsystem(Constants.IntakeConstants.INTAKE_LOWER_INTAKE_ID,Constants.IntakeConstants.INTAKE_SPIN_MOTOR_ID, "rio"); @@ -56,7 +57,7 @@ public GameRobotContainer() { ArmSubsystem.Arm.ENCODER_DIO_SLOT); m_PizzaBoxSubsystem = new PizzaBoxSubsystem(PizzaBoxSubsystem.PizzaBox.PIZZABOX_ID, - "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT); + "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT, PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT_THE_SECOND); //m_ledsubsystem = new LEDSubsystem(Constants.LEDConstants.ledPortNumber); @@ -70,7 +71,7 @@ public GameRobotContainer() { m_ReactionSubsystem = new ReactionSubsystem(Constants.ReactionConstants.reactionID, Constants.ReactionConstants.reactionCAN); - closedFieldRel = new TeleopDrive( + closedFieldRel = new TeleopDrive( m_drivebase, () -> MathUtil.applyDeadband(-driverController.getRawAxis(1), OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(-driverController.getRawAxis(0), OperatorConstants.LEFT_X_DEADBAND), @@ -79,7 +80,7 @@ public GameRobotContainer() { configurePathPlannerCommands(); - autoChooser = AutoBuilder.buildAutoChooser("2N-S2-A3"); + autoChooser = AutoBuilder.buildAutoChooser("Copy of Hajel middle bottom 2"); m_drivebase.setDefaultCommand(closedFieldRel); @@ -97,82 +98,64 @@ private void configureBindings() { //RE-ENABLE BUTTONS; driverController.y().onTrue(new ScoreInSpeakerUnderHand(m_PizzaBoxSubsystem, m_ArmSubsystem)); - driverController.a().onTrue(new ScoreInAmp(m_PizzaBoxSubsystem, m_ArmSubsystem)); + driverController.a().onTrue(new ScoreInAmp(m_PizzaBoxSubsystem, m_ArmSubsystem, closedFieldRel)); driverController.b().onTrue(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem,m_ArmSubsystem, m_IntakeSubsystem, 10)); driverController.x().whileTrue(new DecreaseSpeed(closedFieldRel)); driverController.rightStick().onTrue(new ScoreInSpeakerHigh(m_PizzaBoxSubsystem, m_ArmSubsystem)); - //driverController.leftStick().onTrue(new ChangeRobotOrientation(closedFieldRel)); + driverController.back().onTrue(new ChangeRobotOrientation(closedFieldRel)); - //driverController.rightBumper().onTrue(new BackSpeaker(closedFieldRel)); + //driverController.rightBumper().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()->m_drivebase.getPose())))); //driverController.leftBumper().onTrue(new FaceSpeaker(closedFieldRel)); - driverController.start().onTrue(new InstantCommand(m_drivebase::zeroGyro)); + driverController.rightBumper().onTrue(new Aimbot(closedFieldRel, m_ArmSubsystem, m_PizzaBoxSubsystem, m_drivebase)); + driverController.leftBumper().onTrue(new FaceAmp(closedFieldRel)); + //driverController.start().onTrue(new InstantCommand(m_drivebase::zeroGyro)); + driverController.start().onTrue(new InstantCommand(m_drivebase::setHeading)); /* Operator Controllers */ - operatorController.y().onTrue(new PrepClimb(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem)); + operatorController.y().onTrue(new PrepClimb(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem, m_PizzaBoxSubsystem)); operatorController.b().onTrue(new ClimbAndShoot(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_PizzaBoxSubsystem, m_ReactionSubsystem)); operatorController.a().onTrue(new UnClimb(m_ClimbSubsystem, m_ArmSubsystem, m_ReactionSubsystem)); - operatorController.x().onTrue(new UnClimbPartTwoThatWillBringDownTheMotor(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem)); + operatorController.x().onTrue(new UnClimbPartTwoThatWillBringDownTheMotor(m_ClimbSubsystem, m_drivebase, m_ArmSubsystem, m_ReactionSubsystem, m_PizzaBoxSubsystem)); operatorController.start().onTrue(new StopClimb(m_ClimbSubsystem)); operatorController.rightBumper().onTrue(new ArmEmergencyStop(m_ArmSubsystem, m_PizzaBoxSubsystem)); operatorController.leftBumper().onTrue(new IntakeEmergencyStop(m_IntakeSubsystem)); operatorController.leftStick().whileTrue(new LockHeadingToSourceForIntake(closedFieldRel, m_ArmSubsystem, m_PizzaBoxSubsystem)); - // operatorController. ?? ().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 242).getEntry().getDouble(245))); - operatorController.rightStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, Shuffleboard.getTab("Arm").add("Angle", 240).getEntry().getDouble(245))); - + GenericEntry s = Shuffleboard.getTab("Arm").add("Angle", 236).getEntry(); + operatorController.rightStick().onTrue(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, ()->s.getDouble(236))); } private void configurePathPlannerCommands() { - NamedCommands.registerCommand("Wait (half a second)", new WaitCommand(0.5)); - NamedCommands.registerCommand("Wait (one second)", new WaitCommand(1)); - - NamedCommands.registerCommand("Intake", new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem,m_ArmSubsystem, m_IntakeSubsystem)); - - NamedCommands.registerCommand("Amp", new ScoreInAmp(m_PizzaBoxSubsystem, m_ArmSubsystem)); - - NamedCommands.registerCommand("Speaker (underhand)", new ScoreInSpeakerUnderHand(m_PizzaBoxSubsystem, m_ArmSubsystem)); - NamedCommands.registerCommand("Speaker (subwoofer)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 227.22)); + NamedCommands.registerCommand("Intake", new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem,m_ArmSubsystem, m_IntakeSubsystem)); - NamedCommands.registerCommand("Speaker (A1)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248)/*, - new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase)*/)); - NamedCommands.registerCommand("Speaker (A2)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248)/*, - new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase)*/)); - NamedCommands.registerCommand("Speaker (A3)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), - new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); + NamedCommands.registerCommand("Speaker (underhand)", new ScoreInSpeakerUnderHand(m_PizzaBoxSubsystem, m_ArmSubsystem)); + NamedCommands.registerCommand("Speaker (subwoofer)", new ScoreInSpeakerHigh(m_PizzaBoxSubsystem, m_ArmSubsystem)); - NamedCommands.registerCommand("Speaker (A1-A2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); - NamedCommands.registerCommand("Speaker (A2-A3)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); + NamedCommands.registerCommand("Speaker (A1)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), + new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); + NamedCommands.registerCommand("Speaker (A2)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245), + new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); + NamedCommands.registerCommand("Speaker (A3)", new ParallelDeadlineGroup(new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 248), + new rotateinPlace(()-> UtilMath.BackSpeakerTheta(m_drivebase.getPose()), m_drivebase))); - NamedCommands.registerCommand("Speaker (A3-A2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); - NamedCommands.registerCommand("Speaker (A2-A1)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245)); + NamedCommands.registerCommand("Speaker (S1)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); + NamedCommands.registerCommand("Speaker (S2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236)); + NamedCommands.registerCommand("Speaker (S3)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); - NamedCommands.registerCommand("Speaker (S1)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); - NamedCommands.registerCommand("Speaker (S2)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236)); - NamedCommands.registerCommand("Speaker (S3)", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241)); + NamedCommands.registerCommand("Speaker (S1) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) + .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); + NamedCommands.registerCommand("Speaker (S2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236) + .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); + NamedCommands.registerCommand("Speaker (S3) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) + .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (S1) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (S2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 236) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (S3) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 241) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - - NamedCommands.registerCommand("Speaker (A1-A2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (A2-A3) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - - NamedCommands.registerCommand("Speaker (A3-A2) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - NamedCommands.registerCommand("Speaker (A2-A1) then Intake", new ScoreInSpeakerAdjustable(m_PizzaBoxSubsystem, m_ArmSubsystem, 245) - .andThen(new PickUpFromGroundAndPassToPizzaBox(m_PizzaBoxSubsystem, m_ArmSubsystem, m_IntakeSubsystem))); - } + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -190,12 +173,14 @@ public void setMotorBrake(boolean brake) } public Command teleopInitReset() { - return new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem) - .andThen(new IntakeResetArm(m_IntakeSubsystem)); + return new ResetArm(m_ArmSubsystem, m_PizzaBoxSubsystem).withTimeout(3) + .alongWith(new IntakeResetArm(m_IntakeSubsystem)).withTimeout(3) + .alongWith(new Retract(m_ReactionSubsystem)); } public Command autoInitReset() { - return new IntakeResetArm(m_IntakeSubsystem); + return new IntakeResetArm(m_IntakeSubsystem).withTimeout(3) + .alongWith(new Retract(m_ReactionSubsystem)); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7b160542..e3346442 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -59,22 +59,20 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and use the subsystems needed - // for the specific robot - // LimelightHelpers.setStreamMode_PiPSecondary("limelight"); - - // Shuffleboard.getTab("GameTab").addCamera("Vision", "limelight", "http://limelight.local:5800").withSize(4,3).withPosition(5, 0); - // String logfolder = "/home/lvuser"; // Logger.addDataReceiver(new WPILOGWriter(logfolder)); // Logger.addDataReceiver(new NT4Publisher()); // Logger.start(); - //SignalLogger.enableAutoLogging(true); - //SignalLogger.start(); + SignalLogger.setPath("/home/lvuser/log/"); + SignalLogger.enableAutoLogging(true); + SignalLogger.start(); DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); + + UtilMath.overhand.put(1.33, 236d); //S2 + UtilMath.overhand.put(2.45, 245d); //A2 switch (container){ case GAME: @@ -103,11 +101,6 @@ public void robotInit() { m_baseContainer = new ReactionArmContainer(); break; } - - //UtilMath.treeOverHand.put(); - - //UtilMath.treeUnderHand.put(); - } /** diff --git a/src/main/java/frc/robot/SetupShuffleboard.java b/src/main/java/frc/robot/SetupShuffleboard.java index 7e0b50e2..97bbffd9 100644 --- a/src/main/java/frc/robot/SetupShuffleboard.java +++ b/src/main/java/frc/robot/SetupShuffleboard.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.shuffleboard.*; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; @@ -15,6 +16,9 @@ import frc.robot.commands.CancelAllCommands; import frc.robot.commands.Arm.ResetArm; import frc.robot.commands.Arm.ScoreInTrapStutter; +import frc.robot.commands.Arm.SpinToArmAngle; +import frc.robot.commands.Arm.SwingBackServoTheSecond; +import frc.robot.commands.Arm.SwingForwardServoTheSecond; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorUp; import frc.robot.commands.ClimberCommands.ClimbParts.ClimbAndShoot; @@ -31,6 +35,7 @@ import frc.robot.commands.ReactionArmCommands.Extend; import frc.robot.commands.ReactionArmCommands.Retract; import frc.robot.commands.driveCommands.ResetTeleopDrive; +import frc.robot.commands.driveCommands.StraightenWheelCommand; import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.commands.driveCommands.LockHeadingToSourceForIntake; import frc.robot.subsystems.ArmSubsystem; @@ -57,37 +62,65 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p // LimelightHelpers.setStreamMode_PiPSecondary("limelight"); // Shuffleboard.getTab("GameTab").addCamera("Vision", "limelight", "http://limelight.local:5800").withSize(4,3).withPosition(5, 0); - Shuffleboard.getTab("GameTab").add("Field", swerve.getField2d()) - .withSize(3, 2) - .withPosition(0, 0); + //Climb stuff (all in climb tab) + + if(DriverStation.isTest()) + { + Shuffleboard.getTab("Climb").add("climb prep", new PrepClimb(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(4, 0); + Shuffleboard.getTab("Climb").add("climb & shoot", new ClimbAndShoot(climbSubsystem, swerve, armSubsystem, pizzaBoxSubsystem, reactionSubsystem)).withPosition(5, 0); + Shuffleboard.getTab("Climb").addDouble("climb distance left", () -> climbSubsystem.getPositionLeft()).withPosition(0, 0); + Shuffleboard.getTab("Climb").addDouble("climb distance right", () -> climbSubsystem.getPositionRight()).withPosition(1, 0); + Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem, armSubsystem)).withPosition(1, 1); + Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem, armSubsystem)).withPosition(0, 1); + + Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); + Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem, pizzaBoxSubsystem)).withPosition(5, 1); + + Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); + + Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); + + Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); + } + + // Shuffleboard.getTab("Pizza Box").add("270", new SpinToArmAngle(armSubsystem, 270)); + // Shuffleboard.getTab("Pizza Box").add("Swing Servo Forward", new SwingForwardServoTheSecond(pizzaBoxSubsystem)); + // Shuffleboard.getTab("Pizza Box").add("Swing Servo back", new SwingBackServoTheSecond(pizzaBoxSubsystem)); + + Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); + + Shuffleboard.getTab("Climb").addBoolean("bot left limit", () -> climbSubsystem.getBotLeftLimit()).withPosition(2, 1); + Shuffleboard.getTab("Climb").addBoolean("bot right limit", () -> climbSubsystem.getBotRightLimit()).withPosition(3, 1); + Shuffleboard.getTab("Climb").addBoolean("top left limit", () -> climbSubsystem.getTopLeftLimit()).withPosition(2, 0); + Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); + + // Shuffleboard.getTab("GameTab").add("Field", swerve.getField2d()) + // .withSize(3, 2) + // .withPosition(0, 0); Shuffleboard.getTab("GameTab").add("Autonomous Chooser", chooser) .withSize(2, 1) .withPosition(0, 2); - Shuffleboard.getTab("GameTab").add("Disable Pose Estimator", new toggleLimelightPoseEstimation(limelightSubsystem)) - .withSize(1,1) - .withPosition(2,2); - Shuffleboard.getTab("GameTab").add("Toggle Camera Mode", new toggleCameraMode(limelightSubsystem)) - .withSize(1,1) - .withPosition(1,3); + // Shuffleboard.getTab("GameTab").add("Disable Pose Estimator", new toggleLimelightPoseEstimation(limelightSubsystem)) + // .withSize(1,1) + // .withPosition(2,2); + // Shuffleboard.getTab("GameTab").add("Toggle Camera Mode", new toggleCameraMode(limelightSubsystem)) + // .withSize(1,1) + // .withPosition(1,3); - Shuffleboard.getTab("GameTab").addBoolean("Note In Pizza Box", ()-> pizzaBoxSubsystem.hasNote) - .withPosition(0,3); + // Shuffleboard.getTab("GameTab").addBoolean("Note In Pizza Box", ()-> pizzaBoxSubsystem.hasNote) + // .withPosition(0,3); Shuffleboard.getTab("GameTab").addBoolean("Arm Running", ()-> !armSubsystem.isEmergencyStop()) - .withSize(1, 1) .withPosition(3, 0); Shuffleboard.getTab("GameTab").addBoolean("Intake Running", ()-> !intakeSubsystem.isEmergencyStop()) - .withSize(1, 1) .withPosition(4, 0); Shuffleboard.getTab("GameTab").add("Reset Arm", new ResetArm(armSubsystem, pizzaBoxSubsystem) ) - .withSize(1,1) .withPosition(3,1); Shuffleboard.getTab("GameTab").add("Reset Intake", new IntakeResetArm(intakeSubsystem)) - .withSize(1,1) .withPosition(4,1); - Shuffleboard.getTab("GameTab").add("IntakeRejectNote", new IntakeRejectNote(intakeSubsystem)) + Shuffleboard.getTab("GameTab").add("Intake Reject Note", new IntakeRejectNote(intakeSubsystem)) .withPosition(4,2); Shuffleboard.getTab("GameTab").add("Extend Reaction Bar", new Extend(reactionSubsystem)) .withPosition(3, 3); @@ -99,45 +132,16 @@ public static void setupShuffleboard(SwerveSubsystem swerve, PizzaBoxSubsystem p .withPosition(5,3); Shuffleboard.getTab("GameTab").add("CLEAR ALL COMMANDS", new CancelAllCommands()).withPosition(7,3); Shuffleboard.getTab("GameTab").add("RESET TELEOP DRIVE", new ResetTeleopDrive(teleopDrive)).withPosition(9,0); + // Shuffleboard.getTab("GameTab").add("Straighten", new StraightenWheelCommand(swerve)).withPosition(9, 1); - Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); - - - //Climb stuff (all in climb tab) - Shuffleboard.getTab("Climb").addDouble("climb distance left", () -> climbSubsystem.getPositionLeft()).withPosition(0, 0); - Shuffleboard.getTab("Climb").addDouble("climb distance right", () -> climbSubsystem.getPositionRight()).withPosition(1, 0); - Shuffleboard.getTab("Climb").addBoolean("bot left limit", () -> climbSubsystem.getBotLeftLimit()).withPosition(2, 1); - Shuffleboard.getTab("Climb").addBoolean("bot right limit", () -> climbSubsystem.getBotRightLimit()).withPosition(3, 1); - Shuffleboard.getTab("Climb").addBoolean("top left limit", () -> climbSubsystem.getTopLeftLimit()).withPosition(2, 0); - Shuffleboard.getTab("Climb").addBoolean("top right limit", () -> climbSubsystem.getTopRightLimit()).withPosition(3, 0); - - Shuffleboard.getTab("Climb").add("motor down", new MotorDown(climbSubsystem)).withPosition(1, 1); - Shuffleboard.getTab("Climb").add("motor up", new MotorUp(climbSubsystem)).withPosition(0, 1); - - Shuffleboard.getTab("Climb").add("climb prep", new PrepClimb(climbSubsystem, swerve, armSubsystem, reactionSubsystem)).withPosition(4, 0); - Shuffleboard.getTab("Climb").add("climb & shoot", new ClimbAndShoot(climbSubsystem, swerve, armSubsystem, pizzaBoxSubsystem, reactionSubsystem)).withPosition(5, 0); - Shuffleboard.getTab("Climb").add("unclimb1", new UnClimb(climbSubsystem, armSubsystem, reactionSubsystem)).withPosition(4, 1); - Shuffleboard.getTab("Climb").add("unclimb2", new UnClimbPartTwoThatWillBringDownTheMotor(climbSubsystem, swerve, armSubsystem, reactionSubsystem)).withPosition(5, 1); - - Shuffleboard.getTab("Climb").add("STOP CLIMB!!!!", new StopClimb(climbSubsystem)).withSize(2, 1).withPosition(2, 2); - - Shuffleboard.getTab("Climb").addDouble("Reaction Bar Angle", ()-> reactionSubsystem.getPos()).withPosition(9, 4); - - - Shuffleboard.getTab("System Check").add("check", new SystemCheck(armSubsystem, climbSubsystem, intakeSubsystem, pizzaBoxSubsystem, reactionSubsystem, swerve)); - - Shuffleboard.getTab("Arm").addDouble("distance from speaker", ()-> UtilMath.distanceFromSpeaker(swerve.getPose())); - - Shuffleboard.getTab("Arm").add("stutter trap", new ScoreInTrapStutter(pizzaBoxSubsystem, armSubsystem)); + // Shuffleboard.getTab("TEST COMMAND").add("TEST", new LockHeadingToSourceForIntake(teleopDrive, armSubsystem, pizzaBoxSubsystem)); + Shuffleboard.getTab("System Check").add("check", new SystemCheck(armSubsystem, climbSubsystem, intakeSubsystem, pizzaBoxSubsystem, reactionSubsystem, swerve, teleopDrive)); + DataLogManager.log("rotate in place P: " + Constants.DriveConstants.kP); DataLogManager.log("rotate in place I: " + Constants.DriveConstants.kI); DataLogManager.log("rotate in place D: " + Constants.DriveConstants.kD); } - @Override - public void periodic() { - // This method will be called once per scheduler run - } } diff --git a/src/main/java/frc/robot/Util/UtilMath.java b/src/main/java/frc/robot/Util/UtilMath.java index cd374c5b..452ebf32 100644 --- a/src/main/java/frc/robot/Util/UtilMath.java +++ b/src/main/java/frc/robot/Util/UtilMath.java @@ -4,6 +4,8 @@ package frc.robot.Util; +import java.util.function.Supplier; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.DriverStation; @@ -18,6 +20,9 @@ public class UtilMath { public static final int RED_SOURCE_HEADING_ANGLE = -120; public static final int BLUE_SOURCE_HEADING_ANGLE = -60; + public static InterpolatingDoubleTreeMap overhand = new InterpolatingDoubleTreeMap(); + + public static double caclucateRotateTheta(Pose2d pose, double targetX, double targetY){ @@ -109,4 +114,8 @@ public static double distanceFromSpeaker(Pose2d pose) { return Math.sqrt(Math.pow(RED_SPEAKER_X - robotX, 2) + Math.pow(RED_SPEAKER_Y - robotY, 2)); } } + + public static double distanceFromSpeaker(Supplier pose) { + return distanceFromSpeaker(pose.get()); + } } diff --git a/src/main/java/frc/robot/commands/Arm/Aimbot.java b/src/main/java/frc/robot/commands/Arm/Aimbot.java new file mode 100644 index 00000000..ebbca92b --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/Aimbot.java @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.Arm; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Util.UtilMath; +import frc.robot.commands.driveCommands.DecreaseSpeed; +import frc.robot.commands.driveCommands.FaceAmp; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class Aimbot extends SequentialCommandGroup { + /** Creates a new Aimbot. */ + public Aimbot(TeleopDrive t, ArmSubsystem a, PizzaBoxSubsystem p, SwerveSubsystem s) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + Commands.runOnce(()-> {t.isSlow = true; t.isBackSpeaker = true;}), + new ScoreInSpeakerAdjustable(p, a, ()-> UtilMath.overhand.get(UtilMath.distanceFromSpeaker(()-> s.getPose()))), + Commands.runOnce(()-> {t.isSlow = false; t.isBackSpeaker = false;}) + ); + } +} diff --git a/src/main/java/frc/robot/commands/Arm/ResetArm.java b/src/main/java/frc/robot/commands/Arm/ResetArm.java index 850464c8..f81a24eb 100644 --- a/src/main/java/frc/robot/commands/Arm/ResetArm.java +++ b/src/main/java/frc/robot/commands/Arm/ResetArm.java @@ -2,6 +2,7 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.PizzaBoxSubsystem; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -19,7 +20,8 @@ public ResetArm(ArmSubsystem armSubsystem, PizzaBoxSubsystem pizzaBoxSubsystem) } - public void initialize() { + public void initialize() { + DataLogManager.log("Command Start: Reset Arm"); Command c = CommandScheduler.getInstance().requiring(armSubsystem); if(c != null) { c.cancel(); @@ -32,8 +34,9 @@ public void initialize() { armSubsystem.stopArmMotor(); pizzaBoxSubsystem.stopPizzaBoxMotor(); - new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).schedule(); + armSubsystem.targetArmAngle(ArmSubsystem.Arm.INTAKE_ANGLE); pizzaBoxSubsystem.setServoAngle(50); + pizzaBoxSubsystem.setServoAngleTheSecond(0); } @@ -46,6 +49,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + DataLogManager.log("Command End: Reset Arm"); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java b/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java index 3b25c79e..64c41131 100644 --- a/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java +++ b/src/main/java/frc/robot/commands/Arm/ScoreInAmp.java @@ -5,6 +5,7 @@ package frc.robot.commands.Arm; import frc.robot.subsystems.PizzaBoxSubsystem; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -14,7 +15,7 @@ public class ScoreInAmp extends SequentialCommandGroup { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) //velocity = 100 for testing shooting - public ScoreInAmp(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem) { + public ScoreInAmp(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem, TeleopDrive drive) { addCommands( new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.AMP_ANGLE).withTimeout(3), new SpinNoteContainerMotor(pizzaBoxSubsystem, 40, 100), @@ -26,6 +27,7 @@ public ScoreInAmp(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE), Commands.runOnce(() -> { pizzaBoxSubsystem.hasNote = false; + drive.faceAmp = false; }) ); } diff --git a/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java b/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java index efec504c..d2acd8f8 100644 --- a/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java +++ b/src/main/java/frc/robot/commands/Arm/ScoreInSpeakerAdjustable.java @@ -32,4 +32,21 @@ public ScoreInSpeakerAdjustable(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsyste }) ); } + + public ScoreInSpeakerAdjustable(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem, DoubleSupplier angleSupplier) { + addCommands( + new SpinArmAndPizzaBox(pizzaBoxSubsystem, armSubsystem, angleSupplier, 100).withTimeout(2), + new SpinNoteContainerMotor(pizzaBoxSubsystem, 100, 100), + Commands.waitUntil(()->pizzaBoxSubsystem.isAtVelocity(90)).withTimeout(0.5), + new SwingForwardServo(pizzaBoxSubsystem), + Commands.waitSeconds(.2), + new SwingBackServo(pizzaBoxSubsystem), + Commands.waitSeconds(0.2), + new StopNoteContainerMotor(pizzaBoxSubsystem), + new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).withTimeout(0.1), + Commands.runOnce(() -> { + pizzaBoxSubsystem.hasNote = false; + }) + ); + } } diff --git a/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java b/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java index dc00b8d6..534c83eb 100644 --- a/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java +++ b/src/main/java/frc/robot/commands/Arm/ScoreInTrapStutter.java @@ -5,6 +5,7 @@ package frc.robot.commands.Arm; import frc.robot.subsystems.PizzaBoxSubsystem; +import frc.robot.Constants; import frc.robot.subsystems.ArmSubsystem; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; @@ -17,7 +18,7 @@ public class ScoreInTrapStutter extends SequentialCommandGroup { public ScoreInTrapStutter(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem) { addCommands( new PrintCommand("Score in trp stutter initialize"), - new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.TRAP_ANGLE).withTimeout(3)); + new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.TRAP_ANGLE).withTimeout(3)); for(int index = 0; index < 8; index++) @@ -34,11 +35,11 @@ public ScoreInTrapStutter(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armS addCommands( new StopNoteContainerMotor(pizzaBoxSubsystem), - new SpinToArmAngle(armSubsystem, 260).withTimeout(1), + new SpinToArmAngle(armSubsystem, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.runOnce(() -> { pizzaBoxSubsystem.hasNote = false; }), - new PrintCommand("scorein trap stutter finished") + new PrintCommand("scorein trap stutter finished") ); } diff --git a/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java b/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java index e8ef9080..c058976c 100644 --- a/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java +++ b/src/main/java/frc/robot/commands/Arm/SpinArmAndPizzaBox.java @@ -7,6 +7,8 @@ import frc.robot.subsystems.PizzaBoxSubsystem; import frc.robot.subsystems.ArmSubsystem; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; public class SpinArmAndPizzaBox extends Command { @@ -14,6 +16,7 @@ public class SpinArmAndPizzaBox extends Command { private ArmSubsystem armSubsystem; private double angle; private double vel; + private DoubleSupplier angleUpdate; @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) @@ -25,6 +28,14 @@ public SpinArmAndPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armS this.vel = vel; } + public SpinArmAndPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, ArmSubsystem armSubsystem, DoubleSupplier angle, double vel) { + this.pizzaBoxSubsystem = pizzaBoxSubsystem; + this.armSubsystem = armSubsystem; + this.angle = angle.getAsDouble(); + this.vel = vel; + angleUpdate = angle; + } + // Called when the command is initially scheduled. @Override public void initialize() { @@ -34,6 +45,9 @@ public void initialize() { @Override public void execute() { + if (angleUpdate != null) { + armSubsystem.targetArmAngle(angleUpdate.getAsDouble()); + } if(armSubsystem.encoderGetAngle() > PizzaBoxSubsystem.PizzaBox.START_SPIN_DEGREE) { pizzaBoxSubsystem.spinPizzaBoxMotor(vel, 500); diff --git a/src/main/java/frc/robot/commands/Arm/SpinArmCommand.java b/src/main/java/frc/robot/commands/Arm/SpinArmCommand.java new file mode 100644 index 00000000..70545c53 --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/SpinArmCommand.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.Arm; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ArmSubsystem; + +public class SpinArmCommand extends Command { + /** Creates a new SpinArmCommand. */ + private final ArmSubsystem m_ArmSubsystem; + public SpinArmCommand(ArmSubsystem m_ArmSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.m_ArmSubsystem = m_ArmSubsystem; + addRequirements(m_ArmSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_ArmSubsystem.booster = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_ArmSubsystem.booster = false; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java b/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java index f0bb4385..69f136c5 100644 --- a/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java +++ b/src/main/java/frc/robot/commands/Arm/SpinToArmAngle.java @@ -23,13 +23,11 @@ public SpinToArmAngle(ArmSubsystem armSubsystem, double angle) { // Called when the command is initially scheduled. @Override public void initialize() { - System.out.println("Spin to arm angle initialize"); armSubsystem.targetArmAngle(angle); } @Override public void end(boolean interrupted){ - System.out.println("Spin to arm angle finished"); } diff --git a/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java new file mode 100644 index 00000000..d8d931b2 --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/SwingBackServoTheSecond.java @@ -0,0 +1,33 @@ +package frc.robot.commands.Arm; + +import frc.robot.subsystems.PizzaBoxSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwingBackServoTheSecond extends Command{ + + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final PizzaBoxSubsystem pizzaBoxSubsystem; + + public SwingBackServoTheSecond(PizzaBoxSubsystem pizzaBoxSubsystem) + { + this.pizzaBoxSubsystem = pizzaBoxSubsystem; + addRequirements(pizzaBoxSubsystem); + } + + public void initialize() { + pizzaBoxSubsystem.setServoAngleTheSecond(0); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java new file mode 100644 index 00000000..66015480 --- /dev/null +++ b/src/main/java/frc/robot/commands/Arm/SwingForwardServoTheSecond.java @@ -0,0 +1,32 @@ +package frc.robot.commands.Arm; +import frc.robot.subsystems.PizzaBoxSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + + +public class SwingForwardServoTheSecond extends Command{ + + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final PizzaBoxSubsystem pizzaBoxSubsystem; + + public SwingForwardServoTheSecond(PizzaBoxSubsystem pizzaBoxSubsystem) + { + this.pizzaBoxSubsystem = pizzaBoxSubsystem; + addRequirements(pizzaBoxSubsystem); + } + public void initialize() { + pizzaBoxSubsystem.setServoAngleTheSecond(200); + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java index f292de27..4a940233 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorDown.java @@ -5,34 +5,36 @@ package frc.robot.commands.ClimberCommands.ActuallyMovesMotors; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; public class MotorDown extends Command { /** Creates a new MotorDown. */ private Climbsubsystem climbersubsystem; + private ArmSubsystem armsubsystem; //constructor that takes in a Climbsubsystem object and a SwerveSubsystem obj - public MotorDown(Climbsubsystem c) { + public MotorDown(Climbsubsystem c, ArmSubsystem a) { + climbersubsystem = c; + armsubsystem = a; addRequirements(this.climbersubsystem); } @Override public void initialize() { - System.out.println("Initialize motor down"); climbersubsystem.downMotor(); } @Override - public void execute() { + public void execute() { + climbersubsystem.armAngleCheck = armsubsystem.checkEncoderAngleForClimb(); } @Override public void end(boolean interrupted) { - System.out.println("finish motor down"); } @Override diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java index e340654b..0cc41b0a 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ActuallyMovesMotors/MotorUp.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ClimbConstants; +import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -13,28 +14,29 @@ public class MotorUp extends Command { /** Creates a new MotorUp. */ private final Climbsubsystem climbersubsystem; + private final ArmSubsystem armsubsystem; //constructor that takes in a Climbsubsystem object and a SwerveSubsystem obj - public MotorUp(Climbsubsystem c) { + public MotorUp(Climbsubsystem c, ArmSubsystem a) { climbersubsystem = c; + armsubsystem = a; addRequirements(this.climbersubsystem); } @Override public void initialize() { - System.out.println("Initialize motor up"); climbersubsystem.upMotor(); } @Override public void execute() { + climbersubsystem.armAngleCheck = armsubsystem.checkEncoderAngleForClimb(); } @Override public void end(boolean interrupted) { - System.out.println("finish motor up"); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java index 3e865798..48b6bd49 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbDown.java @@ -19,9 +19,9 @@ public class ClimbDown extends SequentialCommandGroup { public ClimbDown(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a) { addCommands( - new MotorUp(c), + new MotorUp(c, a), new ParallelCommandGroup(new DriveForClimb(s, 0.4)/* , new Retract(r)*/), - new MotorDown(c) + new MotorDown(c, a) ); // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java index 0b338ab3..826edd17 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/AutoClimb/ClimbUp.java @@ -23,9 +23,9 @@ public class ClimbUp extends SequentialCommandGroup { public ClimbUp(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { addCommands( new ParallelCommandGroup(new SpinToArmAngle(a, 135)/* , new Extend(r)*/), - new SequentialCommandGroup(new WaitCommand(0.5), new MotorUp(c)), + new SequentialCommandGroup(new WaitCommand(0.5), new MotorUp(c, a)), new DriveForClimb(s, -0.4), - new MotorDown(c) + new MotorDown(c, a) ); // Use addRequirements() here to declare subsystem dependencies. diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java index 6ef08d87..2773f19a 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/ClimbAndShoot.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.Arm.*; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; import frc.robot.commands.ReactionArmCommands.Extend; @@ -27,10 +29,13 @@ public ClimbAndShoot(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, PizzaB // Add your commands in the addCommands() call, e.g. addCommands ( Commands.runOnce(()->DataLogManager.log("Command Start: ClimbAndShoot")), - new SpinToArmAngle(a, 260).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(0.5), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorDown(c).alongWith(new Extend(r)).withTimeout(3), + new MotorDown(c, a).alongWith(new Extend(r)).withTimeout(3), new ScoreInTrapStutter(p, a), + new SpinNoteContainerMotor(p, -5, 100), + new WaitCommand(1.6), + new StopNoteContainerMotor(p), Commands.runOnce(()->DataLogManager.log("Command End: ClimbAndShoot")) ); diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java index 6d3e1d6b..ef434943 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/PrepClimb.java @@ -11,22 +11,29 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; +import frc.robot.commands.Arm.SwingForwardServoTheSecond; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorUp; -import frc.robot.commands.ReactionArmCommands.Extend; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem; import frc.robot.subsystems.ReactionSubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem.PizzaBox; import frc.robot.subsystems.swervedrive.SwerveSubsystem; public class PrepClimb extends SequentialCommandGroup { /** Creates a new PrepClimb. */ - public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { + public PrepClimb(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r, PizzaBoxSubsystem p) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PrepClimb")), - new SpinToArmAngle(a, 260).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(4), + new SwingForwardServoTheSecond(p), + new WaitCommand(0.7), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(1), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorUp(c).withTimeout(5), + new MotorUp(c, a).withTimeout(5), Commands.runOnce(()->DataLogManager.log("Command End: PrepClimb")) ); } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java index ecf3f3bf..465ae9f3 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimb.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorUp; import frc.robot.commands.ReactionArmCommands.Retract; @@ -27,9 +28,10 @@ public UnClimb(Climbsubsystem c, ArmSubsystem a, ReactionSubsystem r) { // addCommands(new FooCommand(), new BarCommand()); addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimb")), - new SpinToArmAngle(a, 260).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(0.5), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorUp(c).alongWith(new Retract(r)).withTimeout(5), + new MotorUp(c, a).withTimeout(5), + new Retract(r).withTimeout(1), Commands.runOnce(()->DataLogManager.log("Command End: UnClimb")) ); } diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java index 6b0021bd..e956a4c7 100644 --- a/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java +++ b/src/main/java/frc/robot/commands/ClimberCommands/ClimbParts/UnClimbPartTwoThatWillBringDownTheMotor.java @@ -8,12 +8,17 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Constants; import frc.robot.commands.Arm.SpinToArmAngle; +import frc.robot.commands.Arm.SwingBackServoTheSecond; import frc.robot.commands.ClimberCommands.ActuallyMovesMotors.MotorDown; import frc.robot.commands.ReactionArmCommands.Retract; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; +import frc.robot.subsystems.PizzaBoxSubsystem; import frc.robot.subsystems.ReactionSubsystem; +import frc.robot.subsystems.ArmSubsystem.Arm; import frc.robot.subsystems.swervedrive.SwerveSubsystem; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -21,13 +26,17 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class UnClimbPartTwoThatWillBringDownTheMotor extends SequentialCommandGroup { /** Creates a new UnClimbPartTwoThatWillBringDownTheMotor. */ - public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r) { + public UnClimbPartTwoThatWillBringDownTheMotor(Climbsubsystem c, SwerveSubsystem s, ArmSubsystem a, ReactionSubsystem r, PizzaBoxSubsystem p) { addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: UnClimbPartTwo")), - new SpinToArmAngle(a, 135).withTimeout(1), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ANGLE).withTimeout(0.5), Commands.waitUntil(()->a.checkEncoderAngleForClimb()), - new MotorDown(c).withTimeout(3), + new MotorDown(c, a).withTimeout(3), + new SpinToArmAngle(a, Constants.ClimbConstants.CLIMB_ARM_ARNGLE_FOR_SERVO).withTimeout(2.5), + new SwingBackServoTheSecond(p), + new WaitCommand(0.7), + new SpinToArmAngle(a, Arm.INTAKE_ANGLE), Commands.runOnce(()->DataLogManager.log("Command End: UnClimbPartTwo")) ); } diff --git a/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java b/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java index 49ac8731..96deeeed 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/IntakePassNoteToPizzaBox.java @@ -31,10 +31,10 @@ public IntakePassNoteToPizzaBox(IntakeSubsystem subsystem, PizzaBoxSubsystem piz // Called when the command is initially scheduled. @Override public void initialize() { - System.out.println("Intake pass note to pizzabox initialize"); + pizzaBoxSubsystem.spinPizzaBoxMotor(-60, 100); currentSensorValue = true; intakeSubsystem.spinIntakeMotor(0.8, 100); - pizzaBoxSubsystem.spinPizzaBoxMotor(-60, 100); + //pizzaBoxSubsystem.spinPizzaBoxMotor(-60, 100); initTimer = Timer.getFPGATimestamp(); } @@ -47,13 +47,8 @@ public void execute() { // runs once when isFinished is called @Override public void end(boolean interrupted) { - System.out.println("Intake pass note to pizzabox finished"); pizzaBoxSubsystem.stopPizzaBoxMotor(); intakeSubsystem.stopIntakeMotors(); - // if(interrupted) { - // System.out.println("************************************************************************************************************************************************************************************************************************************************************************************"); - // andThen(new IntakeRejectNote(intakeSubsystem)); - // } } // Returns true when the command should end; called every cycle diff --git a/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java b/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java index 3042be6b..6243945a 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/IntakePickUpFromGroundPID.java @@ -12,7 +12,7 @@ public class IntakePickUpFromGroundPID extends PIDCommand { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final IntakeSubsystem intakeSubsystem; - private static PIDController intakeController = new PIDController(.025, .001, .0); + private static PIDController intakeController = new PIDController(.03, 0, .0); private double velocity; private double accleration; diff --git a/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java b/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java index c3a14767..3da6be15 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/IntakeResetArm.java @@ -6,6 +6,7 @@ import frc.robot.Constants; import frc.robot.subsystems.IntakeSubsystem; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -21,6 +22,7 @@ public IntakeResetArm(IntakeSubsystem subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { + DataLogManager.log("Command Start: Reset Intake Arm"); Command i = CommandScheduler.getInstance().requiring(intakeSubsystem); if(i != null) { @@ -29,7 +31,7 @@ public void initialize() { intakeSubsystem.stopArmMotor(); intakeSubsystem.stopIntakeMotors(); - new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION).schedule(); + new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION).withTimeout(3).schedule(); } // Called every time the scheduler runs while the command is scheduled. @@ -42,7 +44,7 @@ public void execute() { // runs once when isFinished is called @Override public void end(boolean interrupted) { - + DataLogManager.log("Command End: Reset Intake Arm"); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java index 48c4d2be..9b8faaad 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/PickUpFromGroundAndPassToPizzaBox.java @@ -24,8 +24,9 @@ public PickUpFromGroundAndPassToPizzaBox(PizzaBoxSubsystem pizzaBoxSubsystem, Ar addCommands( Commands.runOnce(()->DataLogManager.log("Command Start: PickUpFromGroundAndPassToPizzaBox")), new SpinToArmAngle(armSubsystem, ArmSubsystem.Arm.INTAKE_ANGLE).withTimeout(2) - .alongWith((new IntakePickUpFromGroundPID(intakeSubsystem, 0.8, 0.0).withTimeout(intakeTimeout)) - .andThen(new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION))) + .alongWith((new IntakePickUpFromGroundPID(intakeSubsystem, 0.7, 0.0).withTimeout(intakeTimeout)) + .andThen(new SpinIntakePID(intakeSubsystem, Constants.IntakeConstants.UP_POSITION)).withTimeout(5)) + .alongWith(new SpinNoteContainerMotor(pizzaBoxSubsystem, -60, 100)) .andThen(new IntakePassNoteToPizzaBox(intakeSubsystem, pizzaBoxSubsystem).withTimeout(6)) .andThen(Commands.runOnce(() -> { pizzaBoxSubsystem.hasNote = true; diff --git a/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java b/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java index 024c5a6c..b4a360fe 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/SpinIntakePID.java @@ -13,7 +13,7 @@ public class SpinIntakePID extends PIDCommand { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final IntakeSubsystem intakeSubsystem; - private static PIDController intakeController = new PIDController(.025, .001, .0); + private static PIDController intakeController = new PIDController(.03, 0, .0); public SpinIntakePID(IntakeSubsystem intakeSubsystem, final double targetAngle) { super(intakeController, ()-> intakeSubsystem.encoderGetAngle(), () -> targetAngle, diff --git a/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java b/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java index 5d5f3bee..f7794641 100644 --- a/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java +++ b/src/main/java/frc/robot/commands/ReactionArmCommands/Extend.java @@ -10,8 +10,8 @@ public class Extend extends Command { private final ReactionSubsystem m_Subsystem; - private double currPos; //position in rotation - private double endPos; + //private double currPos; //position in rotation + //private double endPos; /** Creates a new Extend. */ public Extend(ReactionSubsystem subsystem) { // Use addRequirements() here to declare subsystem dependencies. @@ -22,30 +22,27 @@ public Extend(ReactionSubsystem subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - currPos = m_Subsystem.getPos(); - endPos = Constants.ReactionConstants.extendedPosition; //adds the current position to the final which is the end pos. + //currPos = m_Subsystem.getPos(); + //endPos = Constants.ReactionConstants.extendedPosition; //adds the current position to the final which is the end pos. m_Subsystem.spinForward(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - currPos = m_Subsystem.getPos(); + //currPos = m_Subsystem.getPos(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_Subsystem.stop(); + // m_Subsystem.stop(); } // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance) - { - return true; - } - return false; + //return (Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance); + return m_Subsystem.PIDcontroller.atSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java b/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java index 3281aa6b..dc78f375 100644 --- a/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java +++ b/src/main/java/frc/robot/commands/ReactionArmCommands/Retract.java @@ -11,8 +11,8 @@ public class Retract extends Command { /** Creates a new Retract. */ private final ReactionSubsystem m_Subsystem; -private double currPos; //position in rotation -private double endPos; +//private double currPos; //position in rotation +//private double endPos; public Retract(ReactionSubsystem subsystem) { // Use addRequirements() here to declare subsystem dependencies. this.m_Subsystem = subsystem; @@ -22,32 +22,28 @@ public Retract(ReactionSubsystem subsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - currPos = m_Subsystem.getPos(); - endPos = Constants.ReactionConstants.retractedPosition; //adds the current position to the final which is the end pos. + // currPos = m_Subsystem.getPos(); + // endPos = Constants.ReactionConstants.retractedPosition; //adds the current position to the final which is the end pos. m_Subsystem.spinBackward(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - currPos = m_Subsystem.getPos(); + // currPos = m_Subsystem.getPos(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_Subsystem.stop(); + //m_Subsystem.stop(); } // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance || currPos >0) - { - return true; - } - return false; - + // Math.abs(currPos - endPos) < Constants.ReactionConstants.tolerance || currPos >0); + return m_Subsystem.PIDcontroller.atSetpoint(); } } diff --git a/src/main/java/frc/robot/commands/SystemCheck.java b/src/main/java/frc/robot/commands/SystemCheck.java index 89381e74..c784ee33 100644 --- a/src/main/java/frc/robot/commands/SystemCheck.java +++ b/src/main/java/frc/robot/commands/SystemCheck.java @@ -15,6 +15,7 @@ import frc.robot.commands.ClimberCommands.ClimbParts.UnClimbPartTwoThatWillBringDownTheMotor; import frc.robot.commands.IntakeCommands.PickUpFromGroundAndPassToPizzaBox; import frc.robot.commands.ReactionArmCommands.*; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.Climbsubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -28,17 +29,17 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class SystemCheck extends SequentialCommandGroup { /** Creates a new SystemCheck. */ - public SystemCheck(ArmSubsystem a, Climbsubsystem c, IntakeSubsystem i, PizzaBoxSubsystem p, ReactionSubsystem r, SwerveSubsystem s) { + public SystemCheck(ArmSubsystem a, Climbsubsystem c, IntakeSubsystem i, PizzaBoxSubsystem p, ReactionSubsystem r, SwerveSubsystem s, TeleopDrive t) { addCommands( //TO-DO: Add condition for different systemcheck if arm/intake is broken - new PrepClimb(c, s, a, r), new WaitCommand(2), + new PrepClimb(c, s, a, r, p), new WaitCommand(2), new ClimbAndShoot(c, s, a, p, r), new WaitCommand(2), new UnClimb(c, a, r), new WaitCommand(2), - new UnClimbPartTwoThatWillBringDownTheMotor(c, s, a, r), new WaitCommand(2), + new UnClimbPartTwoThatWillBringDownTheMotor(c, s, a, r, p), new WaitCommand(2), new PickUpFromGroundAndPassToPizzaBox(p, a, i), new WaitCommand(2), new PickUpFromGroundAndPassToPizzaBox(p, a, i), new WaitCommand(2), - new ScoreInAmp(p, a), new WaitCommand(2), + new ScoreInAmp(p, a, t), new WaitCommand(2), new ScoreInSpeakerUnderHand(p, a), new WaitCommand(2) ); diff --git a/src/main/java/frc/robot/commands/driveCommands/FaceAmp.java b/src/main/java/frc/robot/commands/driveCommands/FaceAmp.java new file mode 100644 index 00000000..985ab6a8 --- /dev/null +++ b/src/main/java/frc/robot/commands/driveCommands/FaceAmp.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.driveCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.swervedrive.drivebase.TeleopDrive; + +public class FaceAmp extends Command { + /** Creates a new FaceAmp. */ + private final TeleopDrive drive; + public FaceAmp(TeleopDrive driveBase) { + drive = driveBase; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + drive.faceAmp = !drive.faceAmp; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java new file mode 100644 index 00000000..6c09ba09 --- /dev/null +++ b/src/main/java/frc/robot/commands/driveCommands/StraightenWheelCommand.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.driveCommands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +public class StraightenWheelCommand extends Command { + /** Creates a new StraightenWheelCommand. */ + private final SwerveSubsystem m_Subsystem; + public StraightenWheelCommand(SwerveSubsystem m_Subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.m_Subsystem = m_Subsystem; + addRequirements(m_Subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_Subsystem.drive(new Translation2d(1, 0), 0, false); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java index a2b7f7b1..d3d6de96 100644 --- a/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java +++ b/src/main/java/frc/robot/commands/driveCommands/rotateinPlace.java @@ -47,7 +47,7 @@ public void initialize() { currTheta = m_Subsystem.getHeading().getDegrees(); PID.setSetpoint(targetTheta); PID.setTolerance(Constants.DriveConstants.THETA_TOLERANCE, Constants.DriveConstants.STEADY_STATE_TOLERANCE); - PID.setPID(Constants.DriveConstants.kP, Constants.DriveConstants.kI, Constants.DriveConstants.kD); + // PID.setPID(0.005, 0, 0); PID.enableContinuousInput(-180, 180); } @@ -56,10 +56,12 @@ public void initialize() { public void execute() { currTheta = m_Subsystem.getHeading().getDegrees(); - angleRate = PID.calculate(currTheta) / 4; - m_Subsystem.drive(pose, -angleRate, true); + angleRate = PID.calculate(currTheta); + m_Subsystem.drive(pose, angleRate, true); - SmartDashboard.putNumber("Rotate In Place Angle PID Output", angleRate / 4); + SmartDashboard.putNumber("Rotate In Place Angle PID Output", angleRate); + SmartDashboard.putNumber("Rotate In Place Angle curr angle", currTheta); + SmartDashboard.putNumber("Rotate In Place Angle goal", PID.getSetpoint()); } diff --git a/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java b/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java index 38d24d57..558ff3ae 100644 --- a/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java +++ b/src/main/java/frc/robot/commands/ledcommands/ChangeLEDToGreen.java @@ -37,7 +37,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - System.out.println("green command executed succesfully"); return ledSubsystem.getColor(1).equals(new Color(0, 255, 0)); } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java index 754503d0..5ccfe62a 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/TeleopDrive.java @@ -33,6 +33,7 @@ public class TeleopDrive extends Command private final SwerveController controller; public boolean isFaceSpeaker = false; public boolean isBackSpeaker = false; + public boolean faceAmp = false; public boolean isSlow; public boolean isHeadingLock; private final PIDController PID; @@ -55,7 +56,6 @@ public TeleopDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, this.PID = new PIDController(Constants.DriveConstants.kP, Constants.DriveConstants.kI, Constants.DriveConstants.kD); this.PID.setTolerance(Constants.FaceSpeakerConstants.THETA_TOLERANCE, Constants.FaceSpeakerConstants.STEADY_STATE_TOLERANCE); - this.PID.setPID(Constants.FaceSpeakerConstants.kP, Constants.FaceSpeakerConstants.kI, Constants.FaceSpeakerConstants.kD); this.PID.enableContinuousInput(-180, 180); // Use addRequirements() here to declare subsystem dependencies. @@ -79,46 +79,59 @@ public void execute() double xVelocity = Math.pow(vX.getAsDouble(), 3); double yVelocity = Math.pow(vY.getAsDouble(), 3); double angVelocity = Math.pow(omega.getAsDouble(), 3); + if(DriverStation.getAlliance().isPresent() && - DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + DriverStation.getAlliance().get() == DriverStation.Alliance.Red && driveMode.getAsBoolean()) { xVelocity *= -1; yVelocity *= -1; } - - currTheta = swerve.getHeading().getDegrees(); + SmartDashboard.putNumber("Robot Rotation", currTheta); + + if(isSlow) + { + double slowFactor = 0.25; + xVelocity *= slowFactor; + yVelocity *= slowFactor; + angVelocity *= slowFactor; + } - // Drive using raw values. if(isFaceSpeaker) { - PID.setSetpoint(UtilMath.FrontSpeakerTheta(swerve.getPose())); + double ang = UtilMath.FrontSpeakerTheta(swerve.getPose()); + PID.setSetpoint(ang); angVelocity = PID.calculate(currTheta); + SmartDashboard.putNumber("isFaceSpeaker Goal", ang); + SmartDashboard.putNumber("isFaceSpeaker Result", angVelocity); } else if(isBackSpeaker) { - PID.setSetpoint(UtilMath.BackSpeakerTheta(swerve.getPose())); + double ang = UtilMath.BackSpeakerTheta(swerve.getPose()); + PID.setSetpoint(ang); angVelocity = PID.calculate(currTheta); + SmartDashboard.putNumber("isBackSpeaker Goal", ang); + SmartDashboard.putNumber("isBackSpeaker Result", angVelocity); } - if(isSlow) - { - double slowFactor = 0.25; - xVelocity *= slowFactor; - yVelocity *= slowFactor; - angVelocity *= slowFactor; + if (faceAmp) { + PID.setSetpoint(90); + angVelocity = PID.calculate(currTheta); + SmartDashboard.putNumber("faceAmp Goal", 90); + SmartDashboard.putNumber("faceAmp Result", angVelocity); } if(isHeadingLock) { - PID.setSetpoint(UtilMath.SourceIntakeHeading(swerve.getPose())); - double result = PID.calculate(currTheta)/4; - angVelocity += result; - SmartDashboard.putNumber("heading Lock Result", result); + double theta = UtilMath.SourceIntakeHeading(swerve.getPose()); + PID.setSetpoint(theta); + double result = PID.calculate(currTheta); + angVelocity += result; + SmartDashboard.putNumber("heading Lock Goal", theta); + SmartDashboard.putNumber("heading Lock Result", result); } - swerve.drive(new Translation2d(xVelocity * swerve.maximumSpeed, yVelocity * swerve.maximumSpeed), angVelocity * controller.config.maxAngularVelocity, driveMode.getAsBoolean()); diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 16f38f84..47d24b1b 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -30,31 +30,33 @@ public static final class Arm { public static final int kSlotIdx = 0; public static final int kPIDLoopIdx = 0; public static final int kTimeoutMs = 30; - public static final int ARM_MAX_ANGLE = 335; + public static final int ARM_MAX_ANGLE = 330; public static final int ARM_MIN_ANGLE = 10; public static final int ROTATION_TO_DEGREES = 360; public static final double GEAR_RATIO = 118.587767088; public static final double ENCODER_RAW_TO_ROTATION = 8132.; public static final double ENCODER_OFFSET = 21.8; public static final int ARM_ID = 18; + public static final int MAX_VOLT = 12; + // - public static final double KP = 6; - public static final double KI = 0.1; - public static final double KD = 0.15; + public static final double KP = 8; + public static final double KI = 0; + public static final double KD = 0; public static final double KSVOLTS = 1.5; - public static final double KGVOLTS = .0; - public static final double KVVOLTS = 2; + public static final double KGVOLTS = 0; + public static final double KVVOLTS = 1.5; public static final double KAVOLTS = 0; - public static final double VEL = 100 * Math.PI / 180; - public static final double ACC = 180 * Math.PI / 180; + public static final double VEL = 230 * Math.PI / 180; + public static final double ACC = 360 * Math.PI / 180; // //Arm ID Jalen Tolbert public static final int ENCODER_DIO_SLOT = 0; public static final int AMP_ANGLE = 300; - public static final int TRAP_ANGLE = 290; - public static final int SPEAKER_LOW_ANGLE = 165; - public static final int SPEAKER_HIGH_ANGLE = 238; - public static final int INTAKE_ANGLE = 60; + public static final int TRAP_ANGLE = 275; + public static final int SPEAKER_LOW_ANGLE = 160; + public static final int SPEAKER_HIGH_ANGLE = 236; + public static final int INTAKE_ANGLE = 63; public static final int CLIMBING_ANGLE = 45; } @@ -63,6 +65,7 @@ public static final class Arm { private ArmFeedforward armFeedForward; public boolean emergencyStop = false; private double prevArmAngle; + public boolean booster = false; public ArmSubsystem(int armId, String armCanbus, int channel1) { @@ -82,12 +85,6 @@ public ArmSubsystem(int armId, String armCanbus, int channel1) Shuffleboard.getTab("Arm").addDouble("Encoder Angle", ()->encoderGetAngle()); Shuffleboard.getTab("Arm").addDouble("Goal in degrees", ()->getController().getGoal().position * (180/Math.PI)); - - if(DriverStation.isTest()) { - Shuffleboard.getTab("Arm").addDouble("Arm Stator Current", () -> m_arm.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Arm").addDouble("Arm Rotor Velocity", () -> m_arm.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Arm").addDouble("Arm Temperature", () -> m_arm.getDeviceTemp().getValueAsDouble()); - } DataLogManager.log("Arm P: " + Arm.KP); DataLogManager.log("Arm I: " + Arm.KI); @@ -103,12 +100,34 @@ public ArmSubsystem(int armId, String armCanbus, int channel1) //Looking at the left of the robot, counterclockwise arm spin is positive public void spinArm(double speed) { - if(speed < -15) { - speed = -15; + if(speed < -Arm.MAX_VOLT) { + speed = -Arm.MAX_VOLT; } - else if (speed > 15) { - speed = 15; + else if (speed > Arm.MAX_VOLT) { + speed = Arm.MAX_VOLT; + } + + if(encoderGetAngle() >= 300) { + if (speed > 1) { + speed = 1; + } } + + if(encoderGetAngle() >= 332) { + speed = -1; + } + + if(isEmergencyStop()) { + speed = 0; + } + + if(booster) + { + speed = Arm.MAX_VOLT; + } + + SmartDashboard.putNumber("Arm speed", speed); + VoltageOut armSpinRequest = new VoltageOut(speed, true, false, false, false); m_arm.setControl(armSpinRequest); } @@ -146,15 +165,20 @@ public boolean checkEncoderAngleForClimb() { @Override public void useOutput(double output, State setPoint) { - //Comment out for testing purposes + double currentArmAngle = encoderGetAngle(); + if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { + emergencyStop = true; + } + prevArmAngle = currentArmAngle; + + double feedForward = armFeedForward.calculate(setPoint.position, setPoint.velocity); if(!isEmergencyStop()) { SmartDashboard.putNumber("Arm PID output", output); SmartDashboard.putNumber("Arm feed forward", feedForward); - SmartDashboard.putNumber("Arm speed", output + feedForward); - SmartDashboard.putNumber("Arm FF setPoint Position", setPoint.position); - SmartDashboard.putNumber("Arm FF setPoint velocity", setPoint.velocity); + SmartDashboard.putNumber("Arm FF setPoint Position", setPoint.position * 180 / Math.PI); + SmartDashboard.putNumber("Arm FF setPoint velocity", setPoint.velocity * 180 / Math.PI); spinArm(output + feedForward); } else @@ -173,12 +197,4 @@ public double getMeasurement() { return encoderGetAngle() * Math.PI/180; } - - @Override - public void periodic() { - double currentArmAngle = encoderGetAngle(); - if (Math.abs(currentArmAngle - prevArmAngle) >= 50) { - emergencyStop = true; - } - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Climbsubsystem.java b/src/main/java/frc/robot/subsystems/Climbsubsystem.java index 1b1c5334..ddc483ab 100644 --- a/src/main/java/frc/robot/subsystems/Climbsubsystem.java +++ b/src/main/java/frc/robot/subsystems/Climbsubsystem.java @@ -36,6 +36,7 @@ public class Climbsubsystem extends SubsystemBase { private boolean checkForUp = false; private boolean emergencyStop = false; + public boolean armAngleCheck = true; DigitalInput bottomLeft = new DigitalInput(ClimbConstants.BOT_LEFT_LIMIT_ID); DigitalInput bottomRight = new DigitalInput(ClimbConstants.BOT_RIGHT_LIMIT_ID); @@ -155,16 +156,16 @@ public void downMotor() { rightPIDcontroller.setGoal(.3); //0.3 } - public void downMotorHarmony() { - checkForUp = false; - - } - @Override public void periodic() { double leftPIDvalue = leftPIDcontroller.calculate(getPositionLeft()); double rightPIDvalue = rightPIDcontroller.calculate(getPositionRight()); + if (!armAngleCheck) { + leftPIDvalue = 0; + rightPIDvalue = 0; + } + if (checkForUp && getTopLeftLimit()) { leftPIDvalue = 0; } @@ -172,10 +173,10 @@ public void periodic() { rightPIDvalue = 0; } - if (!checkForUp && getBotLeftLimit()) { + if (!checkForUp && getBotLeftLimit() && getPositionLeft() > -15) { leftPIDvalue = 0; } - if (!checkForUp && getBotRightLimit()) { + if (!checkForUp && getBotRightLimit() && getPositionRight() < 15) { rightPIDvalue = 0; } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 87031bfc..57cfeeec 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -47,20 +47,7 @@ public IntakeSubsystem(int lowerIntakeId, int spinIntakeId, String can) { UtilMotor.configMotorSupplyCurrent(m_spinIntake, 80); Shuffleboard.getTab("Intake").addDouble("Encoder Angle", () -> encoderGetAngle()); - Shuffleboard.getTab("Intake").addBoolean("Sensor value", () -> isNotePresent()); - - if(DriverStation.isTest()) { - Shuffleboard.getTab("Intake").addDouble("Intake Arm Stator Current", () -> m_moveIntakeArm.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Intake Arm Rotor Velocity", () -> m_moveIntakeArm.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Intake Arm Temperature", () -> m_moveIntakeArm.getDeviceTemp().getValueAsDouble()); - - Shuffleboard.getTab("Intake").addDouble("Spin Intake Stator Current", () -> m_spinIntake.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Spin Intake Rotor Velocity", () -> m_spinIntake.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Intake").addDouble("Spin Intake Temperature", () -> m_spinIntake.getDeviceTemp().getValueAsDouble()); - } - - //m_Encoder.reset(); // Logger.recordOutput("Intake/EncoderAngle", encoderGetAngle()); // Logger.recordOutput("Intake/SensorValue", isNotePresent()); @@ -84,11 +71,11 @@ else if (intakeMotorVelocity < -1) { } if (!emergencyStop) { - // m_spinIntake.set(-intakeMotorVelocity); + m_spinIntake.set(-intakeMotorVelocity); - intakeMotorVelocity *= 100; - spinRequest1 = new VelocityVoltage(-intakeMotorVelocity, 150, true, 0, 0,false, false, false); - m_spinIntake.setControl(spinRequest1); + // intakeMotorVelocity *= 100; + // spinRequest1 = new VelocityVoltage(-intakeMotorVelocity, 150, true, 0, 0,false, false, false); + // m_spinIntake.setControl(spinRequest1); SmartDashboard.putNumber("Intake spin motor speed", intakeMotorVelocity); } diff --git a/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java b/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java index e5d63934..bb7f68ac 100644 --- a/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java +++ b/src/main/java/frc/robot/subsystems/PizzaBoxSubsystem.java @@ -25,6 +25,7 @@ public static final class PizzaBox { public static final int PIZZABOX_ID = 23; public static final int SERVO_PWN_SLOT = 0; + public static final int SERVO_PWN_SLOT_THE_SECOND = 1; public static final int START_SPIN_DEGREE = 180; } @@ -32,20 +33,15 @@ public static final class PizzaBox { private TalonFX m_pizzaBox; private Servo m_servo; + private Servo m_servo_the_second; - public PizzaBoxSubsystem(int pizzaBoxId, String pizzaBoxCanbus, int channelServo) + public PizzaBoxSubsystem(int pizzaBoxId, String pizzaBoxCanbus, int channelServo, int channelServoTheSecond) { m_pizzaBox = new TalonFX(pizzaBoxId, pizzaBoxCanbus); + m_servo_the_second = new Servo(channelServoTheSecond); m_servo = new Servo(channelServo); - UtilMotor.configMotor(m_pizzaBox, .5, 0, 0, .12, 15, 80, false); //UtilMotor.configMotorStatorCurrent(m_pizzaBox, 80); - - if(DriverStation.isTest()) { - Shuffleboard.getTab("Pizza").addDouble("Pizzabox Stator Current", () -> m_pizzaBox.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Pizza").addDouble("Pizzabox Rotor Velocity", () -> m_pizzaBox.getRotorVelocity().getValueAsDouble()); - Shuffleboard.getTab("Pizza").addDouble("Pizzabox Temperature", () -> m_pizzaBox.getDeviceTemp().getValueAsDouble()); - } } //Spins "Pizzabox" motor: velocity in rotations/sec and acceleration in rotations/sec^2 @@ -57,11 +53,17 @@ public void spinPizzaBoxMotor(double velocity, double acceleration) { public void setServoAngle(double angle) { m_servo.setAngle(angle); } + public void setServoAngleTheSecond(double angle) { + m_servo_the_second.setAngle(angle); + } //Returns the servo postion from 0.0 to 1.0 (0 degrees to 180 degrees) public double getServoAngle() { return m_servo.getAngle(); } + public double getServoAngleTheSecond() { + return m_servo_the_second.getAngle(); + } //Stops pizzaBox motor public void stopPizzaBoxMotor() { diff --git a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java index af5a8635..bf0b1ed1 100644 --- a/src/main/java/frc/robot/subsystems/ReactionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ReactionSubsystem.java @@ -6,32 +6,49 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + import com.ctre.phoenix6.signals.NeutralModeValue; public class ReactionSubsystem extends SubsystemBase { private TalonFX m_reactionArm; + private final double REACTION_BAR_PID_KP = 0.09; + private final double REACTION_BAR_PID_KI = 0; + private final double REACTION_BAR_PID_KD = 0; + public PIDController PIDcontroller = new PIDController(REACTION_BAR_PID_KP, REACTION_BAR_PID_KI, REACTION_BAR_PID_KD); + /** Creates a new ReactionSubsystem. */ public ReactionSubsystem(int armID, String canbus) { m_reactionArm = new TalonFX(armID, canbus); MotorOutputConfigs motorOutput = new MotorOutputConfigs(); motorOutput.NeutralMode = NeutralModeValue.Brake; m_reactionArm.getConfigurator().apply(motorOutput); - + PIDcontroller.setTolerance(Constants.ReactionConstants.tolerance); } @Override public void periodic() { // This method will be called once per scheduler run + double PIDvalue = PIDcontroller.calculate(getPos()); + if (PIDvalue > 1) + PIDvalue = 1; + + if (PIDvalue < -1) + PIDvalue = -1; + + m_reactionArm.set(PIDvalue); } public void spinForward() { - m_reactionArm.set(-0.1); + PIDcontroller.setSetpoint(-2.5); } public void spinBackward() { - m_reactionArm.set(0.1); + PIDcontroller.setSetpoint(-0.8); } public void stop() { @@ -40,8 +57,7 @@ public void stop() public double getPos() { - double posVal = m_reactionArm.getRotorPosition().getValue(); - return posVal; + return m_reactionArm.getRotorPosition().getValue(); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index c041e7eb..0b2f6c00 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -45,9 +45,6 @@ public class SwerveSubsystem extends SubsystemBase */ public double maximumSpeed = Units.feetToMeters(14.5); //14.5 - private StructArrayPublisher swerveStatePublisher = NetworkTableInstance.getDefault() - .getStructArrayTopic("Swerve State", SwerveModuleState.struct).publish(); - /** * Initialize {@link SwerveDrive} with the directory provided. * @@ -70,7 +67,7 @@ public SwerveSubsystem(File directory) System.out.println("}"); // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created. - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.NONE; + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; // 3/8/2024 changed from HIGH to NONE to see if prevent loop overrun try { @@ -81,7 +78,7 @@ public SwerveSubsystem(File directory) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection(true); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. setupPathPlanner(); @@ -97,11 +94,6 @@ public SwerveSubsystem(File directory) TalonFX driveMotorTalon = (TalonFX)driveMotor.getMotor(); TalonFX angleMotorTalon = (TalonFX) angleMotor.getMotor(); - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Drive Motor StatorCurrent", () -> driveMotorTalon.getStatorCurrent().getValueAsDouble()); - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Angle Motor StatorCurrent", () -> angleMotorTalon.getStatorCurrent().getValueAsDouble()); - - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Drive Motor Temp", () -> driveMotorTalon.getDeviceTemp().getValueAsDouble()); - Shuffleboard.getTab("Drive Train").addDouble("Drive Train Module " + i + " Angle Motor Temp", () -> angleMotorTalon.getDeviceTemp().getValueAsDouble()); } } } @@ -130,13 +122,13 @@ public void setupPathPlanner() new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class new PIDConstants(1,0,0), // Translation PID constants - new PIDConstants(7.75,0,0), + new PIDConstants(9,0,0), // Rotation PID constants 4.5, // Max module speed, in m/s swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() + new ReplanningConfig(true, true) // Default path replanning config. See the API for the options here ), () -> { @@ -326,7 +318,6 @@ public void drive(ChassisSpeeds velocity) @Override public void periodic() { - swerveStatePublisher.set(swerveDrive.getStates()); } @Override @@ -549,7 +540,19 @@ public Field2d getField2d() public void setHeading() { - resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(Math.PI))); + double xPos = getPose().getX(); + double yPos = getPose().getY(); + + if(DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == DriverStation.Alliance.Red) + { + resetOdometry(new Pose2d(xPos, yPos, new Rotation2d(Math.PI))); + } else if (DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) + { + resetOdometry(new Pose2d(xPos, yPos, new Rotation2d(0))); + } + //resetOdometry(new Pose2d(new Translation2d(), new Rotation2d(Math.PI))); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/testcontainers/ArmContainer.java b/src/main/java/frc/robot/testcontainers/ArmContainer.java index 30d0fd3c..9e3fb4f7 100644 --- a/src/main/java/frc/robot/testcontainers/ArmContainer.java +++ b/src/main/java/frc/robot/testcontainers/ArmContainer.java @@ -20,8 +20,8 @@ public class ArmContainer implements BaseContainer { ArmSubsystem arm = new ArmSubsystem(ArmSubsystem.Arm.ARM_ID, "CAN_Network", ArmSubsystem.Arm.ENCODER_DIO_SLOT); - PizzaBoxSubsystem pizzaBox = new PizzaBoxSubsystem(PizzaBoxSubsystem.PizzaBox.PIZZABOX_ID, - "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT); + // PizzaBoxSubsystem pizzaBox = new PizzaBoxSubsystem(PizzaBoxSubsystem.PizzaBox.PIZZABOX_ID, + // "rio", PizzaBoxSubsystem.PizzaBox.SERVO_PWN_SLOT); private IntakeSubsystem intakeSubsystem = new IntakeSubsystem(Constants.IntakeConstants.INTAKE_LOWER_INTAKE_ID,Constants.IntakeConstants.INTAKE_SPIN_MOTOR_ID, "rio"); @@ -36,8 +36,8 @@ private void configureBindings() { arm.enable(); //Pick Up and Put in PizzaBox Command - m_driverController.a().onTrue(new PickUpFromGroundAndPassToPizzaBox(pizzaBox, arm, intakeSubsystem)); - m_driverController.x().whileTrue(new SpinNoteContainerMotor(pizzaBox, 400, 100)); + // m_driverController.a().onTrue(new PickUpFromGroundAndPassToPizzaBox(pizzaBox, arm, intakeSubsystem)); + // m_driverController.x().whileTrue(new SpinNoteContainerMotor(pizzaBox, 400, 100)); }