diff --git a/simgui-ds.json b/simgui-ds.json index ae2dc78..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/simgui.json b/simgui.json index 0b45c4f..9a81d43 100644 --- a/simgui.json +++ b/simgui.json @@ -26,6 +26,7 @@ "/SmartDashboard/Swerve/Module 2": "Mechanism2d", "/SmartDashboard/Swerve/Module 3": "Mechanism2d", "/SmartDashboard/Visualizers/Arm": "Mechanism2d", + "/SmartDashboard/Visualizers/Arm-Elevator": "Mechanism2d", "/SmartDashboard/Visualizers/Elevator": "Mechanism2d", "/SmartDashboard/Visualizers/Superstructure": "Mechanism2d" }, @@ -47,7 +48,7 @@ "visible": true } }, - "/SmartDashboard/Visualizers/Superstructure": { + "/SmartDashboard/Visualizers/Arm-Elevator": { "window": { "visible": true } @@ -95,13 +96,8 @@ "Server": { "Publishers": { "open": true - }, - "open": true - }, - "visible": true - }, - "NetworkTables View": { - "visible": false + } + } }, "Plot": { "Plot <0>": { @@ -113,8 +109,7 @@ 0.0, 0.8500000238418579 ], - "height": 0, - "height": 246, + "height": 9, "series": [ { "color": [ @@ -147,8 +142,7 @@ } ], "window": { - "name": "Elevator Target Vs Current Height", - "visible": false + "name": "Elevator Target Vs Current Height" } }, "Plot <1>": { @@ -161,7 +155,6 @@ 0.8500000238418579 ], "height": 4, - "height": 252, "series": [ { "color": [ @@ -194,8 +187,7 @@ } ], "window": { - "name": "Arm Target vs Current Angle", - "visible": false + "name": "Arm Target vs Current Angle" } } } diff --git a/src/main/deploy/pathplanner/autos/Blue G + 1 Algae (1 + 1).auto b/src/main/deploy/pathplanner/autos/Blue G + 1 Algae (1 + 1).auto new file mode 100644 index 0000000..968278e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue G + 1 Algae (1 + 1).auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Mid Bottom to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue G to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue GH Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Bottom Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue G + 2 Algae (1 + 2).auto b/src/main/deploy/pathplanner/autos/Blue G + 2 Algae (1 + 2).auto new file mode 100644 index 0000000..c5b12b7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue G + 2 Algae (1 + 2).auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Mid Bottom to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue G to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue IJ Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Bottom Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue G + 3 Algae (1 + 3).auto b/src/main/deploy/pathplanner/autos/Blue G + 3 Algae (1 + 3).auto new file mode 100644 index 0000000..f2ed2f1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue G + 3 Algae (1 + 3).auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Mid Bottom to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue G to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue IJ Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Barge 3 to EF Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue EF Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Bottom Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue H + 1 Algae (1 + 1).auto b/src/main/deploy/pathplanner/autos/Blue H + 1 Algae (1 + 1).auto new file mode 100644 index 0000000..89ba4b6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue H + 1 Algae (1 + 1).auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Mid Top to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue H to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue GH Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Top Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue H + 2 Algae (1 + 2).auto b/src/main/deploy/pathplanner/autos/Blue H + 2 Algae (1 + 2).auto new file mode 100644 index 0000000..c2cc92b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue H + 2 Algae (1 + 2).auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Mid Top to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue H to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue IJ Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Top Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue H + 3 Algae (1 + 3).auto b/src/main/deploy/pathplanner/autos/Blue H + 3 Algae (1 + 3).auto new file mode 100644 index 0000000..d08e31b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue H + 3 Algae (1 + 3).auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Blue Mid Top to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue H to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue IJ Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue Barge 3 to EF Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Blue EF Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Blue Top Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..c869b4c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "aaaa" + } + }, + { + "type": "path", + "data": { + "pathName": "aaa" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red G + 1 Algae (1 + 1).auto b/src/main/deploy/pathplanner/autos/Red G + 1 Algae (1 + 1).auto new file mode 100644 index 0000000..9a560ed --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red G + 1 Algae (1 + 1).auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Mid Bottom to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Red G to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red GH Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Bottom Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red G + 2 Algae (1 + 2).auto b/src/main/deploy/pathplanner/autos/Red G + 2 Algae (1 + 2).auto new file mode 100644 index 0000000..da1c254 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red G + 2 Algae (1 + 2).auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Mid Bottom to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Red G to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red IJ Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Bottom Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red G + 3 Algae (1 + 3).auto b/src/main/deploy/pathplanner/autos/Red G + 3 Algae (1 + 3).auto new file mode 100644 index 0000000..88f65d3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red G + 3 Algae (1 + 3).auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Mid Bottom to G" + } + }, + { + "type": "path", + "data": { + "pathName": "Red G to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red IJ Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Barge 3 to EF Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red EF Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Bottom Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red H + 1 Algae (1 + 1).auto b/src/main/deploy/pathplanner/autos/Red H + 1 Algae (1 + 1).auto new file mode 100644 index 0000000..ea0d8cd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red H + 1 Algae (1 + 1).auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Mid Top to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Red H to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red GH Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Top Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red H + 2 Algae (1 + 2).auto b/src/main/deploy/pathplanner/autos/Red H + 2 Algae (1 + 2).auto new file mode 100644 index 0000000..27c7ce5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red H + 2 Algae (1 + 2).auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Mid Top to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Red H to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red IJ Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Top Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red H + 3 Algae (1 + 3).auto b/src/main/deploy/pathplanner/autos/Red H + 3 Algae (1 + 3).auto new file mode 100644 index 0000000..6d5ba4c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red H + 3 Algae (1 + 3).auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Red Mid Top to H" + } + }, + { + "type": "path", + "data": { + "pathName": "Red H to GH Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red GH Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Barge 3 to IJ Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red IJ Algae to Barge 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Red Barge 3 to EF Algae" + } + }, + { + "type": "path", + "data": { + "pathName": "Red EF Algae to Barge 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Red Top Autons", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue AB.path b/src/main/deploy/pathplanner/paths/Blue AB.path index 15b3813..b3960b0 100644 --- a/src/main/deploy/pathplanner/paths/Blue AB.path +++ b/src/main/deploy/pathplanner/paths/Blue AB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.258745029994419, - "y": 4.000363159179687 + "x": 3.153311593191964, + "y": 4.056981985909598 }, "prevControl": null, "nextControl": { - "x": 3.2713230579418937, - "y": 3.7170926007311036 + "x": 3.1658896211394385, + "y": 3.773711427461014 }, "isLocked": false, "linkedName": "Blue A" }, { "anchor": { - "x": 3.237576729910714, - "y": 3.6696275983537943 + "x": 3.165655517578125, + "y": 3.745935494559151 }, "prevControl": { - "x": 3.2139588422094723, - "y": 3.918509488776805 + "x": 3.1420376298768833, + "y": 3.994817384982162 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue ABCD Algae.path b/src/main/deploy/pathplanner/paths/Blue ABCD Algae.path new file mode 100644 index 0000000..b9156a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue ABCD Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.1795806884765625, + "y": 3.9121194022042407 + }, + "prevControl": null, + "nextControl": { + "x": 3.152801513671875, + "y": 3.469319370814732 + }, + "isLocked": false, + "linkedName": "Blue AB Algae" + }, + { + "anchor": { + "x": 3.932611083984375, + "y": 2.7850987025669642 + }, + "prevControl": { + "x": 2.932611083984375, + "y": 2.7850987025669642 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue CD Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Blue Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Barge 1 + Barge 2.path b/src/main/deploy/pathplanner/paths/Blue Barge 1 + Barge 2.path new file mode 100644 index 0000000..f8d3c81 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Barge 1 + Barge 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 7.280582536969865 + }, + "prevControl": null, + "nextControl": { + "x": 7.302855387595889, + "y": 7.749066169239989 + }, + "isLocked": false, + "linkedName": "Blue Barge 1" + }, + { + "anchor": { + "x": 7.3, + "y": 6.090413992745535 + }, + "prevControl": { + "x": 7.269238140330965, + "y": 5.712091045409606 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Barge 2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Barge 3 to EF Algae.path b/src/main/deploy/pathplanner/paths/Blue Barge 3 to EF Algae.path new file mode 100644 index 0000000..68982fc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Barge 3 to EF Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 4.996803501674107 + }, + "prevControl": null, + "nextControl": { + "x": 6.316773768833705, + "y": 2.2173801967075897 + }, + "isLocked": false, + "linkedName": "Blue Barge 3" + }, + { + "anchor": { + "x": 5.250605555943079, + "y": 2.9040492466517858 + }, + "prevControl": { + "x": 5.762929425920759, + "y": 1.9403560093470984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue EF Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "Blue To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Barge 3 to IJ Algae.path b/src/main/deploy/pathplanner/paths/Blue Barge 3 to IJ Algae.path new file mode 100644 index 0000000..cc2f16e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Barge 3 to IJ Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 4.996803501674107 + }, + "prevControl": null, + "nextControl": { + "x": 5.780119105747767, + "y": 6.098677280970982 + }, + "isLocked": false, + "linkedName": "Blue Barge 3" + }, + { + "anchor": { + "x": 5.036270141601562, + "y": 5.243937029157366 + }, + "prevControl": { + "x": 5.613782174246652, + "y": 6.43344247000558 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue IJ Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "Blue To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Barge 3.path b/src/main/deploy/pathplanner/paths/Blue Barge 3.path new file mode 100644 index 0000000..a8de490 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 4.996803501674107 + }, + "prevControl": null, + "nextControl": { + "x": 7.306832938492079, + "y": 4.429454377636027 + }, + "isLocked": false, + "linkedName": "Blue Barge 3" + }, + { + "anchor": { + "x": 7.3, + "y": 4.0460662841796875 + }, + "prevControl": { + "x": 7.326019304296016, + "y": 4.294708589115817 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Bottom to E.path b/src/main/deploy/pathplanner/paths/Blue Bottom to E.path index 0ffa493..5fac649 100644 --- a/src/main/deploy/pathplanner/paths/Blue Bottom to E.path +++ b/src/main/deploy/pathplanner/paths/Blue Bottom to E.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.146367100306919, - "y": 2.961790248325893 + "x": 7.043586077008928, + "y": 2.833760288783482 }, "prevControl": null, "nextControl": { - "x": 5.37975987018341, - "y": 2.9925964828272886 + "x": 5.316819562964643, + "y": 2.813839497556888 }, "isLocked": false, "linkedName": "Blue Bottom Start" }, { "anchor": { - "x": 5.076107352120536, - "y": 2.961790248325893 + "x": 5.108242361886161, + "y": 2.833760288783482 }, "prevControl": { - "x": 6.204489207954774, - "y": 2.9088323871335353 + "x": 6.511719784636691, + "y": 2.8439019987208347 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue C to HP.path b/src/main/deploy/pathplanner/paths/Blue C to HP.path index 2bd779e..873320d 100644 --- a/src/main/deploy/pathplanner/paths/Blue C to HP.path +++ b/src/main/deploy/pathplanner/paths/Blue C to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.855027988978794, - "y": 2.9843867710658483 + "x": 3.784075927734375, + "y": 2.8778821672712045 }, "prevControl": null, "nextControl": { - "x": 1.9929960345923734, - "y": 0.9965224116572569 + "x": 1.772198058835988, + "y": 0.8323176781486934 }, "isLocked": false, "linkedName": "Blue C" }, { "anchor": { - "x": 1.6107801164899553, - "y": 0.6035906110491066 + "x": 1.6300611223493302, + "y": 0.7061676025390625 }, "prevControl": { - "x": 3.302453767164401, - "y": 2.394714405213484 + "x": 3.6673834940017462, + "y": 2.750659293932905 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Blue To HP", "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue CD.path b/src/main/deploy/pathplanner/paths/Blue CD.path index 1bfbee9..04d0d84 100644 --- a/src/main/deploy/pathplanner/paths/Blue CD.path +++ b/src/main/deploy/pathplanner/paths/Blue CD.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.855027988978794, - "y": 2.9843867710658483 + "x": 3.784075927734375, + "y": 2.8778821672712045 }, "prevControl": null, "nextControl": { - "x": 4.411091609639583, - "y": 2.618491159825125 + "x": 4.340139548395165, + "y": 2.511986556030481 }, "isLocked": false, "linkedName": "Blue C" }, { "anchor": { - "x": 4.179234531947544, - "y": 2.7651035853794643 + "x": 4.066455950055803, + "y": 2.720726667131696 }, "prevControl": { - "x": 3.631568462729578, - "y": 3.1062101728477565 + "x": 3.5187898808378373, + "y": 3.0618332545999882 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue D to HP.path b/src/main/deploy/pathplanner/paths/Blue D to HP.path index 489b031..43907bc 100644 --- a/src/main/deploy/pathplanner/paths/Blue D to HP.path +++ b/src/main/deploy/pathplanner/paths/Blue D to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.179234531947544, - "y": 2.7651035853794643 + "x": 4.066455950055803, + "y": 2.720726667131696 }, "prevControl": null, "nextControl": { - "x": 1.852563155276703, - "y": 0.7987139613765637 + "x": 1.8537256414828387, + "y": 0.8558666265749801 }, "isLocked": false, "linkedName": "Blue D" }, { "anchor": { - "x": 1.6107801164899553, - "y": 0.6035906110491066 + "x": 1.6300611223493302, + "y": 0.7061676025390625 }, "prevControl": { - "x": 3.7465983147945137, - "y": 2.401736081659627 + "x": 3.8420803413067204, + "y": 2.5344439999055717 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Blue To HP", "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue E to HP.path b/src/main/deploy/pathplanner/paths/Blue E to HP.path index 9a5c410..20a6045 100644 --- a/src/main/deploy/pathplanner/paths/Blue E to HP.path +++ b/src/main/deploy/pathplanner/paths/Blue E to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.076107352120536, - "y": 2.961790248325893 + "x": 5.108242361886161, + "y": 2.833760288783482 }, "prevControl": null, "nextControl": { - "x": 1.6682323882223793, - "y": 0.7595495018393912 + "x": 2.9461172921316963, + "y": 1.5098488943917416 }, "isLocked": false, "linkedName": "Blue E" }, { "anchor": { - "x": 1.6107801164899553, - "y": 0.6035906110491066 + "x": 1.6300611223493302, + "y": 0.7061676025390625 }, "prevControl": { - "x": 4.54455805635309, - "y": 2.467982186512902 + "x": 4.186623461381711, + "y": 2.307647543552958 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Blue To HP", "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue EF Algae to Barge 3.path b/src/main/deploy/pathplanner/paths/Blue EF Algae to Barge 3.path new file mode 100644 index 0000000..9ca19c3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue EF Algae to Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.250605555943079, + "y": 2.9040492466517858 + }, + "prevControl": null, + "nextControl": { + "x": 6.369413975306919, + "y": 2.6329929896763393 + }, + "isLocked": false, + "linkedName": "Blue EF Algae" + }, + { + "anchor": { + "x": 7.3, + "y": 4.996803501674107 + }, + "prevControl": { + "x": 6.404150390625, + "y": 3.427594866071429 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Barge 3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue To Barge", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue EF.path b/src/main/deploy/pathplanner/paths/Blue EF.path index 16322d6..11e093f 100644 --- a/src/main/deploy/pathplanner/paths/Blue EF.path +++ b/src/main/deploy/pathplanner/paths/Blue EF.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.076107352120536, - "y": 2.961790248325893 + "x": 5.108242361886161, + "y": 2.833760288783482 }, "prevControl": null, "nextControl": { - "x": 5.473969377790179, - "y": 3.1778089250837054 + "x": 5.506104387555804, + "y": 3.0497789655412944 }, "isLocked": false, "linkedName": "Blue E" }, { "anchor": { - "x": 5.42387956891741, - "y": 3.1112435477120535 + "x": 5.446323067801339, + "y": 2.9757154192243305 }, "prevControl": { - "x": 5.137585525225876, - "y": 2.9188451602046657 + "x": 5.160029024109805, + "y": 2.7833170317169427 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue EFGH.path b/src/main/deploy/pathplanner/paths/Blue EFGH.path new file mode 100644 index 0000000..aecc18e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue EFGH.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.250605555943079, + "y": 2.9040492466517858 + }, + "prevControl": null, + "nextControl": { + "x": 6.250605555943078, + "y": 2.9040492466517858 + }, + "isLocked": false, + "linkedName": "Blue EF Algae" + }, + { + "anchor": { + "x": 5.8211805071149545, + "y": 4.1560904366629465 + }, + "prevControl": { + "x": 5.8211294991629465, + "y": 3.5018114362444193 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue GH Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue G to GH Algae.path b/src/main/deploy/pathplanner/paths/Blue G to GH Algae.path new file mode 100644 index 0000000..ece4ed6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue G to GH Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.814549473353794, + "y": 4.018959416841206 + }, + "prevControl": null, + "nextControl": { + "x": 7.540658569335937, + "y": 3.995721435546875 + }, + "isLocked": false, + "linkedName": "Blue G" + }, + { + "anchor": { + "x": 5.8211805071149545, + "y": 4.1560904366629465 + }, + "prevControl": { + "x": 7.569018990652901, + "y": 4.191081891741071 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue GH Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue GH Algae to Barge 3.path b/src/main/deploy/pathplanner/paths/Blue GH Algae to Barge 3.path new file mode 100644 index 0000000..342a3cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue GH Algae to Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8211805071149545, + "y": 4.1560904366629465 + }, + "prevControl": null, + "nextControl": { + "x": 7.167092978376024, + "y": 4.906645816142244 + }, + "isLocked": false, + "linkedName": "Blue GH Algae" + }, + { + "anchor": { + "x": 7.3, + "y": 4.996803501674107 + }, + "prevControl": { + "x": 5.981079939780231, + "y": 4.197919706270037 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Barge 3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue To Barge", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue GH.path b/src/main/deploy/pathplanner/paths/Blue GH.path index 7ccb855..35dab48 100644 --- a/src/main/deploy/pathplanner/paths/Blue GH.path +++ b/src/main/deploy/pathplanner/paths/Blue GH.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 5.730692400251115, + "x": 5.814549473353794, "y": 4.018959416841206 }, "prevControl": null, "nextControl": { - "x": 5.733025547933166, + "x": 5.816882621035845, "y": 4.476566602264367 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 5.737221418108259, + "x": 5.832912336077008, "y": 4.340860954083895 }, "prevControl": { - "x": 5.736206188870759, + "x": 5.8318971068395085, "y": 4.090863015473203 }, "nextControl": null, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue H to GH Algae.path b/src/main/deploy/pathplanner/paths/Blue H to GH Algae.path new file mode 100644 index 0000000..026b458 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue H to GH Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.832912336077008, + "y": 4.340860954083895 + }, + "prevControl": null, + "nextControl": { + "x": 7.579271589006696, + "y": 4.376954868861606 + }, + "isLocked": false, + "linkedName": "Blue H" + }, + { + "anchor": { + "x": 5.8211805071149545, + "y": 4.1560904366629465 + }, + "prevControl": { + "x": 7.565040370396205, + "y": 4.138390677315848 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue GH Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to A.path b/src/main/deploy/pathplanner/paths/Blue HP to A.path index 882214c..e0bef6e 100644 --- a/src/main/deploy/pathplanner/paths/Blue HP to A.path +++ b/src/main/deploy/pathplanner/paths/Blue HP to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.616084943498884, - "y": 7.418355015345981 + "x": 1.6300611223493302, + "y": 7.31465584891183 }, "prevControl": null, "nextControl": { - "x": 3.1909915941298803, - "y": 4.099047248041398 + "x": 3.148223666494421, + "y": 4.034178936131832 }, "isLocked": false, "linkedName": "Blue Top HP" }, { "anchor": { - "x": 3.258745029994419, - "y": 4.000363159179687 + "x": 3.153311593191964, + "y": 4.056981985909598 }, "prevControl": { - "x": 1.9151657059057003, - "y": 6.962576113065726 + "x": 1.8943586609313032, + "y": 6.712410237339751 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to B.path b/src/main/deploy/pathplanner/paths/Blue HP to B.path index 6ac3d27..eff0a8a 100644 --- a/src/main/deploy/pathplanner/paths/Blue HP to B.path +++ b/src/main/deploy/pathplanner/paths/Blue HP to B.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6107801164899553, - "y": 0.6035906110491066 + "x": 1.6300611223493302, + "y": 0.7061676025390625 }, "prevControl": null, "nextControl": { - "x": 3.2357603694483883, - "y": 3.760080758508464 + "x": 3.1725272995316844, + "y": 3.861172610633482 }, "isLocked": false, "linkedName": "Blue Bottom HP" }, { "anchor": { - "x": 3.237576729910714, - "y": 3.6696275983537943 + "x": 3.165655517578125, + "y": 3.745935494559151 }, "prevControl": { - "x": 1.939458208577061, - "y": 1.138256866398926 + "x": 2.0116842546274074, + "y": 1.3467681023447247 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to C.path b/src/main/deploy/pathplanner/paths/Blue HP to C.path index fe74003..4c3e6bf 100644 --- a/src/main/deploy/pathplanner/paths/Blue HP to C.path +++ b/src/main/deploy/pathplanner/paths/Blue HP to C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6107801164899553, - "y": 0.6035906110491066 + "x": 1.6300611223493302, + "y": 0.7061676025390625 }, "prevControl": null, "nextControl": { - "x": 3.8438135795056487, - "y": 2.9487957443787645 + "x": 3.843466026936238, + "y": 2.8984841545838695 }, "isLocked": false, "linkedName": "Blue Bottom HP" }, { "anchor": { - "x": 3.855027988978794, - "y": 2.9843867710658483 + "x": 3.784075927734375, + "y": 2.8778821672712045 }, "prevControl": { - "x": 1.9046029937975602, - "y": 0.863970709137234 + "x": 1.6205844988132556, + "y": 0.6787310036528442 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to D.path b/src/main/deploy/pathplanner/paths/Blue HP to D.path index 403b57e..f8d1b18 100644 --- a/src/main/deploy/pathplanner/paths/Blue HP to D.path +++ b/src/main/deploy/pathplanner/paths/Blue HP to D.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6107801164899553, - "y": 0.6035906110491066 + "x": 1.6300611223493302, + "y": 0.7061676025390625 }, "prevControl": null, "nextControl": { - "x": 4.108826093398401, - "y": 2.722808852403046 + "x": 4.058796000769013, + "y": 2.791179915798343 }, "isLocked": false, "linkedName": "Blue Bottom HP" }, { "anchor": { - "x": 4.179234531947544, - "y": 2.7651035853794643 + "x": 4.066455950055803, + "y": 2.720726667131696 }, "prevControl": { - "x": 1.9135212418598027, - "y": 0.8459262774667073 + "x": 1.9793106214057206, + "y": 0.9131563052245901 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to K.path b/src/main/deploy/pathplanner/paths/Blue HP to K.path index e817aed..9114f4d 100644 --- a/src/main/deploy/pathplanner/paths/Blue HP to K.path +++ b/src/main/deploy/pathplanner/paths/Blue HP to K.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.616084943498884, - "y": 7.418355015345981 + "x": 1.6300611223493302, + "y": 7.31465584891183 }, "prevControl": null, "nextControl": { - "x": 3.8482508420035844, - "y": 5.076718013224423 + "x": 3.969184381650222, + "y": 5.136991616227191 }, "isLocked": false, "linkedName": "Blue Top HP" }, { "anchor": { - "x": 3.892620849609375, - "y": 5.088974870954241 + "x": 3.8334006173270088, + "y": 5.176861572265625 }, "prevControl": { - "x": 1.989076023539529, - "y": 7.0504827455307595 + "x": 1.8851673736187446, + "y": 7.041074523570269 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue HP to L.path b/src/main/deploy/pathplanner/paths/Blue HP to L.path index 223f0b1..8130589 100644 --- a/src/main/deploy/pathplanner/paths/Blue HP to L.path +++ b/src/main/deploy/pathplanner/paths/Blue HP to L.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.616084943498884, - "y": 7.418355015345981 + "x": 1.6300611223493302, + "y": 7.31465584891183 }, "prevControl": null, "nextControl": { - "x": 3.354047900318607, - "y": 5.058769876451597 + "x": 3.5442932251718675, + "y": 5.112104734320315 }, "isLocked": false, "linkedName": "Blue Top HP" }, { "anchor": { - "x": 3.5622933523995535, - "y": 4.92284197126116 + "x": 3.5713217599051337, + "y": 5.040517316545759 }, "prevControl": { - "x": 1.6980588778394932, - "y": 7.161554526429133 + "x": 1.6855217874901318, + "y": 7.266571798877964 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue IJ Algae to Barge 3.path b/src/main/deploy/pathplanner/paths/Blue IJ Algae to Barge 3.path new file mode 100644 index 0000000..a25dd8a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue IJ Algae to Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.036270141601562, + "y": 5.243937029157366 + }, + "prevControl": null, + "nextControl": { + "x": 5.616485595703125, + "y": 5.890309797014509 + }, + "isLocked": false, + "linkedName": "Blue IJ Algae" + }, + { + "anchor": { + "x": 7.3, + "y": 4.996803501674107 + }, + "prevControl": { + "x": 5.710595267159598, + "y": 5.503312465122767 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue Barge 3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Blue To Barge", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue IJ.path b/src/main/deploy/pathplanner/paths/Blue IJ.path index d955af6..17f1483 100644 --- a/src/main/deploy/pathplanner/paths/Blue IJ.path +++ b/src/main/deploy/pathplanner/paths/Blue IJ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.134817504882813, - "y": 5.085506330217633 + "x": 5.213420758928571, + "y": 5.145287649972098 }, "prevControl": null, "nextControl": { - "x": 4.746547425039041, - "y": 5.264990075320744 + "x": 4.825150679084799, + "y": 5.324771395075209 }, "isLocked": false, "linkedName": "Blue I" }, { "anchor": { - "x": 4.823158918108259, - "y": 5.275612967354911 + "x": 4.899160766601562, + "y": 5.324784633091517 }, "prevControl": { - "x": 5.049530854623285, - "y": 5.16951986504172 + "x": 5.125532703116588, + "y": 5.218691530778327 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue IJKL Algae.path b/src/main/deploy/pathplanner/paths/Blue IJKL Algae.path new file mode 100644 index 0000000..437a905 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Blue IJKL Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.036270141601562, + "y": 5.243937029157366 + }, + "prevControl": null, + "nextControl": { + "x": 5.043921334402901, + "y": 6.140962873186384 + }, + "isLocked": false, + "linkedName": "Blue IJ Algae" + }, + { + "anchor": { + "x": 3.69481201171875, + "y": 5.124476405552454 + }, + "prevControl": { + "x": 3.7128688267299106, + "y": 6.0124738420758925 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Blue KL Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "Blue Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue J to HP.path b/src/main/deploy/pathplanner/paths/Blue J to HP.path index 3b1ead9..2a029f7 100644 --- a/src/main/deploy/pathplanner/paths/Blue J to HP.path +++ b/src/main/deploy/pathplanner/paths/Blue J to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.823158918108259, - "y": 5.275612967354911 + "x": 4.899160766601562, + "y": 5.324784633091517 }, "prevControl": null, "nextControl": { - "x": 1.4947330269782055, - "y": 7.504836446174904 + "x": 1.8684883628135331, + "y": 7.209159927661123 }, "isLocked": false, "linkedName": "Blue J" }, { "anchor": { - "x": 1.616084943498884, - "y": 7.418355015345981 + "x": 1.6300611223493302, + "y": 7.31465584891183 }, "prevControl": { - "x": 4.287057739796883, - "y": 5.656900215337909 + "x": 4.397941872582589, + "y": 5.558952622852836 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "reversed": false, "folder": "Blue To HP", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue K to HP.path b/src/main/deploy/pathplanner/paths/Blue K to HP.path index d7a5a2e..bf45a6f 100644 --- a/src/main/deploy/pathplanner/paths/Blue K to HP.path +++ b/src/main/deploy/pathplanner/paths/Blue K to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.892620849609375, - "y": 5.088974870954241 + "x": 3.8334006173270088, + "y": 5.176861572265625 }, "prevControl": null, "nextControl": { - "x": 1.6315414845579115, - "y": 7.383482120157296 + "x": 1.7888361703905091, + "y": 7.179391220693517 }, "isLocked": false, "linkedName": "Blue K" }, { "anchor": { - "x": 1.616084943498884, - "y": 7.418355015345981 + "x": 1.6300611223493302, + "y": 7.31465584891183 }, "prevControl": { - "x": 3.631710545702547, - "y": 5.462339942495799 + "x": 3.6965239663373928, + "y": 5.317807433190369 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "reversed": false, "folder": "Blue To HP", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue KL.path b/src/main/deploy/pathplanner/paths/Blue KL.path index 15def11..f043360 100644 --- a/src/main/deploy/pathplanner/paths/Blue KL.path +++ b/src/main/deploy/pathplanner/paths/Blue KL.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.892620849609375, - "y": 5.088974870954241 + "x": 3.8334006173270088, + "y": 5.176861572265625 }, "prevControl": null, "nextControl": { - "x": 3.376974835073141, - "y": 4.854129268172988 + "x": 3.317754602790775, + "y": 4.942015969484372 }, "isLocked": false, "linkedName": "Blue K" }, { "anchor": { - "x": 3.5622933523995535, - "y": 4.92284197126116 + "x": 3.5713217599051337, + "y": 5.040517316545759 }, "prevControl": { - "x": 3.7810355561721734, - "y": 5.043886790599692 + "x": 3.7900639636777536, + "y": 5.1615621358842905 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue L to HP.path b/src/main/deploy/pathplanner/paths/Blue L to HP.path index e63bb9c..bbb75c6 100644 --- a/src/main/deploy/pathplanner/paths/Blue L to HP.path +++ b/src/main/deploy/pathplanner/paths/Blue L to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5622933523995535, - "y": 4.92284197126116 + "x": 3.5713217599051337, + "y": 5.040517316545759 }, "prevControl": null, "nextControl": { - "x": 1.6251256121735873, - "y": 7.408298512761309 + "x": 1.9140223911830354, + "y": 6.993458775111606 }, "isLocked": false, "linkedName": "Blue L" }, { "anchor": { - "x": 1.616084943498884, - "y": 7.418355015345981 + "x": 1.6300611223493302, + "y": 7.31465584891183 }, "prevControl": { - "x": 3.2593672687382833, - "y": 5.321251937264353 + "x": 3.086349239619901, + "y": 5.625750549068774 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "reversed": false, "folder": "Blue To HP", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Mid Bottom to G.path b/src/main/deploy/pathplanner/paths/Blue Mid Bottom to G.path index 4271c7d..82142a6 100644 --- a/src/main/deploy/pathplanner/paths/Blue Mid Bottom to G.path +++ b/src/main/deploy/pathplanner/paths/Blue Mid Bottom to G.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.474950250745046, - "y": 4.02301136513072 + "x": 6.288509261805727, + "y": 4.027414460006102 }, "isLocked": false, "linkedName": "Blue Mid Bottom Start" }, { "anchor": { - "x": 5.730692400251115, + "x": 5.814549473353794, "y": 4.018959416841206 }, "prevControl": { - "x": 6.517362859080646, - "y": 4.013461420883503 + "x": 6.750928312871249, + "y": 4.014739793958772 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Mid Starting Points.path b/src/main/deploy/pathplanner/paths/Blue Mid Starting Points.path index abb8105..198f5b0 100644 --- a/src/main/deploy/pathplanner/paths/Blue Mid Starting Points.path +++ b/src/main/deploy/pathplanner/paths/Blue Mid Starting Points.path @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Mid Top to H.path b/src/main/deploy/pathplanner/paths/Blue Mid Top to H.path index 4d957e8..0fc8b8b 100644 --- a/src/main/deploy/pathplanner/paths/Blue Mid Top to H.path +++ b/src/main/deploy/pathplanner/paths/Blue Mid Top to H.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.14393042241916, - "y": 4.329664694472028 + "x": 6.008068904025894, + "y": 4.3350956881324 }, "isLocked": false, "linkedName": "Blue Mid Top Start" }, { "anchor": { - "x": 5.737221418108259, + "x": 5.832912336077008, "y": 4.340860954083895 }, "prevControl": { - "x": 6.947461776689232, - "y": 4.318375045297009 + "x": 6.898911761331018, + "y": 4.343402942107264 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Starting Points.path b/src/main/deploy/pathplanner/paths/Blue Starting Points.path index 1ffa0f3..705bf78 100644 --- a/src/main/deploy/pathplanner/paths/Blue Starting Points.path +++ b/src/main/deploy/pathplanner/paths/Blue Starting Points.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.12774919782366, - "y": 5.275612967354911 + "x": 7.040423583984375, + "y": 5.324784633091517 }, "prevControl": null, "nextControl": { - "x": 7.126269967215402, - "y": 4.297484479631697 + "x": 7.038944353376117, + "y": 4.3466561453683035 }, "isLocked": false, "linkedName": "Blue Top Start" }, { "anchor": { - "x": 7.146367100306919, - "y": 2.961790248325893 + "x": 7.043586077008928, + "y": 2.833760288783482 }, "prevControl": { - "x": 7.146367100306919, - "y": 4.396418960762031 + "x": 7.043586077008928, + "y": 4.268389001219621 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Top HP.path b/src/main/deploy/pathplanner/paths/Blue Top HP.path index 2e0399c..d8568b1 100644 --- a/src/main/deploy/pathplanner/paths/Blue Top HP.path +++ b/src/main/deploy/pathplanner/paths/Blue Top HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.616084943498884, - "y": 7.418355015345981 + "x": 1.6300611223493302, + "y": 7.31465584891183 }, "prevControl": null, "nextControl": { - "x": 1.5358402730618435, - "y": 7.092322262690061 + "x": 1.5498164519122897, + "y": 6.9886230962559095 }, "isLocked": false, "linkedName": "Blue Top HP" }, { "anchor": { - "x": 1.6107801164899553, - "y": 0.6035906110491066 + "x": 1.6300611223493302, + "y": 0.7061676025390625 }, "prevControl": { - "x": 1.7060444448093226, - "y": 0.8347284654777133 + "x": 1.7253254506686975, + "y": 0.9373054569676692 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Blue Utilities", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Blue Top to J.path b/src/main/deploy/pathplanner/paths/Blue Top to J.path index 0da7820..18009f2 100644 --- a/src/main/deploy/pathplanner/paths/Blue Top to J.path +++ b/src/main/deploy/pathplanner/paths/Blue Top to J.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.12774919782366, - "y": 5.275612967354911 + "x": 7.040423583984375, + "y": 5.324784633091517 }, "prevControl": null, "nextControl": { - "x": 5.133306220857776, - "y": 5.259144698681362 + "x": 5.299942497581686, + "y": 5.36060120961256 }, "isLocked": false, "linkedName": "Blue Top Start" }, { "anchor": { - "x": 4.823158918108259, - "y": 5.275612967354911 + "x": 4.899160766601562, + "y": 5.324784633091517 }, "prevControl": { - "x": 6.563164764891064, - "y": 5.283344662088884 + "x": 6.629233872274842, + "y": 5.329688667225583 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "reversed": false, "folder": "Blue To Reef", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path index e309f98..fdc712d 100644 --- a/src/main/deploy/pathplanner/paths/Mobility.path +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 5.737272426060267, + "x": 5.834901646205356, "y": 4.034793526785714 }, "prevControl": { - "x": 6.122333724434882, + "x": 6.219962944579971, "y": 4.0382880248232755 }, "nextControl": null, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red AB.path b/src/main/deploy/pathplanner/paths/Red AB.path index 87143ee..0a16111 100644 --- a/src/main/deploy/pathplanner/paths/Red AB.path +++ b/src/main/deploy/pathplanner/paths/Red AB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.2367095947265625, - "y": 4.002046421595981 + "x": 3.1608097621372764, + "y": 4.035864693777901 }, "prevControl": null, "nextControl": { - "x": 3.2367095947265625, - "y": 3.7520464215959812 + "x": 3.1608097621372764, + "y": 3.785864693777901 }, "isLocked": false, "linkedName": "Red A" }, { "anchor": { - "x": 3.260224260602678, - "y": 3.6753914969308035 + "x": 3.143314034598214, + "y": 3.6790130615234373 }, "prevControl": { - "x": 3.260224260602678, - "y": 3.9253914969308035 + "x": 3.143314034598214, + "y": 3.9290130615234373 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red ABCD Algae.path b/src/main/deploy/pathplanner/paths/Red ABCD Algae.path new file mode 100644 index 0000000..7213e8c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red ABCD Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.1391313825334817, + "y": 3.8886557442801335 + }, + "prevControl": null, + "nextControl": { + "x": 3.1608607700892857, + "y": 3.0004032679966515 + }, + "isLocked": false, + "linkedName": "Red AB Algae" + }, + { + "anchor": { + "x": 3.908586338588169, + "y": 2.7856597900390625 + }, + "prevControl": { + "x": 2.908586338588169, + "y": 2.7856597900390625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red CD Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": "Red Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Barge 1 + Barge 2.path b/src/main/deploy/pathplanner/paths/Red Barge 1 + Barge 2.path new file mode 100644 index 0000000..0d1f862 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Barge 1 + Barge 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 7.246815272739955 + }, + "prevControl": null, + "nextControl": { + "x": 7.284326388137786, + "y": 6.554161980993306 + }, + "isLocked": false, + "linkedName": "Red Barge 1" + }, + { + "anchor": { + "x": 7.3, + "y": 6.150909423828125 + }, + "prevControl": { + "x": 7.301200364209872, + "y": 6.636377799392722 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Barge 2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Barge 3 to EF Algae.path b/src/main/deploy/pathplanner/paths/Red Barge 3 to EF Algae.path new file mode 100644 index 0000000..4eed4b6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Barge 3 to EF Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 5.004760742187499 + }, + "prevControl": null, + "nextControl": { + "x": 6.64832545689174, + "y": 1.8770041329520089 + }, + "isLocked": false, + "linkedName": "Red Barge 3" + }, + { + "anchor": { + "x": 5.2531049455915175, + "y": 2.9138937813895085 + }, + "prevControl": { + "x": 6.080300903320312, + "y": 1.983916800362723 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red EF Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": "Red To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Barge 3 to IJ Algae.path b/src/main/deploy/pathplanner/paths/Red Barge 3 to IJ Algae.path new file mode 100644 index 0000000..f459a89 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Barge 3 to IJ Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 5.004760742187499 + }, + "prevControl": null, + "nextControl": { + "x": 5.600316074916293, + "y": 6.317807442801339 + }, + "isLocked": false, + "linkedName": "Red Barge 3" + }, + { + "anchor": { + "x": 5.04968523297991, + "y": 5.217973981584821 + }, + "prevControl": { + "x": 5.636225673130579, + "y": 6.324234444754463 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red IJ Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "Red To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Barge 3.path b/src/main/deploy/pathplanner/paths/Red Barge 3.path new file mode 100644 index 0000000..237b741 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.3, + "y": 5.004760742187499 + }, + "prevControl": null, + "nextControl": { + "x": 7.302193373995843, + "y": 4.303336921456915 + }, + "isLocked": false, + "linkedName": "Red Barge 3" + }, + { + "anchor": { + "x": 7.3, + "y": 4.015614536830356 + }, + "prevControl": { + "x": 7.313772612012417, + "y": 4.414996328600405 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Bottom to E.path b/src/main/deploy/pathplanner/paths/Red Bottom to E.path index 4d1242a..995824f 100644 --- a/src/main/deploy/pathplanner/paths/Red Bottom to E.path +++ b/src/main/deploy/pathplanner/paths/Red Bottom to E.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.094236973353794, - "y": 2.9505174909319196 + "x": 6.998903111049106, + "y": 2.8420745849609377 }, "prevControl": null, "nextControl": { - "x": 5.238986367442319, - "y": 2.9368586411254 + "x": 5.357483799251099, + "y": 2.889248211170982 }, "isLocked": false, "linkedName": "Red Bottom Start" }, { "anchor": { - "x": 5.121708461216517, - "y": 2.9505174909319196 + "x": 5.118596976143973, + "y": 2.8420745849609377 }, "prevControl": { - "x": 6.551337045652958, - "y": 2.9554663814966946 + "x": 6.44226138330418, + "y": 2.8801252980695207 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red C to HP.path b/src/main/deploy/pathplanner/paths/Red C to HP.path index badd086..b0eff8b 100644 --- a/src/main/deploy/pathplanner/paths/Red C to HP.path +++ b/src/main/deploy/pathplanner/paths/Red C to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.846458653041294, - "y": 2.94766104561942 + "x": 3.812334333147321, + "y": 2.8759438650948663 }, "prevControl": null, "nextControl": { - "x": 1.599314331027172, - "y": 0.6620582480045325 + "x": 1.6309357473800494, + "y": 0.7227728753462443 }, "isLocked": false, "linkedName": "Red C" }, { "anchor": { - "x": 1.6128204345703125, - "y": 0.6087424142020086 + "x": 1.6112901960100445, + "y": 0.7039232526506691 }, "prevControl": { - "x": 3.4348703924258, - "y": 2.6831440646311107 + "x": 3.3924593441431368, + "y": 2.447297484838467 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Red to HP", "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red CD.path b/src/main/deploy/pathplanner/paths/Red CD.path index 3ff2e5d..8928853 100644 --- a/src/main/deploy/pathplanner/paths/Red CD.path +++ b/src/main/deploy/pathplanner/paths/Red CD.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.846458653041294, - "y": 2.94766104561942 + "x": 3.812334333147321, + "y": 2.8759438650948663 }, "prevControl": null, "nextControl": { - "x": 4.413361031668524, - "y": 2.6743604387555804 + "x": 4.379236711774551, + "y": 2.602643258231027 }, "isLocked": false, "linkedName": "Red C" }, { "anchor": { - "x": 4.155260794503348, - "y": 2.7892303466796875 + "x": 4.084563773018973, + "y": 2.7072095598493298 }, "prevControl": { - "x": 3.5554072788783477, - "y": 3.079261561802456 + "x": 3.4847102573939734, + "y": 2.997240774972098 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red D to HP.path b/src/main/deploy/pathplanner/paths/Red D to HP.path index 3997aeb..9f6ec8f 100644 --- a/src/main/deploy/pathplanner/paths/Red D to HP.path +++ b/src/main/deploy/pathplanner/paths/Red D to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.155260794503348, - "y": 2.7892303466796875 + "x": 4.084563773018973, + "y": 2.7072095598493298 }, "prevControl": null, "nextControl": { - "x": 1.6852161232670793, - "y": 0.6558568766453181 + "x": 1.5877252939555717, + "y": 0.6732116609921128 }, "isLocked": false, "linkedName": "Red D" }, { "anchor": { - "x": 1.6128204345703125, - "y": 0.6087424142020086 + "x": 1.6112901960100445, + "y": 0.7039232526506691 }, "prevControl": { - "x": 3.8820473512452773, - "y": 2.555133264821767 + "x": 3.8269266769561696, + "y": 2.5123972116540427 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Red to HP", "idealStartingState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red E to HP.path b/src/main/deploy/pathplanner/paths/Red E to HP.path index a6ecf5e..96d8b94 100644 --- a/src/main/deploy/pathplanner/paths/Red E to HP.path +++ b/src/main/deploy/pathplanner/paths/Red E to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.121708461216517, - "y": 2.9505174909319196 + "x": 5.118596976143973, + "y": 2.8420745849609377 }, "prevControl": null, "nextControl": { - "x": 1.5902230259586978, - "y": 0.6280089079002749 + "x": 2.5481265325891242, + "y": 1.1956263467929538 }, "isLocked": false, "linkedName": "Red E" }, { "anchor": { - "x": 1.6128204345703125, - "y": 0.6087424142020086 + "x": 1.6112901960100445, + "y": 0.7039232526506691 }, "prevControl": { - "x": 4.382125286936259, - "y": 2.4542273635956438 + "x": 4.720704765559239, + "y": 2.6938576706126884 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Red to HP", "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red EF Algae to Barge 3.path b/src/main/deploy/pathplanner/paths/Red EF Algae to Barge 3.path new file mode 100644 index 0000000..dfc038a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red EF Algae to Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.2531049455915175, + "y": 2.9138937813895085 + }, + "prevControl": null, + "nextControl": { + "x": 6.05229753766741, + "y": 2.338524082728794 + }, + "isLocked": false, + "linkedName": "Red EF Algae" + }, + { + "anchor": { + "x": 7.3, + "y": 5.004760742187499 + }, + "prevControl": { + "x": 6.333045305524553, + "y": 2.23115234375 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Barge 3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red To Barge", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red EF.path b/src/main/deploy/pathplanner/paths/Red EF.path index fafdf8b..629d705 100644 --- a/src/main/deploy/pathplanner/paths/Red EF.path +++ b/src/main/deploy/pathplanner/paths/Red EF.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.121708461216517, - "y": 2.9505174909319196 + "x": 5.118596976143973, + "y": 2.8420745849609377 }, "prevControl": null, "nextControl": { - "x": 5.519570486886161, - "y": 3.166536167689732 + "x": 5.516459001813616, + "y": 3.05809326171875 }, "isLocked": false, "linkedName": "Red E" }, { "anchor": { - "x": 5.4284192766462045, - "y": 3.120731026785714 + "x": 5.474224417550223, + "y": 3.00667724609375 }, "prevControl": { - "x": 5.068663379124232, - "y": 2.913035035671148 + "x": 5.114468520028251, + "y": 2.7989812549791844 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red EFGH Algae.path b/src/main/deploy/pathplanner/paths/Red EFGH Algae.path new file mode 100644 index 0000000..0ce9ee6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red EFGH Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.2531049455915175, + "y": 2.9138937813895085 + }, + "prevControl": null, + "nextControl": { + "x": 6.253104945591516, + "y": 2.9138937813895085 + }, + "isLocked": false, + "linkedName": "Red EF Algae" + }, + { + "anchor": { + "x": 5.797665841238838, + "y": 4.09498291015625 + }, + "prevControl": { + "x": 5.722582135881696, + "y": 3.5291516985212055 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red GH Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red G to GH Algae.path b/src/main/deploy/pathplanner/paths/Red G to GH Algae.path new file mode 100644 index 0000000..6ab461e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red G to GH Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.805061994280134, + "y": 4.034775573328922 + }, + "prevControl": null, + "nextControl": { + "x": 7.560602678571428, + "y": 4.031682041713169 + }, + "isLocked": false, + "linkedName": "Red G" + }, + { + "anchor": { + "x": 5.797665841238838, + "y": 4.09498291015625 + }, + "prevControl": { + "x": 7.572946602957589, + "y": 4.126097760881696 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red GH Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red GH Algae to Barge 3.path b/src/main/deploy/pathplanner/paths/Red GH Algae to Barge 3.path new file mode 100644 index 0000000..2a9b0de --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red GH Algae to Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.797665841238838, + "y": 4.09498291015625 + }, + "prevControl": null, + "nextControl": { + "x": 6.039892140821703, + "y": 4.245731788685146 + }, + "isLocked": false, + "linkedName": "Red GH Algae" + }, + { + "anchor": { + "x": 7.3, + "y": 5.004760742187499 + }, + "prevControl": { + "x": 6.953837402777962, + "y": 4.791802757816577 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Barge 3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red To Barge", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red GH.path b/src/main/deploy/pathplanner/paths/Red GH.path index 51fed99..e9f4ec8 100644 --- a/src/main/deploy/pathplanner/paths/Red GH.path +++ b/src/main/deploy/pathplanner/paths/Red GH.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 5.692028372628348, + "x": 5.805061994280134, "y": 4.034775573328922 }, "prevControl": null, "nextControl": { - "x": 5.70906502859933, + "x": 5.822098650251116, "y": 4.411163251202804 }, "isLocked": false, @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 5.728244018554688, + "x": 5.836890956333705, "y": 4.354403817026238 }, "prevControl": { - "x": 5.7546558861064385, + "x": 5.863302823885456, "y": 4.105802905418036 }, "nextControl": null, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red H to GH Algae.path b/src/main/deploy/pathplanner/paths/Red H to GH Algae.path new file mode 100644 index 0000000..e1ffd6e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red H to GH Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.836890956333705, + "y": 4.354403817026238 + }, + "prevControl": null, + "nextControl": { + "x": 7.554532732282365, + "y": 4.223573957170759 + }, + "isLocked": false, + "linkedName": "Red H" + }, + { + "anchor": { + "x": 5.797665841238838, + "y": 4.09498291015625 + }, + "prevControl": { + "x": 7.536832972935267, + "y": 4.203374808175223 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red GH Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red To Algae", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to A.path b/src/main/deploy/pathplanner/paths/Red HP to A.path index 0234d59..a50b77c 100644 --- a/src/main/deploy/pathplanner/paths/Red HP to A.path +++ b/src/main/deploy/pathplanner/paths/Red HP to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6382734026227679, - "y": 7.407847377232142 + "x": 1.689485386439732, + "y": 7.340108816964285 }, "prevControl": null, "nextControl": { - "x": 3.208055663403095, - "y": 4.078166301232639 + "x": 3.128981408539434, + "y": 3.9642169774844254 }, "isLocked": false, "linkedName": "Red Top HP" }, { "anchor": { - "x": 3.2367095947265625, - "y": 4.002046421595981 + "x": 3.1608097621372764, + "y": 4.035864693777901 }, "prevControl": { - "x": 1.900257598167452, - "y": 6.935154064335665 + "x": 1.8760913701347872, + "y": 6.962044082202689 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to B.path b/src/main/deploy/pathplanner/paths/Red HP to B.path index e602072..54321f2 100644 --- a/src/main/deploy/pathplanner/paths/Red HP to B.path +++ b/src/main/deploy/pathplanner/paths/Red HP to B.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6128204345703125, - "y": 0.6087424142020086 + "x": 1.6112901960100445, + "y": 0.7039232526506691 }, "prevControl": null, "nextControl": { - "x": 3.049246890306277, - "y": 3.6467489822290986 + "x": 3.0171465135713023, + "y": 3.643388768299755 }, "isLocked": false, "linkedName": "Red Bottom HP" }, { "anchor": { - "x": 3.260224260602678, - "y": 3.6753914969308035 + "x": 3.143314034598214, + "y": 3.6790130615234373 }, "prevControl": { - "x": 1.7945999702624478, - "y": 0.9690990351545485 + "x": 1.834505775722884, + "y": 1.0759906059617488 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to C.path b/src/main/deploy/pathplanner/paths/Red HP to C.path index dd3099e..2c48e6a 100644 --- a/src/main/deploy/pathplanner/paths/Red HP to C.path +++ b/src/main/deploy/pathplanner/paths/Red HP to C.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6128204345703125, - "y": 0.6087424142020086 + "x": 1.6112901960100445, + "y": 0.7039232526506691 }, "prevControl": null, "nextControl": { - "x": 3.858893049650237, - "y": 2.9029611229076124 + "x": 3.8249172502954747, + "y": 2.894524426751204 }, "isLocked": false, "linkedName": "Red Bottom HP" }, { "anchor": { - "x": 3.846458653041294, - "y": 2.94766104561942 + "x": 3.812334333147321, + "y": 2.8759438650948663 }, "prevControl": { - "x": 1.9880224301540965, - "y": 0.9603112792286335 + "x": 1.9210233857459387, + "y": 1.0403100002437538 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to D.path b/src/main/deploy/pathplanner/paths/Red HP to D.path index 9ebf19b..f8d7b58 100644 --- a/src/main/deploy/pathplanner/paths/Red HP to D.path +++ b/src/main/deploy/pathplanner/paths/Red HP to D.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6128204345703125, - "y": 0.6087424142020086 + "x": 1.6112901960100445, + "y": 0.7039232526506691 }, "prevControl": null, "nextControl": { - "x": 3.9485106894048396, - "y": 2.6049474983276104 + "x": 3.885807482128166, + "y": 2.524098233305265 }, "isLocked": false, "linkedName": "Red Bottom HP" }, { "anchor": { - "x": 4.155260794503348, - "y": 2.7892303466796875 + "x": 4.084563773018973, + "y": 2.7072095598493298 }, "prevControl": { - "x": 2.044110201306561, - "y": 0.9854113501771831 + "x": 1.8609278402581966, + "y": 0.8718454280926664 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -119.99999999999999 + "rotation": 59.99999999999999 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to K.path b/src/main/deploy/pathplanner/paths/Red HP to K.path index 92ee781..d7156d5 100644 --- a/src/main/deploy/pathplanner/paths/Red HP to K.path +++ b/src/main/deploy/pathplanner/paths/Red HP to K.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6382734026227679, - "y": 7.407847377232142 + "x": 1.689485386439732, + "y": 7.340108816964285 }, "prevControl": null, "nextControl": { - "x": 3.878188245147361, - "y": 5.1324266824821905 + "x": 4.280580620681201, + "y": 4.833970847634817 }, "isLocked": false, "linkedName": "Red Top HP" }, { "anchor": { - "x": 3.8905805315290176, - "y": 5.093667602539062 + "x": 3.8606388636997764, + "y": 5.211597987583705 }, "prevControl": { - "x": 1.9468585429382765, - "y": 7.091880486390446 + "x": 1.8167620052540583, + "y": 7.163064343873003 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP to L.path b/src/main/deploy/pathplanner/paths/Red HP to L.path index 43f5dc3..d6ccddf 100644 --- a/src/main/deploy/pathplanner/paths/Red HP to L.path +++ b/src/main/deploy/pathplanner/paths/Red HP to L.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6382734026227679, - "y": 7.407847377232142 + "x": 1.689485386439732, + "y": 7.340108816964285 }, "prevControl": null, "nextControl": { - "x": 3.457423456190929, - "y": 5.125499294399303 + "x": 3.6015237117052994, + "y": 5.014436102401614 }, "isLocked": false, "linkedName": "Red Top HP" }, { "anchor": { - "x": 3.568975394112723, - "y": 4.930391148158481 + "x": 3.575963483537946, + "y": 5.054136439732143 }, "prevControl": { - "x": 1.801851201901177, - "y": 7.200187092467079 + "x": 1.8171902409350595, + "y": 7.173619580701783 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red HP.path b/src/main/deploy/pathplanner/paths/Red HP.path index ef3787f..eeaed27 100644 --- a/src/main/deploy/pathplanner/paths/Red HP.path +++ b/src/main/deploy/pathplanner/paths/Red HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6382734026227679, - "y": 7.407847377232142 + "x": 1.689485386439732, + "y": 7.340108816964285 }, "prevControl": null, "nextControl": { - "x": 1.63133918458931, - "y": 6.617148906433243 + "x": 1.682551168406274, + "y": 6.549410346165387 }, "isLocked": false, "linkedName": "Red Top HP" }, { "anchor": { - "x": 1.6128204345703125, - "y": 0.6087424142020086 + "x": 1.6112901960100445, + "y": 0.7039232526506691 }, "prevControl": { - "x": 1.6075084089186695, - "y": 1.3259010121823729 + "x": 1.6059781703584015, + "y": 1.4210818506310336 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -125.00000000000001 + "rotation": 55.0 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red IJ Algae to Barge 3.path b/src/main/deploy/pathplanner/paths/Red IJ Algae to Barge 3.path new file mode 100644 index 0000000..d72206e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red IJ Algae to Barge 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.04968523297991, + "y": 5.217973981584821 + }, + "prevControl": null, + "nextControl": { + "x": 5.754666137695312, + "y": 5.9416748046875 + }, + "isLocked": false, + "linkedName": "Red IJ Algae" + }, + { + "anchor": { + "x": 7.3, + "y": 5.004760742187499 + }, + "prevControl": { + "x": 6.155333600725446, + "y": 5.477145385742188 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red Barge 3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Red To Barge", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red IJ.path b/src/main/deploy/pathplanner/paths/Red IJ.path index 52b1ebb..7f2da2b 100644 --- a/src/main/deploy/pathplanner/paths/Red IJ.path +++ b/src/main/deploy/pathplanner/paths/Red IJ.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.123136683872767, - "y": 5.03521248953683 + "x": 5.213777814592634, + "y": 5.123864310128348 }, "prevControl": null, "nextControl": { - "x": 4.690181187220981, - "y": 5.266686575753348 + "x": 4.7808223179408476, + "y": 5.355338396344866 }, "isLocked": false, "linkedName": "Red I" }, { "anchor": { - "x": 4.792452130998884, - "y": 5.256076921735492 + "x": 4.905434744698661, + "y": 5.344677734375 }, "prevControl": { - "x": 5.11966814313616, - "y": 5.071581159319197 + "x": 5.232650756835937, + "y": 5.160181971958705 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red IJKL Algae.path b/src/main/deploy/pathplanner/paths/Red IJKL Algae.path new file mode 100644 index 0000000..fa9a8da --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Red IJKL Algae.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.04968523297991, + "y": 5.217973981584821 + }, + "prevControl": null, + "nextControl": { + "x": 6.049685232979909, + "y": 5.217973981584821 + }, + "isLocked": false, + "linkedName": "Red IJ Algae" + }, + { + "anchor": { + "x": 3.701647077287946, + "y": 5.114784894670759 + }, + "prevControl": { + "x": 2.701647077287946, + "y": 5.114784894670759 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Red KL Algae" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "Red Utilities", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red J to HP.path b/src/main/deploy/pathplanner/paths/Red J to HP.path index 1cd8a83..2628157 100644 --- a/src/main/deploy/pathplanner/paths/Red J to HP.path +++ b/src/main/deploy/pathplanner/paths/Red J to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 4.792452130998884, - "y": 5.256076921735492 + "x": 4.905434744698661, + "y": 5.344677734375 }, "prevControl": null, "nextControl": { - "x": 1.471010925773443, - "y": 7.518856016198161 + "x": 1.4297410010846314, + "y": 7.52529115222486 }, "isLocked": false, "linkedName": "Red J" }, { "anchor": { - "x": 1.6382734026227679, - "y": 7.407847377232142 + "x": 1.689485386439732, + "y": 7.340108816964285 }, "prevControl": { - "x": 4.258436616161161, - "y": 5.5719258573002515 + "x": 4.451065765986906, + "y": 5.612570304105246 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "reversed": false, "folder": "Red to HP", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red K to HP.path b/src/main/deploy/pathplanner/paths/Red K to HP.path index faf0703..7284bfe 100644 --- a/src/main/deploy/pathplanner/paths/Red K to HP.path +++ b/src/main/deploy/pathplanner/paths/Red K to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.8905805315290176, - "y": 5.093667602539062 + "x": 3.8606388636997764, + "y": 5.211597987583705 }, "prevControl": null, "nextControl": { - "x": 1.6416601055392235, - "y": 7.424079419647595 + "x": 1.6496369020951236, + "y": 7.363624286522119 }, "isLocked": false, "linkedName": "Red K" }, { "anchor": { - "x": 1.6382734026227679, - "y": 7.407847377232142 + "x": 1.689485386439732, + "y": 7.340108816964285 }, "prevControl": { - "x": 3.651845055231881, - "y": 5.329284785681173 + "x": 3.8277045754022385, + "y": 5.2510913207732255 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "reversed": false, "folder": "Red to HP", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red KL.path b/src/main/deploy/pathplanner/paths/Red KL.path index d673516..f058f78 100644 --- a/src/main/deploy/pathplanner/paths/Red KL.path +++ b/src/main/deploy/pathplanner/paths/Red KL.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.8905805315290176, - "y": 5.093667602539062 + "x": 3.8606388636997764, + "y": 5.211597987583705 }, "prevControl": null, "nextControl": { - "x": 3.689198458382694, - "y": 4.945528274150924 + "x": 3.6592567905534525, + "y": 5.063458659195566 }, "isLocked": false, "linkedName": "Red K" }, { "anchor": { - "x": 3.568975394112723, - "y": 4.930391148158481 + "x": 3.575963483537946, + "y": 5.054136439732143 }, "prevControl": { - "x": 3.787717597885343, - "y": 5.051435967497012 + "x": 3.794705687310566, + "y": 5.175181259070674 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red L to HP.path b/src/main/deploy/pathplanner/paths/Red L to HP.path index 256d48e..16999f7 100644 --- a/src/main/deploy/pathplanner/paths/Red L to HP.path +++ b/src/main/deploy/pathplanner/paths/Red L to HP.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.568975394112723, - "y": 4.930391148158481 + "x": 3.575963483537946, + "y": 5.054136439732143 }, "prevControl": null, "nextControl": { - "x": 1.7643756013137868, - "y": 7.234197445147019 + "x": 1.7405041729681656, + "y": 7.242334327193635 }, "isLocked": false, "linkedName": "Red L" }, { "anchor": { - "x": 1.6382734026227679, - "y": 7.407847377232142 + "x": 1.689485386439732, + "y": 7.340108816964285 }, "prevControl": { - "x": 3.408722130314159, - "y": 5.1710270250006385 + "x": 3.426942910576803, + "y": 5.200275946362243 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 125.00000000000001 + "rotation": -55.0 }, "reversed": false, "folder": "Red to HP", "idealStartingState": { "velocity": 0, - "rotation": 119.99999999999999 + "rotation": -59.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Mid Bottom to G.path b/src/main/deploy/pathplanner/paths/Red Mid Bottom to G.path index cd1c3fd..c145bd0 100644 --- a/src/main/deploy/pathplanner/paths/Red Mid Bottom to G.path +++ b/src/main/deploy/pathplanner/paths/Red Mid Bottom to G.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.268582153320312, + "x": 7.18824462890625, "y": 4.034775573328922 }, "prevControl": null, "nextControl": { - "x": 5.92272673909937, - "y": 4.0356050564880865 + "x": 6.08980618876901, + "y": 4.028068283915934 }, "isLocked": false, "linkedName": "Red Mid Bottom Start" }, { "anchor": { - "x": 5.692028372628348, + "x": 5.805061994280134, "y": 4.034775573328922 }, "prevControl": { - "x": 6.722446112569312, - "y": 4.040282646111361 + "x": 6.789901761850722, + "y": 4.033161594708823 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Mid Starting Points.path b/src/main/deploy/pathplanner/paths/Red Mid Starting Points.path index bbf0541..b66e4bb 100644 --- a/src/main/deploy/pathplanner/paths/Red Mid Starting Points.path +++ b/src/main/deploy/pathplanner/paths/Red Mid Starting Points.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 7.268582153320312, + "x": 7.18824462890625, "y": 4.034775573328922 }, "prevControl": { - "x": 7.280335641043525, + "x": 7.199998116629462, "y": 4.327692234182717 }, "nextControl": null, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Mid Top to H.path b/src/main/deploy/pathplanner/paths/Red Mid Top to H.path index a1143cc..2d92fa0 100644 --- a/src/main/deploy/pathplanner/paths/Red Mid Top to H.path +++ b/src/main/deploy/pathplanner/paths/Red Mid Top to H.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.9645073915660936, - "y": 4.386163418077595 + "x": 6.087630468450763, + "y": 4.3488064036661225 }, "isLocked": false, "linkedName": "Red Mid Top Start" }, { "anchor": { - "x": 5.728244018554688, + "x": 5.836890956333705, "y": 4.354403817026238 }, "prevControl": { - "x": 6.732249417954785, - "y": 4.363940837977704 + "x": 6.8518354245764925, + "y": 4.3678763677548975 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Starting Points.path b/src/main/deploy/pathplanner/paths/Red Starting Points.path index 13f69d7..356f504 100644 --- a/src/main/deploy/pathplanner/paths/Red Starting Points.path +++ b/src/main/deploy/pathplanner/paths/Red Starting Points.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.117394583565848, - "y": 5.256076921735492 + "x": 7.029762922014509, + "y": 5.344677734375 }, "prevControl": null, "nextControl": { - "x": 7.11591535295759, - "y": 4.277948434012278 + "x": 7.0282836914062505, + "y": 4.366549246651786 }, "isLocked": false, "linkedName": "Red Top Start" }, { "anchor": { - "x": 7.094236973353794, - "y": 2.9505174909319196 + "x": 6.998903111049106, + "y": 2.8420745849609377 }, "prevControl": { - "x": 7.094236973353794, - "y": 4.385146203368059 + "x": 6.998903111049106, + "y": 4.276703297397077 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -59.99999999999999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": "Red Utilities", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Red Top to J.path b/src/main/deploy/pathplanner/paths/Red Top to J.path index 6e0ca12..77807aa 100644 --- a/src/main/deploy/pathplanner/paths/Red Top to J.path +++ b/src/main/deploy/pathplanner/paths/Red Top to J.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.117394583565848, - "y": 5.256076921735492 + "x": 7.029762922014509, + "y": 5.344677734375 }, "prevControl": null, "nextControl": { - "x": 6.007288626756819, - "y": 5.267465740618087 + "x": 5.7821959109673795, + "y": 5.364720492475522 }, "isLocked": false, "linkedName": "Red Top Start" }, { "anchor": { - "x": 4.792452130998884, - "y": 5.256076921735492 + "x": 4.905434744698661, + "y": 5.344677734375 }, "prevControl": { - "x": 5.871214920205553, - "y": 5.255530296688935 + "x": 6.1646245211584745, + "y": 5.333728021604713 }, "nextControl": null, "isLocked": false, @@ -35,20 +35,20 @@ "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "reversed": false, "folder": "Red to Reef", "idealStartingState": { "velocity": 0, - "rotation": 59.99999999999999 + "rotation": -119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/aaa.path b/src/main/deploy/pathplanner/paths/aaa.path new file mode 100644 index 0000000..84e6247 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/aaa.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 6.5 + }, + "prevControl": null, + "nextControl": { + "x": 1.480386284722223, + "y": 6.579640480324074 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9353947230747766, + "y": 4.067336600167411 + }, + "prevControl": { + "x": 1.0471397569444432, + "y": 5.209032479745369 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/aaaa.path b/src/main/deploy/pathplanner/paths/aaaa.path new file mode 100644 index 0000000..7fb3ed9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/aaaa.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.399774605887276, + "y": 6.5 + }, + "prevControl": null, + "nextControl": { + "x": 6.472598162549824, + "y": 6.505752443576317 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 6.5 + }, + "prevControl": { + "x": 3.2500037601773997, + "y": 6.4986288418906195 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 600.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index d24c2d9..b92869f 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,49 +1,53 @@ { - "robotWidth": 0.74, - "robotLength": 0.74, + "robotWidth": 0.9186500042, + "robotLength": 0.9440500042, "holonomicMode": true, "pathFolders": [ + "Blue To Algae", + "Blue To Barge", "Blue To HP", "Blue To Reef", "Blue Utilities", + "Red To Barge", + "Red To Algae", "Red Utilities", - "Tests", "Red to HP", - "Red to Reef" + "Red to Reef", + "Tests" ], "autoFolders": [ "Blue Bottom Autons", "Blue Top Autons", - "Tests", "Red Bottom Autons", - "Red Top Autons" + "Red Top Autons", + "Tests" ], "defaultMaxVel": 4.0, "defaultMaxAccel": 5.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxAngVel": 400.0, + "defaultMaxAngAccel": 900.0, "defaultNominalVoltage": 12.0, - "robotMass": 61.23, + "robotMass": 63.05, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.051, "driveGearing": 6.48, - "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "maxDriveSpeed": 4.93, + "driveMotorType": "krakenX60FOC", + "driveCurrentLimit": 120.0, + "wheelCOF": 1.542, + "flModuleX": 0.3, + "flModuleY": 0.3, + "frModuleX": 0.3, + "frModuleY": -0.3, + "blModuleX": -0.3, + "blModuleY": 0.3, + "brModuleX": -0.3, + "brModuleY": -0.3, "bumperOffsetX": 0.0, - "bumperOffsetY": 0.0, + "bumperOffsetY": 0.01905, "robotFeatures": [ - "{\"name\":\"Funnel\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.225,\"y\":0.0},\"size\":{\"width\":0.4,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", - "{\"name\":\"Shooter\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.225,\"y\":-0.15},\"size\":{\"width\":0.125,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" + "{\"name\":\"Funnel\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.275,\"y\":0.0},\"size\":{\"width\":0.4,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Shooter\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.27,\"y\":0.125},\"size\":{\"width\":0.15,\"length\":0.25},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}" ] } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index b8c39bd..2ae9729 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -54,7 +54,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { auto = robot.getAutonomousCommand(); - + if (auto != null) { auto.schedule(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 77e98ad..288eb60 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -13,6 +13,19 @@ import com.stuypulse.robot.commands.arm.ArmOffsetTargetDown; import com.stuypulse.robot.commands.arm.ArmOffsetTargetUp; import com.stuypulse.robot.commands.arm.ArmOverrideVoltage; +import com.stuypulse.robot.commands.arm.ArmToClimb; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL2; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL3; +import com.stuypulse.robot.commands.arm.algae.ArmToBarge; +import com.stuypulse.robot.commands.arm.algae.ArmToHoldAlgae; +import com.stuypulse.robot.commands.arm.coral.ArmToL2Back; +import com.stuypulse.robot.commands.arm.coral.ArmToL2Front; +import com.stuypulse.robot.commands.arm.coral.ArmToL3Back; +import com.stuypulse.robot.commands.arm.coral.ArmToL3Front; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Back; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; import com.stuypulse.robot.commands.autons.EDCB.FourPieceEDCB; import com.stuypulse.robot.commands.autons.EDCB.OnePieceE; import com.stuypulse.robot.commands.autons.EDCB.ThreeHalfPieceEDC; @@ -25,23 +38,45 @@ import com.stuypulse.robot.commands.autons.JKLA.TwoPieceJK; import com.stuypulse.robot.commands.autons.misc.DoNothingAuton; import com.stuypulse.robot.commands.autons.misc.Mobility; -import com.stuypulse.robot.commands.autons.misc.OnePieceG; -import com.stuypulse.robot.commands.autons.misc.OnePieceH; +import com.stuypulse.robot.commands.autons.GAlgae.OneGOneAlgae; +import com.stuypulse.robot.commands.autons.GAlgae.OneGThreeAlgae; +import com.stuypulse.robot.commands.autons.GAlgae.OneGTwoAlgae; +import com.stuypulse.robot.commands.autons.GAlgae.OnePieceG; +import com.stuypulse.robot.commands.autons.HAlgae.OneHOneAlgae; +import com.stuypulse.robot.commands.autons.HAlgae.OneHThreeAlgae; +import com.stuypulse.robot.commands.autons.HAlgae.OneHTwoAlgae; +import com.stuypulse.robot.commands.autons.HAlgae.OnePieceH; import com.stuypulse.robot.commands.autons.tests.CurvyLineTest; import com.stuypulse.robot.commands.autons.tests.RSquareTest; import com.stuypulse.robot.commands.autons.tests.SquareTest; import com.stuypulse.robot.commands.autons.tests.StraightLineTest; +import com.stuypulse.robot.commands.autons.tests.testing; import com.stuypulse.robot.commands.climb.ClimbClimb; +import com.stuypulse.robot.commands.climb.ClimbIdle; import com.stuypulse.robot.commands.climb.ClimbOpen; import com.stuypulse.robot.commands.climb.ClimbOverrideVoltage; import com.stuypulse.robot.commands.elevator.ElevatorOffsetTargetDown; import com.stuypulse.robot.commands.elevator.ElevatorOffsetTargetUp; import com.stuypulse.robot.commands.elevator.ElevatorOverrideVoltage; +import com.stuypulse.robot.commands.elevator.ElevatorToClimb; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToHoldAlgae; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL2; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL3; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToBarge; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL2Back; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL2Front; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL3Back; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL3Front; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Back; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotMoveOperatorOffsetDown; import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotMoveOperatorOffsetUp; import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotToAlgaeGroundPickup; import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotToCoralGroundPickup; import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotToL1; +import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotToProcessor; import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotToStow; import com.stuypulse.robot.commands.froggy.pivot.FroggyPivotWaitUntilAtTargetAngle; import com.stuypulse.robot.commands.froggy.roller.FroggyRollerIntakeAlgae; @@ -51,58 +86,48 @@ import com.stuypulse.robot.commands.froggy.roller.FroggyRollerStop; import com.stuypulse.robot.commands.funnel.FunnelDefaultCommand; import com.stuypulse.robot.commands.funnel.FunnelReverse; +import com.stuypulse.robot.commands.leds.LEDApplyPattern; import com.stuypulse.robot.commands.leds.LEDDefaultCommand; -import com.stuypulse.robot.commands.leds.LEDSolidColor; import com.stuypulse.robot.commands.shooter.ShooterAcquireAlgae; import com.stuypulse.robot.commands.shooter.ShooterAcquireCoral; import com.stuypulse.robot.commands.shooter.ShooterShootAlgae; import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; import com.stuypulse.robot.commands.shooter.ShooterShootForwards; import com.stuypulse.robot.commands.shooter.ShooterStop; -import com.stuypulse.robot.commands.superstructure.SuperStructureToAlgaeL2; -import com.stuypulse.robot.commands.superstructure.SuperStructureToAlgaeL3; -import com.stuypulse.robot.commands.superstructure.SuperStructureToBarge; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; -import com.stuypulse.robot.commands.superstructure.SuperStructureToL2Back; -import com.stuypulse.robot.commands.superstructure.SuperStructureToL2Front; -import com.stuypulse.robot.commands.superstructure.SuperStructureToL3Back; -import com.stuypulse.robot.commands.superstructure.SuperStructureToL3Front; -import com.stuypulse.robot.commands.superstructure.SuperStructureToL4Back; -import com.stuypulse.robot.commands.superstructure.SuperStructureToL4Front; -import com.stuypulse.robot.commands.superstructure.SuperStructureWaitUntilAtTarget; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAligned; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveAlignedToBarge; -import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranch; +import com.stuypulse.robot.commands.swerve.SwerveDriveNudgeForward; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchReady; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; import com.stuypulse.robot.commands.swerve.SwerveDriveWaitUntilAlignedToBarge; -import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranch; -import com.stuypulse.robot.commands.swerve.SwerveDriveSeedFieldRelative; - import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.arm.Arm; +import com.stuypulse.robot.subsystems.arm.Arm.ArmState; import com.stuypulse.robot.subsystems.climb.Climb; import com.stuypulse.robot.subsystems.elevator.Elevator; +import com.stuypulse.robot.subsystems.elevator.Elevator.ElevatorState; import com.stuypulse.robot.subsystems.froggy.Froggy; import com.stuypulse.robot.subsystems.froggy.Froggy.PivotState; import com.stuypulse.robot.subsystems.funnel.Funnel; import com.stuypulse.robot.subsystems.led.LEDController; import com.stuypulse.robot.subsystems.shooter.Shooter; -import com.stuypulse.robot.subsystems.superstructure.SuperStructure; -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; +import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.swerve.Telemetry; +import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.PathUtil.AutonConfig; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { @@ -114,11 +139,11 @@ public class RobotContainer { // Subsystem private final CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); private final Telemetry telemetry = new Telemetry(Settings.Swerve.Constraints.MAX_VELOCITY.get()); + private final LimelightVision vision = LimelightVision.getInstance(); private final Funnel funnel = Funnel.getInstance(); private final Shooter shooter = Shooter.getInstance(); private final Elevator elevator = Elevator.getInstance(); private final Arm arm = Arm.getInstance(); - private final SuperStructure superStructure = SuperStructure.getInstance(); private final Climb climb = Climb.getInstance(); private final Froggy froggy = Froggy.getInstance(); private final LEDController leds = LEDController.getInstance(); @@ -146,7 +171,7 @@ private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); funnel.setDefaultCommand(new FunnelDefaultCommand()); leds.setDefaultCommand(new LEDDefaultCommand()); - shooter.setDefaultCommand(new ShooterAcquireCoral().andThen(new BuzzController(driver)).onlyIf(() -> !shooter.hasCoral())); + shooter.setDefaultCommand(new ShooterAcquireCoral().andThen(new BuzzController(driver)).onlyIf(() -> !shooter.hasCoral() && arm.getState() != ArmState.HOLD_ALGAE)); } /***************/ @@ -156,136 +181,182 @@ private void configureDefaultCommands() { private void configureDriverButtonBindings() { driver.getDPadUp().onTrue(new SwerveDriveSeedFieldRelative()); - driver.getDPadRight().whileTrue(new SwerveDriveDriveAligned(driver, Rotation2d.kZero)); // manual shoot depending on whatever states robot is in: either score barge, forwards/backwards on reef, or processor/L1 driver.getDPadRight() .whileTrue(new ConditionalCommand( + new ConditionalCommand( + new FroggyRollerShootAlgae(), + new FroggyRollerShootCoral(), + () -> froggy.getPivotState() == PivotState.PROCESSOR_SCORE_ANGLE), new ConditionalCommand( new ShooterShootAlgae(), new ConditionalCommand( - new ShooterShootForwards(), - new ShooterShootBackwards().onlyIf(() -> shooter.shouldShootBackwards()), - () -> shooter.shouldShootForward()), - () -> superStructure.getTargetState() == SuperStructureTargetState.BARGE), - new ConditionalCommand( - new FroggyRollerShootCoral(), - new FroggyRollerShootAlgae().onlyIf(() -> froggy.getPivotState() == PivotState.PROCESSOR_SCORE_ANGLE), - () -> froggy.getPivotState() == PivotState.L1_SCORE_ANGLE), - () -> superStructure.isInScoreState())) + new ShooterShootBackwards(), + new ShooterShootForwards(), + shooter::shouldShootBackwards + ), + () -> arm.getState() == ArmState.BARGE), + () -> froggy.getPivotState() == PivotState.L1_SCORE_ANGLE || froggy.getPivotState() == PivotState.PROCESSOR_SCORE_ANGLE)) .onFalse(new ShooterStop()) .onFalse(new FroggyRollerStop()); - // ground algae pickup + // ground algae intake and send elevator/arm to feed driver.getLeftTriggerButton() .onTrue(new FroggyPivotToAlgaeGroundPickup()) .whileTrue(new FroggyRollerIntakeAlgae().andThen(new BuzzController(driver))) + .onTrue(new ElevatorToFeed()) + .onTrue(new ArmToFeed()) .onFalse(new FroggyPivotToStow()); + // Algae processor score driver.getLeftBumper() - .onTrue(new FroggyRollerShootAlgae()) + .whileTrue(new FroggyPivotToProcessor() + .andThen(new FroggyPivotWaitUntilAtTargetAngle()) + .andThen(new FroggyRollerShootAlgae())) .onFalse(new FroggyRollerStop()); + // Ground coral intake and send elevator/arm to feed driver.getRightTriggerButton() .onTrue(new FroggyPivotToCoralGroundPickup()) .whileTrue(new FroggyRollerIntakeCoral()) + .onTrue(new ElevatorToFeed()) + .onTrue(new ArmToFeed()) .onFalse(new FroggyPivotToStow()); + // L1 coral score driver.getRightBumper() .whileTrue(new FroggyPivotToL1() .andThen(new FroggyPivotWaitUntilAtTargetAngle()) .andThen(new FroggyRollerShootCoral())) - .onFalse(new FroggyRollerStop().andThen(new FroggyPivotToStow())); + .onFalse(new FroggyRollerStop()) + .onFalse(new FroggyPivotToStow()); + // L4 coral score driver.getTopButton() - .whileTrue( - new ConditionalCommand( - new SuperStructureToL4Front() - .andThen(new SuperStructureWaitUntilAtTarget() - .alongWith(new SwerveDrivePIDToNearestBranch(4, true) - .alongWith(new LEDSolidColor(Color.kYellow)))) - .andThen(new ShooterShootBackwards()), - new SuperStructureToL4Back() - .andThen(new SuperStructureWaitUntilAtTarget() - .alongWith(new SwerveDrivePIDToNearestBranch(4, false) - .alongWith(new LEDSolidColor(Color.kYellow)))) - .andThen(new ShooterShootForwards()), - () -> swerve.isFrontFacingReef()) - ) - .onFalse(new SuperStructureToFeed()) + .whileTrue(new ConditionalCommand( + new SwerveDrivePIDToNearestBranchReady(true) + .until(() -> elevator.getState() == ElevatorState.L4_FRONT && elevator.atTargetHeight() && arm.getState() == ArmState.L4_FRONT && arm.atTargetAngle()) + .andThen(new SwerveDrivePIDToNearestBranchScore(4, true)) + .alongWith(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToL4Front().alongWith(new ArmToL4Front())) + .onlyIf(() -> elevator.getState() != ElevatorState.L4_FRONT || arm.getState() != ArmState.L4_FRONT) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget()))) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) // check again since robot may have moved + .andThen(new ShooterShootBackwards().alongWith(new SwerveDriveNudgeForward())), + new SwerveDrivePIDToNearestBranchReady(false) + .until(() -> elevator.getState() == ElevatorState.L4_BACK && elevator.atTargetHeight() && arm.getState() == ArmState.L4_BACK && arm.atTargetAngle()) + .andThen(new SwerveDrivePIDToNearestBranchScore(4, false)) + .alongWith(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToL4Back().alongWith(new ArmToL4Back())) + .onlyIf(() -> elevator.getState() != ElevatorState.L4_BACK || arm.getState() != ArmState.L4_BACK) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget()))) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) // check again since robot may have moved + .andThen(new ShooterShootForwards()), + () -> swerve.isFrontFacingReef())) + .onFalse(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToFeed().alongWith(new ArmToFeed()))) .onFalse(new ShooterStop()); - + + // L3 coral score driver.getRightButton() - .whileTrue( - new ConditionalCommand( - new SuperStructureToL3Front() - .andThen(new SuperStructureWaitUntilAtTarget() - .alongWith(new SwerveDrivePIDToNearestBranch(3, true) - .alongWith(new LEDSolidColor(Color.kYellow)))) - .andThen(new ShooterShootBackwards()), - new SuperStructureToL3Back() - .andThen(new SuperStructureWaitUntilAtTarget() - .alongWith(new SwerveDrivePIDToNearestBranch(3, false) - .alongWith(new LEDSolidColor(Color.kYellow)))) - .andThen(new ShooterShootForwards()), - () -> swerve.isFrontFacingReef()) - ) - .onFalse(new SuperStructureToFeed()) + .whileTrue(new ConditionalCommand( + new SwerveDrivePIDToNearestBranchReady(true) + .until(() -> elevator.getState() == ElevatorState.L3_FRONT && elevator.atTargetHeight() && arm.getState() == ArmState.L3_FRONT && arm.atTargetAngle()) + .andThen(new SwerveDrivePIDToNearestBranchScore(3, true)) + .alongWith(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToL3Front().alongWith(new ArmToL3Front())) + .onlyIf(() -> elevator.getState() != ElevatorState.L3_FRONT || arm.getState() != ArmState.L3_FRONT) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget()))) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) // check again since robot may have moved + .andThen(new ShooterShootBackwards().alongWith(new SwerveDriveNudgeForward())), + new SwerveDrivePIDToNearestBranchReady(false) + .until(() -> elevator.getState() == ElevatorState.L3_BACK && elevator.atTargetHeight() && arm.getState() == ArmState.L3_BACK && arm.atTargetAngle()) + .andThen(new SwerveDrivePIDToNearestBranchScore(3, false)) + .alongWith(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToL3Back().alongWith(new ArmToL3Back())) + .onlyIf(() -> elevator.getState() != ElevatorState.L3_BACK || arm.getState() != ArmState.L3_BACK) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget()))) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) // check again since robot may have moved + .andThen(new ShooterShootForwards()), + () -> swerve.isFrontFacingReef())) + .onFalse(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToFeed().alongWith(new ArmToFeed()))) .onFalse(new ShooterStop()); + // L2 coral score driver.getBottomButton() - .whileTrue( - new ConditionalCommand( - new SuperStructureToL2Front() - .andThen(new SuperStructureWaitUntilAtTarget() - .alongWith(new SwerveDrivePIDToNearestBranch(2, true) - .alongWith(new LEDSolidColor(Color.kYellow)))) - .andThen(new ShooterShootForwards()), - new SuperStructureToL2Back() - .andThen(new SuperStructureWaitUntilAtTarget() - .alongWith(new SwerveDrivePIDToNearestBranch(2, false) - .alongWith(new LEDSolidColor(Color.kYellow)))) - .andThen(new ShooterShootForwards()), - () -> swerve.isFrontFacingReef()) - ) - .onFalse(new SuperStructureToFeed()) + .whileTrue(new ConditionalCommand( + new SwerveDrivePIDToNearestBranchReady(true) + .until(() -> elevator.getState() == ElevatorState.L2_FRONT && elevator.atTargetHeight() && arm.getState() == ArmState.L2_FRONT && arm.atTargetAngle()) + .andThen(new SwerveDrivePIDToNearestBranchScore(2, true)) + .alongWith(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToL2Front().alongWith(new ArmToL2Front())) + .onlyIf(() -> elevator.getState() != ElevatorState.L2_FRONT || arm.getState() != ArmState.L2_FRONT) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget()))) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) // check again since robot may have moved + .andThen(new ShooterShootForwards().alongWith(new SwerveDriveNudgeForward())), + new SwerveDrivePIDToNearestBranchReady(false) + .until(() -> elevator.getState() == ElevatorState.L2_BACK && elevator.atTargetHeight() && arm.getState() == ArmState.L2_BACK && arm.atTargetAngle()) + .andThen(new SwerveDrivePIDToNearestBranchScore(2, false)) + .alongWith(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToL2Back().alongWith(new ArmToL2Back())) + .onlyIf(() -> elevator.getState() != ElevatorState.L2_BACK || arm.getState() != ArmState.L2_BACK) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget()))) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) // check again since robot may have moved + .andThen(new ShooterShootForwards()), + () -> swerve.isFrontFacingReef())) + .onFalse(new WaitUntilCommand(() -> swerve.isClearFromReef()) + .andThen(new ElevatorToFeed().alongWith(new ArmToFeed()))) .onFalse(new ShooterStop()); + // Barge score driver.getLeftButton() .whileTrue(new SwerveDriveDriveAlignedToBarge(driver)) - .whileTrue(new SuperStructureToBarge() - .andThen(new SuperStructureWaitUntilAtTarget() + .whileTrue(new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget()) .alongWith(new SwerveDriveWaitUntilAlignedToBarge() - .alongWith(new LEDSolidColor(Color.kYellow)))) + .alongWith(new LEDApplyPattern(Settings.LED.ALIGN_COLOR)))) .andThen(new ShooterShootAlgae())) - .onFalse(new SuperStructureToFeed()) + .onFalse(new ElevatorToFeed().alongWith(new ArmToFeed())) .onFalse(new ShooterStop()); + // Acquire Reef Algae L3 driver.getDPadLeft() - .whileTrue(new SuperStructureToAlgaeL3() - .andThen(new ShooterAcquireAlgae())) - .onFalse(new SuperStructureToFeed()); + .whileTrue(new ElevatorToAlgaeL3().alongWith(new ArmToAlgaeL3())) + .whileTrue(new ShooterAcquireAlgae()) + .onFalse(new ElevatorToHoldAlgae().alongWith(new ArmToHoldAlgae())); + // Acquire Reef Algae L2 driver.getDPadDown() - .whileTrue(new SuperStructureToAlgaeL2() - .andThen(new ShooterAcquireAlgae())) - .onFalse(new SuperStructureToFeed()); - - driver.getLeftMenuButton().onTrue(new ClimbOpen()); - driver.getRightMenuButton().onTrue(new ClimbClimb()); + .whileTrue(new ElevatorToAlgaeL2().alongWith(new ArmToAlgaeL2())) + .whileTrue(new ShooterAcquireAlgae()) + .onFalse(new ElevatorToHoldAlgae().alongWith(new ArmToHoldAlgae())); + + driver.getLeftMenuButton() + .onTrue(new ElevatorToClimb().alongWith(new ArmToClimb()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + .andThen(new ClimbOpen())); + + driver.getRightMenuButton() + .onTrue(new ClimbClimb()) + .onFalse(new ClimbIdle()); } private void configureOperatorButtonBindings() { + // Elevator Voltage Override new Trigger(() -> Math.abs(operator.getLeftStick().y) > Settings.Operator.Elevator.VOLTAGE_OVERRIDE_DEADBAND) .whileTrue(new ElevatorOverrideVoltage(() -> -operator.getLeftStick().y > 0 ? (-operator.getLeftStick().y * Math.abs(Settings.Operator.Elevator.MAX_VOLTAGE_UP)) : (-operator.getLeftStick().y * Math.abs(Settings.Operator.Elevator.MAX_VOLTAGE_DOWN)))); + // Arm Voltage Override new Trigger(() -> Math.abs(operator.getRightStick().x) > Settings.Operator.Arm.VOLTAGE_OVERRIDE_DEADBAND) .whileTrue(new ArmOverrideVoltage(() -> operator.getRightStick().x > 0 ? (operator.getRightStick().x * Math.abs(Settings.Operator.Arm.MAX_VOLTAGE_UP)) : (operator.getRightStick().x * Math.abs(Settings.Operator.Arm.MAX_VOLTAGE_DOWN)))); + // Shoot Backwards operator.getLeftTriggerButton() .whileTrue(new ConditionalCommand( new ShooterShootForwards().alongWith(new FunnelReverse()), @@ -293,28 +364,44 @@ private void configureOperatorButtonBindings() { () -> shooter.shouldShootBackwards())) .onFalse(new ShooterStop()); + // Shoot Forwards + operator.getRightTriggerButton() + .whileTrue(new ConditionalCommand( + new ShooterShootBackwards(), + new ShooterShootForwards(), + () -> shooter.shouldShootBackwards())) + .onFalse(new ShooterStop()); + + // Froggy pivot offsets operator.getLeftBumper().whileTrue(new FroggyPivotMoveOperatorOffsetUp()); operator.getRightBumper().whileTrue(new FroggyPivotMoveOperatorOffsetDown()); - operator.getLeftMenuButton().onTrue(new ClimbOverrideVoltage(Settings.Operator.Climb.CLIMB_DOWN_VOLTAGE)); - operator.getRightMenuButton().onTrue(new ClimbOverrideVoltage(Settings.Operator.Climb.CLIMB_UP_VOLTAGE)); - - operator.getTopButton().onTrue(swerve.isFrontFacingReef() ? new SuperStructureToL4Front() : new SuperStructureToL4Back()); - operator.getRightButton().onTrue(swerve.isFrontFacingReef() ? new SuperStructureToL3Front() : new SuperStructureToL3Back()); - operator.getBottomButton().onTrue(swerve.isFrontFacingReef() ? new SuperStructureToL2Front() : new SuperStructureToL2Back()); - operator.getLeftButton().onTrue(new SuperStructureToBarge()); - + // Climb voltage overrides + operator.getLeftMenuButton().whileTrue(new ClimbOverrideVoltage(Settings.Operator.Climb.CLIMB_DOWN_VOLTAGE)); + operator.getRightMenuButton().whileTrue(new ClimbOverrideVoltage(Settings.Operator.Climb.CLIMB_UP_VOLTAGE)); + + // Arm/Elevator to heights + operator.getTopButton().onTrue(swerve.isFrontFacingReef() + ? new ElevatorToL4Front().alongWith(new ArmToL4Front()) + : new ElevatorToL4Back().alongWith(new ArmToL4Back())); + operator.getRightButton().onTrue(swerve.isFrontFacingReef() + ? new ElevatorToL3Front().alongWith(new ArmToL3Front()) + : new ElevatorToL3Back().alongWith(new ArmToL3Back())); + operator.getBottomButton().onTrue(swerve.isFrontFacingReef() + ? new ElevatorToL2Front().alongWith(new ArmToL2Front()) + : new ElevatorToL2Back().alongWith(new ArmToL2Back())); + + operator.getLeftButton() + .onTrue(new ElevatorToBarge()) + .onTrue(new ArmToBarge()); + + // Elevator offsets operator.getDPadUp().onTrue(new ElevatorOffsetTargetUp()); operator.getDPadDown().onTrue(new ElevatorOffsetTargetDown()); + // Arm offsets operator.getDPadLeft().onTrue(new ArmOffsetTargetDown()); operator.getDPadRight().onTrue(new ArmOffsetTargetUp()); - - operator.getRightTriggerButton() - .whileTrue(new ConditionalCommand( - new ShooterShootBackwards(), - new ShooterShootForwards(), - () -> shooter.shouldShootBackwards())); } /**************/ @@ -325,6 +412,11 @@ public void configureAutons() { swerve.configureAutoBuilder(); + AutonConfig testa = new AutonConfig("testa", testing::new, + "Red Bottom to E", "Red E to HP", "Red HP to D", "Red D to HP", "Red HP to C", "Blue C to HP", "Red HP to B"); + + testa.registerRed(autonChooser); + autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); /** TOP AUTONS **/ @@ -426,6 +518,58 @@ public void configureAutons() { BLUE_FOUR_PIECE_EDCB.registerBlue(autonChooser); RED_FOUR_PIECE_EDCB.registerRed(autonChooser); + /** TOP ALGAE AUTONS **/ + + AutonConfig BLUE_H_ONE_ALGAE = new AutonConfig("1 Piece H + 1 Algae", OneHOneAlgae::new, + "Blue Mid Top to H", "Blue H to GH Algae", "Blue GH Algae to Barge 3"); + AutonConfig RED_H_ONE_ALGAE = new AutonConfig("1 Piece H + 1 Algae", OneHOneAlgae::new, + "Red Mid Top to H", "Red H to GH Algae", "Red GH Algae to Barge 3"); + + AutonConfig BLUE_H_TWO_ALGAE = new AutonConfig("1 Piece H + 2 Algae", OneHTwoAlgae::new, + "Blue Mid Top to H", "Blue H to GH Algae", "Blue GH Algae to Barge 3", "Blue Barge 3 to IJ Algae", "Blue IJ Algae to Barge 3"); + AutonConfig RED_H_TWO_ALGAE = new AutonConfig("1 Piece H + 2 Algae", OneHTwoAlgae::new, + "Red Mid Top to H", "Red H to GH Algae", "Red GH Algae to Barge 3", "Red Barge 3 to IJ Algae", "Red IJ Algae to Barge 3"); + + AutonConfig BLUE_H_THREE_ALGAE = new AutonConfig("1 Piece H + 3 Algae", OneHThreeAlgae::new, + "Blue Mid Top to H", "Blue H to GH Algae", "Blue GH Algae to Barge 3", "Blue Barge 3 to IJ Algae", "Blue IJ Algae to Barge 3", "Blue Barge 3 to EF Algae", "Blue EF Algae to Barge 3"); + AutonConfig RED_H_THREE_ALGAE = new AutonConfig("1 Piece H + 3 Algae", OneHThreeAlgae::new, + "Red Mid Top to H", "Red H to GH Algae", "Red GH Algae to Barge 3", "Red Barge 3 to IJ Algae", "Red IJ Algae to Barge 3", "Red Barge 3 to EF Algae", "Red EF Algae to Barge 3"); + + BLUE_H_ONE_ALGAE.registerBlue(autonChooser); + RED_H_ONE_ALGAE.registerRed(autonChooser); + + BLUE_H_TWO_ALGAE.registerBlue(autonChooser); + RED_H_TWO_ALGAE.registerRed(autonChooser); + + BLUE_H_THREE_ALGAE.registerBlue(autonChooser); + RED_H_THREE_ALGAE.registerRed(autonChooser); + + /** BOTTOM ALGAE AUTONS **/ + + AutonConfig BLUE_G_ONE_ALGAE = new AutonConfig("1 Piece G + 1 Algae", OneGOneAlgae::new, + "Blue Mid Bottom to G", "Blue G to GH Algae", "Blue GH Algae to Barge 3"); + AutonConfig RED_G_ONE_ALGAE = new AutonConfig("1 Piece G + 1 Algae", OneGOneAlgae::new, + "Red Mid Bottom to G", "Red G to GH Algae", "Red GH Algae to Barge 3"); + + AutonConfig BLUE_G_TWO_ALGAE = new AutonConfig("1 Piece G + 2 Algae", OneGTwoAlgae::new, + "Blue Mid Bottom to G", "Blue G to GH Algae", "Blue GH Algae to Barge 3", "Blue Barge 3 to IJ Algae", "Blue IJ Algae to Barge 3"); + AutonConfig RED_G_TWO_ALGAE = new AutonConfig("1 Piece G + 2 Algae", OneGTwoAlgae::new, + "Red Mid Bottom to G", "Red G to GH Algae", "Red GH Algae to Barge 3", "Red Barge 3 to IJ Algae", "Red IJ Algae to Barge 3"); + + AutonConfig BLUE_G_THREE_ALGAE = new AutonConfig("1 Piece G + 3 Algae", OneGThreeAlgae::new, + "Blue Mid Bottom to G", "Blue G to GH Algae", "Blue GH Algae to Barge 3", "Blue Barge 3 to IJ Algae", "Blue IJ Algae to Barge 3", "Blue Barge 3 to EF Algae", "Blue EF Algae to Barge 3"); + AutonConfig RED_G_THREE_ALGAE = new AutonConfig("1 Piece G + 3 Algae", OneGThreeAlgae::new, + "Red Mid Bottom to G", "Red G to GH Algae", "Red GH Algae to Barge 3", "Red Barge 3 to IJ Algae", "Red IJ Algae to Barge 3", "Red Barge 3 to EF Algae", "Red EF Algae to Barge 3"); + + BLUE_G_ONE_ALGAE.registerBlue(autonChooser); + RED_G_ONE_ALGAE.registerRed(autonChooser); + + BLUE_G_TWO_ALGAE.registerBlue(autonChooser); + RED_G_TWO_ALGAE.registerRed(autonChooser); + + BLUE_G_THREE_ALGAE.registerBlue(autonChooser); + RED_G_THREE_ALGAE.registerRed(autonChooser); + /** TESTS **/ AutonConfig BLUE_MOBILITY = new AutonConfig("Mobility", Mobility::new, @@ -440,6 +584,9 @@ public void configureAutons() { "Square Top", "Square Right", "Square Bottom", "Square Left"); AutonConfig RSQUARE_TEST = new AutonConfig("RSquare Test", RSquareTest::new, "RSquare Top", "RSquare Right", "RSquare Bottom", "RSquare Left"); + AutonConfig testPath = new AutonConfig("testing", testing::new, + "Red Bottom to E", "Red E to HP", "Red HP to D", "Red D to HP", "Red HP to C", "Blue C to HP", "Red HP to B"); + BLUE_MOBILITY.registerBlue(autonChooser); RED_MOBILITY.registerRed(autonChooser); @@ -447,25 +594,34 @@ public void configureAutons() { CURVY_LINE_TEST.registerRed(autonChooser); SQUARE_TEST.registerRed(autonChooser); RSQUARE_TEST.registerRed(autonChooser); + testPath.registerRed(autonChooser); SmartDashboard.putData("Autonomous", autonChooser); } public void configureSysids() { - autonChooser.setDefaultOption("Swerve Quasi Forward", swerve.sysIdQuasistatic(Direction.kForward)); - autonChooser.setDefaultOption("Swerve Quasi Backward", swerve.sysIdQuasistatic(Direction.kReverse)); - autonChooser.setDefaultOption("Swerve Dynamic Forward", swerve.sysIdDynamic(Direction.kForward)); - autonChooser.setDefaultOption("Swerve Dynamic Backward", swerve.sysIdDynamic(Direction.kReverse)); - - autonChooser.setDefaultOption("Elevator Quasi Forward", elevator.getSysIdQuasistatic(Direction.kForward)); - autonChooser.setDefaultOption("Elevator Quasi Backward", elevator.getSysIdQuasistatic(Direction.kReverse)); - autonChooser.setDefaultOption("Elevator Dynamic Forward", elevator.getSysIdDynamic(Direction.kForward)); - autonChooser.setDefaultOption("Elevator Dynamic Backward", elevator.getSysIdDynamic(Direction.kReverse)); - - autonChooser.setDefaultOption("Arm Quasi Forward", arm.getSysIdQuasistatic(Direction.kForward)); - autonChooser.setDefaultOption("Arm Quasi Backward", arm.getSysIdQuasistatic(Direction.kReverse)); - autonChooser.setDefaultOption("Arm Dynamic Forward", arm.getSysIdDynamic(Direction.kForward)); - autonChooser.setDefaultOption("Arm Dynamic Backward", arm.getSysIdDynamic(Direction.kReverse)); + autonChooser.addOption("Swerve Quasi Forward", swerve.sysIdQuasistatic(Direction.kForward)); + autonChooser.addOption("Swerve Quasi Backward", swerve.sysIdQuasistatic(Direction.kReverse)); + autonChooser.addOption("Swerve Dynamic Forward", swerve.sysIdDynamic(Direction.kForward)); + autonChooser.addOption("Swerve Dynamic Backward", swerve.sysIdDynamic(Direction.kReverse)); + + SysIdRoutine elevatorSysIdRoutine = elevator.getSysIdRoutine(); + autonChooser.addOption("Elevator Quasi Forward", elevatorSysIdRoutine.quasistatic(Direction.kForward)); + autonChooser.addOption("Elevator Quasi Backward", elevatorSysIdRoutine.quasistatic(Direction.kReverse)); + autonChooser.addOption("Elevator Dynamic Forward", elevatorSysIdRoutine.dynamic(Direction.kForward)); + autonChooser.addOption("Elevator Dynamic Backward", elevatorSysIdRoutine.dynamic(Direction.kReverse)); + + SysIdRoutine armSysIdRoutine = arm.getSysIdRoutine(); + autonChooser.addOption("Arm Quasi Forward", armSysIdRoutine.quasistatic(Direction.kForward)); + autonChooser.addOption("Arm Quasi Backward", armSysIdRoutine.quasistatic(Direction.kReverse)); + autonChooser.addOption("Arm Dynamic Forward", armSysIdRoutine.dynamic(Direction.kForward)); + autonChooser.addOption("Arm Dynamic Backward", armSysIdRoutine.dynamic(Direction.kReverse)); + + SysIdRoutine froggyPivotSysIdRoutine = froggy.getFroggySysIdRoutine(); + autonChooser.addOption("Froggy Pivot Quasi Forward", froggyPivotSysIdRoutine.quasistatic(Direction.kForward)); + autonChooser.addOption("Froggy Pivot Quasi Backward", froggyPivotSysIdRoutine.quasistatic(Direction.kReverse)); + autonChooser.addOption("Froggy Pivot Dynamic Forward", froggyPivotSysIdRoutine.dynamic(Direction.kForward)); + autonChooser.addOption("Froggy Pivot Dynamic Backward", froggyPivotSysIdRoutine.dynamic(Direction.kReverse)); } public Command getAutonomousCommand() { diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToStow.java b/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimb.java similarity index 80% rename from src/main/java/com/stuypulse/robot/commands/arm/ArmToStow.java rename to src/main/java/com/stuypulse/robot/commands/arm/ArmToClimb.java index 4440e11..c55c00b 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToStow.java +++ b/src/main/java/com/stuypulse/robot/commands/arm/ArmToClimb.java @@ -8,8 +8,8 @@ import com.stuypulse.robot.subsystems.arm.Arm.ArmState; -public class ArmToStow extends ArmSetState { - public ArmToStow() { - super(ArmState.STOW); +public class ArmToClimb extends ArmSetState { + public ArmToClimb() { + super(ArmState.CLIMB); } } diff --git a/src/main/java/com/stuypulse/robot/commands/arm/algae/ArmToHoldAlgae.java b/src/main/java/com/stuypulse/robot/commands/arm/algae/ArmToHoldAlgae.java new file mode 100644 index 0000000..607700d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/arm/algae/ArmToHoldAlgae.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.arm.algae; + +import com.stuypulse.robot.commands.arm.ArmSetState; +import com.stuypulse.robot.subsystems.arm.Arm; + +public class ArmToHoldAlgae extends ArmSetState { + public ArmToHoldAlgae() { + super(Arm.ArmState.HOLD_ALGAE); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/FourPieceEDCB.java b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/FourPieceEDCB.java index 7cb72dd..ec34308 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/FourPieceEDCB.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/FourPieceEDCB.java @@ -1,48 +1,95 @@ package com.stuypulse.robot.commands.autons.EDCB; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class FourPieceEDCB extends SequentialCommandGroup { public FourPieceEDCB(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on E - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on D new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine(), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on C new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on B new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[6]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[6]), - new ScoreRoutine() + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/OnePieceE.java b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/OnePieceE.java index be93bd1..ec68c0e 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/OnePieceE.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/OnePieceE.java @@ -1,24 +1,39 @@ package com.stuypulse.robot.commands.autons.EDCB; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class OnePieceE extends SequentialCommandGroup { public OnePieceE(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on E - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine() + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreeHalfPieceEDC.java b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreeHalfPieceEDC.java index a738c55..02df0b5 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreeHalfPieceEDC.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreeHalfPieceEDC.java @@ -1,45 +1,82 @@ package com.stuypulse.robot.commands.autons.EDCB; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class ThreeHalfPieceEDC extends SequentialCommandGroup { public ThreeHalfPieceEDC(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on E - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on D new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine(), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), - // To HP< Score on C + // To HP, Score on C new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new ScoreRoutine(), - + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + // To HP, Acquire Coral new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreePieceEDC.java b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreePieceEDC.java index e914dcf..5b78c84 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreePieceEDC.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/ThreePieceEDC.java @@ -1,40 +1,77 @@ package com.stuypulse.robot.commands.autons.EDCB; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class ThreePieceEDC extends SequentialCommandGroup { public ThreePieceEDC(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on E - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on D new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on C new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new ScoreRoutine() + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/TwoPieceED.java b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/TwoPieceED.java index 6164eb4..831d7ae 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/EDCB/TwoPieceED.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/EDCB/TwoPieceED.java @@ -1,32 +1,59 @@ package com.stuypulse.robot.commands.autons.EDCB; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class TwoPieceED extends SequentialCommandGroup { public TwoPieceED(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on E - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on D new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine() + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGOneAlgae.java b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGOneAlgae.java new file mode 100644 index 0000000..17b1567 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGOneAlgae.java @@ -0,0 +1,73 @@ +package com.stuypulse.robot.commands.autons.GAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL2; +import com.stuypulse.robot.commands.arm.algae.ArmToBarge; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL2; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToBarge; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterAcquireAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OneGOneAlgae extends SequentialCommandGroup { + + public OneGOneAlgae(PathPlannerPath... paths) { + + addCommands( + + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on G + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + + // Score Preload on G + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + + // Acquire GH Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ElevatorToAlgaeL2().alongWith(new ArmToAlgaeL2()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGThreeAlgae.java b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGThreeAlgae.java new file mode 100644 index 0000000..e9818e3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGThreeAlgae.java @@ -0,0 +1,92 @@ +package com.stuypulse.robot.commands.autons.GAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL2; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL3; +import com.stuypulse.robot.commands.arm.algae.ArmToBarge; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL2; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL3; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToBarge; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterAcquireAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OneGThreeAlgae extends SequentialCommandGroup { + + public OneGThreeAlgae(PathPlannerPath... paths) { + + addCommands( + + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on G + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + + // Acquire GH Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ElevatorToAlgaeL2().alongWith(new ArmToAlgaeL2()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae(), + + // Acquire IJ Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new ElevatorToAlgaeL3().alongWith(new ArmToAlgaeL3()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae(), + + // Acquire EF Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + new ElevatorToAlgaeL3().alongWith(new ArmToAlgaeL3()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[6]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGTwoAlgae.java b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGTwoAlgae.java new file mode 100644 index 0000000..ab78b87 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OneGTwoAlgae.java @@ -0,0 +1,78 @@ +package com.stuypulse.robot.commands.autons.GAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL2; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL3; +import com.stuypulse.robot.commands.arm.algae.ArmToBarge; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL2; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL3; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToBarge; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterAcquireAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OneGTwoAlgae extends SequentialCommandGroup { + + public OneGTwoAlgae(PathPlannerPath... paths) { + + addCommands( + + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on G + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + + // Acquire GH Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ElevatorToAlgaeL2().alongWith(new ArmToAlgaeL2()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae(), + + // Acquire IJ Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new ElevatorToAlgaeL3().alongWith(new ArmToAlgaeL3()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OnePieceG.java b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OnePieceG.java new file mode 100644 index 0000000..b13988d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/GAlgae/OnePieceG.java @@ -0,0 +1,42 @@ +package com.stuypulse.robot.commands.autons.GAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OnePieceG extends SequentialCommandGroup { + + public OnePieceG(PathPlannerPath... paths) { + + addCommands( + + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on G + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHOneAlgae.java b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHOneAlgae.java new file mode 100644 index 0000000..8ff0334 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHOneAlgae.java @@ -0,0 +1,61 @@ +package com.stuypulse.robot.commands.autons.HAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL2; +import com.stuypulse.robot.commands.arm.algae.ArmToBarge; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL2; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToBarge; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterAcquireAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OneHOneAlgae extends SequentialCommandGroup { + + public OneHOneAlgae(PathPlannerPath... paths) { + + addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on H + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + + // Acquire GH Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ElevatorToAlgaeL2().alongWith(new ArmToAlgaeL2()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHThreeAlgae.java b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHThreeAlgae.java new file mode 100644 index 0000000..9ab18fb --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHThreeAlgae.java @@ -0,0 +1,92 @@ +package com.stuypulse.robot.commands.autons.HAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL2; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL3; +import com.stuypulse.robot.commands.arm.algae.ArmToBarge; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL2; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL3; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToBarge; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterAcquireAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OneHThreeAlgae extends SequentialCommandGroup { + + public OneHThreeAlgae(PathPlannerPath... paths) { + + addCommands( + + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on H + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + + // Acquire GH Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ElevatorToAlgaeL2().alongWith(new ArmToAlgaeL2()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae(), + + // Acquire IJ Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new ElevatorToAlgaeL3().alongWith(new ArmToAlgaeL3()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae(), + + // Acquire EF Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + new ElevatorToAlgaeL3().alongWith(new ArmToAlgaeL3()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[6]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHTwoAlgae.java b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHTwoAlgae.java new file mode 100644 index 0000000..c6e5d90 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OneHTwoAlgae.java @@ -0,0 +1,77 @@ +package com.stuypulse.robot.commands.autons.HAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL2; +import com.stuypulse.robot.commands.arm.algae.ArmToAlgaeL3; +import com.stuypulse.robot.commands.arm.algae.ArmToBarge; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL2; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToAlgaeL3; +import com.stuypulse.robot.commands.elevator.algae.ElevatorToBarge; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterAcquireAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootAlgae; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OneHTwoAlgae extends SequentialCommandGroup { + + public OneHTwoAlgae(PathPlannerPath... paths) { + + addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on H + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), + + // Acquire GH Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ElevatorToAlgaeL2().alongWith(new ArmToAlgaeL2()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae(), + + // Acquire IJ Algae, Score on Barge + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new ElevatorToAlgaeL3().alongWith(new ArmToAlgaeL3()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterAcquireAlgae(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new ElevatorToBarge().alongWith(new ArmToBarge()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new ShooterShootAlgae() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OnePieceH.java b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OnePieceH.java new file mode 100644 index 0000000..87ee4b4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/HAlgae/OnePieceH.java @@ -0,0 +1,41 @@ +package com.stuypulse.robot.commands.autons.HAlgae; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class OnePieceH extends SequentialCommandGroup { + + public OnePieceH(PathPlannerPath... paths) { + + addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Score Preload on H + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/FourPieceJKLA.java b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/FourPieceJKLA.java index 0eb8ebc..65be5bf 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/FourPieceJKLA.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/FourPieceJKLA.java @@ -1,48 +1,95 @@ package com.stuypulse.robot.commands.autons.JKLA; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class FourPieceJKLA extends SequentialCommandGroup { public FourPieceJKLA(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on J - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on K new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine(), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on L new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on A new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[6]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[6]), - new ScoreRoutine() + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/OnePieceJ.java b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/OnePieceJ.java index 0d4c3f6..d0d4467 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/OnePieceJ.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/OnePieceJ.java @@ -1,22 +1,39 @@ package com.stuypulse.robot.commands.autons.JKLA; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class OnePieceJ extends SequentialCommandGroup { public OnePieceJ(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on J - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine() + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreeHalfPieceJKL.java b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreeHalfPieceJKL.java index e329ba7..353997f 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreeHalfPieceJKL.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreeHalfPieceJKL.java @@ -1,45 +1,82 @@ package com.stuypulse.robot.commands.autons.JKLA; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class ThreeHalfPieceJKL extends SequentialCommandGroup { public ThreeHalfPieceJKL(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on J - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on K new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on L new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new ScoreRoutine(), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Acquire Coral new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreePieceJKL.java b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreePieceJKL.java index 809bda9..82538a1 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreePieceJKL.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/ThreePieceJKL.java @@ -1,40 +1,77 @@ package com.stuypulse.robot.commands.autons.JKLA; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class ThreePieceJKL extends SequentialCommandGroup { public ThreePieceJKL(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on J - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on K new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on L new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), - new ScoreRoutine() + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/TwoPieceJK.java b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/TwoPieceJK.java index 1fdbd30..e36d227 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/JKLA/TwoPieceJK.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/JKLA/TwoPieceJK.java @@ -1,32 +1,59 @@ package com.stuypulse.robot.commands.autons.JKLA; import com.pathplanner.lib.path.PathPlannerPath; -import com.stuypulse.robot.commands.autons.routines.ScoreRoutine; -import com.stuypulse.robot.commands.superstructure.SuperStructureToFeed; +import com.stuypulse.robot.commands.arm.ArmToFeed; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; +import com.stuypulse.robot.commands.shooter.ShooterStop; +import com.stuypulse.robot.commands.elevator.ElevatorToFeed; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class TwoPieceJK extends SequentialCommandGroup { public TwoPieceJK(PathPlannerPath... paths) { addCommands( + new SwerveDriveResetPoseToStartOfPath(paths[0]), // Score Preload on J - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - new ScoreRoutine(), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ), + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop(), // To HP, Score on K new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - new SuperStructureToFeed() + new ElevatorToFeed().alongWith(new ArmToFeed()) + ), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new WaitUntilCommand(() -> Shooter.getInstance().hasCoral()) + .andThen( + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight().alongWith(new ArmWaitUntilAtTarget())) + ) ), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new ScoreRoutine() + new SwerveDrivePIDToNearestBranchScore(4, true) + .andThen(new ShooterShootBackwards()), + new WaitUntilCommand(() -> !Shooter.getInstance().hasCoral()), + new ShooterStop() ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/misc/Mobility.java b/src/main/java/com/stuypulse/robot/commands/autons/misc/Mobility.java index 7078d78..c6bf428 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/misc/Mobility.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/misc/Mobility.java @@ -13,7 +13,9 @@ public Mobility(PathPlannerPath... paths) { addCommands( new SwerveDriveResetPoseToStartOfPath(paths[0]), - // Drive straight + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + // Drives straight CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ); diff --git a/src/main/java/com/stuypulse/robot/commands/autons/routines/ScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/autons/routines/ScoreRoutine.java index 902a460..c0af9cb 100644 --- a/src/main/java/com/stuypulse/robot/commands/autons/routines/ScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/autons/routines/ScoreRoutine.java @@ -2,11 +2,13 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; +import com.stuypulse.robot.commands.arm.coral.ArmToL4Front; +import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; +import com.stuypulse.robot.commands.elevator.coral.ElevatorToL4Front; import com.stuypulse.robot.commands.shooter.ShooterShootBackwards; import com.stuypulse.robot.commands.shooter.ShooterStop; -import com.stuypulse.robot.commands.superstructure.SuperStructureToL4Front; -import com.stuypulse.robot.commands.superstructure.SuperStructureWaitUntilAtTarget; -import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranch; +import com.stuypulse.robot.commands.swerve.SwerveDrivePIDToNearestBranchScore; import com.stuypulse.robot.constants.Settings; public class ScoreRoutine extends SequentialCommandGroup { @@ -15,9 +17,11 @@ public ScoreRoutine() { addCommands( - new SuperStructureToL4Front() - .andThen(new SuperStructureWaitUntilAtTarget().alongWith(new SwerveDrivePIDToNearestBranch(4, true))) - .andThen(new ShooterShootBackwards()), + new ElevatorToL4Front().alongWith(new ArmToL4Front()) + .andThen(new ElevatorWaitUntilAtTargetHeight() + .alongWith(new ArmWaitUntilAtTarget()) + .alongWith(new SwerveDrivePIDToNearestBranchScore(4, true))) + .andThen(new ShooterShootBackwards()), new WaitCommand(Settings.Auton.SHOOTER_WAIT_TIME), new ShooterStop() diff --git a/src/main/java/com/stuypulse/robot/commands/autons/tests/testing.java b/src/main/java/com/stuypulse/robot/commands/autons/tests/testing.java new file mode 100644 index 0000000..c5516d6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/autons/tests/testing.java @@ -0,0 +1,29 @@ +package com.stuypulse.robot.commands.autons.tests; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.swerve.SwerveDriveResetPoseToStartOfPath; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class testing extends SequentialCommandGroup { + + public testing(PathPlannerPath... paths) { + + addCommands( + + new SwerveDriveResetPoseToStartOfPath(paths[0]), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[5]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[6]) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/arm/ArmToVerticalDown.java b/src/main/java/com/stuypulse/robot/commands/climb/ClimbClose.java similarity index 60% rename from src/main/java/com/stuypulse/robot/commands/arm/ArmToVerticalDown.java rename to src/main/java/com/stuypulse/robot/commands/climb/ClimbClose.java index 1b21286..6c37310 100644 --- a/src/main/java/com/stuypulse/robot/commands/arm/ArmToVerticalDown.java +++ b/src/main/java/com/stuypulse/robot/commands/climb/ClimbClose.java @@ -4,12 +4,12 @@ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.arm; +package com.stuypulse.robot.commands.climb; -import com.stuypulse.robot.subsystems.arm.Arm.ArmState; +import com.stuypulse.robot.subsystems.climb.Climb.ClimbState; -public class ArmToVerticalDown extends ArmSetState { - public ArmToVerticalDown() { - super(ArmState.VERTICAL_DOWN); +public class ClimbClose extends ClimbToState { + public ClimbClose() { + super(ClimbState.CLOSED); } } diff --git a/src/main/java/com/stuypulse/robot/commands/climb/ClimbStow.java b/src/main/java/com/stuypulse/robot/commands/climb/ClimbIdle.java similarity index 81% rename from src/main/java/com/stuypulse/robot/commands/climb/ClimbStow.java rename to src/main/java/com/stuypulse/robot/commands/climb/ClimbIdle.java index 10183da..035a7ed 100644 --- a/src/main/java/com/stuypulse/robot/commands/climb/ClimbStow.java +++ b/src/main/java/com/stuypulse/robot/commands/climb/ClimbIdle.java @@ -8,8 +8,8 @@ import com.stuypulse.robot.subsystems.climb.Climb.ClimbState; -public class ClimbStow extends ClimbToState { - public ClimbStow() { - super(ClimbState.STOW); +public class ClimbIdle extends ClimbToState { + public ClimbIdle() { + super(ClimbState.IDLE); } } diff --git a/src/main/java/com/stuypulse/robot/commands/climb/ClimbWaitUntilAtTarget.java b/src/main/java/com/stuypulse/robot/commands/climb/ClimbWaitUntilAtTarget.java deleted file mode 100644 index 8bcf0cb..0000000 --- a/src/main/java/com/stuypulse/robot/commands/climb/ClimbWaitUntilAtTarget.java +++ /dev/null @@ -1,11 +0,0 @@ -package com.stuypulse.robot.commands.climb; - -import com.stuypulse.robot.subsystems.climb.Climb; - -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class ClimbWaitUntilAtTarget extends WaitUntilCommand{ - public ClimbWaitUntilAtTarget() { - super(() -> Climb.getInstance().atTargetAngle()); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToClimb.java b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToClimb.java new file mode 100644 index 0000000..12b1be7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/ElevatorToClimb.java @@ -0,0 +1,15 @@ +/************************ PROJECT MARY *************************/ +/* Copyright (c) 2025 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands.elevator; + +import com.stuypulse.robot.subsystems.elevator.Elevator.ElevatorState; + +public class ElevatorToClimb extends ElevatorToHeight { + public ElevatorToClimb() { + super(ElevatorState.CLIMB); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/algae/ElevatorToHoldAlgae.java b/src/main/java/com/stuypulse/robot/commands/elevator/algae/ElevatorToHoldAlgae.java new file mode 100644 index 0000000..229ef5c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/elevator/algae/ElevatorToHoldAlgae.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.elevator.algae; + +import com.stuypulse.robot.commands.elevator.ElevatorToHeight; +import com.stuypulse.robot.subsystems.elevator.Elevator.ElevatorState; + +public class ElevatorToHoldAlgae extends ElevatorToHeight{ + public ElevatorToHoldAlgae() { + super(ElevatorState.HOLD_ALGAE); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/elevator/coral/ElevatorToL3Back.java b/src/main/java/com/stuypulse/robot/commands/elevator/coral/ElevatorToL3Back.java index bfe8b66..be0e61b 100644 --- a/src/main/java/com/stuypulse/robot/commands/elevator/coral/ElevatorToL3Back.java +++ b/src/main/java/com/stuypulse/robot/commands/elevator/coral/ElevatorToL3Back.java @@ -11,6 +11,6 @@ public class ElevatorToL3Back extends ElevatorToHeight { public ElevatorToL3Back() { - super(ElevatorState.ALGAE_L3); + super(ElevatorState.L3_BACK); } } diff --git a/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeAlgae.java b/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeAlgae.java index 4bb4fff..3029887 100644 --- a/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeAlgae.java @@ -26,12 +26,17 @@ public void initialize() { } @Override - public void end(boolean interrupted) { - froggy.setRollerState(RollerState.STOP); + public boolean isFinished() { + return froggy.isStalling(); } @Override - public boolean isFinished() { - return froggy.isStalling(); + public void end(boolean interrupted) { + if (!interrupted) { + froggy.setRollerState(RollerState.HOLD_ALGAE); + } + else { + froggy.setRollerState(RollerState.STOP); + } } } diff --git a/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeCoral.java b/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeCoral.java index d31bfc6..d7a84df 100644 --- a/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeCoral.java +++ b/src/main/java/com/stuypulse/robot/commands/froggy/roller/FroggyRollerIntakeCoral.java @@ -26,12 +26,12 @@ public void initialize() { } @Override - public void end(boolean interrupted) { - froggy.setRollerState(RollerState.STOP); + public boolean isFinished() { + return froggy.isStalling(); } @Override - public boolean isFinished() { - return froggy.isStalling(); + public void end(boolean interrupted) { + froggy.setRollerState(RollerState.STOP); } } diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDSolidColor.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java similarity index 72% rename from src/main/java/com/stuypulse/robot/commands/leds/LEDSolidColor.java rename to src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java index 420f85e..48845c5 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDSolidColor.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDApplyPattern.java @@ -9,22 +9,21 @@ import com.stuypulse.robot.subsystems.led.LEDController; import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; -public class LEDSolidColor extends Command { +public class LEDApplyPattern extends Command { protected final LEDController leds; - protected final Color selectedColor; + protected final LEDPattern pattern; - public LEDSolidColor(Color color) { + public LEDApplyPattern(LEDPattern pattern) { leds = LEDController.getInstance(); - selectedColor = color; + this.pattern = pattern; addRequirements(leds); } @Override public void execute() { - leds.applyPattern(LEDPattern.solid(selectedColor)); + leds.applyPattern(pattern); } } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java index f73a3e8..3094d32 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireAlgae.java @@ -32,6 +32,11 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - shooter.setState(ShooterState.STOP); + if (!interrupted) { + shooter.setState(ShooterState.HOLD_ALGAE); + } + else { + shooter.setState(ShooterState.STOP); + } } } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java index 76c832d..652831b 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java @@ -9,9 +9,9 @@ import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ShooterStop extends Command { +public class ShooterStop extends InstantCommand { private final Shooter shooter; diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToAlgaeL2.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToAlgaeL2.java deleted file mode 100644 index 24b887b..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToAlgaeL2.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToAlgaeL2 extends SuperStructureToState{ - public SuperStructureToAlgaeL2() { - super(SuperStructureTargetState.ALGAE_L2); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToAlgaeL3.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToAlgaeL3.java deleted file mode 100644 index 7960ba1..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToAlgaeL3.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToAlgaeL3 extends SuperStructureToState{ - public SuperStructureToAlgaeL3() { - super(SuperStructureTargetState.ALGAE_L3); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToBarge.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToBarge.java deleted file mode 100644 index 786b0a9..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToBarge.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToBarge extends SuperStructureToState{ - public SuperStructureToBarge() { - super(SuperStructureTargetState.BARGE); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToFeed.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToFeed.java deleted file mode 100644 index 7af0c47..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToFeed.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToFeed extends SuperStructureToState{ - public SuperStructureToFeed() { - super(SuperStructureTargetState.FEED); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL2Back.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL2Back.java deleted file mode 100644 index 67c6783..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL2Back.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToL2Back extends SuperStructureToState{ - public SuperStructureToL2Back() { - super(SuperStructureTargetState.L2_BACK); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL2Front.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL2Front.java deleted file mode 100644 index 6520a23..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL2Front.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToL2Front extends SuperStructureToState{ - public SuperStructureToL2Front() { - super(SuperStructureTargetState.L2_FRONT); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL3Back.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL3Back.java deleted file mode 100644 index 1d73ca9..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL3Back.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToL3Back extends SuperStructureToState{ - public SuperStructureToL3Back() { - super(SuperStructureTargetState.L3_BACK); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL3Front.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL3Front.java deleted file mode 100644 index 8e89c60..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL3Front.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToL3Front extends SuperStructureToState{ - public SuperStructureToL3Front() { - super(SuperStructureTargetState.L3_FRONT); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL4Back.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL4Back.java deleted file mode 100644 index ce59950..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL4Back.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToL4Back extends SuperStructureToState{ - public SuperStructureToL4Back() { - super(SuperStructureTargetState.L4_BACK); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL4Front.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL4Front.java deleted file mode 100644 index b8178e9..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToL4Front.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToL4Front extends SuperStructureToState{ - public SuperStructureToL4Front() { - super(SuperStructureTargetState.L4_FRONT); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToState.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToState.java deleted file mode 100644 index 7afa67e..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToState.java +++ /dev/null @@ -1,22 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure; -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class SuperStructureToState extends InstantCommand{ - private final SuperStructure superStructure; - private final SuperStructureTargetState targetState; - - public SuperStructureToState(SuperStructureTargetState state) { - this.superStructure = SuperStructure.getInstance(); - this.targetState = state; - addRequirements(superStructure); - } - - @Override - public void initialize() { - superStructure.setTargetState(targetState); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToStow.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToStow.java deleted file mode 100644 index 1e35c80..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureToStow.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.subsystems.superstructure.SuperStructure.SuperStructureTargetState; - -public class SuperStructureToStow extends SuperStructureToState{ - public SuperStructureToStow() { - super(SuperStructureTargetState.STOW); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureWaitUntilAtTarget.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureWaitUntilAtTarget.java deleted file mode 100644 index 45f1fed..0000000 --- a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperStructureWaitUntilAtTarget.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.stuypulse.robot.commands.superstructure; - -import com.stuypulse.robot.commands.arm.ArmWaitUntilAtTarget; -import com.stuypulse.robot.commands.elevator.ElevatorWaitUntilAtTargetHeight; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; - -public class SuperStructureWaitUntilAtTarget extends ParallelCommandGroup{ - public SuperStructureWaitUntilAtTarget() { - super( - new ArmWaitUntilAtTarget(), - new ElevatorWaitUntilAtTargetHeight() - ); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNudgeForward.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNudgeForward.java new file mode 100644 index 0000000..013b117 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNudgeForward.java @@ -0,0 +1,33 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveNudgeForward extends Command { + private final CommandSwerveDrivetrain swerve; + + public SwerveDriveNudgeForward() { + swerve = CommandSwerveDrivetrain.getInstance(); + addRequirements(swerve); + } + + @Override + public void execute() { + swerve.setControl(swerve.getRobotCentricSwerveRequest() + .withVelocityX(Settings.Swerve.NUDGE_FORWARD_SPEED_METERS_PER_SECOND) + .withVelocityY(0) + .withRotationalRate(0) + ); + } + + @Override + public void end(boolean interrupted) { + swerve.setControl(swerve.getRobotCentricSwerveRequest() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(0)); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranch.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranch.java deleted file mode 100644 index 300f5f8..0000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranch.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.constants.Field; - -public class SwerveDrivePIDToNearestBranch extends SwerveDrivePIDToPose{ - public SwerveDrivePIDToNearestBranch(int level, boolean isScoringFrontSide) { - super(() -> Field.getClosestBranch().getTargetPose(level, isScoringFrontSide)); - } -} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranchReady.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranchReady.java new file mode 100644 index 0000000..b5d5849 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranchReady.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; + +public class SwerveDrivePIDToNearestBranchReady extends SwerveDrivePIDToPose{ + public SwerveDrivePIDToNearestBranchReady(boolean isScoringFrontSide) { + super(() -> Field.getClosestBranch().getReadyPose(isScoringFrontSide)); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranchScore.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranchScore.java new file mode 100644 index 0000000..65c6544 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToNearestBranchScore.java @@ -0,0 +1,9 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; + +public class SwerveDrivePIDToNearestBranchScore extends SwerveDrivePIDToPose{ + public SwerveDrivePIDToNearestBranchScore(int level, boolean isScoringFrontSide) { + super(() -> Field.getClosestBranch().getScorePose(level, isScoringFrontSide)); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToPose.java index d57ba4e..dcc3a67 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDrivePIDToPose.java @@ -8,8 +8,10 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.robot.util.TranslationMotionProfileIan; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; import com.stuypulse.stuylib.math.Vector2D; import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; import com.stuypulse.stuylib.streams.booleans.BStream; @@ -21,6 +23,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -53,8 +56,8 @@ public SwerveDrivePIDToPose(Supplier poseSupplier) { swerve = CommandSwerveDrivetrain.getInstance(); controller = new HolonomicController( - new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), - new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), + new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD).add(new MotorFeedforward(0, 0.8, 0).position()), + new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD).add(new MotorFeedforward(0, 0.8, 0).position()), new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.MAX_ANGULAR_ACCELERATION))); @@ -88,11 +91,18 @@ public SwerveDrivePIDToPose withTolerance(Number x, Number y, Number theta) { // the VStream needs to be recreated everytime the command is scheduled to allow the target tranlation to jump to the start of the path private VStream getNewTranslationSetpointGenerator() { - VStream targetTranslationRelativeToStart = VStream.create(() -> new Vector2D(targetPose.getTranslation().minus(startingPose.getTranslation()))) - .filtered( - new VMotionProfile(Settings.Swerve.Alignment.Constraints.MAX_VELOCITY, Settings.Swerve.Alignment.Constraints.MAX_ACCELERATION)); - - return VStream.create(() -> targetTranslationRelativeToStart.get().add(new Vector2D(startingPose.getTranslation()))); + return VStream.create(() -> new Vector2D(targetPose.getTranslation())) + .filtered(new TranslationMotionProfileIan( + Settings.Swerve.Alignment.Constraints.MAX_VELOCITY, + Settings.Swerve.Alignment.Constraints.MAX_ACCELERATION, + new Vector2D(swerve.getPose().getTranslation()), + swerve.getFieldRelativeSpeeds())); + + // VStream targetTranslationRelativeToStart = VStream.create(() -> new Vector2D(targetPose.getTranslation().minus(startingPose.getTranslation()))) + // .filtered( + // new VMotionProfile(Settings.Swerve.Alignment.Constraints.MAX_VELOCITY, Settings.Swerve.Alignment.Constraints.MAX_ACCELERATION)); + + // return VStream.create(() -> targetTranslationRelativeToStart.get().add(new Vector2D(startingPose.getTranslation()))); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 87b618e..595f473 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -8,7 +8,8 @@ public interface Cameras { public CameraInfo[] LimelightCameras = new CameraInfo[] { - new CameraInfo("limelight", new Pose3d()) + new CameraInfo("limelight-funnel", new Pose3d(Units.inchesToMeters(-13.819), Units.inchesToMeters(-2.250), Units.inchesToMeters(6.829), new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(-20), Units.degreesToRadians(180)))), + new CameraInfo("limelight-shooter", new Pose3d(Units.inchesToMeters(9.0), Units.inchesToMeters(-9.226), Units.inchesToMeters(10.0), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(-13.1), Units.degreesToRadians(-18.65)))) }; public static class CameraInfo { diff --git a/src/main/java/com/stuypulse/robot/constants/Constants.java b/src/main/java/com/stuypulse/robot/constants/Constants.java index c93f199..68bc3f1 100644 --- a/src/main/java/com/stuypulse/robot/constants/Constants.java +++ b/src/main/java/com/stuypulse/robot/constants/Constants.java @@ -11,10 +11,10 @@ public interface Constants { - double LENGTH_WITH_BUMPERS_METERS = Units.inchesToMeters(30); - double WIDTH_WITH_BUMPERS_METERS = Units.inchesToMeters(30); + double LENGTH_WITH_BUMPERS_METERS = Units.inchesToMeters(37.16); + double WIDTH_WITH_BUMPERS_METERS = Units.inchesToMeters(36.16); - double SHOOTER_Y_OFFSET = Units.inchesToMeters(8.5); + double SHOOTER_Y_OFFSET = Units.inchesToMeters(3.16 + 0.6); public interface Elevator { double MIN_HEIGHT_METERS = Units.inchesToMeters(40.85); // FROM FLOOR TO TOP OF ELEVATOR @@ -38,32 +38,32 @@ public interface Encoders { } public interface Arm { - double GEAR_RATIO = 50.0 / 3.0; + double GEAR_RATIO = 30.0; double DISTANCE_FROM_PIVOT_TO_TOP_OF_ELEVATOR = Units.inchesToMeters(5); // Current used for sim only - Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(216.992516 + 90); + Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(201.848576 + 90); double ARM_LENGTH = Units.inchesToMeters(29); double MASS_KG = Units.lbsToKilograms(12.8); double MOMENT_OF_INERTIA = MASS_KG * ARM_LENGTH * ARM_LENGTH / 3; - - Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(-98.638641); - Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(240); } public interface Froggy { - double GEAR_RATIO = 8.0 / 1.0; + double GEAR_RATIO = 48.0; - Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-56.584441); - Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(94.812699); + Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-51); + Rotation2d MAXIMUM_ANGLE = Rotation2d.fromDegrees(105); - Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(139.420866); + Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(219.815204 - 90); } public interface Climb { double GEAR_RATIO = 75.0; - Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(0.0); + Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(259); + + Rotation2d MIN_ANGLE = Rotation2d.kZero; + Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(265); } public interface LED { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 69970b0..4cf7fb0 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -132,12 +132,13 @@ public static boolean isValidAprilTagId(int id) { /*** REEF POSITIONS ***/ Translation2d REEF_CENTER = new Translation2d(Units.inchesToMeters(144.0 + 93.5 / 2), WIDTH / 2); + double CENTER_OF_REEF_TO_REEF_FACE = Units.inchesToMeters(32.75); double CENTER_OF_TROUGH_TO_BRANCH = Units.inchesToMeters(13.0/2.0); public enum CoralBranch { A,B,C,D,E,F,G,H,I,J,K,L; - public Pose2d getTargetPose(int level, boolean isScoringFrontSide) { + public Pose2d getScorePose(int level, boolean isScoringFrontSide) { Pose3d correspondingAprilTagPose; switch (this) { case A, B: @@ -174,7 +175,7 @@ public Pose2d getTargetPose(int level, boolean isScoringFrontSide) { targetDistanceFromReef = isScoringFrontSide ? Settings.Swerve.Alignment.Targets.TARGET_DISTANCE_FROM_REEF_L4_FRONT : Settings.Swerve.Alignment.Targets.TARGET_DISTANCE_FROM_REEF_L4_BACK; break; default: - throw new IllegalArgumentException("Branch level provided to CoralBranch.getTargetPose() was invalid. Should be in range [2,4]"); + throw new IllegalArgumentException("Branch level provided to CoralBranch.getScorePose() was invalid. Should be in range [2,4]"); } return correspondingAprilTagPose.toPose2d().transformBy( @@ -184,6 +185,37 @@ public Pose2d getTargetPose(int level, boolean isScoringFrontSide) { isScoringFrontSide ? Rotation2d.k180deg : Rotation2d.kZero)); } + public Pose2d getReadyPose(boolean isScoringFrontSide) { + Pose3d correspondingAprilTagPose; + switch (this) { + case A, B: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_AB.getLocation() : NamedTags.RED_AB.getLocation(); + break; + case C, D: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_CD.getLocation() : NamedTags.RED_CD.getLocation(); + break; + case E, F: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_EF.getLocation() : NamedTags.RED_EF.getLocation(); + break; + case G, H: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_GH.getLocation() : NamedTags.RED_GH.getLocation(); + break; + case I, J: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_IJ.getLocation() : NamedTags.RED_IJ.getLocation(); + break; + case K, L: + default: + correspondingAprilTagPose = Robot.isBlue() ? NamedTags.BLUE_KL.getLocation() : NamedTags.RED_KL.getLocation(); + break; + } + + return correspondingAprilTagPose.toPose2d().transformBy( + new Transform2d( + Constants.LENGTH_WITH_BUMPERS_METERS/2 + Settings.CLEARANCE_DISTANCE_FROM_REEF + 0.1, // The 0.1 is a buffer to ensure the robot gets past the clearance + CENTER_OF_TROUGH_TO_BRANCH * (this.isLeftPeg() ? -1 : 1) + Constants.SHOOTER_Y_OFFSET * (isScoringFrontSide ? 1 : -1), + isScoringFrontSide ? Rotation2d.k180deg : Rotation2d.kZero)); + } + public boolean isLeftPeg() { return switch (this) { case A, C, E, G, I, K -> true; @@ -209,7 +241,7 @@ public static CoralBranch getClosestBranch() { for (CoralBranch branch : CoralBranch.values()) { // just pick a random level for the branch (in this case 2) since it doesnt really matter - double distance = CommandSwerveDrivetrain.getInstance().getPose().minus(branch.getTargetPose(2, true)).getTranslation().getNorm(); + double distance = CommandSwerveDrivetrain.getInstance().getPose().minus(branch.getScorePose(2, true)).getTranslation().getNorm(); if (distance < closestDistance) { closestDistance = distance; nearestBranch = branch; @@ -255,7 +287,7 @@ public Pose2d getTargetPose() { break; } return correspondingAprilTag.toPose2d() - .transformBy(new Transform2d(Constants.LENGTH_WITH_BUMPERS_METERS / 2, 0, Rotation2d.fromDegrees(180))); + .transformBy(new Transform2d(Constants.LENGTH_WITH_BUMPERS_METERS / 2 + (isHighAlgae() ? Settings.Swerve.Alignment.Targets.TARGET_DISTANCE_FROM_ALGAE_L3 : Settings.Swerve.Alignment.Targets.TARGET_DISTANCE_FROM_ALGAE_L2), 0, Rotation2d.fromDegrees(180))); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index cfeb174..859dd23 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -12,8 +12,8 @@ public interface Gains { public interface Swerve { public interface Alignment { - PIDConstants XY = new PIDConstants(1.0, 0, 0.0); - PIDConstants THETA = new PIDConstants(12.988, 0, 0.77717); + PIDConstants XY = new PIDConstants(3.0, 0, 0.0); + PIDConstants THETA = new PIDConstants(3.0, 0, 0.1); } } @@ -33,17 +33,49 @@ public interface FF { } public interface Arm { - public interface PID { - double kP = 9.2822; - double kI = 0.0; - double kD = 1.1381; + public interface Coral { + public interface PID { + double kP = 0.049744; + double kI = 0.0; + double kD = 0.0; + } + + public interface FF { + double kS = 0.076049; + double kV = 0.011872; + double kA = 0.0011738; + double kG = 0.69106; + } } - public interface FF { - double kS = 0.084998; - double kV = 2.095; - double kA = 0.59073; - double kG = 0.90574; + public interface Empty { + public interface PID { + double kP = 0.022194; + double kI = 0.0; + double kD = 0.00021311; + } + + public interface FF { + double kS = 0.041733; + double kV = 0.011199; + double kA = 0.00123655; + double kG = 0.60473; + } + } + + public interface Algae { + public interface PID { + double kP = 0.014094; + double kI = 0.0; + double kD = 0.0; + } + + public interface FF { + double kS = 0.078914; + double kV = 0.011835; + double kA = 0.0011797; + double kG = 0.69059; + } } } @@ -58,7 +90,8 @@ public interface FF { double kS = 0.0; double kV = 0.0; double kA = 0.0; - double kG = 0.0; + double CORAL_kG = 0.0; + double ALGAE_kG = 0.0; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index dc338b2..69b13e2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -55,12 +55,8 @@ public interface Arm { .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Arm.PID.kP, Gains.Arm.PID.kI, Gains.Arm.PID.kD, 0) - .withFFConstants(Gains.Arm.FF.kS, Gains.Arm.FF.kV, Gains.Arm.FF.kA, Gains.Arm.FF.kG, 0) - .withGravityType(GravityTypeValue.Arm_Cosine) .withSensorToMechanismRatio(Constants.Arm.GEAR_RATIO); // .withRemoteSensor(Ports.Arm.ABSOLUTE_ENCODER, FeedbackSensorSourceValue.FusedCANcoder, Constants.Arm.GEAR_RATIO) - // .withMotionProfile(Settings.Arm.MAX_VEL.getDegrees(), Settings.Arm.MAX_ACCEL.getDegrees()); } public interface Froggy { @@ -76,11 +72,13 @@ public interface Froggy { .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Froggy.PID.kP, Gains.Froggy.PID.kI, Gains.Froggy.PID.kD, 0) - .withFFConstants(Gains.Froggy.FF.kS, Gains.Froggy.FF.kV, Gains.Froggy.FF.kA, Gains.Froggy.FF.kG, 0) + .withFFConstants(Gains.Froggy.FF.kS, Gains.Froggy.FF.kV, Gains.Froggy.FF.kA, Gains.Froggy.FF.CORAL_kG, 0) + .withPIDConstants(Gains.Froggy.PID.kP, Gains.Froggy.PID.kI, Gains.Froggy.PID.kD, 1) + .withFFConstants(Gains.Froggy.FF.kS, Gains.Froggy.FF.kV, Gains.Froggy.FF.kA, Gains.Froggy.FF.ALGAE_kG, 1) .withGravityType(GravityTypeValue.Arm_Cosine) .withSensorToMechanismRatio(Constants.Froggy.GEAR_RATIO) .withRemoteSensor(Ports.Froggy.ABSOLUTE_ENCODER, FeedbackSensorSourceValue.FusedCANcoder, Constants.Froggy.GEAR_RATIO) - .withMotionProfile(Settings.Froggy.MAX_VEL_ROTATIONS_PER_S, Settings.Froggy.MAX_ACCEL_ROTATIONS_PER_S_PER_S); + .withMotionProfile(Settings.Froggy.MAX_VEL.getRotations(), Settings.Froggy.MAX_ACCEL.getRotations()); } public interface Elevator { diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 926a4c7..fe07828 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -22,12 +22,12 @@ public interface Froggy { public interface Climb { int MOTOR = 20; - // int ABSOLUTE_ENCODER = 0; + int ABSOLUTE_ENCODER = 3; } public interface Arm { int MOTOR = 32; - int ABSOLUTE_ENCODER = 5; + int ABSOLUTE_ENCODER = 8; } public interface Shooter { @@ -42,7 +42,7 @@ public interface Funnel { public interface Elevator { int MOTOR = 31; - int BOTTOM_SWITCH = 8; + int BOTTOM_SWITCH = 0; } public interface LED { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 9b5b861..3f5c723 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -31,30 +31,31 @@ public interface Settings { String CANIVORE_NAME = "CANIVORE"; - double CLEARANCE_DISTANCE_FROM_REEF = 0.4; + double CLEARANCE_DISTANCE_FROM_REEF = 0.75; // From bumper double CLEARANCE_DISTANCE_FROM_CENTERLINE_FOR_BARGE = Settings.Swerve.Alignment.Targets.TARGET_DISTANCE_FROM_CENTERLINE_FOR_BARGE + 0.4; public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); SmartBoolean ARM = new SmartBoolean("Enabled Subsystems/Arm Is Enabled", true); - SmartBoolean ELEVATOR = new SmartBoolean("Enabled Subsystems/Elevator Is Enabled", false); - SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", false); - SmartBoolean FUNNEL = new SmartBoolean("Enabled Subsystems/Funnel Is Enabled", false); - SmartBoolean CLIMB = new SmartBoolean("Enabled Subsystems/Climb Is Enabled", false); + SmartBoolean ELEVATOR = new SmartBoolean("Enabled Subsystems/Elevator Is Enabled", true); + SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); + SmartBoolean FUNNEL = new SmartBoolean("Enabled Subsystems/Funnel Is Enabled", true); + SmartBoolean CLIMB = new SmartBoolean("Enabled Subsystems/Climb Is Enabled", true); SmartBoolean FROGGY = new SmartBoolean("Enabled Subsystems/Froggy Is Enabled", false); - SmartBoolean VISION = new SmartBoolean("Enabled Subsystems/Vision Is Enabled", false); + SmartBoolean VISION = new SmartBoolean("Enabled Subsystems/Vision Is Enabled", true); } public interface Swerve { - double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.03; - + double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; + double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; + double NUDGE_FORWARD_SPEED_METERS_PER_SECOND = 0.15; public interface Constraints { double MAX_MODULE_SPEED = 4.9; - SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity (m per s)", 3.0); - SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration (m per s^2)", 5.0); - SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity (rad per s)", Units.degreesToRadians(360)); - SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration (rad per s^2)", Units.degreesToRadians(720)); + SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity (m per s)", 4.0); + SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration (m per s^2)", 15.0); + SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity (rad per s)", Units.degreesToRadians(400)); + SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration (rad per s^2)", Units.degreesToRadians(900)); PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( @@ -66,18 +67,17 @@ public interface Constraints { public interface Alignment { public interface Constraints { + SmartNumber MAX_VELOCITY = new SmartNumber("Alignment/Constraints/Max Velocity (m per s)", 4.0); + SmartNumber MAX_ACCELERATION = new SmartNumber("Alignment/Constraints/Max Acceleration (m per s^2)", 12.0); - SmartNumber MAX_VELOCITY = new SmartNumber("Alignment/Constraints/Max Velocity (m per s)", 3.0); - SmartNumber MAX_ACCELERATION = new SmartNumber("Alignment/Constraints/Max Acceleration (m per s^2)", 5.0); - - SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Alignment/Constraints/Max Angular Velocity (rad per s)", Units.degreesToRadians(360)); - SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Alignment/Constraints/Max Angular Acceleration (rad per s^2)", Units.degreesToRadians(720)); + SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Alignment/Constraints/Max Angular Velocity (rad per s)", Units.degreesToRadians(400)); + SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Alignment/Constraints/Max Angular Acceleration (rad per s^2)", Units.degreesToRadians(900)); } public interface Tolerances { - SmartNumber X_TOLERANCE = new SmartNumber("Alignment/Tolerances/X Tolerance (m)", 0.02); - SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Tolerances/Y Tolerance (m)", 0.02); - SmartNumber THETA_TOLERANCE = new SmartNumber("Alignment/Tolerances/Theta Tolerance (rad)", Units.degreesToRadians(4)); + SmartNumber X_TOLERANCE = new SmartNumber("Alignment/Tolerances/X Tolerance (m)", 0.05); + SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Tolerances/Y Tolerance (m)", 0.05); + SmartNumber THETA_TOLERANCE = new SmartNumber("Alignment/Tolerances/Theta Tolerance (rad)", Units.degreesToRadians(5)); SmartNumber MAX_VELOCITY_WHEN_ALIGNED = new SmartNumber("Alignment/Tolerances/Max Velocity When Aligned", 0.15); @@ -85,11 +85,13 @@ public interface Tolerances { } public interface Targets { + // DISTANCE FROM REEF TO BUMPER + double TARGET_DISTANCE_FROM_REEF_L2_FRONT = -0.02; + double TARGET_DISTANCE_FROM_REEF_L3_FRONT = -0.02; + double TARGET_DISTANCE_FROM_REEF_L4_FRONT = -0.02; + double TARGET_DISTANCE_FROM_REEF_L2_BACK = 0.0; - double TARGET_DISTANCE_FROM_REEF_L2_FRONT = 0.0; double TARGET_DISTANCE_FROM_REEF_L3_BACK = 0.0; - double TARGET_DISTANCE_FROM_REEF_L3_FRONT = 0.0; - double TARGET_DISTANCE_FROM_REEF_L4_FRONT = 0.0; double TARGET_DISTANCE_FROM_REEF_L4_BACK = 0.0; double TARGET_DISTANCE_FROM_ALGAE_L2 = 0.0; @@ -105,21 +107,23 @@ public interface Vision { } public interface Shooter { - SmartNumber CORAL_SHOOT_SPEED_FORWARD = new SmartNumber("Coral Shoot Speed Forward", 0.75); - SmartNumber CORAL_SHOOT_SPEED_REVERSE = new SmartNumber("Coral Shoot Speed Reverse", -0.75); - SmartNumber CORAL_ACQUIRE_SPEED = new SmartNumber("Coral Acquire Speed", 0.3); - SmartNumber ALGAE_ACQUIRE_SPEED = new SmartNumber("Algae Acquire Speed", -0.45); - SmartNumber ALGAE_SHOOT_SPEED = new SmartNumber("Algae Shoot Speed", 0.45); + SmartNumber CORAL_SHOOT_SPEED_FORWARD = new SmartNumber("Shooter/Target Speeds/Coral Shoot Speed Forward", 0.75); + SmartNumber CORAL_SHOOT_SPEED_REVERSE = new SmartNumber("Shooter/Target Speeds/Coral Shoot Speed Reverse", -0.75); + SmartNumber CORAL_ACQUIRE_SPEED = new SmartNumber("Shooter/Target Speeds/Coral Acquire Speed", 0.15); + SmartNumber ALGAE_ACQUIRE_SPEED = new SmartNumber("Shooter/Target Speeds/Algae Acquire Speed", -1.0); + SmartNumber ALGAE_SHOOT_SPEED = new SmartNumber("Shooter/Target Speeds/Algae Shoot Speed", 1.0); - double HAS_CORAL_DEBOUNCE = 0.2; + SmartNumber ALGAE_HOLD_SPEED = new SmartNumber("Shooter/Target Speeds/Algae Hol Speed", -0.1); + + double HAS_CORAL_DEBOUNCE = 0.0; double STALL_DETECTION_DEBOUNCE = 0.5; - double STALL_CURRENT_THRESHOLD = 30; + double STALL_CURRENT_THRESHOLD = 80; } public interface Funnel { - SmartNumber FORWARD_SPEED = new SmartNumber("Funnel/Forward Speed", 0.4); - SmartNumber REVERSE_SPEED = new SmartNumber("Funnel/Reverse Speed", -0.4); + SmartNumber FORWARD_SPEED = new SmartNumber("Funnel/Forward Speed", 1.0); + SmartNumber REVERSE_SPEED = new SmartNumber("Funnel/Reverse Speed", -1.0); double STALL_CURRENT = 30; double STALL_DETECTION_TIME = 0.25; @@ -130,32 +134,38 @@ public interface Funnel { public interface Elevator { - double MAX_VELOCITY_METERS_PER_SECOND = 2.0; + double MAX_VELOCITY_METERS_PER_SECOND = 1.5; double MAX_ACCEL_METERS_PER_SECOND_PER_SECOND = 2.0; - double FEED_HEIGHT_METERS = 1.0; + double FEED_HEIGHT_METERS = 1.13; // Coral - double FRONT_L2_HEIGHT_METERS = 1.609; - double FRONT_L3_HEIGHT_METERS = 1.073; - double FRONT_L4_HEIGHT_METERS = 1.708; + double FRONT_L2_HEIGHT_METERS = 1.592002; + double FRONT_L3_HEIGHT_METERS = 1.037354; + double FRONT_L4_HEIGHT_METERS = 1.656494; double BACK_L2_HEIGHT_METERS = 1.106; double BACK_L3_HEIGHT_METERS = 1.161; double BACK_L4_HEIGHT_METERS = 1.836; // Algae - double BARGE_HEIGHT_METERS = 2.073; + double BARGE_HEIGHT_METERS = Constants.Elevator.MAX_HEIGHT_METERS; double ALGAE_L2_HEIGHT_METERS = 1.4; double ALGAE_L3_HEIGHT_METERS = 1.8; + double HOLD_ALGAE_HEIGHT_METERS = 1.2; + + double CLIMB_HEIGHT_METERS = Constants.Elevator.MIN_HEIGHT_METERS + 0.1; double HEIGHT_TOLERANCE_METERS = 0.04; } public interface Arm { - Rotation2d L2_ANGLE_FRONT = Rotation2d.fromDegrees(-83.385); - Rotation2d L3_ANGLE_FRONT = Rotation2d.fromDegrees(60.6); - Rotation2d L4_ANGLE_FRONT = Rotation2d.fromDegrees(75.0); + Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(-82); // Angle that arm makes when resting against the funnel + Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(201); + + Rotation2d L2_ANGLE_FRONT = Rotation2d.fromDegrees(-69.345703); + Rotation2d L3_ANGLE_FRONT = Rotation2d.fromDegrees(65.302734); + Rotation2d L4_ANGLE_FRONT = Rotation2d.fromDegrees(55.361328); Rotation2d L2_ANGLE_BACK = Rotation2d.fromDegrees(171.5); Rotation2d L3_ANGLE_BACK = Rotation2d.fromDegrees(146.1); @@ -163,51 +173,52 @@ public interface Arm { Rotation2d ALGAE_L2_ANGLE = Rotation2d.fromDegrees(7); Rotation2d ALGAE_L3_ANGLE = Rotation2d.fromDegrees(9); - Rotation2d BARGE_ANGLE = Rotation2d.fromDegrees(60.0); + Rotation2d BARGE_ANGLE = Rotation2d.fromDegrees(66.0); - Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(-90); - Rotation2d FEED_ANGLE = Rotation2d.fromDegrees(-93); + Rotation2d FEED_ANGLE = MIN_ANGLE.plus(Rotation2d.fromDegrees(0)); + Rotation2d HOLD_ALGAE = MIN_ANGLE.plus(Rotation2d.fromDegrees(4)); + + Rotation2d CLIMB_ANGLE = MAX_ANGLE.minus(Rotation2d.fromDegrees(5)); - Rotation2d MAX_VEL = Rotation2d.fromDegrees(150.0); - Rotation2d MAX_ACCEL = Rotation2d.fromDegrees(200.0); + Rotation2d MAX_VEL = Rotation2d.fromDegrees(200.0); + Rotation2d MAX_ACCEL = Rotation2d.fromDegrees(400.0); Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); } public interface Froggy { - Rotation2d STOW_ANGLE = Rotation2d.fromDegrees(0); - Rotation2d ALGAE_GROUND_PICKUP_ANGLE = Rotation2d.fromDegrees(0); - Rotation2d CORAL_GROUND_PICKUP_ANGLE = Rotation2d.fromDegrees(0); + Rotation2d STOW_ANGLE = Constants.Froggy.MAXIMUM_ANGLE; + Rotation2d ALGAE_GROUND_PICKUP_ANGLE = Rotation2d.fromDegrees(3.5); + Rotation2d CORAL_GROUND_PICKUP_ANGLE = Constants.Froggy.MINIMUM_ANGLE; Rotation2d GOLF_TEE_ALGAE_PICKUP_ANGLE = Rotation2d.fromDegrees(0); - Rotation2d L1_SCORING_ANGLE = Rotation2d.fromDegrees(0); + Rotation2d L1_SCORING_ANGLE = Rotation2d.fromDegrees(41); Rotation2d PROCESSOR_SCORE_ANGLE = Rotation2d.fromDegrees(0); Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1.0); - double ALGAE_INTAKE_SPEED = 0.5; - double ALGAE_OUTTAKE_SPEED = 0.5; - - double CORAL_INTAKE_SPEED = 0.5; - double CORAL_OUTTAKE_SPEED = 0.5; - - double HOLD_ALGAE_SPEED = 0.0; + SmartNumber ALGAE_INTAKE_SPEED = new SmartNumber("Froggy/Roller/Target Speeds/Algae Intake Speed", 1.0); + SmartNumber ALGAE_OUTTAKE_SPEED = new SmartNumber("Froggy/Roller/Target Speeds/Algae Outtake Speed", -1.0); + SmartNumber CORAL_INTAKE_SPEED = new SmartNumber("Froggy/Roller/Target Speeds/Coral Intake Speed", -1.0); + SmartNumber CORAL_OUTTAKE_SPEED = new SmartNumber("Froggy/Roller/Target Speeds/Coral Outtake Speed", 1.0); + SmartNumber HOLD_ALGAE_SPEED = new SmartNumber("Froggy/Roller/Target Speeds/Hold Algae Speed", 0.0); - double CORAL_STALL_CURRENT_THRESHOLD = 20.0; - double ALGAE_STALL_CURRENT_THRESHOLD = 20.0; + double CORAL_STALL_CURRENT_THRESHOLD = 80.0; + double ALGAE_STALL_CURRENT_THRESHOLD = 80.0; double STALL_DEBOUNCE_TIME = 0.0; - double MAX_VEL_ROTATIONS_PER_S = 1.0; - double MAX_ACCEL_ROTATIONS_PER_S_PER_S = 1.0; + Rotation2d MAX_VEL = Rotation2d.fromDegrees(100); + Rotation2d MAX_ACCEL = Rotation2d.fromDegrees(100); } public interface Climb { - double RESET_STALL_CURRENT = 40; - double RESET_VOLTAGE = -1.0; + double DEFAULT_VOLTAGE = 4; // Used for normal movement + double OPEN_VOLTAGE_LOW = 1; // Used when getting close to the open angle + double CLIMB_VOLTAGE = 12; // Used when climbing - Rotation2d CLOSED_ANGLE = Rotation2d.fromDegrees(162.0); - Rotation2d OPEN_ANGLE = Rotation2d.fromDegrees(0.0); - Rotation2d CLIMBED_ANGLE = Rotation2d.fromDegrees(0.0); + Rotation2d OPEN_ANGLE = Rotation2d.fromDegrees(3.0); + Rotation2d CLOSED_ANGLE = Rotation2d.fromDegrees(165); + Rotation2d CLIMBED_ANGLE = Rotation2d.fromDegrees(245); - Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(2.0); + Rotation2d ANGLE_TOLERANCE_FOR_CLOSED = Rotation2d.fromDegrees(8); } public interface LED { @@ -233,7 +244,7 @@ public interface Driver { double BUZZ_INTENSITY = 1.0; public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.05); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.08); SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); @@ -243,7 +254,7 @@ public interface Drive { } public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.08); SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); @@ -283,6 +294,6 @@ public interface Arm { } public interface Auton { - double SHOOTER_WAIT_TIME = 0.2; + double SHOOTER_WAIT_TIME = 0.75; } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index c4c2bef..99bf937 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -12,6 +12,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.ArmElevatorVisualizer; import com.stuypulse.stuylib.math.SLMath; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,7 +23,7 @@ public abstract class Arm extends SubsystemBase { - public static final Arm instance; + private static final Arm instance; static { if (Robot.isReal()) { @@ -38,7 +39,6 @@ public static Arm getInstance() { } public enum ArmState { - STOW(Settings.Arm.STOW_ANGLE), FEED(Settings.Arm.FEED_ANGLE), L2_FRONT(Settings.Arm.L2_ANGLE_FRONT), L2_BACK(Settings.Arm.L2_ANGLE_BACK), @@ -48,13 +48,14 @@ public enum ArmState { L4_BACK(Settings.Arm.L4_ANGLE_BACK), ALGAE_L2(Settings.Arm.ALGAE_L2_ANGLE), ALGAE_L3(Settings.Arm.ALGAE_L3_ANGLE), + HOLD_ALGAE(Settings.Arm.HOLD_ALGAE), BARGE(Settings.Arm.BARGE_ANGLE), - VERTICAL_DOWN(Rotation2d.fromDegrees(-90)); + CLIMB(Settings.Arm.CLIMB_ANGLE); private Rotation2d targetAngle; private ArmState(Rotation2d targetAngle) { - this.targetAngle = Rotation2d.fromDegrees(SLMath.clamp(targetAngle.getDegrees(), Constants.Arm.MIN_ANGLE.getDegrees(), Constants.Arm.MAX_ANGLE.getDegrees())); + this.targetAngle = Rotation2d.fromDegrees(SLMath.clamp(targetAngle.getDegrees(), Settings.Arm.MIN_ANGLE.getDegrees(), Settings.Arm.MAX_ANGLE.getDegrees())); } public Rotation2d getTargetAngle() { @@ -69,7 +70,7 @@ public boolean isFrontSide() { private ArmState state; protected Arm() { - this.state = ArmState.STOW; + this.state = ArmState.FEED; } public ArmState getState() { @@ -86,14 +87,17 @@ public void setState(ArmState state) { public abstract boolean atTargetAngle(); public abstract void setVoltageOverride(Optional voltage); + public abstract double getVoltageOverride(); + public abstract void setOperatorOffset(Rotation2d offset); public abstract Rotation2d getOperatorOffset(); - public abstract Command getSysIdQuasistatic(SysIdRoutine.Direction direction); - public abstract Command getSysIdDynamic(SysIdRoutine.Direction direction); + public abstract SysIdRoutine getSysIdRoutine(); @Override public void periodic() { + ArmElevatorVisualizer.getInstance().updateArmAngle(getCurrentAngle()); + SmartDashboard.putString("Arm/State", getState().toString()); SmartDashboard.putBoolean("Arm/At Target Angle", atTargetAngle()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index f17be3d..340947d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -11,95 +11,85 @@ import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.elevator.Elevator; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.ArmDriveFeedForward; +import com.stuypulse.robot.util.ArmElevatorFeedForward; +import com.stuypulse.robot.util.SettableNumber; +import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.control.feedforward.ArmFeedforward; import com.stuypulse.stuylib.control.feedforward.MotorFeedforward; -import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.Optional; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.hardware.TalonFX; public class ArmImpl extends Arm { private TalonFX motor; - // private CANcoder absoluteEncoder; private DutyCycleEncoder absoluteEncoder; private Controller controller; private Optional voltageOverride; private Rotation2d operatorOffset; - - private SysIdRoutine sysidRoutine; - private boolean isRunningSysid; + private SettableNumber kP, kI, kD, kS, kV, kA, kG; public ArmImpl() { super(); motor = new TalonFX(Ports.Arm.MOTOR); Motors.Arm.MOTOR_CONFIG.configure(motor); - motor.setPosition(Constants.Arm.MIN_ANGLE.getRotations()); + motor.setPosition(Settings.Arm.MIN_ANGLE.getRotations()); - // absoluteEncoder = new CANcoder(Ports.Arm.ABSOLUTE_ENCODER); absoluteEncoder = new DutyCycleEncoder(Ports.Arm.ABSOLUTE_ENCODER); absoluteEncoder.setInverted(true); - // MagnetSensorConfigs magnetConfig = new MagnetSensorConfigs() - // .withMagnetOffset(Constants.Arm.ANGLE_OFFSET) - // .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive); - - // absoluteEncoder.getConfigurator().apply(magnetConfig); - - MotionProfile motionProfile = new MotionProfile(Settings.Arm.MAX_VEL.getRotations(), Settings.Arm.MAX_ACCEL.getRotations()); - motionProfile.reset(Settings.Arm.STOW_ANGLE.getRotations()); - - controller = new MotorFeedforward(Gains.Arm.FF.kS, Gains.Arm.FF.kV, Gains.Arm.FF.kA).position() - .add(new ArmFeedforward(Gains.Arm.FF.kG)) - .add(new PIDController(Gains.Arm.PID.kP, Gains.Arm.PID.kI, Gains.Arm.PID.kD)) + MotionProfile motionProfile = new MotionProfile(Settings.Arm.MAX_VEL.getDegrees(), Settings.Arm.MAX_ACCEL.getDegrees()); + motionProfile.reset(Settings.Arm.MIN_ANGLE.getDegrees()); + + kP = new SettableNumber(Gains.Arm.Empty.PID.kP); + kI = new SettableNumber(Gains.Arm.Empty.PID.kI); + kD = new SettableNumber(Gains.Arm.Empty.PID.kD); + kS = new SettableNumber(Gains.Arm.Empty.FF.kS); + kV = new SettableNumber(Gains.Arm.Empty.FF.kV); + kA = new SettableNumber(Gains.Arm.Empty.FF.kA); + kG = new SettableNumber(Gains.Arm.Empty.FF.kG); + + controller = new MotorFeedforward(kS, kV, kA).position() + .add(new ArmFeedforward(kG)) + .add(new ArmDriveFeedForward(kG, CommandSwerveDrivetrain.getInstance()::getRobotRelativeXAccelGs)) + .add(new PIDController(kP, kI, kD)) .setSetpointFilter(motionProfile); voltageOverride = Optional.empty(); operatorOffset = Rotation2d.kZero; - - isRunningSysid = false; - - sysidRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - null, - edu.wpi.first.units.Units.Volts.of(5), - null, - state -> SignalLogger.writeString("SysIdArm_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - motor.setVoltage(output.in(edu.wpi.first.units.Units.Volts)); - isRunningSysid = true; - }, - state -> { - SignalLogger.writeDouble("Arm Position (rotations)", getCurrentAngle().getRotations()); - SignalLogger.writeDouble("Arm Velocity (rotations per s)", motor.getVelocity().getValueAsDouble()); - SignalLogger.writeDouble("Arm Voltage", motor.getMotorVoltage().getValueAsDouble()); - }, - this)); } @Override - public Command getSysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysidRoutine.quasistatic(direction); - } - - @Override - public Command getSysIdDynamic(SysIdRoutine.Direction direction) { - return sysidRoutine.dynamic(direction); + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Arm", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> getCurrentAngle().getDegrees(), + () -> Units.rotationsToDegrees(motor.getVelocity().getValueAsDouble()), + () -> motor.getMotorVoltage().getValueAsDouble(), + getInstance() + ); } @Override @@ -108,17 +98,16 @@ public boolean atTargetAngle() { } private Rotation2d getTargetAngle() { - return getState().getTargetAngle().plus(operatorOffset); + return Rotation2d.fromDegrees( + SLMath.clamp(getState().getTargetAngle().plus(operatorOffset).getDegrees(), Settings.Arm.MIN_ANGLE.getDegrees(), Settings.Arm.MAX_ANGLE.getDegrees())); } @Override public Rotation2d getCurrentAngle() { - // double degrees = Units.rotationsToDegrees(absoluteEncoder.get() - Constants.Arm.ANGLE_OFFSET.getRotations()); - // if (degrees < Constants.Arm.MIN_ANGLE.minus(Rotation2d.fromDegrees(5)).getDegrees()) { - // degrees += 360; - // } - // return Rotation2d.fromDegrees(degrees); - return Rotation2d.fromRotations(motor.getPosition().getValueAsDouble()); + double encoderAngle = absoluteEncoder.get() - Constants.Arm.ANGLE_OFFSET.getRotations(); + return Rotation2d.fromRotations(encoderAngle > Settings.Arm.MIN_ANGLE.minus(Rotation2d.fromDegrees(15)).getRotations() + ? encoderAngle + : encoderAngle + 1); } @Override @@ -126,6 +115,16 @@ public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; } + @Override + public double getVoltageOverride() { + if (voltageOverride.isPresent()) { + return voltageOverride.get(); + } + else { + return 0; + } + } + @Override public void setOperatorOffset(Rotation2d offset) { this.operatorOffset = offset; @@ -136,24 +135,65 @@ public Rotation2d getOperatorOffset() { return this.operatorOffset; } + private void updateGains() { + if (getState() == ArmState.HOLD_ALGAE || getState() == ArmState.BARGE) { + kP.set(Gains.Arm.Algae.PID.kP); + kI.set(Gains.Arm.Algae.PID.kI); + kD.set(Gains.Arm.Algae.PID.kD); + kS.set(Gains.Arm.Algae.FF.kS); + kV.set(Gains.Arm.Algae.FF.kV); + kA.set(Gains.Arm.Algae.FF.kA); + kG.set(Gains.Arm.Algae.FF.kG); + } else if (Shooter.getInstance().hasCoral()) { + kP.set(Gains.Arm.Coral.PID.kP); + kI.set(Gains.Arm.Coral.PID.kI); + kD.set(Gains.Arm.Coral.PID.kD); + kS.set(Gains.Arm.Coral.FF.kS); + kV.set(Gains.Arm.Coral.FF.kV); + kA.set(Gains.Arm.Coral.FF.kA); + kG.set(Gains.Arm.Coral.FF.kG); + } else { + kP.set(Gains.Arm.Empty.PID.kP); + kI.set(Gains.Arm.Empty.PID.kI); + kD.set(Gains.Arm.Empty.PID.kD); + kS.set(Gains.Arm.Empty.FF.kS); + kV.set(Gains.Arm.Empty.FF.kV); + kA.set(Gains.Arm.Empty.FF.kA); + kG.set(Gains.Arm.Empty.FF.kG); + } + } + @Override public void periodic() { super.periodic(); - if (Settings.EnabledSubsystems.ARM.get() && !isRunningSysid) { + updateGains(); + + SmartDashboard.putNumber("Arm/kG", kG.get()); + + if (Settings.EnabledSubsystems.ARM.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - // motor.setControl(new MotionMagicVoltage(getTargetAngle().getRotations())); - motor.setVoltage(controller.update(getTargetAngle().getRotations(), getCurrentAngle().getRotations())); + motor.setVoltage(controller.update(getTargetAngle().getDegrees(), getCurrentAngle().getDegrees())); } } else { motor.setVoltage(0); } - SmartDashboard.putNumber("Climb/Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Climb/Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putBoolean("Arm/Is Voltage Override Present", voltageOverride.isPresent()); + SmartDashboard.putNumber("Arm/Voltage Override", getVoltageOverride()); + + SmartDashboard.putNumber("Arm/Setpoint (deg)", controller.getSetpoint()); + SmartDashboard.putNumber("Arm/Angle Error (deg)", controller.getError()); + + SmartDashboard.putBoolean("Arm/Absolute Encoder is Connected", absoluteEncoder.isConnected()); + SmartDashboard.putNumber("Arm/Absolute Encoder Value raw (deg)", Units.rotationsToDegrees(absoluteEncoder.get())); + + SmartDashboard.putNumber("Arm/Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Arm/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Arm/Stator Current", motor.getStatorCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmSim.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmSim.java index b7d2611..5483704 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmSim.java @@ -8,11 +8,14 @@ import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; +import static edu.wpi.first.units.Units.Second; + import java.util.Optional; import com.ctre.phoenix6.SignalLogger; import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -40,19 +43,16 @@ public class ArmSim extends Arm { private Optional voltageOverride; private Rotation2d operatorOffset; - private SysIdRoutine sysidRoutine; - private boolean isRunningSysid; - protected ArmSim() { sim = new SingleJointedArmSim( DCMotor.getKrakenX60(1), Constants.Arm.GEAR_RATIO, Constants.Arm.MOMENT_OF_INERTIA, Constants.Arm.ARM_LENGTH, - Constants.Arm.MIN_ANGLE.getRadians(), - Constants.Arm.MAX_ANGLE.getRadians(), + Settings.Arm.MIN_ANGLE.getRadians(), + Settings.Arm.MAX_ANGLE.getRadians(), false, - Settings.Arm.STOW_ANGLE.getRadians() + Settings.Arm.MIN_ANGLE.getRadians() ); LinearSystem armSystem = LinearSystemId.createSingleJointedArmSystem( @@ -80,40 +80,24 @@ protected ArmSim() { Settings.Arm.MAX_VEL.getRadians(), Settings.Arm.MAX_ACCEL.getRadians() ); - motionProfile.reset(Settings.Arm.STOW_ANGLE.getRadians()); + motionProfile.reset(Settings.Arm.MIN_ANGLE.getRadians()); voltageOverride = Optional.empty(); operatorOffset = Rotation2d.kZero; - - isRunningSysid = false; - - sysidRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - null, - edu.wpi.first.units.Units.Volts.of(5), - null, - state -> SignalLogger.writeString("SysIdArm_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - sim.setInputVoltage(output.in(edu.wpi.first.units.Units.Volts)); - isRunningSysid = true; - }, - state -> { - SignalLogger.writeDouble("Arm Position (rad)", getCurrentAngle().getRadians()); - SignalLogger.writeDouble("Arm Velocity (rad per s)", sim.getVelocityRadPerSec()); - SignalLogger.writeDouble("Arm Voltage", controller.getU(0)); - }, - this)); } @Override - public Command getSysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysidRoutine.quasistatic(direction); - } - - @Override - public Command getSysIdDynamic(SysIdRoutine.Direction direction) { - return sysidRoutine.dynamic(direction); + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 3, + 7, + "Arm", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> getCurrentAngle().getDegrees(), + () -> Units.radiansToDegrees(sim.getVelocityRadPerSec()), + () -> voltageOverride.get(), + getInstance() + ); } @Override @@ -135,6 +119,16 @@ public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; } + @Override + public double getVoltageOverride() { + if (voltageOverride.isPresent()) { + return voltageOverride.get(); + } + else { + return 0; + } + } + @Override public void setOperatorOffset(Rotation2d offset) { this.operatorOffset = offset; @@ -157,21 +151,18 @@ public void periodic() { controller.correct(VecBuilder.fill(sim.getAngleRads(), sim.getVelocityRadPerSec())); controller.predict(Settings.DT); - if (Settings.EnabledSubsystems.ARM.get() && !isRunningSysid) { + if (Settings.EnabledSubsystems.ARM.get()) { if (voltageOverride.isPresent()) { sim.setInputVoltage(voltageOverride.get()); - sim.update(Settings.DT); } else { sim.setInputVoltage(controller.getU(0)); - sim.update(Settings.DT); } } else { sim.setInputVoltage(0); } - SmartDashboard.putNumber("Arm/Current Angle (deg)", getCurrentAngle().getDegrees()); - SmartDashboard.putNumber("Arm/Target Angle (deg)", getTargetAngle().getDegrees()); + sim.update(Settings.DT); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/climb/Climb.java b/src/main/java/com/stuypulse/robot/subsystems/climb/Climb.java index d1aa640..69b0bee 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climb/Climb.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climb/Climb.java @@ -8,10 +8,13 @@ import java.util.Optional; +import com.stuypulse.robot.constants.Constants; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.stuylib.math.SLMath; + +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public abstract class Climb extends SubsystemBase { private static final Climb instance; @@ -25,15 +28,27 @@ public static Climb getInstance() { } public enum ClimbState { - STOW, - OPEN, - CLIMBING, + CLOSED(Settings.Climb.CLOSED_ANGLE), + OPEN(Settings.Climb.OPEN_ANGLE), + CLIMBING(Settings.Climb.CLIMBED_ANGLE), + IDLE(Rotation2d.kZero); // Filler angle (wont be used) + + private Rotation2d targetAngle; + + private ClimbState(Rotation2d targetAngle) { + this.targetAngle = Rotation2d.fromDegrees( + SLMath.clamp(targetAngle.getDegrees(), Constants.Climb.MIN_ANGLE.getDegrees(), Constants.Climb.MAX_ANGLE.getDegrees())); + } + + public Rotation2d getTargetAngle() { + return this.targetAngle; + } } private ClimbState state; protected Climb() { - this.state = ClimbState.STOW; + this.state = ClimbState.CLOSED; } public ClimbState getState() { @@ -45,16 +60,10 @@ public void setState(ClimbState state) { setVoltageOverride(Optional.empty()); } - public abstract boolean atTargetAngle(); - public abstract void setVoltageOverride(Optional voltage); - public abstract Command getSysIdQuasistatic(SysIdRoutine.Direction direction); - public abstract Command getSysIdDynamic(SysIdRoutine.Direction direction); - @Override public void periodic() { SmartDashboard.putString("Climb/State", state.toString()); - SmartDashboard.putBoolean("Climb/At Target Angle", atTargetAngle()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/climb/ClimbImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climb/ClimbImpl.java index bdbeea4..1fed541 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climb/ClimbImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climb/ClimbImpl.java @@ -6,96 +6,45 @@ package com.stuypulse.robot.subsystems.climb; +import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.Units; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.Optional; -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; public class ClimbImpl extends Climb { private TalonFX motor; - // private CANcoder absoluteEncoder; + private DutyCycleEncoder absoluteEncoder; private Optional voltageOverride; - private boolean hasBeenReset; - - private SysIdRoutine sysidRoutine; - private boolean isRunningSysid; - protected ClimbImpl() { super(); motor = new TalonFX(Ports.Climb.MOTOR); Motors.Climb.MOTOR_CONFIG.configure(motor); motor.setPosition(Settings.Climb.OPEN_ANGLE.getRotations()); - // absoluteEncoder = new CANcoder(Ports.Climb.ABSOLUTE_ENCODER); - - // MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs() - // .withMagnetOffset(Constants.Climb.ANGLE_OFFSET.getRotations()) - // .withSensorDirection(SensorDirectionValue.Clockwise_Positive); - - // absoluteEncoder.getConfigurator().apply(magnetSensorConfigs); + absoluteEncoder = new DutyCycleEncoder(Ports.Climb.ABSOLUTE_ENCODER); + absoluteEncoder.setInverted(false); voltageOverride = Optional.empty(); - - hasBeenReset = false; - - isRunningSysid = false; - - sysidRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - null, - Units.Volts.of(6), - null), - new SysIdRoutine.Mechanism( - output -> { - motor.setVoltage(output.in(Units.Volts)); - isRunningSysid = true; - }, - state -> SignalLogger.writeString("SysIdElevator_State", state.toString()), - this)); - } - - @Override - public Command getSysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysidRoutine.quasistatic(direction); - } - - @Override - public Command getSysIdDynamic(SysIdRoutine.Direction direction) { - return sysidRoutine.dynamic(direction); } private Rotation2d getTargetAngle() { - switch (getState()) { - case STOW: - return Settings.Climb.CLOSED_ANGLE; - case OPEN: - return Settings.Climb.OPEN_ANGLE; - case CLIMBING: - return Settings.Climb.CLIMBED_ANGLE; - default: - return Settings.Climb.CLOSED_ANGLE; - } + return getState().getTargetAngle(); } private Rotation2d getCurrentAngle() { - return Rotation2d.fromRotations(motor.getPosition().getValueAsDouble()); - } - - @Override - public boolean atTargetAngle() { - return Math.abs(getTargetAngle().getDegrees() - getCurrentAngle().getDegrees()) < Settings.Climb.ANGLE_TOLERANCE.getDegrees(); + return absoluteEncoder.get() - Constants.Climb.ANGLE_OFFSET.getRotations() < Constants.Climb.MIN_ANGLE.minus(Rotation2d.fromDegrees(10)).getRotations() + ? Rotation2d.fromRotations(absoluteEncoder.get() - Constants.Climb.ANGLE_OFFSET.getRotations() + 1) + : Rotation2d.fromRotations(absoluteEncoder.get() - Constants.Climb.ANGLE_OFFSET.getRotations()); } @Override @@ -105,29 +54,66 @@ public void setVoltageOverride(Optional voltage) { @Override public void periodic() { - if (!hasBeenReset && !isRunningSysid) { - motor.setVoltage(Settings.Climb.RESET_VOLTAGE); - if (Math.abs(motor.getStatorCurrent().getValueAsDouble()) > Settings.Climb.RESET_STALL_CURRENT) { - hasBeenReset = true; - motor.setPosition(Settings.Climb.OPEN_ANGLE.getRotations()); - } - } - if (Settings.EnabledSubsystems.CLIMB.get() && !isRunningSysid) { + super.periodic(); + if (Settings.EnabledSubsystems.CLIMB.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(new PositionVoltage(getTargetAngle().getRotations())); + double angleErrorDegrees = getTargetAngle().getDegrees() - getCurrentAngle().getDegrees(); + SmartDashboard.putNumber("Climb/Angle Error (deg)", angleErrorDegrees); + + if (getState() == ClimbState.IDLE) { + motor.setVoltage(0); + } + else if (getState() == ClimbState.OPEN) { + if (angleErrorDegrees < 0) { + if (Math.abs(angleErrorDegrees) < 15) { + motor.setVoltage(-Settings.Climb.OPEN_VOLTAGE_LOW); + } + else { + motor.setVoltage(-Settings.Climb.DEFAULT_VOLTAGE); + } + } + else { + motor.setVoltage(0); + } + } + else if (getState() == ClimbState.CLOSED) { + if (Math.abs(angleErrorDegrees) > Settings.Climb.ANGLE_TOLERANCE_FOR_CLOSED.getDegrees()) { + if (getCurrentAngle().getDegrees() > Settings.Climb.CLOSED_ANGLE.getDegrees()) { + motor.setVoltage(-Settings.Climb.DEFAULT_VOLTAGE); + } + else { + motor.setVoltage(Settings.Climb.DEFAULT_VOLTAGE); + } + } + else { + motor.setVoltage(0); + } + } + else if (getState() == ClimbState.CLIMBING) { + if (getCurrentAngle().getDegrees() < Settings.Climb.CLIMBED_ANGLE.getDegrees()) { + motor.setVoltage(Settings.Climb.CLIMB_VOLTAGE); + } + else { + motor.setVoltage(0); + } + } } } else { motor.setVoltage(0); } + SmartDashboard.putNumber("Climb/Absolute Encoder angle raw (deg)", Units.rotationsToDegrees(absoluteEncoder.get())); + SmartDashboard.putNumber("Climb/Absolute Encoder angle (deg)", Units.rotationsToDegrees(absoluteEncoder.get() - Constants.Climb.ANGLE_OFFSET.getRotations())); + SmartDashboard.putNumber("Climb/Current Angle (deg)", getCurrentAngle().getDegrees()); SmartDashboard.putNumber("Climb/Target Angle (deg)", getTargetAngle().getDegrees()); SmartDashboard.putNumber("Climb/Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Climb/Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Climb/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Climb/Stator Current", motor.getStatorCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java index 97e3f76..a5f6310 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/Elevator.java @@ -11,6 +11,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.ArmElevatorVisualizer; import com.stuypulse.stuylib.math.SLMath; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -35,7 +36,6 @@ public static Elevator getInstance() { } public enum ElevatorState { - STOW(Constants.Elevator.MIN_HEIGHT_METERS), FEED(Settings.Elevator.FEED_HEIGHT_METERS), L2_FRONT(Settings.Elevator.FRONT_L2_HEIGHT_METERS), L3_FRONT(Settings.Elevator.FRONT_L3_HEIGHT_METERS), @@ -45,8 +45,10 @@ public enum ElevatorState { L4_BACK(Settings.Elevator.BACK_L4_HEIGHT_METERS), ALGAE_L2(Settings.Elevator.ALGAE_L2_HEIGHT_METERS), ALGAE_L3(Settings.Elevator.ALGAE_L3_HEIGHT_METERS), + HOLD_ALGAE(Settings.Elevator.HOLD_ALGAE_HEIGHT_METERS), BARGE(Settings.Elevator.BARGE_HEIGHT_METERS), - BOTTOM(Constants.Elevator.MIN_HEIGHT_METERS); + BOTTOM(Constants.Elevator.MIN_HEIGHT_METERS), + CLIMB(Settings.Elevator.CLIMB_HEIGHT_METERS); private Number targetHeight; @@ -62,7 +64,7 @@ public double getTargetHeight() { private ElevatorState state; protected Elevator() { - this.state = ElevatorState.STOW; + this.state = ElevatorState.FEED; } public void setState(ElevatorState state) { @@ -78,15 +80,18 @@ public ElevatorState getState() { public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); + public abstract double getAccelGs(); + public abstract void setVoltageOverride(Optional voltage); public abstract void setOperatorOffset(double offset); public abstract double getOperatorOffset(); - public abstract Command getSysIdQuasistatic(SysIdRoutine.Direction direction); - public abstract Command getSysIdDynamic(SysIdRoutine.Direction direction); + public abstract SysIdRoutine getSysIdRoutine(); @Override public void periodic() { + ArmElevatorVisualizer.getInstance().updateElevatorHeight(getCurrentHeight()); + SmartDashboard.putString("Elevator/State", state.toString()); SmartDashboard.putNumber("Elevator/Target Height (m)", getState().getTargetHeight()); SmartDashboard.putNumber("Elevator/Current Height (m)", getCurrentHeight()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java index 8777235..552919d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorImpl.java @@ -7,25 +7,17 @@ package com.stuypulse.robot.subsystems.elevator; import com.stuypulse.stuylib.math.SLMath; -import com.stuypulse.stuylib.network.SmartNumber; - import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Velocity; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; - import java.util.Optional; -import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -36,9 +28,6 @@ public class ElevatorImpl extends Elevator { private Optional voltageOverride; private double operatorOffset; - private SysIdRoutine sysidRoutine; - private boolean isRunningSysid; - protected ElevatorImpl() { super(); motor = new TalonFX(Ports.Elevator.MOTOR, Settings.CANIVORE_NAME); @@ -49,32 +38,20 @@ protected ElevatorImpl() { voltageOverride = Optional.empty(); operatorOffset = 0; - - isRunningSysid = false; - - sysidRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - null, - Units.Volts.of(5), - null, - state -> SignalLogger.writeString("SysIdElevator_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - motor.setVoltage(output.in(Units.Volts)); - isRunningSysid = true; - }, - null, - this)); - } - - @Override - public Command getSysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysidRoutine.quasistatic(direction); } @Override - public Command getSysIdDynamic(SysIdRoutine.Direction direction) { - return sysidRoutine.dynamic(direction); + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 7, + "Elevator", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> getCurrentHeight(), + () -> motor.getVelocity().getValueAsDouble(), + () -> motor.getMotorVoltage().getValueAsDouble(), + getInstance() + ); } @Override @@ -83,7 +60,7 @@ public double getCurrentHeight() { } private double getTargetHeight() { - return getState().getTargetHeight() + operatorOffset; + return SLMath.clamp(getState().getTargetHeight() + operatorOffset, Constants.Elevator.MIN_HEIGHT_METERS, Constants.Elevator.MAX_HEIGHT_METERS); } @Override @@ -91,6 +68,11 @@ public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS; } + @Override + public double getAccelGs() { + return motor.getAcceleration().getValueAsDouble(); + } + private boolean atBottom() { return !bumpSwitchBottom.get(); } @@ -118,7 +100,7 @@ public void periodic() { motor.setPosition(Constants.Elevator.MIN_HEIGHT_METERS); } - if (Settings.EnabledSubsystems.ELEVATOR.get() && !isRunningSysid) { + if (Settings.EnabledSubsystems.ELEVATOR.get()) { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } @@ -130,11 +112,10 @@ public void periodic() { motor.setVoltage(0); } - - SmartDashboard.putNumber("Elevator/Motor Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Elevator/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Elevator/Stator Current", motor.getStatorCurrent().getValueAsDouble()); - - SmartDashboard.putNumber("Elevator/Supply Voltage", motor.getSupplyVoltage().getValueAsDouble()); SmartDashboard.putNumber("Elevator/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Elevator/Accel Gs", getAccelGs()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java index 3d84a87..f9564d3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java +++ b/src/main/java/com/stuypulse/robot/subsystems/elevator/ElevatorSimu.java @@ -6,6 +6,9 @@ package com.stuypulse.robot.subsystems.elevator; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.Derivative; +import com.stuypulse.stuylib.streams.numbers.filters.IFilter; import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; import java.util.Optional; @@ -13,6 +16,7 @@ import com.ctre.phoenix6.SignalLogger; import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -37,12 +41,11 @@ public class ElevatorSimu extends Elevator { private final LinearSystemLoop controller; private final MotionProfile motionProfile; + private IStream accel; + private Optional voltageOverride; private double operatorOffset; - private SysIdRoutine sysidRoutine; - private boolean isRunningSysid; - protected ElevatorSimu() { sim = new ElevatorSim( DCMotor.getKrakenX60(1), @@ -53,6 +56,8 @@ protected ElevatorSimu() { Constants.Elevator.MAX_HEIGHT_METERS, false, Constants.Elevator.MIN_HEIGHT_METERS); + + sim.setState(Constants.Elevator.MIN_HEIGHT_METERS, 0); LinearSystem elevatorSystem = LinearSystemId.createElevatorSystem( DCMotor.getKrakenX60(1), @@ -82,34 +87,25 @@ protected ElevatorSimu() { motionProfile.reset(Constants.Elevator.MIN_HEIGHT_METERS); + accel = IStream.create(() -> sim.getVelocityMetersPerSecond()) + .filtered(new Derivative()); + voltageOverride = Optional.empty(); operatorOffset = 0; - - isRunningSysid = false; - - sysidRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - null, - Units.Volts.of(4), - null, - state -> SignalLogger.writeString("SysIdElevator_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - isRunningSysid = true; - sim.setInputVoltage(output.in(Units.Volts)); - }, - null, - this)); - } - - @Override - public Command getSysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysidRoutine.quasistatic(direction); } @Override - public Command getSysIdDynamic(SysIdRoutine.Direction direction) { - return sysidRoutine.dynamic(direction); + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 7, + "Elevator", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> getCurrentHeight(), + () -> sim.getVelocityMetersPerSecond(), + () -> voltageOverride.get(), + getInstance() + ); } private double getTargetHeight() { @@ -126,6 +122,11 @@ public boolean atTargetHeight() { return Math.abs(getTargetHeight() - getCurrentHeight()) < Settings.Elevator.HEIGHT_TOLERANCE_METERS; } + @Override + public double getAccelGs() { + return accel.get(); + } + @Override public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; @@ -153,18 +154,18 @@ public void periodic() { controller.correct(VecBuilder.fill(sim.getPositionMeters(), sim.getVelocityMetersPerSecond())); controller.predict(Settings.DT); - if (Settings.EnabledSubsystems.ELEVATOR.get() && !isRunningSysid) { + if (Settings.EnabledSubsystems.ELEVATOR.get()) { if (voltageOverride.isPresent()) { sim.setInputVoltage(0); - sim.update(Settings.DT); } else { sim.setInputVoltage(controller.getU(0)); - sim.update(Settings.DT); } } else { sim.setInputVoltage(0); } + + sim.update(Settings.DT); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/froggy/Froggy.java b/src/main/java/com/stuypulse/robot/subsystems/froggy/Froggy.java index 34a19ed..4a6155d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/froggy/Froggy.java +++ b/src/main/java/com/stuypulse/robot/subsystems/froggy/Froggy.java @@ -12,7 +12,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public abstract class Froggy extends SubsystemBase { @@ -46,10 +48,10 @@ public Rotation2d getTargetAngle() { } public enum RollerState { - INTAKE_CORAL(-Settings.Froggy.CORAL_INTAKE_SPEED), + INTAKE_CORAL(Settings.Froggy.CORAL_INTAKE_SPEED), INTAKE_ALGAE(Settings.Froggy.ALGAE_INTAKE_SPEED), SHOOT_CORAL(Settings.Froggy.CORAL_OUTTAKE_SPEED), - SHOOT_ALGAE(-Settings.Froggy.ALGAE_OUTTAKE_SPEED), + SHOOT_ALGAE(Settings.Froggy.ALGAE_OUTTAKE_SPEED), HOLD_ALGAE(Settings.Froggy.HOLD_ALGAE_SPEED), STOP(0); @@ -97,6 +99,8 @@ public void setRollerState(RollerState state) { public abstract void setPivotOperatorOffset(Rotation2d offset); public abstract Rotation2d getPivotOperatorOffset(); + public abstract SysIdRoutine getFroggySysIdRoutine(); + @Override public void periodic() { SmartDashboard.putString("Froggy/Pivot State", getPivotState().toString()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/froggy/FroggyImpl.java b/src/main/java/com/stuypulse/robot/subsystems/froggy/FroggyImpl.java index acef7d1..9d1df52 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/froggy/FroggyImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/froggy/FroggyImpl.java @@ -13,34 +13,32 @@ import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.MotionProfile; import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.Optional; -import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.SensorDirectionValue; public class FroggyImpl extends Froggy { private TalonFX rollerMotor; private TalonFX pivotMotor; - // private CANcoder absoluteEncoder; private DutyCycleEncoder absoluteEncoder; - private Controller controller; - private BStream isStalling; private Optional pivotVoltageOverride; @@ -53,23 +51,10 @@ protected FroggyImpl() { pivotMotor = new TalonFX(Ports.Froggy.PIVOT); Motors.Froggy.PIVOT_MOTOR_CONFIG.configure(pivotMotor); - - // absoluteEncoder = new CANcoder(Ports.Froggy.ABSOLUTE_ENCODER); - - // MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs() - // .withMagnetOffset(Constants.Froggy.ANGLE_OFFSET) - // .withSensorDirection(SensorDirectionValue.Clockwise_Positive); - - // absoluteEncoder.getConfigurator().apply(magnetSensorConfigs); absoluteEncoder = new DutyCycleEncoder(Ports.Froggy.ABSOLUTE_ENCODER); absoluteEncoder.setInverted(true); - controller = new MotorFeedforward(Gains.Froggy.FF.kS, Gains.Froggy.FF.kV, Gains.Froggy.FF.kA).position() - .add(new ArmFeedforward(Gains.Froggy.FF.kG)) - .add(new PIDController(Gains.Froggy.PID.kP, Gains.Froggy.PID.kI, Gains.Froggy.PID.kD)) - .setSetpointFilter(new MotionProfile(Settings.Froggy.MAX_VEL_ROTATIONS_PER_S, Settings.Froggy.MAX_ACCEL_ROTATIONS_PER_S_PER_S)); - isStalling = BStream.create(() -> { switch (getRollerState()) { case INTAKE_CORAL: @@ -85,6 +70,20 @@ protected FroggyImpl() { pivotOperatorOffset = Rotation2d.kZero; } + @Override + public SysIdRoutine getFroggySysIdRoutine() { + return SysId.getRoutine( + 2, + 9, + "Froggy Pivot", + voltage -> setPivotVoltageOverride(Optional.of(voltage)), + () -> getCurrentAngle().getRotations(), + () -> pivotMotor.getVelocity().getValueAsDouble(), + () -> pivotMotor.getMotorVoltage().getValueAsDouble(), + getInstance() + ); + } + private Rotation2d getCurrentAngle() { // return Rotation2d.fromRotations(absoluteEncoder.getAbsolutePosition().getValueAsDouble()); return Rotation2d.fromRotations(absoluteEncoder.get() - Constants.Froggy.ANGLE_OFFSET.getRotations()); @@ -124,14 +123,22 @@ public Rotation2d getPivotOperatorOffset() { public void periodic() { super.periodic(); + pivotMotor.setPosition(getCurrentAngle().getRotations()); + if (Settings.EnabledSubsystems.FROGGY.get()) { rollerMotor.set(getRollerState().getTargetSpeed().doubleValue()); if (pivotVoltageOverride.isPresent()) { pivotMotor.setVoltage(pivotVoltageOverride.get()); } else { - // pivotMotor.setControl(new MotionMagicVoltage(getTargetAngle().getRotations())); - pivotMotor.setVoltage(controller.update(getTargetAngle().getRotations(), getCurrentAngle().getRotations())); + if (getRollerState() == RollerState.HOLD_ALGAE && isStalling()) { + // Algae + pivotMotor.setControl(new MotionMagicVoltage(getTargetAngle().getRotations()).withSlot(1)); + } + else { + // Coral + pivotMotor.setControl(new MotionMagicVoltage(getTargetAngle().getRotations()).withSlot(0)); + } } } else { @@ -139,14 +146,22 @@ public void periodic() { pivotMotor.setVoltage(0); } - SmartDashboard.putBoolean("Froggy/At Target Angle", isAtTargetAngle()); + // PIVOT + SmartDashboard.putBoolean("Froggy/Pivot/At Target Angle", isAtTargetAngle()); + SmartDashboard.putNumber("Froggy/Pivot/Raw Encoder Angle (deg)", Units.rotationsToDegrees(absoluteEncoder.get())); + + SmartDashboard.putNumber("Froggy/Pivot/Supply Current", pivotMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Froggy/Pivot/Stator Current", pivotMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Froggy/Pivot/Voltage", pivotMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Froggy/Current Angle (deg)", getCurrentAngle().getDegrees()); - SmartDashboard.putNumber("Froggy/Target Angle (deg)", getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Froggy/Pivot/Current Angle (deg)", getCurrentAngle().getDegrees()); + SmartDashboard.putNumber("Froggy/Pivot/Target Angle (deg)", getTargetAngle().getDegrees()); - SmartDashboard.putBoolean("Froggy/Is Stalling", isStalling()); + // ROLLER + SmartDashboard.putBoolean("Froggy/Roller/Is Stalling", isStalling()); - SmartDashboard.putNumber("Froggy/Roller Voltage", rollerMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Froggy/Roller Current", rollerMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Froggy/Roller/Voltage", rollerMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Froggy/Roller/Supply Current", rollerMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Froggy/Roller/Stator Current", rollerMotor.getStatorCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/funnel/FunnelImpl.java b/src/main/java/com/stuypulse/robot/subsystems/funnel/FunnelImpl.java index 91c2dc2..24bff09 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/funnel/FunnelImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/funnel/FunnelImpl.java @@ -6,8 +6,6 @@ package com.stuypulse.robot.subsystems.funnel; -import org.ejml.equation.IntegerSequence.For; - import com.ctre.phoenix6.hardware.TalonFX; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; @@ -62,8 +60,10 @@ public void periodic() { motor.set(0); } - SmartDashboard.putNumber("Funnel/Current", motor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putBoolean("Funnel/IR Sensor", irSensor.get()); + SmartDashboard.putNumber("Funnel/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Funnel/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putBoolean("Funnel/IR Sensor raw", irSensor.get()); SmartDashboard.putBoolean("Funnel/Has Coral", hasCoral()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index 6634ea8..d92be0e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -7,7 +7,7 @@ package com.stuypulse.robot.subsystems.shooter; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.superstructure.SuperStructure; +import com.stuypulse.robot.subsystems.arm.Arm; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,6 +30,7 @@ public enum ShooterState { SHOOT_CORAL_FORWARD(Settings.Shooter.CORAL_SHOOT_SPEED_FORWARD), SHOOT_CORAL_REVERSE(Settings.Shooter.CORAL_SHOOT_SPEED_REVERSE), SHOOT_ALGAE(Settings.Shooter.ALGAE_SHOOT_SPEED), + HOLD_ALGAE(Settings.Shooter.ALGAE_HOLD_SPEED), STOP(0); private Number speed; @@ -61,7 +62,7 @@ public void setState(ShooterState state) { public abstract boolean isStalling(); public boolean shouldShootForward() { - switch (SuperStructure.getInstance().getTargetState()) { + switch (Arm.getInstance().getState()) { case L2_FRONT, L2_BACK, L3_BACK, L4_BACK: return true; default: @@ -70,7 +71,7 @@ public boolean shouldShootForward() { } public boolean shouldShootBackwards() { - switch (SuperStructure.getInstance().getTargetState()) { + switch (Arm.getInstance().getState()) { case L3_FRONT, L4_FRONT: return true; default: diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 9820428..b581abc 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -60,8 +60,8 @@ public void periodic() { } SmartDashboard.putNumber("Shooter/Voltage", motor.getMotorVoltage().getValueAsDouble()); - - SmartDashboard.putNumber("Shooter/Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Shooter/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Shooter/Stator Current", motor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putBoolean("Shooter/Has Coral", hasCoral()); SmartDashboard.putBoolean("Shooter/Is Stalling", isStalling()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/SuperStructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/SuperStructure.java deleted file mode 100644 index f1c916a..0000000 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/SuperStructure.java +++ /dev/null @@ -1,147 +0,0 @@ -package com.stuypulse.robot.subsystems.superstructure; - -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.arm.Arm; -import com.stuypulse.robot.subsystems.arm.Arm.ArmState; -import com.stuypulse.robot.subsystems.elevator.Elevator; -import com.stuypulse.robot.subsystems.elevator.Elevator.ElevatorState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class SuperStructure extends SubsystemBase{ - private static final SuperStructure instance; - - static { - instance = new SuperStructure(); - } - - public static SuperStructure getInstance() { - return instance; - } - - public enum SuperStructureTargetState { - FEED(ArmState.FEED, ElevatorState.FEED), - STOW(ArmState.STOW, ElevatorState.STOW), - L2_FRONT(ArmState.L2_FRONT, ElevatorState.L2_FRONT), - L2_BACK(ArmState.L2_BACK, ElevatorState.L2_BACK), - L3_FRONT(ArmState.L3_FRONT, ElevatorState.L3_FRONT), - L3_BACK(ArmState.L3_BACK, ElevatorState.L3_BACK), - L4_FRONT(ArmState.L4_FRONT, ElevatorState.L4_FRONT), - L4_BACK(ArmState.L4_BACK, ElevatorState.L4_BACK), - ALGAE_L2(ArmState.ALGAE_L2, ElevatorState.ALGAE_L2), - ALGAE_L3(ArmState.ALGAE_L3, ElevatorState.ALGAE_L3), - BARGE(ArmState.BARGE, ElevatorState.BARGE); - - private ArmState targetArmState; - private ElevatorState targetElevatorState; - - private SuperStructureTargetState(ArmState targetArmState, ElevatorState targetElevatorState) { - this.targetArmState = targetArmState; - this.targetElevatorState = targetElevatorState; - } - - public ArmState getTargetArmState() { - return this.targetArmState; - } - - public ElevatorState getTargetElevatorState() { - return this.targetElevatorState; - } - } - - private SuperStructureTargetState targetState; - - private Arm arm; - private Elevator elevator; - - private SuperStructureVisualizer visualizer; - - private SuperStructure() { - this.targetState = SuperStructureTargetState.STOW; - - this.arm = Arm.getInstance(); - this.elevator = Elevator.getInstance(); - - visualizer = SuperStructureVisualizer.getInstance(); - } - - public void setTargetState(SuperStructureTargetState state) { - this.targetState = state; - } - - public SuperStructureTargetState getTargetState() { - return this.targetState; - } - - public boolean isInScoreState() { - switch (getTargetState()) { - case L2_FRONT: - case L2_BACK: - case L3_FRONT: - case L3_BACK: - case L4_FRONT: - case L4_BACK: - case ALGAE_L2: - case ALGAE_L3: - case BARGE: - return true; - default: - return false; - } - } - - private boolean canMoveDownFromBarge() { - double distanceFromCenterLine = Math.abs(Field.LENGTH / 2 - CommandSwerveDrivetrain.getInstance().getPose().getX()); - return distanceFromCenterLine > Settings.CLEARANCE_DISTANCE_FROM_CENTERLINE_FOR_BARGE; - } - - private void updateArmElevatorTargetStates() { - ArmState currentArmState = arm.getState(); - ArmState targetArmState = getTargetState().getTargetArmState(); - - ElevatorState currentElevatorState = elevator.getState(); - ElevatorState targetElevatorState = getTargetState().getTargetElevatorState(); - - if (currentArmState == ArmState.BARGE && currentElevatorState == ElevatorState.BARGE && !canMoveDownFromBarge()) { - return; - } - - if (getTargetState() == SuperStructureTargetState.FEED) { - if ((currentElevatorState == ElevatorState.FEED && elevator.atTargetHeight()) || !Settings.EnabledSubsystems.ELEVATOR.get()) { - arm.setState(ArmState.FEED); - } - else if (((currentArmState == ArmState.VERTICAL_DOWN && arm.atTargetAngle()) || !Settings.EnabledSubsystems.ARM.get()) && currentElevatorState != ElevatorState.FEED) { - elevator.setState(ElevatorState.FEED); - } - else { - arm.setState(ArmState.VERTICAL_DOWN); - } - } - else { - if ((targetElevatorState != currentElevatorState && arm.atTargetAngle()) || !Settings.EnabledSubsystems.ARM.get()) { - if (currentArmState == ArmState.FEED) { - arm.setState(ArmState.VERTICAL_DOWN); - } - else { - elevator.setState(targetElevatorState); - } - } - else if ((targetArmState != currentArmState && elevator.atTargetHeight()) || !Settings.EnabledSubsystems.ELEVATOR.get()) { - arm.setState(targetArmState); - } - } - } - - @Override - public void periodic() { - updateArmElevatorTargetStates(); - - visualizer.update(elevator.getCurrentHeight(), arm.getCurrentAngle()); - - SmartDashboard.putString("Super Structure/Target Arm State", getTargetState().getTargetArmState().toString()); - SmartDashboard.putString("Super Structure/Target Elevator State", getTargetState().getTargetElevatorState().toString()); - } -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 3252537..7a5d698 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -19,10 +19,12 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Constants; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; +import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.Vector2D; import edu.wpi.first.math.Matrix; @@ -41,6 +43,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; /** @@ -70,11 +73,13 @@ public static CommandSwerveDrivetrain getInstance() { private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric() .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) + .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) .withDesaturateWheelSpeeds(true); private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) + .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) .withDesaturateWheelSpeeds(true); public SwerveRequest.FieldCentric getFieldCentricSwerveRequest() { @@ -335,16 +340,20 @@ public Pose2d getPose() { return getState().Pose; } + public double getRobotRelativeXAccelGs() { + return getPigeon2().getAccelerationX().getValueAsDouble() * getPose().getRotation().getCos(); + } + public void configureAutoBuilder() { try{ AutoBuilder.configure( this::getPose, this::resetPose, this::getChassisSpeeds, - (speeds, feedforwards) -> setChassisSpeeds(speeds), + this::setChassisSpeedsAuton, new PPHolonomicDriveController(Gains.Swerve.Alignment.XY, Gains.Swerve.Alignment.THETA), RobotConfig.fromGUISettings(), - () -> false, + () -> true, instance ); // PathPlannerLogging.setLogActivePathCallback((poses) -> Odometry.getInstance().getField().getObject("path").setPoses(poses)); @@ -374,15 +383,25 @@ public SwerveModuleState[] getModuleStates() { return moduleStates; } - private ChassisSpeeds getChassisSpeeds() { + public ChassisSpeeds getChassisSpeeds() { return getKinematics().toChassisSpeeds(getModuleStates()); } + public Vector2D getFieldRelativeSpeeds() { + return new Vector2D(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond) + .rotate(Angle.fromRotation2d(getPose().getRotation())); + } + private void setChassisSpeeds(ChassisSpeeds robotSpeeds) { ChassisSpeeds speeds = new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, -robotSpeeds.omegaRadiansPerSecond); setControl(new SwerveRequest.RobotCentric().withVelocityX(speeds.vxMetersPerSecond).withVelocityY(speeds.vyMetersPerSecond).withRotationalRate(speeds.omegaRadiansPerSecond)); } + private void setChassisSpeedsAuton(ChassisSpeeds robotSpeeds) { + ChassisSpeeds speeds = new ChassisSpeeds(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond, -robotSpeeds.omegaRadiansPerSecond); + setControl(new SwerveRequest.RobotCentric().withVelocityX(speeds.vxMetersPerSecond).withVelocityY(speeds.vyMetersPerSecond).withRotationalRate(-speeds.omegaRadiansPerSecond)); + } + public boolean isFrontFacingReef() { Vector2D reefCenterToRobot = new Vector2D(getPose().getTranslation().minus(Field.REEF_CENTER)); Rotation2d robotHeading = getPose().getRotation(); @@ -396,10 +415,18 @@ public boolean isAlignedToBargeX() { < Settings.Swerve.Alignment.Tolerances.X_TOLERANCE.get(); } + public boolean isClearFromReef() { + return Field.REEF_CENTER.getDistance(getPose().getTranslation()) > (Settings.CLEARANCE_DISTANCE_FROM_REEF + Field.CENTER_OF_REEF_TO_REEF_FACE + Constants.LENGTH_WITH_BUMPERS_METERS / 2); + } + @Override public void periodic() { SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", getChassisSpeeds().vxMetersPerSecond); SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", getChassisSpeeds().vyMetersPerSecond); + + SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", getFieldRelativeSpeeds().x); + SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); + SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", getChassisSpeeds().omegaRadiansPerSecond); for (int i = 0; i < 4; i++) { @@ -407,9 +434,15 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Target Speed (m per s)", getModule(i).getTargetState().speedMetersPerSecond); SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Angle (deg)", getModule(i).getCurrentState().angle.getDegrees() % 360); SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Target Angle (deg)", getModule(i).getTargetState().angle.getDegrees() % 360); + SmartDashboard.putNumber("Swerve/Modules/Module " + i + "/Stator Current", getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); } + SmartDashboard.putNumber("Swerve/Gyro/Accel x (g)", getPigeon2().getAccelerationX().getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Gyro/Accel y (g)", getPigeon2().getAccelerationY().getValueAsDouble()); + SmartDashboard.putNumber("Swerve/Gyro/Robot Relative Accel x (g)", getRobotRelativeXAccelGs()); + SmartDashboard.putBoolean("Swerve/Is Front Facing Reef", isFrontFacingReef()); + SmartDashboard.putBoolean("Swerve/Is clear from Reef", isClearFromReef()); Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? getPose() : Field.transformToOppositeAlliance(getPose())); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index caf814a..2c67dbd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -31,8 +31,8 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.14362).withKI(0.0).withKD(0.0) - .withKS(0.14752).withKV(0.011085); + .withKP(0.23046).withKI(0.0).withKD(0.0) + .withKS(0.13806).withKV(0.21423).withKA(0.18032); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -86,7 +86,7 @@ public class TunerConstants { private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; + private static final boolean kInvertRightSide = false; private static final int kPigeonId = 9; @@ -126,44 +126,44 @@ public class TunerConstants { .withDriveFrictionVoltage(kDriveFrictionVoltage); - // Front Left + // Front Left (Module 0) private static final int kFrontLeftDriveMotorId = 13; private static final int kFrontLeftSteerMotorId = 12; private static final int kFrontLeftEncoderId = 2; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(Units.degreesToRotations(85.781250 + 180)); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(Units.degreesToRotations(-87.363281)); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; private static final Distance kFrontLeftXPos = Inches.of(12); private static final Distance kFrontLeftYPos = Inches.of(12); - // Front Right + // Front Right (Module 1) private static final int kFrontRightDriveMotorId = 15; private static final int kFrontRightSteerMotorId = 14; private static final int kFrontRightEncoderId = 3; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.158447265625); + private static final Angle kFrontRightEncoderOffset = Rotations.of(Units.degreesToRotations(233.964844)); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; private static final Distance kFrontRightXPos = Inches.of(12); private static final Distance kFrontRightYPos = Inches.of(-12); - // Back Left + // Back Left (Module 2) private static final int kBackLeftDriveMotorId = 11; private static final int kBackLeftSteerMotorId = 10; private static final int kBackLeftEncoderId = 1; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.167236328125); + private static final Angle kBackLeftEncoderOffset = Rotations.of(Units.degreesToRotations(58.359375)); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; private static final Distance kBackLeftXPos = Inches.of(-12); private static final Distance kBackLeftYPos = Inches.of(12); - // Back Right + // Back Right (Module 3) private static final int kBackRightDriveMotorId = 17; private static final int kBackRightSteerMotorId = 16; private static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.35888671875); + private static final Angle kBackRightEncoderOffset = Rotations.of(Units.degreesToRotations(-51.152344)); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 7334588..62508f5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -9,6 +9,7 @@ import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -37,9 +38,9 @@ private LimelightVision() { robotRelativePose.getX(), robotRelativePose.getY(), robotRelativePose.getZ(), - robotRelativePose.getRotation().getX(), - robotRelativePose.getRotation().getY(), - robotRelativePose.getRotation().getZ() + Units.radiansToDegrees(robotRelativePose.getRotation().getX()), + Units.radiansToDegrees(robotRelativePose.getRotation().getY()), + Units.radiansToDegrees(robotRelativePose.getRotation().getZ()) ); } diff --git a/src/main/java/com/stuypulse/robot/util/ArmDriveFeedForward.java b/src/main/java/com/stuypulse/robot/util/ArmDriveFeedForward.java new file mode 100644 index 0000000..04c0767 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/ArmDriveFeedForward.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.streams.numbers.IStream; + +import edu.wpi.first.math.util.Units; + +public class ArmDriveFeedForward extends Controller { + + private final Number kG; + private final IStream xAccelInGs; + + public ArmDriveFeedForward(Number kG, IStream xAccelInGs) { + this.kG = kG; + this.xAccelInGs = xAccelInGs; + } + + @Override + protected double calculate(double setpoint, double measurement) { + return -(kG.doubleValue() * Math.sin(Units.degreesToRadians(measurement)) * xAccelInGs.get()); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/ArmElevatorFeedForward.java b/src/main/java/com/stuypulse/robot/util/ArmElevatorFeedForward.java new file mode 100644 index 0000000..ed39ab9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/ArmElevatorFeedForward.java @@ -0,0 +1,23 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.streams.numbers.IStream; + +import edu.wpi.first.math.util.Units; + +public class ArmElevatorFeedForward extends Controller { + + private final Number kG; + private final IStream elevatorAccelInGs; + + public ArmElevatorFeedForward(Number kG, IStream elevatorAccelInGs) { + this.kG = kG; + this.elevatorAccelInGs = elevatorAccelInGs; + } + + @Override + protected double calculate(double setpoint, double measurement) { + return kG.doubleValue() * Math.cos(Units.degreesToRadians(measurement)) * elevatorAccelInGs.get(); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/SuperStructureVisualizer.java b/src/main/java/com/stuypulse/robot/util/ArmElevatorVisualizer.java similarity index 85% rename from src/main/java/com/stuypulse/robot/subsystems/superstructure/SuperStructureVisualizer.java rename to src/main/java/com/stuypulse/robot/util/ArmElevatorVisualizer.java index 8c8d49c..29535de 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/SuperStructureVisualizer.java +++ b/src/main/java/com/stuypulse/robot/util/ArmElevatorVisualizer.java @@ -1,4 +1,4 @@ -package com.stuypulse.robot.subsystems.superstructure; +package com.stuypulse.robot.util; import com.stuypulse.robot.constants.Constants; @@ -11,14 +11,14 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -public class SuperStructureVisualizer { - private static final SuperStructureVisualizer instance; +public class ArmElevatorVisualizer { + private static final ArmElevatorVisualizer instance; static { - instance = new SuperStructureVisualizer(); + instance = new ArmElevatorVisualizer(); } - public static SuperStructureVisualizer getInstance() { + public static ArmElevatorVisualizer getInstance() { return instance; } @@ -30,13 +30,15 @@ public static SuperStructureVisualizer getInstance() { private final MechanismRoot2d elevatorCarriageTopFront; private final MechanismRoot2d elevatorCarriageTopBack; + private double elevatorHeight; + private final MechanismRoot2d armPivot; private final MechanismLigament2d arm; private double width; private double height; - private SuperStructureVisualizer() { + private ArmElevatorVisualizer() { width = Constants.Arm.ARM_LENGTH * 2 + Units.inchesToMeters(3); height = Constants.Elevator.MAX_HEIGHT_METERS + Constants.Arm.ARM_LENGTH + Units.inchesToMeters(3); @@ -110,6 +112,8 @@ private SuperStructureVisualizer() { new Color8Bit(Color.kOrange) )); + elevatorHeight = Constants.Elevator.MIN_HEIGHT_METERS; + // ARM armPivot = superStructure.getRoot("Arm Pivot", width/2, Constants.Elevator.MIN_HEIGHT_METERS - Constants.Arm.DISTANCE_FROM_PIVOT_TO_TOP_OF_ELEVATOR); @@ -124,13 +128,18 @@ private SuperStructureVisualizer() { armPivot.append(arm); } - public void update(double elevatorHeight, Rotation2d armAngle) { + public void updateElevatorHeight(double elevatorHeight) { + this.elevatorHeight = elevatorHeight; elevatorCarriageTopFront.setPosition((width + Constants.Elevator.WIDTH) / 2 - Units.inchesToMeters(2), elevatorHeight); elevatorCarriageTopBack.setPosition((width - Constants.Elevator.WIDTH) / 2 + Units.inchesToMeters(2), elevatorHeight); + SmartDashboard.putData("Visualizers/Arm-Elevator", superStructure); + } + + public void updateArmAngle(Rotation2d armAngle) { armPivot.setPosition(width/2, elevatorHeight - Constants.Arm.DISTANCE_FROM_PIVOT_TO_TOP_OF_ELEVATOR); arm.setAngle(armAngle); - SmartDashboard.putData("Visualizers/Superstructure", superStructure); + SmartDashboard.putData("Visualizers/Arm-Elevator", superStructure); } } diff --git a/src/main/java/com/stuypulse/robot/util/HolonomicController.java b/src/main/java/com/stuypulse/robot/util/HolonomicController.java index 5591a76..5db84b7 100644 --- a/src/main/java/com/stuypulse/robot/util/HolonomicController.java +++ b/src/main/java/com/stuypulse/robot/util/HolonomicController.java @@ -1,7 +1,7 @@ package com.stuypulse.robot.util; +import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.math.Angle; import edu.wpi.first.math.geometry.Pose2d; @@ -10,11 +10,11 @@ import edu.wpi.first.util.sendable.SendableBuilder; public class HolonomicController implements Sendable { - private PIDController xController; - private PIDController yController; + private Controller xController; + private Controller yController; private AngleController angleController; - public HolonomicController(PIDController xController, PIDController yController, AngleController angleController) { + public HolonomicController(Controller xController, Controller yController, AngleController angleController) { this.xController = xController; this.yController = yController; this.angleController = angleController; diff --git a/src/main/java/com/stuypulse/robot/util/SettableNumber.java b/src/main/java/com/stuypulse/robot/util/SettableNumber.java new file mode 100644 index 0000000..abb786b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SettableNumber.java @@ -0,0 +1,37 @@ +package com.stuypulse.robot.util; + +public class SettableNumber extends Number { + private double value; + + public SettableNumber(double value) { + this.value = value; + } + + public void set(double value) { + this.value = value; + } + + public double get() { + return value; + } + + @Override + public int intValue() { + return (int) value; + } + + @Override + public long longValue() { + return (long) value; + } + + @Override + public float floatValue() { + return (float) value; + } + + @Override + public double doubleValue() { + return (double) value; + } +} diff --git a/src/main/java/com/stuypulse/robot/util/SysId.java b/src/main/java/com/stuypulse/robot/util/SysId.java new file mode 100644 index 0000000..3bb62c9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SysId.java @@ -0,0 +1,42 @@ +package com.stuypulse.robot.util; + +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volt; +import static edu.wpi.first.units.Units.Volts; + +import java.util.function.Consumer; +import java.util.function.Supplier; + +import com.ctre.phoenix6.SignalLogger; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class SysId { + public static SysIdRoutine getRoutine( + double rampRate, + double stepVoltage, + String subsystemName, + Consumer voltageSetter, + Supplier positionSupplier, + Supplier velocitySupplier, + Supplier voltageSupplier, + Subsystem subsystemInstance + ) { + return new SysIdRoutine( + new SysIdRoutine.Config( + Units.Volts.of(rampRate).per(Second), + Units.Volts.of(stepVoltage), + null, + state -> SignalLogger.writeString(subsystemName + " SysId-State", state.toString())), + new SysIdRoutine.Mechanism( + output -> voltageSetter.accept(output.in(Volts)), + state -> { + SignalLogger.writeDouble(subsystemName + " Position", positionSupplier.get()); + SignalLogger.writeDouble(subsystemName + " Velocity", velocitySupplier.get()); + SignalLogger.writeDouble(subsystemName + " Voltage", voltageSupplier.get()); + }, + subsystemInstance)); + } +} diff --git a/src/main/java/com/stuypulse/robot/util/TranslationMotionProfileIan.java b/src/main/java/com/stuypulse/robot/util/TranslationMotionProfileIan.java new file mode 100644 index 0000000..4b0845c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/TranslationMotionProfileIan.java @@ -0,0 +1,107 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.vectors.filters.VFilter; +import com.stuypulse.stuylib.util.StopWatch; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class TranslationMotionProfileIan implements VFilter { + + // Default number of times to apply filter (helps accuracy) + private static final int kDefaultSteps = 64; + + // Stopwatch to Track dt + private StopWatch mTimer; + + // Limits for each of the derivatives + private Number mVelLimit; + private Number mAccelLimit; + + // The last output / velocity + private Vector2D mOutput; + private Vector2D mVelocity; + + // Number of times to apply filter (helps accuracy) + private final int mSteps; + + public TranslationMotionProfileIan( + Number velLimit, + Number accelLimit, + Vector2D startingTranslation, + Vector2D startingVelocity, + int steps) + { + mTimer = new StopWatch(); + + mVelLimit = velLimit; + mAccelLimit = accelLimit; + + mOutput = startingTranslation; + mVelocity = startingVelocity; + + mSteps = steps; + } + + public TranslationMotionProfileIan(Number velLimit, Number accelLimit, Vector2D startingTranslation, Vector2D startingVelocity) { + this(velLimit, accelLimit, startingTranslation, startingVelocity, kDefaultSteps); + } + + public TranslationMotionProfileIan(Number velLimit, Number accelLimit) { + this(velLimit, accelLimit, Vector2D.kOrigin, Vector2D.kOrigin, kDefaultSteps); + } + + public Vector2D get(Vector2D target) { + double dt = mTimer.reset() / mSteps; + + for (int i = 0; i < mSteps; ++i) { + // if there is a accel limit, limit the amount the velocity can change + if (0 < mAccelLimit.doubleValue()) { + // amount of windup in system (how long it would take to slow down) + double windup = mVelocity.magnitude() / mAccelLimit.doubleValue(); + + // If the windup is too small, just use normal algorithm to limit acceleration + if (windup < dt) { + // Calculate acceleration needed to reach target + Vector2D accel = target.sub(mOutput).div(dt).sub(mVelocity); + + // Try to reach it while abiding by accel limit + mVelocity = mVelocity.add(accel.clamp(dt * mAccelLimit.doubleValue())); + } else { + // the position it would end up if it attempted to come to a full stop + Vector2D windA = + mVelocity.mul(0.5 * (dt + windup)); // windup caused by acceleration + Vector2D future = mOutput.add(windA); // where the robot will end up + + // Calculate acceleration needed to come to stop at target throughout windup + Vector2D accel = target.sub(future).div(windup); + + // Try to reach it while abiding by accel limit + mVelocity = mVelocity.add(accel.clamp(dt * mAccelLimit.doubleValue())); + } + + } else { + // make the velocity the difference between target and current + mVelocity = target.sub(mOutput).div(dt); + } + + // if there is an velocity limit, limit the velocity + if (0 < mVelLimit.doubleValue()) { + mVelocity = mVelocity.clamp(mVelLimit.doubleValue()); + } + + // adjust output by calculated velocity + mOutput = mOutput.add(mVelocity.mul(dt)); + } + + // Field.FIELD2D.getObject("Translation Motion Profile Ian").setPose(!Robot.isBlue() + // ? new Pose2d(mOutput.x, mOutput.y, new Rotation2d()) + // : Field.transformToOppositeAlliance(new Pose2d(mOutput.x, mOutput.y, new Rotation2d()))); + + return mOutput; + } +}