diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..b1479f5 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,43 @@ +{ + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "GroundCoral": { + "open": true + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/Pose": "Field2d", + "/SmartDashboard/Alerts": "Alerts", + "/SmartDashboard/Arm": "Subsystem", + "/SmartDashboard/ArmMinus": "Command", + "/SmartDashboard/ArmPlus": "Command", + "/SmartDashboard/Auto Chooser": "String Chooser", + "/SmartDashboard/Elevator": "Subsystem", + "/SmartDashboard/Elevator duty cycle up": "Command", + "/SmartDashboard/EndEffector": "Subsystem", + "/SmartDashboard/Face Hex": "Command", + "/SmartDashboard/Face Net": "Command", + "/SmartDashboard/GroundCoral": "Subsystem", + "/SmartDashboard/Module 0": "Mechanism2d", + "/SmartDashboard/Module 1": "Mechanism2d", + "/SmartDashboard/Module 2": "Mechanism2d", + "/SmartDashboard/Module 3": "Mechanism2d", + "/SmartDashboard/Reset Gyro": "Command", + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/Wrist": "Subsystem", + "/SmartDashboard/Zero Arm": "Command", + "/SmartDashboard/Zero Wrist": "Command" + } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "696" + } +} diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..9591fdb 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "2025BuildSeason" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/README.md b/README.md index 1560f68..6677a2a 100644 --- a/README.md +++ b/README.md @@ -1 +1,2 @@ -# 2025BuildSeason \ No newline at end of file +# 2025BuildSeason +The official robot code for the 2025 onseason robot, Hail Mary \ No newline at end of file diff --git a/build.gradle b/build.gradle index d6f1f22..66e0041 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.peterabeles.gversion" version "1.10" } diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/deploy/pathplanner/autos/LeftSide.auto b/src/main/deploy/pathplanner/autos/LeftSide.auto new file mode 100644 index 0000000..8acb4fc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftSide.auto @@ -0,0 +1,139 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "gotoreef" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "thentosource" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "backtoreef" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "tosource2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "end" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L1" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/centermove.auto b/src/main/deploy/pathplanner/autos/Middle.auto similarity index 59% rename from src/main/deploy/pathplanner/autos/centermove.auto rename to src/main/deploy/pathplanner/autos/Middle.auto index 0b097e3..8e4a60a 100644 --- a/src/main/deploy/pathplanner/autos/centermove.auto +++ b/src/main/deploy/pathplanner/autos/Middle.auto @@ -7,13 +7,19 @@ { "type": "path", "data": { - "pathName": "CenterTrussMove" + "pathName": "MiddleOne" + } + }, + { + "type": "named", + "data": { + "name": "L4" } } ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MiddleNet.auto b/src/main/deploy/pathplanner/autos/MiddleNet.auto new file mode 100644 index 0000000..d5c952b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MiddleNet.auto @@ -0,0 +1,74 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middlestart" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "middlemove" + } + }, + { + "type": "named", + "data": { + "name": "L2Algae" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "middletonet" + } + }, + { + "type": "named", + "data": { + "name": "AlgaeUp" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Barge" + } + }, + { + "type": "path", + "data": { + "pathName": "bargetoreef" + } + }, + { + "type": "named", + "data": { + "name": "L3Algae" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NearCenterTruss.auto b/src/main/deploy/pathplanner/autos/NearCenterTruss.auto deleted file mode 100644 index b27f2ac..0000000 --- a/src/main/deploy/pathplanner/autos/NearCenterTruss.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterTrussToSource" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceToReef" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefToSource" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceToReef2" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NearCenterTruss2.auto b/src/main/deploy/pathplanner/autos/NearCenterTruss2.auto deleted file mode 100644 index c280e82..0000000 --- a/src/main/deploy/pathplanner/autos/NearCenterTruss2.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterTrussToReefER" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefERToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefEL" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "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 deleted file mode 100644 index e51a5ee..0000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": null - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/middletour.auto b/src/main/deploy/pathplanner/autos/Old RightSide.auto similarity index 61% rename from src/main/deploy/pathplanner/autos/middletour.auto rename to src/main/deploy/pathplanner/autos/Old RightSide.auto index 89b07ad..decbbb1 100644 --- a/src/main/deploy/pathplanner/autos/middletour.auto +++ b/src/main/deploy/pathplanner/autos/Old RightSide.auto @@ -7,67 +7,67 @@ { "type": "path", "data": { - "pathName": "MiddleToReefCR" + "pathName": "First Test" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "ReefCRToSourceB" + "name": "L4" } }, { "type": "path", "data": { - "pathName": "SourceBToReefAL" + "pathName": "Second Test" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "ReefALToSourceR" + "name": "Intake" } }, { "type": "path", "data": { - "pathName": "SourceRToReefFR" + "pathName": "Third Test" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "ReefFRToSourceR" + "name": "L4" } }, { "type": "path", "data": { - "pathName": "SourceRToReefEL" + "pathName": "Fourth Test" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "ReefELToSourceR" + "name": "Intake" } }, { "type": "path", "data": { - "pathName": "SourceRToReefAR" + "pathName": "Fifth Test" } }, { "type": "named", "data": { - "name": "hello" + "name": "L4" } } ] } }, - "resetOdom": true, + "resetOdom": false, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RightSide.auto b/src/main/deploy/pathplanner/autos/RightSide.auto new file mode 100644 index 0000000..c29613b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RightSide.auto @@ -0,0 +1,139 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "First Test" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Second Test" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Third Test" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Fourth Test" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Fifth Test" + } + }, + { + "type": "named", + "data": { + "name": "AfterIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Auto (Simulation).auto b/src/main/deploy/pathplanner/autos/Test Auto (Simulation).auto deleted file mode 100644 index 7caa292..0000000 --- a/src/main/deploy/pathplanner/autos/Test Auto (Simulation).auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Example Path" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "New Path" - } - }, - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "path", - "data": { - "pathName": "L4" - } - }, - { - "type": "path", - "data": { - "pathName": null - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/edge2.auto b/src/main/deploy/pathplanner/autos/edge2.auto deleted file mode 100644 index 45cac44..0000000 --- a/src/main/deploy/pathplanner/autos/edge2.auto +++ /dev/null @@ -1,100 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "EdgeToReefCL" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "named", - "data": { - "name": "ScoreL4" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "ReefCLToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefBR" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "named", - "data": { - "name": "ScoreL4" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "ReefBRToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefAR" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "PIDtoNearest" - } - }, - { - "type": "named", - "data": { - "name": "ScoreL4" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/edgemove.auto b/src/main/deploy/pathplanner/autos/edgemove.auto deleted file mode 100644 index b9a251b..0000000 --- a/src/main/deploy/pathplanner/autos/edgemove.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "EdgeMove" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/edgetour.auto b/src/main/deploy/pathplanner/autos/edgetour.auto deleted file mode 100644 index e7ccc3d..0000000 --- a/src/main/deploy/pathplanner/autos/edgetour.auto +++ /dev/null @@ -1,133 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "EdgeToReefCR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefCRToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefCL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefCLToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefBL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefBLToSourceB" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceBToReefAR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefARToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefAL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefALToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefFR" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefFRToSouceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefFL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefFLToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefEL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefELToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefEL" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefELToSourceR" - } - }, - { - "type": "path", - "data": { - "pathName": "SourceRToReefER" - } - }, - { - "type": "path", - "data": { - "pathName": "ReefERToSourceR" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/middlemove.auto b/src/main/deploy/pathplanner/autos/middlemove.auto deleted file mode 100644 index 600be2c..0000000 --- a/src/main/deploy/pathplanner/autos/middlemove.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "MiddleMove" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterTrussMove.path b/src/main/deploy/pathplanner/paths/CenterTrussMove.path deleted file mode 100644 index 2efe92c..0000000 --- a/src/main/deploy/pathplanner/paths/CenterTrussMove.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.53675, - "y": 5.0584999999999996 - }, - "prevControl": null, - "nextControl": { - "x": 7.120696721311476, - "y": 5.019979508196721 - }, - "isLocked": false, - "linkedName": "CenterTruss" - }, - { - "anchor": { - "x": 6.605225409836065, - "y": 5.0584999999999996 - }, - "prevControl": { - "x": 7.432377049180327, - "y": 5.05594262295082 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterTrussToSource.path b/src/main/deploy/pathplanner/paths/CenterTrussToSource.path deleted file mode 100644 index 770cc7a..0000000 --- a/src/main/deploy/pathplanner/paths/CenterTrussToSource.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.516290983606557, - "y": 4.984016393442623 - }, - "prevControl": null, - "nextControl": { - "x": 6.617213114754098, - "y": 5.295696721311476 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.7861680327868852, - "y": 7.03391393442623 - }, - "prevControl": { - "x": 2.505430327868852, - "y": 6.362602459016394 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ex1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -51.48307369289725 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeMove.path b/src/main/deploy/pathplanner/paths/EdgeMove.path deleted file mode 100644 index b45499d..0000000 --- a/src/main/deploy/pathplanner/paths/EdgeMove.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.595250000000001, - "y": 7.25225 - }, - "prevControl": null, - "nextControl": { - "x": 6.192276987840646, - "y": 7.251727329722346 - }, - "isLocked": false, - "linkedName": "Edge" - }, - { - "anchor": { - "x": 5.897950819672132, - "y": 7.25225 - }, - "prevControl": { - "x": 7.2699005723947545, - "y": 7.2338036884881465 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -179.70897632112784 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeToReefDL.path b/src/main/deploy/pathplanner/paths/EdgeToReefDL.path deleted file mode 100644 index 3ce9a60..0000000 --- a/src/main/deploy/pathplanner/paths/EdgeToReefDL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.595250000000001, - "y": 7.25225 - }, - "prevControl": null, - "nextControl": { - "x": 7.991290801914323, - "y": 5.379697812362297 - }, - "isLocked": false, - "linkedName": "Edge" - }, - { - "anchor": { - "x": 6.221618852459016, - "y": 4.288729508196721 - }, - "prevControl": { - "x": 6.750602379014676, - "y": 5.379215035375417 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index b6074a8..0000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.60875, - "y": 6.8915 - }, - "prevControl": null, - "nextControl": { - "x": 1.8743422906562406, - "y": 6.845967803712558 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.6802254098360656, - "y": 5.343647540983607 - }, - "prevControl": { - "x": 1.1203436468772154, - "y": 5.79078396275844 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ep1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 118.44292862436343 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -155.6375469846878 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fifth Test.path b/src/main/deploy/pathplanner/paths/Fifth Test.path new file mode 100644 index 0000000..333b6d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fifth Test.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.212, + "y": 0.91 + }, + "prevControl": null, + "nextControl": { + "x": 1.975866802268245, + "y": 1.5835623150887574 + }, + "isLocked": false, + "linkedName": "balls" + }, + { + "anchor": { + "x": 3.744, + "y": 3.044 + }, + "prevControl": { + "x": 2.5493882365631166, + "y": 1.988008105276134 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.7214285714285711, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 145.85153165654307 + }, + "reversed": false, + "folder": "RightSide", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0079798014414 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/First Test.path b/src/main/deploy/pathplanner/paths/First Test.path new file mode 100644 index 0000000..5dda7c8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/First Test.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.896, + "y": 1.8521102995562126 + }, + "prevControl": null, + "nextControl": { + "x": 5.9438334566074955, + "y": 2.27752134183925 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 2.92 + }, + "prevControl": { + "x": 5.877305612056213, + "y": 2.521506641395464 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Nuts" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.26190476190476186, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0 + }, + "reversed": false, + "folder": "RightSide", + "idealStartingState": { + "velocity": 0, + "rotation": -144.38773779361557 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleToReefCR.path b/src/main/deploy/pathplanner/paths/Fourth Test.path similarity index 59% rename from src/main/deploy/pathplanner/paths/MiddleToReefCR.path rename to src/main/deploy/pathplanner/paths/Fourth Test.path index 088ed83..2b49563 100644 --- a/src/main/deploy/pathplanner/paths/MiddleToReefCR.path +++ b/src/main/deploy/pathplanner/paths/Fourth Test.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.566, - "y": 6.17975 + "x": 4.071960616438356, + "y": 2.8529965753424653 }, "prevControl": null, "nextControl": { - "x": 6.579700584987288, - "y": 5.806952241681442 + "x": 3.08320090859812, + "y": 1.8546234090456206 }, "isLocked": false, - "linkedName": "MiddleStart" + "linkedName": "third" }, { "anchor": { - "x": 5.46975, - "y": 4.922 + "x": 1.212, + "y": 0.91 }, "prevControl": { - "x": 5.71019443267824, - "y": 4.990457832232982 + "x": 2.30773353135748, + "y": 1.5426220492893399 }, "nextControl": null, "isLocked": false, - "linkedName": "ReefCR" + "linkedName": "balls" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 58.172553423326704 + "rotation": 145.0079798014414 }, "reversed": false, - "folder": "ComponentPaths", + "folder": "RightSide", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 150.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/MiddleOne.path similarity index 66% rename from src/main/deploy/pathplanner/paths/New New New Path.path rename to src/main/deploy/pathplanner/paths/MiddleOne.path index e1ab9b1..eb16b00 100644 --- a/src/main/deploy/pathplanner/paths/New New New Path.path +++ b/src/main/deploy/pathplanner/paths/MiddleOne.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 7.05, + "y": 3.91 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 6.389948058621888, + "y": 3.91 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.790250000000001, - "y": 4.960999999999999 + "x": 5.68, + "y": 3.91 }, "prevControl": { - "x": 6.790250000000001, - "y": 4.960999999999999 + "x": 6.015450938895257, + "y": 3.91 }, "nextControl": null, "isLocked": false, @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "reversed": false, - "folder": "ComponentPaths", + "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 117bf66..794be5c 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6802254098360656, - "y": 5.343647540983607 + "x": 2.0, + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 4.680225409836065, - "y": 5.343647540983607 + "x": 3.0, + "y": 7.0 }, "isLocked": false, - "linkedName": "ep1" + "linkedName": null }, { "anchor": { - "x": 1.8580942622950818, - "y": 6.95 + "x": 8.835102739726027, + "y": 6.74464897260274 }, "prevControl": { - "x": 3.0688524590163935, - "y": 7.129815573770491 + "x": 7.835102739726027, + "y": 6.74464897260274 }, "nextControl": null, "isLocked": false, @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -52.125016348901745 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 118.44292862436343 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefALToSourceB.path b/src/main/deploy/pathplanner/paths/ReefALToSourceB.path deleted file mode 100644 index 6db7fa1..0000000 --- a/src/main/deploy/pathplanner/paths/ReefALToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": null, - "nextControl": { - "x": 2.7963988482503943, - "y": 4.814238006000046 - }, - "isLocked": false, - "linkedName": "ReefAL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.376469896774239, - "y": 5.438651880522935 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefALToSourceR.path b/src/main/deploy/pathplanner/paths/ReefALToSourceR.path deleted file mode 100644 index d5ee177..0000000 --- a/src/main/deploy/pathplanner/paths/ReefALToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": null, - "nextControl": { - "x": 2.9054999999999995, - "y": 3.3522499999999997 - }, - "isLocked": false, - "linkedName": "ReefAL" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 1.364999999997346, - "y": 3.556999999994158 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefARToSourceB.path b/src/main/deploy/pathplanner/paths/ReefARToSourceB.path deleted file mode 100644 index 973d33e..0000000 --- a/src/main/deploy/pathplanner/paths/ReefARToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": null, - "nextControl": { - "x": 4.003, - "y": 4.366249999999999 - }, - "isLocked": false, - "linkedName": "ReefAR" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 0.4722500000000003, - "y": 7.07675 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefARToSourceR.path b/src/main/deploy/pathplanner/paths/ReefARToSourceR.path deleted file mode 100644 index db29e1c..0000000 --- a/src/main/deploy/pathplanner/paths/ReefARToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": null, - "nextControl": { - "x": 2.7637177827401294, - "y": 3.528590662447068 - }, - "isLocked": false, - "linkedName": "ReefAR" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.242802121627151, - "y": 2.596402623347232 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefBLToSourceB.path b/src/main/deploy/pathplanner/paths/ReefBLToSourceB.path deleted file mode 100644 index 4b71cf0..0000000 --- a/src/main/deploy/pathplanner/paths/ReefBLToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.50025, - "y": 5.0584999999999996 - }, - "prevControl": null, - "nextControl": { - "x": 3.071038680859481, - "y": 5.479570082428542 - }, - "isLocked": false, - "linkedName": "ReefBL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.4611942558252036, - "y": 6.10867838534708 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -58.49573328079569 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefBRToSourceB.path b/src/main/deploy/pathplanner/paths/ReefBRToSourceB.path deleted file mode 100644 index 6b44e56..0000000 --- a/src/main/deploy/pathplanner/paths/ReefBRToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.8600409836065577, - "y": 5.475512295081967 - }, - "prevControl": null, - "nextControl": { - "x": 2.3419037208153863, - "y": 5.603040389952021 - }, - "isLocked": false, - "linkedName": "ReefBR" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.6688446379516346, - "y": 5.521446137788735 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 122.85572195043298 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefCLToSourceB.path b/src/main/deploy/pathplanner/paths/ReefCLToSourceB.path deleted file mode 100644 index 5ba7994..0000000 --- a/src/main/deploy/pathplanner/paths/ReefCLToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.9822500000000005, - "y": 5.41925 - }, - "prevControl": null, - "nextControl": { - "x": 3.7245000000000004, - "y": 5.858 - }, - "isLocked": false, - "linkedName": "ReefCL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.301, - "y": 6.735499999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 57.680383491819796 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefCRToSourceB.path b/src/main/deploy/pathplanner/paths/ReefCRToSourceB.path deleted file mode 100644 index 3e52ee6..0000000 --- a/src/main/deploy/pathplanner/paths/ReefCRToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.46975, - "y": 4.922 - }, - "prevControl": null, - "nextControl": { - "x": 4.5142500000000005, - "y": 5.94575 - }, - "isLocked": false, - "linkedName": "ReefCR" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 1.9305, - "y": 6.60875 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 58.172553423326704 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefELToSourceR.path b/src/main/deploy/pathplanner/paths/ReefELToSourceR.path deleted file mode 100644 index 670a576..0000000 --- a/src/main/deploy/pathplanner/paths/ReefELToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.92375, - "y": 2.61125 - }, - "prevControl": null, - "nextControl": { - "x": 3.4840908373840813, - "y": 2.328051729177785 - }, - "isLocked": false, - "linkedName": "ReefEL" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.0263976800287327, - "y": 1.4771932976546012 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -60.52411099675415 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefERToSourceB.path b/src/main/deploy/pathplanner/paths/ReefERToSourceB.path deleted file mode 100644 index c869ef4..0000000 --- a/src/main/deploy/pathplanner/paths/ReefERToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 4.894500000000001, - "y": 1.85075 - }, - "isLocked": false, - "linkedName": "ReefER" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 2.5545000000000004, - "y": 1.0415 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -57.8042660652869 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefERToSourceR.path b/src/main/deploy/pathplanner/paths/ReefERToSourceR.path deleted file mode 100644 index 30e2c34..0000000 --- a/src/main/deploy/pathplanner/paths/ReefERToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 5.21625, - "y": 2.4259999999999993 - }, - "isLocked": false, - "linkedName": "ReefER" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.2035, - "y": 1.694749999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -57.8042660652869 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFLToSourceB.path b/src/main/deploy/pathplanner/paths/ReefFLToSourceB.path deleted file mode 100644 index 9b4285c..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFLToSourceB.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 2.807999999999999, - "y": 3.19625 - }, - "isLocked": false, - "linkedName": "ReefFL" - }, - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": { - "x": 1.7945649676983555, - "y": 6.61128607889577 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceB" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFLToSourceR.path b/src/main/deploy/pathplanner/paths/ReefFLToSourceR.path deleted file mode 100644 index e2eff28..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFLToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": null, - "nextControl": { - "x": 4.92925, - "y": 2.6014999999999997 - }, - "isLocked": false, - "linkedName": "ReefFL" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 0.20899999999999963, - "y": 1.1389999999999991 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFRToSouceR.path b/src/main/deploy/pathplanner/paths/ReefFRToSouceR.path deleted file mode 100644 index e48756e..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFRToSouceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.3735000000000004, - "y": 2.9817499999999995 - }, - "prevControl": null, - "nextControl": { - "x": 2.6908909470734246, - "y": 2.43103298536486 - }, - "isLocked": false, - "linkedName": "ReefFR" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 2.216165124821639, - "y": 1.9947836705032231 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 59.036243467926546 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefFRToSourceR.path b/src/main/deploy/pathplanner/paths/ReefFRToSourceR.path deleted file mode 100644 index 1e6e819..0000000 --- a/src/main/deploy/pathplanner/paths/ReefFRToSourceR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.3735000000000004, - "y": 2.9817499999999995 - }, - "prevControl": null, - "nextControl": { - "x": 2.52525, - "y": 3.332749999999999 - }, - "isLocked": false, - "linkedName": "ReefFR" - }, - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": { - "x": 0.9457499999999999, - "y": 1.938499999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "SourceR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 59.036243467926546 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeToReefCR.path b/src/main/deploy/pathplanner/paths/Second Test.path similarity index 60% rename from src/main/deploy/pathplanner/paths/EdgeToReefCR.path rename to src/main/deploy/pathplanner/paths/Second Test.path index 41c6f49..7a0814a 100644 --- a/src/main/deploy/pathplanner/paths/EdgeToReefCR.path +++ b/src/main/deploy/pathplanner/paths/Second Test.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.595250000000001, - "y": 7.25225 + "x": 5.0, + "y": 2.91 }, "prevControl": null, "nextControl": { - "x": 5.9475, - "y": 6.80375 + "x": 4.135470984344181, + "y": 1.7873341192061143 }, "isLocked": false, - "linkedName": "Edge" + "linkedName": "Nuts" }, { "anchor": { - "x": 5.46975, - "y": 4.922 + "x": 1.212, + "y": 0.91 }, "prevControl": { - "x": 7.02725, - "y": 5.764749999999999 + "x": 2.7055593102810627, + "y": 1.5338824426775153 }, "nextControl": null, "isLocked": false, - "linkedName": "ReefCR" + "linkedName": "balls" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 58.172553423326704 + "rotation": 145.0 }, "reversed": false, - "folder": "ComponentPaths", + "folder": "RightSide", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": -150.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefAL.path b/src/main/deploy/pathplanner/paths/SourceBToReefAL.path deleted file mode 100644 index 396fba0..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefAL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.4722500000000003, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": { - "x": 1.9835000000000003, - "y": 3.6447499999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefAR.path b/src/main/deploy/pathplanner/paths/SourceBToReefAR.path deleted file mode 100644 index afa41c8..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefAR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.2853469493415943, - "y": 6.363039664386715 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": { - "x": 2.8409604971839766, - "y": 4.5566264678922686 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefBL.path b/src/main/deploy/pathplanner/paths/SourceBToReefBL.path deleted file mode 100644 index 3ee988c..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefBL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 1.9967451896648978, - "y": 6.562537101862314 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.50025, - "y": 5.0584999999999996 - }, - "prevControl": { - "x": 3.1039096610105723, - "y": 5.437154570087065 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefBL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -58.49573328079569 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefBR.path b/src/main/deploy/pathplanner/paths/SourceBToReefBR.path deleted file mode 100644 index 6c87932..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefBR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 1.818380083850317, - "y": 6.640740137529289 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.8600409836065577, - "y": 5.475512295081967 - }, - "prevControl": { - "x": 2.0097226066154796, - "y": 5.915347603812001 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefBR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 122.85572195043298 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefCL.path b/src/main/deploy/pathplanner/paths/SourceBToReefCL.path deleted file mode 100644 index 6c17455..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefCL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.4722500000000003, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 4.9822500000000005, - "y": 5.41925 - }, - "prevControl": { - "x": 3.9822500000000005, - "y": 5.41925 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefCL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 57.680383491819796 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefCR.path b/src/main/deploy/pathplanner/paths/SourceBToReefCR.path deleted file mode 100644 index 0d1c9df..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefCR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.4722500000000003, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 5.46975, - "y": 4.922 - }, - "prevControl": { - "x": 4.795081967213115, - "y": 5.727254098360657 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefCR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 58.172553423326704 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceBToReefFL.path b/src/main/deploy/pathplanner/paths/SourceBToReefFL.path deleted file mode 100644 index c5ef2f5..0000000 --- a/src/main/deploy/pathplanner/paths/SourceBToReefFL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.4722500000000003, - "y": 7.07675 - }, - "prevControl": null, - "nextControl": { - "x": 2.47225, - "y": 7.07675 - }, - "isLocked": false, - "linkedName": "SourceB" - }, - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": { - "x": 2.92925, - "y": 2.6014999999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefFL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -49.969740728110416 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefAL.path b/src/main/deploy/pathplanner/paths/SourceRToReefAL.path deleted file mode 100644 index 6d56988..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefAL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 2.9835000000000003, - "y": 3.6447499999999997 - }, - "prevControl": { - "x": 1.9835000000000003, - "y": 3.6447499999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefAR.path b/src/main/deploy/pathplanner/paths/SourceRToReefAR.path deleted file mode 100644 index 1a9f0d9..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefAR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.366249999999999 - }, - "prevControl": { - "x": 2.0029999999999997, - "y": 4.366249999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefAR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefEL.path b/src/main/deploy/pathplanner/paths/SourceRToReefEL.path deleted file mode 100644 index 8c75b7d..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefEL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2090000000000005, - "y": 1.1389999999999998 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 4.92375, - "y": 2.61125 - }, - "prevControl": { - "x": 3.92375, - "y": 2.61125 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefEL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -60.52411099675415 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefER.path b/src/main/deploy/pathplanner/paths/SourceRToReefER.path deleted file mode 100644 index 78d9d43..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefER.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2096508560167436, - "y": 1.7839970668582361 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 - }, - "prevControl": { - "x": 4.70942146615192, - "y": 2.6005634886412072 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefER" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -57.8042660652869 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefFL.path b/src/main/deploy/pathplanner/paths/SourceRToReefFL.path deleted file mode 100644 index 7a8f0ba..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefFL.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 3.92925, - "y": 2.6014999999999997 - }, - "prevControl": { - "x": 2.92925, - "y": 2.6014999999999997 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefFL" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 55.71312302279096 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceRToReefFR.path b/src/main/deploy/pathplanner/paths/SourceRToReefFR.path deleted file mode 100644 index 1fc95d0..0000000 --- a/src/main/deploy/pathplanner/paths/SourceRToReefFR.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.2089999999999999, - "y": 1.1389999999999998 - }, - "prevControl": null, - "nextControl": { - "x": 2.2089999999999996, - "y": 1.1389999999999993 - }, - "isLocked": false, - "linkedName": "SourceR" - }, - { - "anchor": { - "x": 3.3735000000000004, - "y": 2.9817499999999995 - }, - "prevControl": { - "x": 2.3735000000000004, - "y": 2.9817499999999995 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ReefFR" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 59.036243467926546 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": 53.972626614896335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceToReef.path b/src/main/deploy/pathplanner/paths/SourceToReef.path deleted file mode 100644 index 3153cf6..0000000 --- a/src/main/deploy/pathplanner/paths/SourceToReef.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.7861680327868852, - "y": 7.03391393442623 - }, - "prevControl": null, - "nextControl": { - "x": 2.786168032786885, - "y": 7.03391393442623 - }, - "isLocked": false, - "linkedName": "ex1" - }, - { - "anchor": { - "x": 3.728176229508197, - "y": 5.18780737704918 - }, - "prevControl": { - "x": 2.728176229508197, - "y": 5.18780737704918 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ex2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.96375653207352 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -51.48307369289725 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceToReef2.path b/src/main/deploy/pathplanner/paths/SourceToReef2.path deleted file mode 100644 index b27fab6..0000000 --- a/src/main/deploy/pathplanner/paths/SourceToReef2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.7861680327868852, - "y": 7.03391393442623 - }, - "prevControl": null, - "nextControl": { - "x": 2.3136270491803277, - "y": 7.669262295081967 - }, - "isLocked": false, - "linkedName": "ex1" - }, - { - "anchor": { - "x": 3.728176229508197, - "y": 5.18780737704918 - }, - "prevControl": { - "x": 3.467163934426229, - "y": 5.784315573770492 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "ex2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 120.96375653207352 - }, - "reversed": false, - "folder": "ComponentPaths", - "idealStartingState": { - "velocity": 0, - "rotation": -51.48307369289725 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Third Test.path b/src/main/deploy/pathplanner/paths/Third Test.path new file mode 100644 index 0000000..5836d3d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Third Test.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.212, + "y": 0.91 + }, + "prevControl": null, + "nextControl": { + "x": 2.1899678254437878, + "y": 1.4052481662968441 + }, + "isLocked": false, + "linkedName": "balls" + }, + { + "anchor": { + "x": 4.071960616438356, + "y": 2.8529965753424653 + }, + "prevControl": { + "x": 3.0149602203648413, + "y": 1.9660679328429413 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "third" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.6452380952380952, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": "RightSide", + "idealStartingState": { + "velocity": 0, + "rotation": 145.0079798014414 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backtoSource.path b/src/main/deploy/pathplanner/paths/backtoSource.path deleted file mode 100644 index 9dac851..0000000 --- a/src/main/deploy/pathplanner/paths/backtoSource.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.03483606557377, - "y": 5.40358606557377 - }, - "prevControl": null, - "nextControl": { - "x": 6.03483606557377, - "y": 5.40358606557377 - }, - "isLocked": false, - "linkedName": "ep2" - }, - { - "anchor": { - "x": 2.0139344262295085, - "y": 6.973975409836065 - }, - "prevControl": { - "x": 1.0139344262295085, - "y": 6.973975409836065 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -37.87498365109819 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 61.38954033403478 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/backtoreef.path b/src/main/deploy/pathplanner/paths/backtoreef.path new file mode 100644 index 0000000..1c79460 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/backtoreef.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.247, + "y": 7.24 + }, + "prevControl": null, + "nextControl": { + "x": 1.847315525552532, + "y": 6.762834260559234 + }, + "isLocked": false, + "linkedName": "leftsource" + }, + { + "anchor": { + "x": 3.955027760709269, + "y": 5.125649359199437 + }, + "prevControl": { + "x": 3.5557756490487864, + "y": 5.4183140577166995 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TwoLeft" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.5775401069518717, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.999999999999996 + }, + "reversed": false, + "folder": "LeftSide", + "idealStartingState": { + "velocity": 0, + "rotation": 36.86989764584405 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleMove.path b/src/main/deploy/pathplanner/paths/bargetoreef.path similarity index 62% rename from src/main/deploy/pathplanner/paths/MiddleMove.path rename to src/main/deploy/pathplanner/paths/bargetoreef.path index 96185dd..5c9c465 100644 --- a/src/main/deploy/pathplanner/paths/MiddleMove.path +++ b/src/main/deploy/pathplanner/paths/bargetoreef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.566, - "y": 6.17975 + "x": 7.71, + "y": 5.5 }, "prevControl": null, "nextControl": { - "x": 6.58125, - "y": 6.158811475409837 + "x": 6.542763157894736, + "y": 5.516365131578947 }, "isLocked": false, - "linkedName": "MiddleStart" + "linkedName": "Barge" }, { "anchor": { - "x": 5.993852459016393, - "y": 6.17975 + "x": 5.13, + "y": 5.17 }, "prevControl": { - "x": 6.988831967213113, - "y": 6.182786885245902 + "x": 5.703026315789472, + "y": 5.555013157894736 }, "nextControl": null, "isLocked": false, @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -29.999999999999996 }, "reversed": false, - "folder": "ComponentPaths", + "folder": "MiddleAndNet", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ReefToSource.path b/src/main/deploy/pathplanner/paths/end.path similarity index 61% rename from src/main/deploy/pathplanner/paths/ReefToSource.path rename to src/main/deploy/pathplanner/paths/end.path index 0a33f0f..fe8bb6f 100644 --- a/src/main/deploy/pathplanner/paths/ReefToSource.path +++ b/src/main/deploy/pathplanner/paths/end.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.728176229508197, - "y": 5.18780737704918 + "x": 1.2471318493150685, + "y": 7.24 }, "prevControl": null, "nextControl": { - "x": 3.4356762295081973, - "y": 6.41630737704918 + "x": 1.933095665104542, + "y": 6.642267709084353 }, "isLocked": false, - "linkedName": "ex2" + "linkedName": "leftsource" }, { "anchor": { - "x": 1.64775, - "y": 7.0865 + "x": 3.37, + "y": 4.13 }, "prevControl": { - "x": 2.40825, - "y": 6.8135 + "x": 2.1965943877551024, + "y": 5.013832908163264 }, "nextControl": null, "isLocked": false, @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -53.13010235415593 + "rotation": 90.0 }, "reversed": false, - "folder": "ComponentPaths", + "folder": "LeftSide", "idealStartingState": { "velocity": 0, - "rotation": 120.96375653207352 + "rotation": 36.86989764584405 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/gotoreef.path b/src/main/deploy/pathplanner/paths/gotoreef.path new file mode 100644 index 0000000..506ebbf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/gotoreef.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.077097602739726, + "y": 6.038441780821918 + }, + "prevControl": null, + "nextControl": { + "x": 5.8472851027397255, + "y": 5.902947007930787 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.91, + "y": 5.19 + }, + "prevControl": { + "x": 5.512861842105263, + "y": 5.699555921052632 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ThreeRight" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "L4", + "waypointRelativePos": 0.72, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": "LeftSide", + "idealStartingState": { + "velocity": 0, + "rotation": -45.000000000000135 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middlemove.path b/src/main/deploy/pathplanner/paths/middlemove.path new file mode 100644 index 0000000..6d78d21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middlemove.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.675, + "y": 3.875 + }, + "prevControl": null, + "nextControl": { + "x": 5.675, + "y": 4.125 + }, + "isLocked": false, + "linkedName": "FourLeft" + }, + { + "anchor": { + "x": 5.81, + "y": 4.09 + }, + "prevControl": { + "x": 5.81, + "y": 3.84 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "FourBall" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "MiddleAndNet", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L4.path b/src/main/deploy/pathplanner/paths/middlestart.path similarity index 63% rename from src/main/deploy/pathplanner/paths/L4.path rename to src/main/deploy/pathplanner/paths/middlestart.path index 589685b..2507876 100644 --- a/src/main/deploy/pathplanner/paths/L4.path +++ b/src/main/deploy/pathplanner/paths/middlestart.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 7.05, + "y": 3.875 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 6.389948058621888, + "y": 3.875 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.03483606557377, - "y": 5.40358606557377 + "x": 5.675, + "y": 3.875 }, "prevControl": { - "x": 4.03483606557377, - "y": 5.40358606557377 + "x": 6.010450938895257, + "y": 3.875 }, "nextControl": null, "isLocked": false, - "linkedName": "ep2" + "linkedName": "FourLeft" } ], "rotationTargets": [], @@ -34,21 +34,21 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 61.38954033403478 + "rotation": -90.0 }, "reversed": false, - "folder": null, + "folder": "MiddleAndNet", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/middletonet.path b/src/main/deploy/pathplanner/paths/middletonet.path new file mode 100644 index 0000000..9391de8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/middletonet.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8, + "y": 4.09 + }, + "prevControl": null, + "nextControl": { + "x": 6.676198630136986, + "y": 5.066669520547945 + }, + "isLocked": false, + "linkedName": "FourBall" + }, + { + "anchor": { + "x": 7.71, + "y": 5.5 + }, + "prevControl": { + "x": 6.748356164383562, + "y": 4.959075342465755 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Barge" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 480.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "MiddleAndNet", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/EdgeToReefCL.path b/src/main/deploy/pathplanner/paths/thentosource.path similarity index 59% rename from src/main/deploy/pathplanner/paths/EdgeToReefCL.path rename to src/main/deploy/pathplanner/paths/thentosource.path index 4353444..b983fc5 100644 --- a/src/main/deploy/pathplanner/paths/EdgeToReefCL.path +++ b/src/main/deploy/pathplanner/paths/thentosource.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.595250000000001, - "y": 7.25225 + "x": 4.91, + "y": 5.19 }, "prevControl": null, "nextControl": { - "x": 6.5325, - "y": 6.7452499999999995 + "x": 4.232166095890411, + "y": 6.033467465753425 }, "isLocked": false, - "linkedName": "Edge" + "linkedName": "ThreeRight" }, { "anchor": { - "x": 4.9822500000000005, - "y": 5.41925 + "x": 1.2471318493150685, + "y": 7.24 }, "prevControl": { - "x": 5.46975, - "y": 6.140750000000001 + "x": 2.7497003424657525, + "y": 6.098047945205479 }, "nextControl": null, "isLocked": false, - "linkedName": "ReefCL" + "linkedName": "leftsource" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 57.680383491819796 + "rotation": 36.87 }, "reversed": false, - "folder": "ComponentPaths", + "folder": "LeftSide", "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": -29.999999999999996 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterTrussToReefER.path b/src/main/deploy/pathplanner/paths/tosource2.path similarity index 57% rename from src/main/deploy/pathplanner/paths/CenterTrussToReefER.path rename to src/main/deploy/pathplanner/paths/tosource2.path index 881c488..57b067a 100644 --- a/src/main/deploy/pathplanner/paths/CenterTrussToReefER.path +++ b/src/main/deploy/pathplanner/paths/tosource2.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.53675, - "y": 5.0584999999999996 + "x": 3.955027760709269, + "y": 5.125649359199437 }, "prevControl": null, "nextControl": { - "x": 7.02, - "y": 4.5417499999999995 + "x": 3.0753959113942013, + "y": 6.154245249610397 }, "isLocked": false, - "linkedName": "CenterTruss" + "linkedName": "TwoLeft" }, { "anchor": { - "x": 5.5185, - "y": 2.8939999999999997 + "x": 1.2471318493150685, + "y": 7.24 }, "prevControl": { - "x": 6.015749999999996, - "y": 3.313249999999999 + "x": 2.4642123287671236, + "y": 5.902714041095891 }, "nextControl": null, "isLocked": false, - "linkedName": "ReefER" + "linkedName": "leftsource" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 4.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 480.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -57.8042660652869 + "rotation": 36.86989764584405 }, "reversed": false, - "folder": "ComponentPaths", + "folder": "LeftSide", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 29.999999999999996 }, "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 344420b..e20e2f0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,36 +1,36 @@ { - "robotWidth": 1.0, - "robotLength": 1.0, + "robotWidth": 0.736, + "robotLength": 0.736, "holonomicMode": true, "pathFolders": [ - "ComponentPaths" + "MiddleAndNet", + "LeftSide", + "RightSide" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 2.0, + "defaultMaxVel": 3, + "defaultMaxAccel": 2.4, "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxAngAccel": 480.0, "defaultNominalVoltage": 12.0, - "robotMass": 110.0, + "robotMass": 54.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.044, + "driveWheelRadius": 0.0406, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", - "driveCurrentLimit": 40.0, + "driveCurrentLimit": 120.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, + "flModuleX": 0.254, + "flModuleY": 0.254, + "frModuleX": 0.254, + "frModuleY": -0.254, + "blModuleX": -0.254, + "blModuleY": 0.254, + "brModuleX": -0.254, + "brModuleY": -0.254, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [ - "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.3,\"y\":0.0},\"size\":{\"width\":0.4,\"length\":0.05},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" - ] + "robotFeatures": [] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/lib/Auto.java similarity index 69% rename from src/main/java/frc/robot/Auto.java rename to src/main/java/frc/lib/Auto.java index a5c8c5e..a1f78f8 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/lib/Auto.java @@ -1,4 +1,4 @@ -package frc.robot; +package frc.team696.lib; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; @@ -12,9 +12,6 @@ import java.util.Optional; import java.util.function.Supplier; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -30,7 +27,6 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -38,12 +34,13 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.subsystems.Swerve; import frc.team696.lib.Dashboards.ShuffleDashboard; import frc.team696.lib.Logging.BackupLogger; import frc.team696.lib.Logging.PLog; import frc.team696.lib.Swerve.SwerveConfigs; import frc.team696.lib.Swerve.SwerveConstants; +import frc.team696.lib.Swerve.SwerveDriveSubsystem; +import frc.team696.lib.Swerve.SwerveModule; /** * Houses all methods related to the Autonomous period and self driving during teleop @@ -72,16 +69,15 @@ public void register() { } public static Auto _instance; - private Swerve _swerve; + private SwerveDriveSubsystem _swerve; private SendableChooser _autoChooser; public SysIdRoutine _driveSysIdRoutine; - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds().withSteerRequestType(SteerRequestType.MotionMagicExpo).withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private Auto (Swerve swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister) { + private Auto (SwerveDriveSubsystem swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister) { _swerve = swerve; - + RobotConfig config = new RobotConfig( SwerveConstants.MASS, SwerveConstants.MOMENT_OF_INERTIA, @@ -101,19 +97,23 @@ private Auto (Swerve swerve, boolean shouldUseGUIValues, NamedCommand... command new Alert("Failed to fetch robotConfig GUI Settings", AlertType.kWarning).set(true); } } - + AutoBuilder.configure( - ()->_swerve.getState().Pose, + _swerve::getPose, _swerve::resetPose, - ()->_swerve.getState().Speeds, - (speeds, feedforwards) -> m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()), + _swerve::getRobotRelativeSpeeds, + (speeds, feedforwards) -> _swerve.Drive(speeds), new PPHolonomicDriveController( - new PIDConstants(10.0, 0.0, 0.0), - new PIDConstants(7.0, 0.0, 0.0)), + new PIDConstants(5.0, 0.0, 0.0), + new PIDConstants(5.0, 0.0, 0.0)), config, - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + () -> { + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, _swerve ); @@ -130,20 +130,54 @@ private Auto (Swerve swerve, boolean shouldUseGUIValues, NamedCommand... command }); _autoChooser = AutoBuilder.buildAutoChooser(); - //SmartDashboard.putData(_autoChooser); ShuffleDashboard.addAutoChooser(_autoChooser); _autoChooser.onChange((command)-> { - //visualize(); - }); + visualize(); + }); + + /* + * Default SysIdRoutine + * + * Copy in your subsystem to add more. + */ + _driveSysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config( + Volts.per(Second).of(1), + Volts.of(3), + Seconds.of(10)), + new SysIdRoutine.Mechanism(_swerve::voltageDriveForward, log -> { + for (int i = 0; i < SwerveModule.s_moduleCount; i++) { + log.motor(SwerveModule.moduleNames[i]) + .voltage(_swerve.getModules()[i].getDriveOutputVoltage()) + .linearPosition(Meters.of(SwerveConstants.DISTANCE_PER_ROTATION.timesDivisor(_swerve.getModules()[i].getDriveMotorPosition()).magnitude())) + .linearVelocity(MetersPerSecond.of(SwerveConstants.DISTANCE_PER_ROTATION.times(_swerve.getModules()[i].getState().speedMetersPerSecond).magnitude())); + } + }, _swerve)); } + + public static Supplier generateSysIDAutos( SysIdRoutine routine ) { + SendableChooser _sysIdChooser = new SendableChooser<>(); + _sysIdChooser.setDefaultOption("None", Commands.none()); + + _sysIdChooser.addOption("Quasistatic Forwards", routine.quasistatic(SysIdRoutine.Direction.kForward)); + _sysIdChooser.addOption("Quasistatic Backwards", routine.quasistatic(SysIdRoutine.Direction.kReverse)); + + _sysIdChooser.addOption("Dynamic Forwards", routine.dynamic(SysIdRoutine.Direction.kForward)); + _sysIdChooser.addOption("Dynamic Backwards", routine.dynamic(SysIdRoutine.Direction.kReverse)); + + SmartDashboard.putData("SysID Test Chooser",_sysIdChooser); + + return _sysIdChooser::getSelected; + } + /** * Initializes all things related to auto (ex. pathplanner) * * @param swerve The swerve subsystem * @param commandsToRegister each command to register for path planner */ - public static void Initialize(Swerve swerve, NamedCommand... commandsToRegister){ + public static void Initialize(SwerveDriveSubsystem swerve, NamedCommand... commandsToRegister){ if (_instance != null) throw new RuntimeException ("Don't Initialize Twice!"); _instance = new Auto(swerve, false, commandsToRegister); @@ -156,7 +190,7 @@ public static void Initialize(Swerve swerve, NamedCommand... commandsToRegister) * @param shouldUseGUIValues Should Pathplanner fetch Robot Config from GUI? * @param commandsToRegister each command to register for path planner */ - public static void Initialize(Swerve swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister){ + public static void Initialize(SwerveDriveSubsystem swerve, boolean shouldUseGUIValues, NamedCommand... commandsToRegister){ if (_instance != null) throw new RuntimeException ("Don't Initialize Twice!"); _instance = new Auto(swerve, shouldUseGUIValues, commandsToRegister); diff --git a/src/main/java/frc/lib/Camera/BaseCam.java b/src/main/java/frc/lib/Camera/BaseCam.java new file mode 100644 index 0000000..4239e5e --- /dev/null +++ b/src/main/java/frc/lib/Camera/BaseCam.java @@ -0,0 +1,91 @@ +package frc.team696.lib.Camera; + +import java.util.Optional; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N3; +import frc.team696.lib.Logging.BackupLogger; +import frc.team696.lib.Logging.PLog; + +/** + * Base Camera Object for handling how cameras will generally be handled + * + *

+ * Extend to add functionality for a specific camera + * + * @see LimeLightCam.java + * @see PhotonVisionCam.java + */ +public abstract class BaseCam { + public class AprilTagResult { + public Pose2d pose; + /* + * Time Should Be In Current Time, Same as Phoenix6 Swerve + */ + public double time; + + public double distToTag; + public int tagCount; + + public double ambiguity; + + public AprilTagResult(Pose2d pose, double time, double distToTag, int tagCount, double ambiguity) { + this.pose = pose; + this.time = time; + this.distToTag = distToTag; + this.tagCount = tagCount; + this.ambiguity = ambiguity; + } + } + + public abstract Optional getEstimate(); + + Vector stdDeviations = VecBuilder.fill(0.7, 0.7, 2); + + public void setStdDeviations(double x, double y, double r) { + stdDeviations = VecBuilder.fill(x, y, r); + } + + @FunctionalInterface + public static interface addVisionEstimate { + void accept(Pose2d p, double d, Vector v); + } + + @FunctionalInterface + public static interface acceptEstimate { + boolean test(AprilTagResult latestResult); + } + + // eventually switch this to taking in a addVisionEstimate + public boolean addVisionEstimate(addVisionEstimate addVisionMeasurement, acceptEstimate checkEstimation) { + Optional oEstimation = this.getEstimate(); + + if (oEstimation.isPresent()) { + AprilTagResult estimation = oEstimation.get(); + try { + if (!checkEstimation.test(estimation)) { + BackupLogger.addToQueue("696/Vision/Rejected Pose", estimation.pose); + return false; + } else { + BackupLogger.addToQueue("696/Vision/Accepted Pose", estimation.pose); + } + } catch (Exception e) { + PLog.fatalException("Camera", e.getMessage(), e); + } + addVisionMeasurement.accept( + estimation.pose, + estimation.time, + stdDeviations); + return true; + } + return false; + } + + public synchronized boolean addVisionEstimate(addVisionEstimate addVisionMeasurement) { + return addVisionEstimate(addVisionMeasurement, (latestResult) -> { + return true; + }); + } +} diff --git a/src/main/java/frc/lib/Camera/LimeLightCam.java b/src/main/java/frc/lib/Camera/LimeLightCam.java new file mode 100644 index 0000000..64ffa87 --- /dev/null +++ b/src/main/java/frc/lib/Camera/LimeLightCam.java @@ -0,0 +1,93 @@ +package frc.team696.lib.Camera; + +import java.util.Optional; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * LimeLight Camera Interface + */ +public class LimeLightCam extends BaseCam { + public String name = ""; + public static int LimeLightCount = 0; + + private NetworkTable _ntTable; + + private boolean useMegaTag2 = false; + + public LimeLightCam(String name, int[] TagsToCheck, boolean useMegaTag2) { + this.name = name; + + if (TagsToCheck.length > 0) { + LimelightHelpers.SetFiducialIDFiltersOverride(name, TagsToCheck); + } + + for (int port = 5800; port <= 5809; port++) { + PortForwarder.add(port + 10 * LimeLightCount, String.format("%s.local", this.name), port); + } + + _ntTable = NetworkTableInstance.getDefault().getTable(name); + + _ntTable.getEntry("ledMode").setNumber(1); // Example of how to use -> This turns off LEDS on the front + + this.useMegaTag2 = useMegaTag2; + + LimeLightCount++; + } + + public LimeLightCam(String name) { + this(name, new int[] {}, false); + } + + public LimeLightCam(String name, boolean useMegaTag2) { + this(name, new int[] {}, useMegaTag2); + } + + public int targetCount() { + double[] t2d = _ntTable.getEntry("t2d").getDoubleArray(new double[0]); + if (t2d.length == 17) { + return (int) t2d[1]; + } + return 0; + } + + public boolean hasTargets() { + return targetCount() > 0; + } + + public double tX() { + return -1 * _ntTable.getEntry("tx").getDouble(0); + } + + public void SetRobotOrientation(Rotation2d curYaw) { + LimelightHelpers.SetRobotOrientation(name, curYaw.getDegrees(), 0, 0, 0, 0, 0); + } + + public Optional getEstimate() { + LimelightHelpers.PoseEstimate latestEstimate; + if (!useMegaTag2 || DriverStation.isDisabled()) { + latestEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue(name); + } else { + latestEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name); + } + + if (latestEstimate == null) + return Optional.empty(); + + if (latestEstimate.tagCount == 0) + return Optional.empty(); + + return Optional.of( + new AprilTagResult(latestEstimate.pose, + Utils.fpgaToCurrentTime(latestEstimate.timestampSeconds), + latestEstimate.avgTagDist, + latestEstimate.tagCount, + latestEstimate.rawFiducials[0].ambiguity)); // Probably not the best but good enough for now + } +} diff --git a/src/main/java/frc/lib/Camera/LimelightHelpers.java b/src/main/java/frc/lib/Camera/LimelightHelpers.java new file mode 100644 index 0000000..dd65ec5 --- /dev/null +++ b/src/main/java/frc/lib/Camera/LimelightHelpers.java @@ -0,0 +1,1268 @@ +//LimelightHelpers v1.9 (REQUIRES 2024.9.1) + +package frc.team696.lib.Camera; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * Default Library Supplied By LimeLight Themselves + * + *

Potentially will be removed in the future. + */ +@SuppressWarnings("unused") +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + /** + * Makes a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles. + * + * @param pose The Pose3d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles. + * + * @param pose The Pose2d object to convert. + * @return The array of doubles representing the pose. + **/ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 11; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/Camera/PhotonVisionCam.java b/src/main/java/frc/lib/Camera/PhotonVisionCam.java new file mode 100644 index 0000000..d794f96 --- /dev/null +++ b/src/main/java/frc/lib/Camera/PhotonVisionCam.java @@ -0,0 +1,77 @@ +package frc.team696.lib.Camera; +/* +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.net.PortForwarder; +import frc.team696.lib.Logging.PLog; + +/** + * PhotonVision Camera Interface + * */ +/* +public class PhotonVisionCam extends BaseCam { + + public String _name; + private Transform3d _position; + + private PhotonCamera _camera; + private PhotonPoseEstimator _estimator; + + public static AprilTagFieldLayout _aprilTagFieldLayout; + + public PhotonVisionCam(String name, Transform3d position) { + this._name = name; + this._position = position; + + _camera = new PhotonCamera(_name); + _estimator = new PhotonPoseEstimator( + _aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + _camera, + _position + ); + _estimator.setMultiTagFallbackStrategy( + PoseStrategy.LOWEST_AMBIGUITY + ); + } + + public Optional getEstimate() { + Optional oLatestEstimate = _estimator.update(); + + if (oLatestEstimate.isEmpty()) return Optional.empty(); + + EstimatedRobotPose latestEstimate = oLatestEstimate.get(); + + return Optional.of( + new AprilTagResult(latestEstimate.estimatedPose.toPose2d(), + latestEstimate.timestampSeconds, + latestEstimate.targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm(), + latestEstimate.targetsUsed.size(), + latestEstimate.targetsUsed.get(0).getPoseAmbiguity())); + } + + static { + try { + _aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + } catch (Exception e) { + PLog.fatalException( + "PhotonVisionCam", + "Failed to load april tag layout.", + e + ); + } + + PortForwarder.add(5800, "photonvision.local", 5800); + + PhotonCamera.setVersionCheckEnabled(false); + } +} +*/ \ No newline at end of file diff --git a/src/main/java/frc/lib/Dashboards/ShuffleDashboard.java b/src/main/java/frc/lib/Dashboards/ShuffleDashboard.java new file mode 100644 index 0000000..1d0d6c8 --- /dev/null +++ b/src/main/java/frc/lib/Dashboards/ShuffleDashboard.java @@ -0,0 +1,35 @@ +package frc.team696.lib.Dashboards; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; + +public class ShuffleDashboard { + private static ShuffleboardTab _mainTab; + + public static final Field2d field = new Field2d(); + + static { + _mainTab = Shuffleboard.getTab("Telemetry"); + + _mainTab.add(field).withPosition(0, 0).withSize(7, 6); + } + + public static ComplexWidget addObject(Sendable object) { + return _mainTab.add(object); + } + + public static ComplexWidget addObject(String name, Sendable object) { + return _mainTab.add(name, object); + } + + public static ShuffleboardTab Tab() { + return _mainTab; + } + + public static void addAutoChooser(Sendable chooser) { + _mainTab.add("Autos", chooser).withPosition(7, 0).withSize(2,1); + } +} diff --git a/src/main/java/frc/lib/Datatypes/InterpolatingTable.java b/src/main/java/frc/lib/Datatypes/InterpolatingTable.java new file mode 100644 index 0000000..942532d --- /dev/null +++ b/src/main/java/frc/lib/Datatypes/InterpolatingTable.java @@ -0,0 +1,39 @@ +package frc.team696.lib.Datatypes; + +import java.util.Map; +import java.util.TreeMap; + +import edu.wpi.first.math.interpolation.Interpolatable; +import frc.team696.lib.Util; + +/** + * Class For creating a table to interpolate between values automatically + * + *

To use must create your custom class that implements Interpolatable to handle how the table will interpolate between nearest datapoints. + */ +public class InterpolatingTable> { + public final TreeMap map = new TreeMap(); + + @SafeVarargs + public InterpolatingTable(Map.Entry... entries) { + for (Map.Entry entry : entries) { + map.putIfAbsent(entry.getKey(), entry.getValue()); + } + } + + /** + * + * @param key Gets the value from the map at this point + * @return Interpolated Value + */ + public T getValue(double key) { + double index = Util.clamp(key, map.firstKey() + 0.0000000001, map.lastKey() - 0.0000000001); + Map.Entry lower = map.floorEntry(index); + Map.Entry higher = map.ceilingEntry(index); + + double t = (index - lower.getKey()) / (higher.getKey() - lower.getKey()); + + return lower.getValue().interpolate(higher.getValue(), t); + } + +} diff --git a/src/main/java/frc/lib/Field.java b/src/main/java/frc/lib/Field.java new file mode 100644 index 0000000..4f9037a --- /dev/null +++ b/src/main/java/frc/lib/Field.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/** + * Add Any Necessary Field Elements Below + * + * To Be Updated Each Year + */ +public class Field { + public static abstract class FieldSide { + public abstract Pose2d FieldElement(); + } + + public static final class RED extends FieldSide { + @Override + public final Pose2d FieldElement() { + return new Pose2d(); + } + } + public static final class BLUE extends FieldSide { + @Override + public final Pose2d FieldElement() { + return new Pose2d(); + } + } + + public static final RED redInstance = new RED(); + public static final BLUE blueInstance = new BLUE(); + + public static final FieldSide getSide() { + if (Util.getAlliance() == Alliance.Blue) { + return blueInstance; + } + return redInstance; + } + + public static final FieldSide getOppositeSide() { + if (Util.getAlliance() == Alliance.Blue) { + return redInstance; + } + return blueInstance; + } + +} diff --git a/src/main/java/frc/lib/HardwareDevices/CANCoderFactory.java b/src/main/java/frc/lib/HardwareDevices/CANCoderFactory.java new file mode 100644 index 0000000..bbbae81 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/CANCoderFactory.java @@ -0,0 +1,89 @@ +package frc.team696.lib.HardwareDevices; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.Timer; +import frc.team696.lib.Logging.PLog; + +/** + * Class For creating and keeping track of cancoder device + * + *

Automatically logs name of the device if it fails to config + * + *

Resets device parameters if it drops out + */ +public class CANCoderFactory { + private final double TIMEOUT = 0.05; + private CANcoder _encoder; + private CANcoderConfiguration _config; + private String _name; + + private boolean _configured = false; + private double _lastConfiguration = -100; + + private Alert configurationAlert; + + public CANCoderFactory(int id, String canBus, CANcoderConfiguration config, String name) { + this._encoder = new CANcoder(id, canBus); + this._name = name; + this._config = config; + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public CANCoderFactory(int id, CANcoderConfiguration config, String name) { + this(id, "rio", config, name); + } + + private boolean configure() { + return configure(false); + } + + public boolean configure(boolean force) { + if (!force && _configured) return true; + if (!force && Timer.getFPGATimestamp() - _lastConfiguration < 3) return false; + + _lastConfiguration = Timer.getFPGATimestamp(); + StatusCode configCode = _encoder.getConfigurator().apply(this._config, TIMEOUT); + + if(configCode.isError()) { + PLog.unusual(_name, "Failed to configure"); + } else { + PLog.info(_name, "Configured"); + if (configCode.isWarning()) { + PLog.unusual(_name, "Config Warning: " + configCode.toString()); + } + + _configured = true; + } + + configurationAlert.set(!_configured); + + return _configured; + } + + + public Rotation2d getPosition() { + if (configure()) { + StatusSignal positionCode = _encoder.getAbsolutePosition(); + if(!positionCode.getStatus().isOK()) { + _configured = false; + return new Rotation2d(); + } + return Rotation2d.fromRotations(positionCode.getValueAsDouble()); + } else + return new Rotation2d(); + } + + + public CANcoder get() { + return _encoder; + } +} diff --git a/src/main/java/frc/lib/HardwareDevices/GyroInterface.java b/src/main/java/frc/lib/HardwareDevices/GyroInterface.java new file mode 100644 index 0000000..3ab6077 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/GyroInterface.java @@ -0,0 +1,15 @@ +package frc.team696.lib.HardwareDevices; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.AngularVelocity; + +/** + * Base interface for switching between different Gyro Devices + */ +public interface GyroInterface { + public abstract Rotation2d getYaw(); + + public abstract void resetYaw(); + + public abstract AngularVelocity getAngularVelocity(); +} diff --git a/src/main/java/frc/lib/HardwareDevices/PigeonFactory.java b/src/main/java/frc/lib/HardwareDevices/PigeonFactory.java new file mode 100644 index 0000000..c2fa762 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/PigeonFactory.java @@ -0,0 +1,143 @@ +package frc.team696.lib.HardwareDevices; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.team696.lib.Logging.PLog; + +/** + * Automatically configures and optimizes pigeon + */ +public class PigeonFactory implements GyroInterface { + + private final double TIMEOUT = 0.05; + private Pigeon2 _gyro; + private Pigeon2Configuration _config; + private String _name; + + private boolean _configured = false; + private double _lastConfiguration = -100; + + public StatusSignal _yawSignal; + public StatusSignal _yawVelocitySignal; + + private Alert configurationAlert; + + public PigeonFactory(int id, String canBus, Pigeon2Configuration config, String name) { + this._gyro = new Pigeon2(id, canBus); + this._name = name; + this._config = config; + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public PigeonFactory(int id, Pigeon2Configuration config, String name) { + this(id, "rio", config, name); + } + + private boolean configure() { + return configure(false); + } + + public boolean configure(boolean force) { + if (!force && _configured) return true; + if (!force && Timer.getFPGATimestamp() - _lastConfiguration < 3) return false; + + _lastConfiguration = Timer.getFPGATimestamp(); + StatusCode configCode = _gyro.getConfigurator().apply(this._config, TIMEOUT); + + _yawSignal = _gyro.getYaw(); + _yawVelocitySignal = _gyro.getAngularVelocityZWorld(); + + if(configCode.isError() || _yawSignal.getStatus().isError() || _yawVelocitySignal.getStatus().isError()) { + PLog.unusual(_name, "Failed to configure"); + } else { + PLog.info(_name, "Configured"); + if (configCode.isWarning()) { + PLog.unusual(_name, "Config Warning: " + configCode.toString()); + } + + _configured = true; + } + + // Actually Used In Swerve + _yawSignal.setUpdateFrequency(100); + _yawVelocitySignal.setUpdateFrequency(100); + + // Useful Other Commands That May Be Used In The Future + _gyro.getPitch().setUpdateFrequency(100); + _gyro.getRoll().setUpdateFrequency(100); + + _gyro.optimizeBusUtilization(); + + configurationAlert.set(!_configured); + + return _configured; + } + + private double getRawYaw(boolean refresh) { + if (configure()) { + if (refresh) { + StatusSignal code = _yawSignal.refresh(); + if(!code.getStatus().isOK()) { + _configured = false; + return 0; + } + } + return MathUtil.inputModulus(_yawSignal.getValueAsDouble(),-180,180); + } else + return 0; + } + + /* + * Angle Of the Gyro in -180, 180 + */ + @Override + public Rotation2d getYaw() { + return Rotation2d.fromDegrees(getRawYaw(false)); + } + + @Override + public void resetYaw() { + _gyro.reset(); + } + + public AngularVelocity getAngularVelocity(boolean refresh) { + if (configure()) { + if (refresh) { + StatusSignal code = _yawVelocitySignal.refresh(); + if(!code.getStatus().isOK()) { + _configured = false; + return RadiansPerSecond.of(0); + } + } + return _yawVelocitySignal.getValue(); + } else + return RadiansPerSecond.of(0); + } + + @Override + public AngularVelocity getAngularVelocity() { + return getAngularVelocity(false); + } + + public Pigeon2 get() { + return _gyro; + } + + public Rotation2d getLatencyAdjustedYaw() { + return Rotation2d.fromDegrees(MathUtil.inputModulus(BaseStatusSignal.getLatencyCompensatedValue(_yawSignal, _yawVelocitySignal).magnitude(),-180,180)); + } +} diff --git a/src/main/java/frc/lib/HardwareDevices/TalonFactory.java b/src/main/java/frc/lib/HardwareDevices/TalonFactory.java new file mode 100644 index 0000000..47c83a2 --- /dev/null +++ b/src/main/java/frc/lib/HardwareDevices/TalonFactory.java @@ -0,0 +1,197 @@ +package frc.team696.lib.HardwareDevices; + +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.Alert.AlertType; +import frc.team696.lib.Logging.PLog; + +/** + * Class For creating and keeping track of talon device + * + *

Automatically logs name of the device if it fails to config + * + *

Resets device parameters if it drops out + */ +public class TalonFactory { + private final double TIMEOUT = 0.05; + private TalonFX _motor; + private TalonFXConfiguration _config; + private String _name; + private DutyCycleOut _DutyCycleControl; + private VoltageOut _VoltageControl; + + private int _followID = -1; + private boolean _opposeDirection = false; + + private boolean _configured = false; + private double _lastConfiguration = -100; + + private Alert configurationAlert; + + public TalonFactory(int id, String canBus, TalonFXConfiguration config, String name) { + this._motor = new TalonFX(id, canBus); + this._name = name; + this._config = config; + _DutyCycleControl = new DutyCycleOut(0); + _VoltageControl = new VoltageOut(0); + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public TalonFactory(int id, CANBus canBus, TalonFXConfiguration config, String name) { + this._motor = new TalonFX(id, canBus); + this._name = name; + this._config = config; + _DutyCycleControl = new DutyCycleOut(0); + _VoltageControl = new VoltageOut(0); + configurationAlert = new Alert(String.format("Failed to configure %s", this._name), AlertType.kError); + configure(); + } + + public TalonFactory(int id, TalonFXConfiguration config, String name) { + this(id, "rio", config, name); + } + + private boolean configure() { + return configure(false); + } + + public boolean configure(boolean force) { + if (!force && _configured) return true; + if (!force && Timer.getFPGATimestamp() - _lastConfiguration < 3) return false; + + _lastConfiguration = Timer.getFPGATimestamp(); + StatusCode configCode = _motor.getConfigurator().apply(this._config, TIMEOUT); + + if(configCode.isError()) { + PLog.unusual(_name, "Failed to configure"); + } else { + PLog.info(_name, "Configured"); + if (configCode.isWarning()) { + PLog.unusual(_name, "Config Warning: " + configCode.toString()); + } + + if (_followID != -1) + _motor.setControl(new Follower(_followID, _opposeDirection)); + + setPosition(0); + + _configured = true; + } + + configurationAlert.set(!_configured); + + return _configured; + } + + public void Follow(int id, boolean opposeDirection) { + _followID = id; + this._opposeDirection = opposeDirection; + if (configure()) + _motor.setControl(new Follower(id, opposeDirection)); + } + + public void Follow(TalonFX master, boolean opposeDirection) { + Follow(master.getDeviceID(), opposeDirection); + } + + public void Follow(TalonFactory master, boolean opposeDirection) { + Follow(master.getID(), opposeDirection); + } + + public int getID() { + return _motor.getDeviceID(); + } + + public String getName(){ + return _name; + } + + public void setPosition(double newPosition) { + if (configure()) + if(!_motor.setPosition(newPosition).isOK()) { + PLog.info(this._name, "Failed To Set Position"); + _configured = false; + } + } + + public void setPosition() { + setPosition(0); + } + + public double getPosition() { + if (configure()) { + StatusSignal positionCode = _motor.getPosition(); + if(!positionCode.getStatus().isOK()) { + _configured = false; + return 0; + } + return positionCode.getValueAsDouble(); + } else + return 0; + } + + public double getVelocity() { + if (configure()) { + StatusSignal velocityCode = _motor.getVelocity(); + if(!velocityCode.getStatus().isOK()) { + _configured = false; + return 0; + } + return velocityCode.getValueAsDouble(); + } else + return 0; + } + + public void PercentOutput(double percent) { + setControl(_DutyCycleControl.withOutput(percent)); + } + + /** @param voltage 0-12 input */ + public void VoltageOut(Voltage voltage) { + setControl(_VoltageControl.withOutput(voltage.in(Volts))); + } + + public void stop() { + _motor.stopMotor(); + } + + public void setControl(ControlRequest request) { + if (configure()) + if (!_motor.setControl(request).isOK()) { + _configured = false; + } + } + + public double getCurrent() { + if (configure()) { + StatusSignal currentCode = _motor.getStatorCurrent(); + if(!currentCode.getStatus().isOK()) { + _configured = false; + return 0; + } + return currentCode.getValueAsDouble(); + } else + return 0; + } + + public TalonFX get() { + return _motor; + } +} diff --git a/src/main/java/frc/lib/Logging/AdvantageKitLog.java b/src/main/java/frc/lib/Logging/AdvantageKitLog.java new file mode 100644 index 0000000..b942d1c --- /dev/null +++ b/src/main/java/frc/lib/Logging/AdvantageKitLog.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. +/* +package frc.team696.lib.Logging; + +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.BuildConstants; + +/** Simple Setup for Advantage Kit */ /* +@SuppressWarnings("resource") +public class AdvantageKitLog { + public static final void setup() { + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + if (RobotBase.isReal()) { + new PowerDistribution(1, ModuleType.kRev); + Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") + } + + Logger.start(); + } +} +*/ \ No newline at end of file diff --git a/src/main/java/frc/lib/Logging/BackupLogger.java b/src/main/java/frc/lib/Logging/BackupLogger.java new file mode 100644 index 0000000..e1f7635 --- /dev/null +++ b/src/main/java/frc/lib/Logging/BackupLogger.java @@ -0,0 +1,161 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib.Logging; + +import java.util.concurrent.ConcurrentLinkedQueue; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.hal.PowerJNI; +import edu.wpi.first.hal.can.CANJNI; +import edu.wpi.first.hal.can.CANStatus; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PowerDistribution; + +/** + * NOT DONE + * + * + * Sick Logger that uses generics to fuck shit up. Who knows how performant it + * is, who cares, it's 1 file! + * + * May Switch to this internally -> less reliance on other libraries, more + * freedom, doesn't hurt anything anyway + * + * Really Meant as a backup/alternative to other loggers, use it if you want, + * idgaf + */ +public class BackupLogger { + public static DataLog log; + + public static final boolean PUBLISH_TO_NT = true; + public static final boolean PUBLISH_TO_LOG = true; + + private static ConcurrentLinkedQueue> logQueue = new ConcurrentLinkedQueue>(); + + private static final CANStatus status = new CANStatus(); + private static PowerDistribution pdh; + + private static logThread lThread; + + static { + DataLogManager.start("/U/logs"); + + log = DataLogManager.getLog(); + + DataLogManager.logConsoleOutput(true); + DataLogManager.logNetworkTables(true); + DriverStation.startDataLog(log, true); + + lThread = new logThread(); + lThread.start(); + } + + public static void stop() { + lThread.isRunning = false; + DataLogManager.stop(); + + for (LogEntry.GenericPublisher p : LogEntry.publishers.values()) { + p.publisher.close(); + } + + for (LogEntry.GenericDataLog p : LogEntry.logEntries.values()) { + p.dataLog.finish(); + } + } + + public static void setPdh(PowerDistribution newPdh) { + pdh = newPdh; + } + + /** + * Call Periodically If Desired + */ + public static void logSystemInformation() { + addToQueue("SystemStats/FPGAVersion", HALUtil.getFPGAVersion()); + addToQueue("SystemStats/FPGARevision", HALUtil.getFPGARevision()); + addToQueue("SystemStats/SerialNumber", HALUtil.getSerialNumber()); + addToQueue("SystemStats/Comments", HALUtil.getComments()); + addToQueue("SystemStats/TeamNumber", HALUtil.getTeamNumber()); + addToQueue("SystemStats/FPGAButton", HALUtil.getFPGAButton()); + addToQueue("SystemStats/SystemActive", HAL.getSystemActive()); + addToQueue("SystemStats/BrownedOut", HAL.getBrownedOut()); + addToQueue("SystemStats/RSLState", HAL.getRSLState()); + addToQueue("SystemStats/SystemTimeValid", HAL.getSystemTimeValid()); + addToQueue("SystemStats/BatteryVoltage", PowerJNI.getVinVoltage()); + addToQueue("SystemStats/BatteryCurrent", PowerJNI.getVinCurrent()); + addToQueue("SystemStats/3v3Rail/Voltage", PowerJNI.getUserVoltage3V3()); + addToQueue("SystemStats/3v3Rail/Current", PowerJNI.getUserCurrent3V3()); + addToQueue("SystemStats/3v3Rail/Active", PowerJNI.getUserActive3V3()); + addToQueue("SystemStats/3v3Rail/CurrentFaults", PowerJNI.getUserCurrentFaults3V3()); + addToQueue("SystemStats/5vRail/Voltage", PowerJNI.getUserVoltage5V()); + addToQueue("SystemStats/5vRail/Current", PowerJNI.getUserCurrent5V()); + addToQueue("SystemStats/5vRail/Active", PowerJNI.getUserActive5V()); + addToQueue("SystemStats/5vRail/CurrentFaults", PowerJNI.getUserCurrentFaults5V()); + addToQueue("SystemStats/6vRail/Voltage", PowerJNI.getUserVoltage6V()); + addToQueue("SystemStats/6vRail/Current", PowerJNI.getUserCurrent6V()); + addToQueue("SystemStats/6vRail/Active", PowerJNI.getUserActive6V()); + addToQueue("SystemStats/6vRail/CurrentFaults", PowerJNI.getUserCurrentFaults6V()); + addToQueue("SystemStats/BrownoutVoltage", PowerJNI.getBrownoutVoltage()); + addToQueue("SystemStats/CPUTempCelcius", PowerJNI.getCPUTemp()); + + CANJNI.getCANStatus(status); + addToQueue("SystemStats/CANBus/Utilization", status.percentBusUtilization); + addToQueue("SystemStats/CANBus/OffCount", status.busOffCount); + addToQueue("SystemStats/CANBus/TxFullCount", status.txFullCount); + addToQueue("SystemStats/CANBus/ReceiveErrorCount", status.receiveErrorCount); + addToQueue("SystemStats/CANBus/TransmitErrorCount", status.transmitErrorCount); + addToQueue("SystemStats/EpochTimeMicros", HALUtil.getFPGATime()); + + if (pdh == null) + return; + + addToQueue("SystemStats/PowerDistribution/Temperature", pdh.getTemperature()); + addToQueue("SystemStats/PowerDistribution/Voltage", pdh.getVoltage()); + addToQueue("SystemStats/PowerDistribution/ChannelCurrent", pdh.getAllCurrents()); + addToQueue("SystemStats/PowerDistribution/TotalCurrent", pdh.getTotalCurrent()); + addToQueue("SystemStats/PowerDistribution/TotalPower", pdh.getTotalPower()); + addToQueue("SystemStats/PowerDistribution/TotalEnergy", pdh.getTotalEnergy()); + addToQueue("SystemStats/PowerDistribution/ChannelCount", pdh.getNumChannels()); + } + + public static void addToQueue(String name, T value) { + if (lThread.isRunning) + logQueue.add(new LogEntry(name, value)); + } + + public static class logThread extends Thread { + public logThread() { + this.setName("LoggerThread"); + this.setDaemon(true); + this.setPriority(MIN_PRIORITY); + } + + public volatile boolean isRunning = true; + + @Override + public void run() { + while (isRunning) { + LogEntry entry = BackupLogger.logQueue.poll(); + if (entry == null) + continue; + try { + if (BackupLogger.PUBLISH_TO_NT) { + LogEntry.GenericPublisher.classToPublisher.get(entry.value.getClass()) + .invoke(LogEntry.publishers.get(entry.name).publisher, entry.value); + } + if (BackupLogger.PUBLISH_TO_LOG) { + LogEntry.GenericDataLog.classToDatalog.get(entry.value.getClass()) + .invoke(LogEntry.logEntries.get(entry.name).dataLog, entry.value); + } + } catch (Exception e) { + PLog.info("Logger", "Publisher Failed to accept value: " + entry.name); + } + } + } + } +} diff --git a/src/main/java/frc/lib/Logging/LogEntry.java b/src/main/java/frc/lib/Logging/LogEntry.java new file mode 100644 index 0000000..40a45a1 --- /dev/null +++ b/src/main/java/frc/lib/Logging/LogEntry.java @@ -0,0 +1,192 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib.Logging; + +import java.lang.reflect.Method; +import java.util.concurrent.ConcurrentHashMap; + +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.Publisher; +import edu.wpi.first.util.datalog.BooleanArrayLogEntry; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLogEntry; +import edu.wpi.first.util.datalog.DoubleArrayLogEntry; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.util.datalog.IntegerArrayLogEntry; +import edu.wpi.first.util.datalog.IntegerLogEntry; +import edu.wpi.first.util.datalog.ProtobufLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.datalog.StructArrayLogEntry; +import edu.wpi.first.util.datalog.StructLogEntry; +import edu.wpi.first.util.protobuf.Protobuf; +import edu.wpi.first.util.protobuf.ProtobufSerializable; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; +import us.hebi.quickbuf.ProtoMessage; + +@SuppressWarnings("unchecked") +/** Known Bug, Fails to print string the first time, caught in a try catch so it's fine, just don't know why it's hpapening + * + * Everything works but int[]s, because they are longs internally Sick + */ +public class LogEntry { + public final T value; + public final String name; + public final Long time; + public LogEntry(String name, T value) { + this.value = value; + this.name = name; + this.time = System.currentTimeMillis(); + + if (BackupLogger.log == null) return; + + if (publishers.containsKey(name)) return; + + if (value instanceof String) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getStringTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new StringLogEntry(BackupLogger.log, name))); + + return; + } + + if (value instanceof Integer || value instanceof Long || value instanceof Short) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getIntegerTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new IntegerLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof int[] || value instanceof long[] || value instanceof short[]) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getIntegerArrayTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new IntegerArrayLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof Double || value instanceof Float) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getDoubleTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new DoubleLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof double[] || value instanceof float[]) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getDoubleArrayTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new DoubleArrayLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof Boolean) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getBooleanTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new BooleanLogEntry(BackupLogger.log, name))); + return; + } + + if (value instanceof boolean[]) { + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getBooleanArrayTopic(name).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), new BooleanArrayLogEntry(BackupLogger.log, name))); + return; + } + + + if(value instanceof StructSerializable) { + try { + Struct structImplementation = (Struct)value.getClass().getDeclaredField("struct").get(null); + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getStructTopic(name, structImplementation).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), StructLogEntry.create(BackupLogger.log, name, structImplementation))); + + } catch ( Exception e ){ + PLog.info("Logger", "Malformed Struct detected: " + name); + } + return; + } + + + if(value instanceof StructSerializable[]) { + try { + Struct structImplementation = (Struct)value.getClass().getComponentType().getDeclaredField("struct").get(null); + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getStructArrayTopic(name, structImplementation).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), StructArrayLogEntry.create(BackupLogger.log, name, structImplementation))); + + } catch ( Exception e ){ + PLog.info("Logger", "Malformed Struct detected: " + name); + } + return; + } + + if (value instanceof ProtobufSerializable) { + try { + Protobuf> protobufImplementation = (Protobuf>)value.getClass().getDeclaredField("proto").get(null); + publishers.put(name, new GenericPublisher<>(value.getClass(), NetworkTableInstance.getDefault().getProtobufTopic(name, protobufImplementation).publish())); + + logEntries.put(name, new GenericDataLog(value.getClass(), ProtobufLogEntry.create(BackupLogger.log, name, protobufImplementation))); + } catch ( Exception e ){ + PLog.info("Logger", "Malformed Protobuf detected: " + name); + } + return; + } + + + + PLog.info("Logger", "failed to categorize logEntry: " + name); + + } + + + /** + * private final StructArrayPublisher swerveModuleDesiredStatePublisher = NetworkTableInstance.getDefault() +.getStructArrayTopic("696/Swerve/DesiredStates", SwerveModuleState.struct).publish(); + + */ + + public static class GenericPublisher { + public T publisher; + public static final ConcurrentHashMap, Method> classToPublisher = new ConcurrentHashMap<>(); + public GenericPublisher(Class type, T publisher) { + this.publisher = publisher; + + if (classToPublisher.containsKey(type)) return; + + Method[] methodsNT = publisher.getClass().getMethods(); + for (Method method : methodsNT) { + if (method.getName().equals("set") && method.getParameterCount() == 1) { + if (method.getParameterTypes()[0].isArray() != type.isArray()) continue; + + classToPublisher.put(type, method); + break; + } + } + } + } + + public static class GenericDataLog { + public T dataLog; + public static final ConcurrentHashMap, Method> classToDatalog = new ConcurrentHashMap<>(); + public GenericDataLog(Class type, T log) { + this.dataLog = log; + + if (classToDatalog.containsKey(type)) return; + + Method[] methodsLOG = dataLog.getClass().getMethods(); + for (Method method : methodsLOG) { + if (method.getName().equals("update") && method.getParameterCount() == 1) { + if (method.getParameterTypes()[0].isArray() != type.isArray()) continue; + // if (!((Object)type).getClass().isAssignableFrom(temp)) continue; + + classToDatalog.put(type, method); + break; + } + } + } + } + public static final ConcurrentHashMap> publishers = new ConcurrentHashMap<>(); + public static final ConcurrentHashMap> logEntries = new ConcurrentHashMap<>(); +} diff --git a/src/main/java/frc/lib/Logging/PLog.java b/src/main/java/frc/lib/Logging/PLog.java new file mode 100644 index 0000000..d97f00f --- /dev/null +++ b/src/main/java/frc/lib/Logging/PLog.java @@ -0,0 +1,91 @@ +package frc.team696.lib.Logging; + +import edu.wpi.first.wpilibj.DriverStation; + +/** Class for printing out to the console */ +public class PLog { + /** + * Log a FATAL error, after which the robot cannot (properly) function.
+ * + * @param category + * @param message + */ + public static void fatal(String category, T message) { + log("Fatal", category, String.valueOf(message)); + + // make it show up on the DS as well + DriverStation.reportError("Fatal Error: " + String.valueOf(message), true); + } + + /** + * Log a FATAL error due to an exception, after which the robot cannot + * (properly) function.
+ * Prints your message, and the exception's name, message, and stacktrace. + * + */ + public static void fatalException(String category, String userMessage, Exception exception) { + String exceptionMessage = String.format("%s -- %s: %s", userMessage, exception.getClass().getSimpleName(), exception.getMessage()); + for (StackTraceElement element : exception.getStackTrace()) { + exceptionMessage += "\n " + element; + } + log("Fatal", category, exceptionMessage); + + } + + public static void fatalException(String category, String userMessage, StackTraceElement[] elements) { + String exceptionMessage = String.format("%s ", userMessage); + for (StackTraceElement element : elements) { + exceptionMessage += "\n " + element; + } + log("Fatal", category, exceptionMessage); + } + + /** + * Log a failure which may kill one function or one thread, however the robot as + * a whole can keep functioning. + * + * @param category + * @param message + */ + public static void recoverable(String category, T message) { + log("Recoverable", category, String.valueOf(message)); + + DriverStation.reportError("Error: " + String.valueOf(message), true); + } + + /** + * Log something which should not happen under normal circumstances and probably + * is a bug, but does not cause anything to crash. + * + * @param category + * @param message + */ + public static void unusual(String category, T message) { + log("Unusual", category, String.valueOf(message)); + } + + /** + * Log a semi-important message which the user should probably see, but does not + * indicate anything is broken. + */ + public static void info(String category, T message) { + log("Info", category, String.valueOf(message)); + } + + /** + * Log a message which is not important during normal operation, but is useful + * if you're trying to debug the robot. + * + * @param category + * @param message + */ + public static void debug(String category, T message) { + log("Debug", category, String.valueOf(message)); + } + + private static void log(String severity, String category, String message) { + String output = String.format("[%s] [%s] %s", severity, category, message); + System.out.println(output); + BackupLogger.addToQueue("prints", output); + } +} diff --git a/src/main/java/frc/lib/Swerve/Commands/TeleopSwerve.java b/src/main/java/frc/lib/Swerve/Commands/TeleopSwerve.java new file mode 100644 index 0000000..a5f9ce3 --- /dev/null +++ b/src/main/java/frc/lib/Swerve/Commands/TeleopSwerve.java @@ -0,0 +1,165 @@ +package frc.team696.lib.Swerve.Commands; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team696.lib.Util; +import frc.team696.lib.Swerve.SwerveConstants; +import frc.team696.lib.Swerve.SwerveDriveSubsystem; + +/** + * Swerve command for driving around during teleop + * + *

Config once and create multiple objects to handle different driving styles + */ +public class TeleopSwerve extends Command { + protected static DoubleSupplier translation = ()->0; + protected static DoubleSupplier strafe = ()->0; + protected static DoubleSupplier rotation = ()->0; + protected static double deadband = 1; // deadband for controller -> defaulted to 1 so you must config swerve + protected static double rotationDeadband = 1; + protected static SwerveDriveSubsystem swerveSubsystem = null; + protected static double lastPeriodicSeconds = 0; + private static PIDController pidController = new PIDController(0.0056, 0.00, 0); + static { + pidController.enableContinuousInput(-180, 180); + pidController.setTolerance(1); + } + private static BooleanSupplier lockRotation; // should lock rotation -> usually a button + + private boolean fieldRelative; // should do fieldRelative controol + private boolean openLoop; // should do openLoop control + private Supplier rotationGoal; // Rotation to lock to once lockRotation has been activated + private DoubleSupplier multiplier = ()->1; // Multiplier to outputted Speed + + public static void config(SwerveDriveSubsystem s, DoubleSupplier x, DoubleSupplier y, DoubleSupplier r, BooleanSupplier rotationLock, double deadBand) { + strafe = x; + translation = y; + rotation = r; + + lockRotation = rotationLock; + + deadband = deadBand; + rotationDeadband = Math.sqrt(2 * Math.pow(deadband, 2)); + swerveSubsystem = s; + } + + public static TeleopSwerve New(){ + return new TeleopSwerve(); + } + + private TeleopSwerve() { + this.fieldRelative = true; + this.openLoop = true; + + this.rotationGoal = ()->null; + + this.multiplier = ()->1; + + addRequirements(swerveSubsystem); + } + + public TeleopSwerve withMultiplier(DoubleSupplier multiplier) { + this.multiplier = multiplier; + return this; + } + + public TeleopSwerve withMultiplier(double multiplier) { + this.multiplier = ()->multiplier; + return this; + } + + public TeleopSwerve withRotationGoal(Supplier goal) { + this.rotationGoal = goal; + return this; + } + + public TeleopSwerve withRotationGoal(Rotation2d goal) { + this.rotationGoal = ()->goal; + return this; + } + + public TeleopSwerve withRobotRelative(boolean robotRelative) { + this.fieldRelative = !robotRelative; + return this; + } + + public TeleopSwerve withOpenLoop(boolean openLoop) { + this.openLoop = openLoop; + return this; + } + + @Override + public void execute() { + double yAxis = translation.getAsDouble(); + double xAxis = strafe.getAsDouble(); + double rAxis = rotation.getAsDouble(); + + Rotation2d theta=Rotation2d.fromDegrees(0); + if(xAxis!=0||yAxis!=0) + theta = new Rotation2d(yAxis, xAxis); + double magnitude = Math.min(Math.sqrt((xAxis * xAxis) + (yAxis * yAxis)), 1); + if (magnitude < deadband) magnitude = 0; + Rotation2d goalRotation = rotationGoal.get(); + if (lockRotation != null && lockRotation.getAsBoolean() && goalRotation != null) { // Rotation Lock To Angle + double pid = pidController.calculate(swerveSubsystem.getPose().getRotation().getDegrees(), goalRotation.getDegrees()); + rAxis = Math.abs(pidController.getError()) > 1 ? Math.abs(Math.pow(pid, 2)) * 1.1 * Math.signum(pid) + pid * 2.2 : 0; + } else { + rAxis = (Math.abs(rAxis) > rotationDeadband) ? Util.map(rAxis * rAxis, rotationDeadband, 1, 0, 1) * Math.signum(rAxis) : 0; + } + + double outputPercent = Math.min(multiplier.getAsDouble(),1); + + double desiredRotation = rAxis * SwerveConstants.MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); + Translation2d desiredTranslation = new Translation2d(Math.pow(magnitude, 2), theta).times(SwerveConstants.MAX_VELOCITY.in(MetersPerSecond)).times(outputPercent); + + /* Untested Portion Begin + double deltaSeconds = Timer.getFPGATimestamp() - lastPeriodicSeconds; + + // Acceleration Limiting Start + Translation2d curVel = swerveSubsystem.getState().velocityXY(); + Translation2d desiredAcceleration = desiredTranslation.minus(curVel).div(deltaSeconds); + double accelerationLimit = SwerveConstants.MAX_ACCELERATION.in(MetersPerSecondPerSecond) * (1 - curVel.getNorm() / SwerveConstants.MAX_VELOCITY.in(MetersPerSecond)); + + desiredAcceleration = desiredAcceleration.div(desiredAcceleration.getNorm()).times(accelerationLimit); + + double accelerationXYScaling = Math.max(Math.max(1 , Math.abs(desiredAcceleration.getX()) / SwerveConstants.MAX_ACCELERATION_X.in(MetersPerSecondPerSecond) ), Math.abs(desiredAcceleration.getY()) / SwerveConstants.MAX_ACCELERATION_Y.in(MetersPerSecondPerSecond)); + desiredAcceleration = desiredAcceleration.div(desiredAcceleration.getNorm()).times(accelerationXYScaling); + + double maxSkidAcceleration = Math.max(1, desiredAcceleration.getNorm() / SwerveConstants.MAX_ACCELERATION_SKID.in(MetersPerSecondPerSecond)); + desiredAcceleration = desiredAcceleration.div(desiredAcceleration.getNorm()).times(maxSkidAcceleration); + + // Acceleration Limiting End + + // Jerk Limiting Start + // Might do Questionable Things because we are using our currentAcceleration which may not be accurate (Why it is in the untested Portion) + + Translation2d curAccel = swerveSubsystem.getState().accelerationXY(); + Translation2d desiredJerk = desiredAcceleration.minus(curAccel).div(deltaSeconds); + double jerkLimit = SwerveConstants.MAX_JERK.in(MetersPerSecondPerSecond.per(Seconds)); + + double jerkScaling = Math.min(jerkLimit, desiredJerk.getNorm()); + + Translation2d scaledJerk = desiredJerk.div(desiredJerk.getNorm()).times(jerkScaling); + + desiredAcceleration = curAccel.plus(scaledJerk.times(deltaSeconds)); + + // Jerk Limiting End + + desiredTranslation = curVel.plus(desiredAcceleration.times(deltaSeconds)); + + lastPeriodicSeconds = Timer.getFPGATimestamp(); + /* Untested Portion End */ + + swerveSubsystem.Drive(desiredTranslation, desiredRotation, fieldRelative, openLoop); + + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/Swerve/SwerveConfigs.java b/src/main/java/frc/lib/Swerve/SwerveConfigs.java new file mode 100644 index 0000000..7cd502f --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveConfigs.java @@ -0,0 +1,107 @@ +package frc.team696.lib.Swerve; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; + +/** + * + * Device Configs of the Swerve Drive + * + *

Only Update Device Configs if you know what you are doing. + */ +public final class SwerveConfigs { + public final static TalonFXConfiguration angle; + public final static TalonFXConfiguration drive; + public final static CANcoderConfiguration canCoder; + public final static Pigeon2Configuration pigeon; + public final static SwerveModuleConstants FRONT_LEFT; + public final static SwerveModuleConstants FRONT_RIGHT; + public final static SwerveModuleConstants BACK_LEFT; + public final static SwerveModuleConstants BACK_RIGHT; + static { + angle = new TalonFXConfiguration(); + drive = new TalonFXConfiguration(); + canCoder = new CANcoderConfiguration(); + pigeon = new Pigeon2Configuration(); + + FRONT_LEFT = new SwerveModuleConstants(); + FRONT_RIGHT = new SwerveModuleConstants(); + BACK_LEFT = new SwerveModuleConstants(); + BACK_RIGHT = new SwerveModuleConstants(); + + /** Swerve CANCoder Configuration */ + canCoder.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + canCoder.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + + /** Swerve Angle Motor Configuration */ + angle.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + angle.MotorOutput.NeutralMode = NeutralModeValue.Coast; + angle.Feedback.SensorToMechanismRatio = SwerveConstants.ANGLE_GEAR_RATIO; + angle.ClosedLoopGeneral.ContinuousWrap = true; + angle.CurrentLimits.SupplyCurrentLimitEnable = true; + angle.CurrentLimits.SupplyCurrentLimit = 25; + angle.CurrentLimits.SupplyCurrentLowerLimit = 60; + angle.CurrentLimits.SupplyCurrentLowerTime = 0.1; + angle.CurrentLimits.StatorCurrentLimitEnable = true; + angle.CurrentLimits.StatorCurrentLimit = 80; + angle.Slot0.kP = 150.0; + angle.Slot0.kI = 0.0; + angle.Slot0.kD = 0.0; + + angle.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0; + angle.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0; + angle.Voltage.PeakForwardVoltage = 12.; + angle.Voltage.PeakReverseVoltage = -12.; + + /** Swerve Drive Motor Configuration */ + drive.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + drive.MotorOutput.NeutralMode = NeutralModeValue.Brake; + drive.Feedback.SensorToMechanismRatio = SwerveConstants.DRIVE_GEAR_RATIO; + drive.CurrentLimits.SupplyCurrentLimitEnable = true; + drive.CurrentLimits.SupplyCurrentLimit = 25; + drive.CurrentLimits.SupplyCurrentLowerLimit = 90; + drive.CurrentLimits.SupplyCurrentLowerTime = 0.2; + drive.CurrentLimits.StatorCurrentLimitEnable = true; + drive.CurrentLimits.StatorCurrentLimit = 110; + drive.Voltage.PeakForwardVoltage = 12.; + drive.Voltage.PeakReverseVoltage = -12.; + + drive.Slot0.kP = 2.; + drive.Slot0.kI = 0.0; + drive.Slot0.kD = 0.0; + drive.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = 0.25; + drive.OpenLoopRamps.VoltageOpenLoopRampPeriod = 0.25; + drive.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.02; + drive.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.02; + + /** Individual Swerve Module Configurations */ + FRONT_LEFT.EncoderId = 0; + FRONT_LEFT.DriveMotorId = 1; + FRONT_LEFT.SteerMotorId = 0; + FRONT_LEFT.EncoderOffset = -0.24-.25; + + FRONT_RIGHT.EncoderId = 1; + FRONT_RIGHT.DriveMotorId = 3; + FRONT_RIGHT.SteerMotorId = 2; + FRONT_RIGHT.EncoderOffset = -0.393-.25; + + BACK_LEFT.EncoderId = 2; + BACK_LEFT.DriveMotorId = 5; + BACK_LEFT.SteerMotorId = 4; + + BACK_LEFT.EncoderOffset = -0.456-.25; + + BACK_RIGHT.EncoderId = 3; + BACK_RIGHT.DriveMotorId = 7; + BACK_RIGHT.SteerMotorId = 6; + BACK_RIGHT.EncoderOffset = -0.03-.25; + + /** Pigeon Configuration */ + pigeon.MountPose.MountPoseYaw = 0; + } +} diff --git a/src/main/java/frc/lib/Swerve/SwerveConstants.java b/src/main/java/frc/lib/Swerve/SwerveConstants.java new file mode 100644 index 0000000..f564408 --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveConstants.java @@ -0,0 +1,116 @@ +package frc.team696.lib.Swerve; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Kilogram; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Minute; +import static edu.wpi.first.units.Units.NewtonMeters; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.DistanceUnit; +import edu.wpi.first.units.LinearAccelerationUnit; +import edu.wpi.first.units.LinearVelocityUnit; +import edu.wpi.first.units.MassUnit; +import edu.wpi.first.units.MultUnit; +import edu.wpi.first.units.TimeUnit; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Force; +import edu.wpi.first.units.measure.LinearAcceleration; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.Mass; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Mult; +import edu.wpi.first.units.measure.Per; +import edu.wpi.first.units.measure.Torque; +import edu.wpi.first.units.measure.Voltage; + +/** + * Constants of the Swerve Drive + * + *

Should Be Updated For Each Robot Accordingly + */ +public class SwerveConstants { + public static final String canBus = "cv"; + + public static final int expectedModuleCount = 4; + + public static final Voltage drivekS = Volts.of(0.667); + public static final Per drivekV = Volts.of(2.44).div(MetersPerSecond.of(1)); + public static final Per drivekA = Volts.of(0.27).div(MetersPerSecondPerSecond.of(1)); + + public static final double DRIVE_GEAR_RATIO = (5.36 / 1.0); + public static final double ANGLE_GEAR_RATIO = (150.0 / 7.0 / 1.0); + + public static final Mass MASS = Pounds.of(140); + + // Distance From The Center To The Wheel, From The Distance Between Two Wheels divided By 2 + public static final Distance WHEELBASE_X = Inches.of(18.5).div(2); + public static final Distance WHEELBASE_Y = Inches.of(18.5).div(2); + public static final Distance DRIVEBASE_RADIUS = Inches.of(Math.sqrt(Math.pow(WHEELBASE_X.in(Inches), 2) + Math.pow(WHEELBASE_Y.in(Inches), 2))); + public static final Distance WHEEL_DIAMETER = Inches.of(3.5); + public static final Distance WHEEL_RADIUS = WHEEL_DIAMETER.div(2); + public static final Distance WHEEL_CIRCUM = WHEEL_DIAMETER.times(Math.PI); + public static final double WHEEL_COEFFICIENT_OF_FRICTION = 1.2; + + public static final Per DISTANCE_PER_ROTATION = WHEEL_CIRCUM.div(DRIVE_GEAR_RATIO).div(Rotations.of(1)); + + // Use Reca.lc to find Stall Torque At Motor Current Limit + // Values For Kraken X60 + public static final AngularVelocity MAX_FREESPIN_VELOCITY = Rotations.of(6000).per(Minute); + // Current Limit / Max Current / Torque At Max Current + public static final Torque MAX_MOTOR_STALL_TORQUE = NewtonMeters.of(SwerveConfigs.drive.CurrentLimits.StatorCurrentLimit / 366 * 7.09); + public static final Force MAX_WHEEL_FORCE = MAX_MOTOR_STALL_TORQUE.times(DRIVE_GEAR_RATIO).div(WHEEL_RADIUS).times(expectedModuleCount); + + public static final LinearVelocity THEORETICAL_MAX_SPEED = MetersPerSecond.of(MAX_FREESPIN_VELOCITY.in(RotationsPerSecond) * DISTANCE_PER_ROTATION.in(Meters.per(Rotation))); + public static final LinearAcceleration THEORETICAL_MAX_ACCELERATION = (MAX_WHEEL_FORCE.div(MASS)); + public static final LinearVelocity MAX_VELOCITY = THEORETICAL_MAX_SPEED.times(0.85); + public static final LinearAcceleration MAX_ACCELERATION = THEORETICAL_MAX_ACCELERATION.times(0.7); + public static final AngularVelocity THEORETICAL_MAX_ANGULAR_VELOCITY = RotationsPerSecond.of(MAX_VELOCITY.in(MetersPerSecond) / (DRIVEBASE_RADIUS.in(Meters) * 2 * Math.PI)); + public static final AngularVelocity MAX_ANGULAR_VELOCITY = THEORETICAL_MAX_ANGULAR_VELOCITY.times(0.95); + + /* Credit To 1690s Software Session Part 1 + * Forward limit -> Max Acceleration Magnitude -> MaxAcc * (1 - curVel / maxVel)-> see TeleopSwerve.java for usage + * Tilt limit -> Limits Magnitude of Accelx and Accely -> MaxFrontAccel & MaxSideAcc + * Skid Limit -> Limits Max Acceleration Of the robot to prevent skidding + */ + public static final LinearAcceleration MAX_ACCELERATION_X = MetersPerSecondPerSecond.of(10000000); + public static final LinearAcceleration MAX_ACCELERATION_Y = MetersPerSecondPerSecond.of(10000000); + public static final LinearAcceleration MAX_ACCELERATION_SKID = MetersPerSecondPerSecond.of(10000000); + + /** + * Jerk Control - My Own + */ + public static final Per MAX_JERK = MetersPerSecondPerSecond.per(Seconds).ofNative(100000000); + + //ASSUMES UNIFORM DISTRIBUTION, SHOULD BE CALCULATED EXPERIMENTALLY + public static final Mult, DistanceUnit> THEORETICAL_MOMENT_OF_INERTIA = MASS.times(DRIVEBASE_RADIUS).times(DRIVEBASE_RADIUS).times(1/12); + + /** + * Credit (https://pathplanner.dev/robot-config.html#module-config-options) + * + * Calculate Kangular and Klinear through SysID + * + * Mass * WheelBase_X * Kangular * Klinear + */ + public static final MomentOfInertia MOMENT_OF_INERTIA = MomentOfInertia.ofBaseUnits(THEORETICAL_MOMENT_OF_INERTIA.in(MultUnit.combine(MultUnit.combine(Kilogram, Meters), Meters)), KilogramSquareMeters); + + public static final Translation2d[] modPositions = { + new Translation2d(WHEELBASE_X, WHEELBASE_Y), // FL + new Translation2d(WHEELBASE_X, WHEELBASE_Y.times(-1)), // FR + new Translation2d(WHEELBASE_X.times(-1), WHEELBASE_Y), // BL + new Translation2d(WHEELBASE_X.times(-1), WHEELBASE_Y.times(-1)) // BR + }; + +} diff --git a/src/main/java/frc/lib/Swerve/SwerveDriveState.java b/src/main/java/frc/lib/Swerve/SwerveDriveState.java new file mode 100644 index 0000000..d837c2f --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveDriveState.java @@ -0,0 +1,164 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.team696.lib.Swerve; + +import java.nio.ByteBuffer; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.util.struct.Struct; +import edu.wpi.first.util.struct.StructSerializable; + +/** + * Holds the current State of the robot and last update. + * + *

Should Not be manually updated + */ +public class SwerveDriveState implements StructSerializable { + public Pose2d pose = new Pose2d(); + public ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds(); + public ChassisSpeeds robotAcceleration = new ChassisSpeeds(); + public double timeStamp = 0; + public double timeSinceLastUpdate = -1; + + /** Use This to Scale trust of camera measurements, decreasing it as you get updates, up to you how you want to change it. */ + public double trust = 1e-9; // Never Set To 0 + + public SwerveDriveState(Pose2d pose, ChassisSpeeds speeds, ChassisSpeeds accelerations, double time, double trust) { + update(pose, speeds, accelerations, time, trust); + } + + public SwerveDriveState(Pose2d pose, ChassisSpeeds speeds, ChassisSpeeds acceleration, double time, double timeSinceLastUpdate, double trust) { + this.pose = pose; + this.robotRelativeSpeeds = speeds; + this.robotAcceleration = acceleration; + this.timeStamp = time; + this.timeSinceLastUpdate = timeSinceLastUpdate; + this.trust = trust; + } + + public SwerveDriveState(SwerveDriveState other) { + this(other.pose, other.robotRelativeSpeeds, other.robotAcceleration, other.timeStamp, other.trust); + this.timeSinceLastUpdate = other.timeSinceLastUpdate; + } + + public SwerveDriveState() { + this(new Pose2d(), new ChassisSpeeds(), new ChassisSpeeds(), 0, 1e-9); + } + + /** + * + * @return Current X,Y velocity of the robot in M/S + */ + public double speed() { + return Math.sqrt(robotRelativeSpeeds.vxMetersPerSecond * robotRelativeSpeeds.vxMetersPerSecond + robotRelativeSpeeds.vyMetersPerSecond * robotRelativeSpeeds.vyMetersPerSecond); + } + + /** + * + * @return Current X,Y velocity of robot in M/S as a Vector + */ + public Translation2d velocityXY() { + return new Translation2d(robotRelativeSpeeds.vxMetersPerSecond, robotRelativeSpeeds.vyMetersPerSecond); + } + + public Translation2d accelerationXY() { + return new Translation2d(robotAcceleration.vxMetersPerSecond, robotAcceleration.vyMetersPerSecond); + } + + /** + * + * @return Current Rotational Velocity of the robot in Rad/s + */ + public double angularVelocity() { + return Math.abs(robotRelativeSpeeds.omegaRadiansPerSecond); + } + + /** + * Used interally for updated estimation + * + * @param pose + * @param speeds + * @param time + * @return + */ + public SwerveDriveState update(Pose2d pose, ChassisSpeeds speeds, ChassisSpeeds accelerations, double time, double slippage) { + this.timeSinceLastUpdate = time - this.timeStamp; + if (timeSinceLastUpdate == 0) timeSinceLastUpdate = 1e-6; + + this.pose = pose; + this.robotRelativeSpeeds = speeds; + this.robotAcceleration = accelerations; + this.timeStamp = time; + + this.trust += slippage; + + return this; + } + + public static final SwerveDriveStateStruct struct = new SwerveDriveStateStruct(); + public static class SwerveDriveStateStruct implements Struct { + @Override + public Class getTypeClass() { + return SwerveDriveState.class; + } + + @Override + public String getTypeName() { + return "SwerveDriveState"; + } + + @Override + public int getSize() { + return Pose2d.struct.getSize() + ChassisSpeeds.struct.getSize() + ChassisSpeeds.struct.getSize() + Double.BYTES * 3; + } + + @Override + public String getSchema() { + return "Pose2d pose;ChassisSpeeds velocity;ChassisSpeeds acceleration;double timestamp;double timeSinceLastUpdate;double trust"; + } + + @Override + public Struct[] getNested() { + return new Struct[] {Pose2d.struct, ChassisSpeeds.struct}; + } + + @Override + public SwerveDriveState unpack(ByteBuffer bb) { + Pose2d pose = Pose2d.struct.unpack(bb); + ChassisSpeeds velocity = ChassisSpeeds.struct.unpack(bb); + ChassisSpeeds acceleration = ChassisSpeeds.struct.unpack(bb); + double time = bb.getDouble(); + double timeSinceLastUpdate = bb.getDouble(); + double trust = bb.getDouble(); + return new SwerveDriveState(pose, velocity, acceleration, time, timeSinceLastUpdate, trust); + } + + @Override + public void pack(ByteBuffer bb, SwerveDriveState value) { + Pose2d.struct.pack(bb, value.pose); + ChassisSpeeds.struct.pack(bb, value.robotRelativeSpeeds); + ChassisSpeeds.struct.pack(bb, value.robotAcceleration); + bb.putDouble(value.timeStamp); + bb.putDouble(value.timeSinceLastUpdate); + bb.putDouble(value.trust); + } + + @Override + public boolean isImmutable() { + return true; + } + } +} + + +/** + * Potential Future work -> Incorporate Gyro Readings into Current Acceleration and Store Jerk, Might be cool + * + * Add system for keeping track of confidence of current pose (1690). Overtime accumulate error from odometry -> reset down when getting vision readings ect. + * + * Will help update vision faster and more reliably, don't need to trust shit vision poses when we have gotten a good one and barely moved! + */ \ No newline at end of file diff --git a/src/main/java/frc/lib/Swerve/SwerveDriveSubsystem.java b/src/main/java/frc/lib/Swerve/SwerveDriveSubsystem.java new file mode 100644 index 0000000..3960bde --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveDriveSubsystem.java @@ -0,0 +1,444 @@ +package frc.team696.lib.Swerve; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import java.util.concurrent.locks.ReadWriteLock; +import java.util.concurrent.locks.ReentrantReadWriteLock; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team696.lib.Util; +import frc.team696.lib.HardwareDevices.PigeonFactory; +import frc.team696.lib.Logging.BackupLogger; + +public abstract class SwerveDriveSubsystem extends SubsystemBase { + private final SwerveModulePosition[] _swervePositions = new SwerveModulePosition[4]; + private final SwerveDrivePoseEstimator _poseEstimator; + + protected final SwerveModule[] _modules; + protected final SwerveDriveKinematics _kinematics; + + protected final PigeonFactory _pigeon; + + protected Rotation2d yawOffset = new Rotation2d(0); + + protected final ReadWriteLock _stateLock; + protected final SwerveDriveState _cachedState; + + private final odometryThread _odometryThread; + + private SwerveModuleState[] swerveModuleDesiredStates = { new SwerveModuleState(), new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState() }; + + public SwerveDriveSubsystem() { + this._stateLock = new ReentrantReadWriteLock(); + this._cachedState = new SwerveDriveState(); + + SwerveModule frontLeft = new SwerveModule(SwerveConfigs.FRONT_LEFT); + SwerveModule frontRight = new SwerveModule(SwerveConfigs.FRONT_RIGHT); + SwerveModule backLeft = new SwerveModule(SwerveConfigs.BACK_LEFT); + SwerveModule backRight = new SwerveModule(SwerveConfigs.BACK_RIGHT); + _modules = new SwerveModule[] { frontLeft, frontRight, backLeft, backRight }; + + _kinematics = new SwerveDriveKinematics(SwerveConstants.modPositions); + + for (int i = 0; i < 4; ++i) { + _swervePositions[i] = _modules[i].getPosition(); + } + + _pigeon = new PigeonFactory(0, SwerveConstants.canBus, SwerveConfigs.pigeon, "Pigeon"); + + _poseEstimator = new SwerveDrivePoseEstimator(_kinematics, getYaw(), _swervePositions, + new Pose2d(0, 0, new Rotation2d(0)), VecBuilder.fill(0.1, 0.1, 0.01), VecBuilder.fill(0.3, 0.3, 0.6)); + + zeroYaw(); + + _odometryThread = new odometryThread(this); + _odometryThread.start(); + } + + /** + * + * @return Current Estimated State Of The Robot + * + * @see SwerveDriveState.java + */ + public SwerveDriveState getState() { + try { + this._stateLock.readLock().lock(); + return this._cachedState; + } finally { + this._stateLock.readLock().unlock(); + } + } + + /** + * + * @return Current Estimated Pose2d Of The Robot + */ + public Pose2d getPose() { + return getState().pose; + } + + /** + * Resets The Pose2d Of The Robot To newPose + * + * @param newPose new Pose2d Of The Robot + */ + public void resetPose(Pose2d newPose) { + try { + this._stateLock.writeLock().lock(); + _poseEstimator.resetPosition(getYaw(), _swervePositions, newPose); + } finally { + this._stateLock.writeLock().unlock(); + } + } + + /** + * Resets The Pose Of the Robot to 0,0,0 + */ + public void resetPose() { + resetPose(new Pose2d()); + } + + /** + * Updated The Yaw Offset Of The Robot + *

+ * Used To 0 out the Gyro When Driving + */ + public void updateYawOffset() { + yawOffset = getPose().getRotation().minus(getYaw()); + } + + /** + * Periodically called 50 Hz + *

+ * Replaces the regular periodic function + */ + public abstract void onUpdate(); + + /** + * + * @return Current Yaw Of the Gyro + * + * @see latencyAdjustedYaw() + */ + public Rotation2d getYaw() { + return _pigeon.getYaw(); + } + + /** + * + * @return Latency Adjusted Yaw Of the Gyro + */ + public Rotation2d latencyAdjustedYaw() { + return _pigeon.getLatencyAdjustedYaw(); + } + + /** + * Zeros the gyro + */ + public void zeroYaw() { + yawOffset = getYaw(); + resetPose(new Pose2d(getPose().getTranslation(), + Rotation2d.fromDegrees(Util.getAlliance() == DriverStation.Alliance.Red ? 180 : 0))); + } + + /** + * + * @return Robot Relatives Speeds + * + *

    + *
  • x is forward
    • + *
  • y is left
    • + *
  • r is counterclock wise
    • + */ + public ChassisSpeeds getRobotRelativeSpeeds() { + return getState().robotRelativeSpeeds; + } + + /** + * + * @return Array Of Modules States + */ + protected SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (SwerveModule mod : _modules) { + states[mod.moduleNumber] = mod.getState(); + } + return states; + } + + /** + * + * @return Array of Module Positions + */ + protected SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (SwerveModule mod : _modules) { + states[mod.moduleNumber] = mod.getPosition(); + } + return states; + } + + /** + * Used To drive the robot during teleop + * + * @param translation in X, Y + * @param rotation in R + * @param fieldRelative should forward be based on the field or robot + * @param isOpenLoop closed or open loop control of the drive wheels. + * Typically openLoop for teleop and closed for autonomous + */ + public void Drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) { + ChassisSpeeds desiredRobotSpeeds = new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + if (fieldRelative) + desiredRobotSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotSpeeds, + getYaw().plus(yawOffset).rotateBy(Rotation2d.fromDegrees((Util.getAlliance() == Alliance.Red ? 180 : 0)))); + + SwerveModuleState[] swerveModuleStates = _kinematics.toSwerveModuleStates(desiredRobotSpeeds); + + setModuleStates(swerveModuleStates, isOpenLoop); + } + + /** + * Used to drive during auto + * + * @param c Chassis Speeds object containing the desired speeds of the robot + */ + public void Drive(ChassisSpeeds c) { + SwerveModuleState[] swerveModuleStates = _kinematics.toSwerveModuleStates(c); + setModuleStates(swerveModuleStates); + } + + public void voltageDriveForward(Voltage volts) { + for (SwerveModule mod : _modules) + mod.setDesiredVoltageForward(volts); + } + + public void voltageRotateForward(Voltage volts) { + for (SwerveModule mod : _modules) + mod.setDesiredVoltageRotation(volts); + } + + /** + * Stops driving + */ + public void doNothing() { + Drive(new ChassisSpeeds()); + } + + /** + * @param desiredStates the desired states of each module, in the order of the + * kinematics (FL, FR, BL, BR) + * @param openLoop Open or closed loop control + */ + public void setModuleStates(SwerveModuleState[] desiredStates, boolean openLoop) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_VELOCITY); + + for (SwerveModule mod : _modules) + mod.setDesiredState(desiredStates[mod.moduleNumber], openLoop); + + this.swerveModuleDesiredStates = desiredStates; + } + + /** + * Using closed loop control + * + * @param desiredStates the desired states of each module, in the order of the + * kinematics (FL, FR, BL, BR) + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + setModuleStates(desiredStates, false); + } + + /** + * + * @return List Of Swerve Modules + */ + public SwerveModule[] getModules() { + return _modules; + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Translation2d position) { + return getPose().getTranslation().getDistance(position); + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Pose2d position) { + return distTo(position.getTranslation()); + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Translation2d position) { + Translation2d delta = getPose().getTranslation().minus(position); + Rotation2d rot = Rotation2d.fromRadians(Math.atan2(delta.getY(), delta.getX())); + return rot; + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Pose2d position) { + return angleTo(position.getTranslation()); + } + + /** + * Don't override periodic, override onUpdate() + * + */ + @Override + public final void periodic() { + if (DriverStation.isDisabled()) { + this.updateYawOffset(); + } + + BackupLogger.addToQueue("Swerve/ModuleStates", getModuleStates()); + + BackupLogger.addToQueue("Swerve/DesiredModuleStates", swerveModuleDesiredStates); + + BackupLogger.addToQueue("Swerve/RobotState", getState()); + + onUpdate(); + } + + /** + * Adds a vision measurement to the current estimation + * + * @param visionPose Vision Pose + * @param visionTimestamp Vision Timestamp + * @param stdDeviations Standard Deviations of vision measurement + */ + public void addVisionMeasurement(Pose2d visionPose, double visionTimestamp, Vector stdDeviations) { + try { + this._stateLock.writeLock().lock(); + + _poseEstimator.addVisionMeasurement(visionPose, visionTimestamp, stdDeviations); + } finally { + this._stateLock.writeLock().unlock(); + } + } + + class odometryThread extends Thread { + private SwerveDriveSubsystem this0; + + private BaseStatusSignal[] _allSignals; + + odometryThread(SwerveDriveSubsystem this0) { + this.this0 = this0; + + this.setName("OdometryThread"); + this.setDaemon(true); + this.setPriority(MIN_PRIORITY + 1); // Slightly Above Minimum + } + + public void run() { + _allSignals = new BaseStatusSignal[this0._modules.length * SwerveModule.SIGNAL_COUNT + 2 + 3]; + for (SwerveModule mod : this0._modules) { + + BaseStatusSignal[] signals = mod.getSignals(); + + for (int i = 0; i < SwerveModule.SIGNAL_COUNT; i++) { + _allSignals[mod.moduleNumber * SwerveModule.SIGNAL_COUNT + i] = signals[i]; + } + } + _allSignals[_allSignals.length - 2] = this0._pigeon._yawSignal; + _allSignals[_allSignals.length - 1] = this0._pigeon._yawVelocitySignal; + + _allSignals[_allSignals.length - 5] = this0._pigeon.get().getAccelerationX(); + _allSignals[_allSignals.length - 4] = this0._pigeon.get().getAccelerationY(); + _allSignals[_allSignals.length - 3] = this0._pigeon.get().getAccelerationZ(); + + BaseStatusSignal.setUpdateFrequencyForAll(250, _allSignals); + ChassisSpeeds acceleration = new ChassisSpeeds(); + + while (true) { + try { + BaseStatusSignal.refreshAll(_allSignals); + + this.this0._stateLock.writeLock().lock(); + + for (int i = 0; i < 4; ++i) { + _swervePositions[i] = _modules[i].getPosition(); + } + + ChassisSpeeds speeds = _kinematics.toChassisSpeeds(getModuleStates()); + + double time = System.currentTimeMillis(); + double timeSinceLastUpdate = time - _cachedState.timeStamp; + if (timeSinceLastUpdate == 0) + timeSinceLastUpdate = 1e-9; + + Pose2d newPose = _poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), latencyAdjustedYaw(), + _swervePositions); + + double slippage = Math.abs(speeds.omegaRadiansPerSecond - _pigeon.getAngularVelocity().in(RadiansPerSecond)) + * Math.sqrt(speeds.vxMetersPerSecond * speeds.vxMetersPerSecond + + speeds.vyMetersPerSecond * speeds.vyMetersPerSecond); + + double measuredSensorAcceleration = Math + .sqrt(Math.pow(_allSignals[_allSignals.length - 3].getValueAsDouble(), 2) + + Math.pow(_allSignals[_allSignals.length - 4].getValueAsDouble(), 2) + + Math.pow(_allSignals[_allSignals.length - 5].getValueAsDouble(), 2)); + + acceleration.vxMetersPerSecond = (speeds.vxMetersPerSecond + - _cachedState.robotRelativeSpeeds.vxMetersPerSecond) / (timeSinceLastUpdate / 1000); + acceleration.vyMetersPerSecond = (speeds.vyMetersPerSecond + - _cachedState.robotRelativeSpeeds.vyMetersPerSecond) / (timeSinceLastUpdate / 1000); + acceleration.omegaRadiansPerSecond = (speeds.omegaRadiansPerSecond + - _cachedState.robotRelativeSpeeds.omegaRadiansPerSecond) / (timeSinceLastUpdate / 1000); + + double accelerationMagnitude = acceleration.vxMetersPerSecond * acceleration.vxMetersPerSecond + + acceleration.vyMetersPerSecond * acceleration.vyMetersPerSecond; + + BackupLogger.addToQueue("sensorAcceleration", + Units.Gs.of(measuredSensorAcceleration - 1).in(Units.MetersPerSecondPerSecond)); + BackupLogger.addToQueue("acceleration", accelerationMagnitude); + + this.this0._cachedState.update( + newPose, + speeds, + acceleration, + time, + slippage); + } finally { + this.this0._stateLock.writeLock().unlock(); + } + Timer.delay(1.0 / 250.0); // Limits To 250 Hz + } + } + } +} diff --git a/src/main/java/frc/lib/Swerve/SwerveModule.java b/src/main/java/frc/lib/Swerve/SwerveModule.java new file mode 100644 index 0000000..8b1aeee --- /dev/null +++ b/src/main/java/frc/lib/Swerve/SwerveModule.java @@ -0,0 +1,185 @@ +package frc.team696.lib.Swerve; + +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.PerUnit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import frc.team696.lib.Util; +import frc.team696.lib.HardwareDevices.CANCoderFactory; +import frc.team696.lib.HardwareDevices.TalonFactory; + +public class SwerveModule{ + public static int s_moduleCount = 0; + public static final String[] moduleNames = {"FL", "FR", "BL", "BR"}; + + private final StatusSignal _encoderSignal; + private final StatusSignal _anglePositionSignal; + private final StatusSignal _angleVelocitySignal; + private final StatusSignal _drivePositionSignal; + private final StatusSignal _driveVelocitySignal; + + public static final int SIGNAL_COUNT = 4; + + public final int moduleNumber; + private Rotation2d _angleOffset; + + private TalonFactory _angleMotor; + private TalonFactory _driveMotor; + private CANCoderFactory _encoder; + + private Rotation2d _lastAngle; + + private final SimpleMotorFeedforward _driveFeedForward = new SimpleMotorFeedforward(SwerveConstants.drivekS.in(Volts), SwerveConstants.drivekV.in(PerUnit.combine(Volts, MetersPerSecond)), SwerveConstants.drivekA.in(PerUnit.combine(Volts,MetersPerSecondPerSecond))); + + private final VelocityVoltage _driveVelocity = new VelocityVoltage(0); + + private final PositionVoltage _anglePosition = new PositionVoltage(0); + + private final double[] _sysidRotationPositions = new double[] {135, -135, -45, 45}; + + + public SwerveModule(SwerveModuleConstants moduleConstants){ + this.moduleNumber = s_moduleCount++; + this._angleOffset = Rotation2d.fromRotations(moduleConstants.EncoderOffset); + /* Angle Encoder Config */ + _encoder = new CANCoderFactory(moduleConstants.EncoderId, SwerveConstants.canBus, SwerveConfigs.canCoder, "Swerve Encoder " + moduleNumber); + + _encoderSignal = ( _encoder.get().getAbsolutePosition()); + + /* Angle Motor Config */ + _angleMotor = new TalonFactory(moduleConstants.SteerMotorId, SwerveConstants.canBus, SwerveConfigs.angle, "Swerve Angle " + moduleNumber); + resetToAbsolute(); + + /* Drive Motor Config */ + _driveMotor = new TalonFactory(moduleConstants.DriveMotorId, SwerveConstants.canBus, SwerveConfigs.drive, "Swerve Drive " + moduleNumber); + + _anglePositionSignal = (_angleMotor.get().getPosition()); + _angleVelocitySignal = (_angleMotor.get().getVelocity()); + _drivePositionSignal = (_driveMotor.get().getPosition()); + _driveVelocitySignal = (_driveMotor.get().getVelocity()); + + BaseStatusSignal.setUpdateFrequencyForAll(100, _encoderSignal, _anglePositionSignal, _angleVelocitySignal, _drivePositionSignal, _driveVelocitySignal); + + ParentDevice.optimizeBusUtilizationForAll(_encoder.get(), _angleMotor.get(), _driveMotor.get()); + + _lastAngle = getState().angle; + } + + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop){ + desiredState.optimize(getState().angle); + desiredState.cosineScale(getState().angle); + Rotation2d angle = ((Math.abs(desiredState.speedMetersPerSecond) <= (SwerveConstants.MAX_VELOCITY.in(Units.MetersPerSecond) * 0.01)) ? _lastAngle : desiredState.angle); + _angleMotor.setControl(_anglePosition.withPosition(angle.getRotations())); + setSpeed(desiredState, isOpenLoop); + _lastAngle = angle; + } + + // For Use In SysID + public void setDesiredVoltageForward(Voltage volts) { + _driveMotor.VoltageOut(volts); + _angleMotor.setControl(_anglePosition.withPosition(0)); + } + public void setDesiredVoltageRotation(Voltage volts) { + _driveMotor.VoltageOut(volts); + _angleMotor.setControl(_anglePosition.withPosition(_sysidRotationPositions[moduleNumber])); + } + public Voltage getDriveOutputVoltage() { + return Volts.of(_driveMotor.get().get() * RobotController.getBatteryVoltage()); + } + + private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop){ + if(isOpenLoop){ + _driveMotor.VoltageOut(Volts.of(desiredState.speedMetersPerSecond / SwerveConstants.MAX_VELOCITY.in(Units.MetersPerSecond)*12)); + } else { + _driveVelocity.Velocity = Util.MPSToRPS(desiredState.speedMetersPerSecond, SwerveConstants.WHEEL_CIRCUM.in(Units.Meters)); + _driveVelocity.FeedForward = _driveFeedForward.calculate(desiredState.speedMetersPerSecond); + _driveMotor.setControl(_driveVelocity); + } + } + + public Rotation2d getCANCoderAngle(boolean refresh){ + if (refresh) { + _encoderSignal.refresh(); + } + + return Rotation2d.fromRotations(_encoderSignal.getValueAsDouble()); + } + + public Rotation2d getCANCoderAngle(){ + return getCANCoderAngle(true); + } + + public void resetToAbsolute(){ + Rotation2d absolutePosition = getCANCoderAngle(true).minus(_angleOffset); + _angleMotor.setPosition(absolutePosition.getRotations()); + } + + public SwerveModuleState getState(boolean refresh){ + if (refresh) { + _driveVelocitySignal.refresh(); + } + return new SwerveModuleState( + Util.RPSToMPS(_driveVelocitySignal.getValueAsDouble(), SwerveConstants.WHEEL_CIRCUM.in(Units.Meters)), + Rotation2d.fromRotations(getAngleMotorPosition(refresh).in(Rotations)) + ); + } + + public SwerveModuleState getState(){ + return getState(false); + } + + public SwerveModulePosition getPosition(){ + return new SwerveModulePosition( + Util.rotationsToMeters(getDriveMotorPosition().in(Rotations), SwerveConstants.WHEEL_CIRCUM.in(Units.Meters)), + Rotation2d.fromRotations(getAngleMotorPosition().in(Rotations)) + ); + } + + public Angle getDriveMotorPosition(boolean refresh) { + if (refresh) { + _drivePositionSignal.refresh(); + _driveVelocitySignal.refresh(); + } + return Rotations.of(BaseStatusSignal.getLatencyCompensatedValue(_drivePositionSignal, _driveVelocitySignal).magnitude()); + } + + public Angle getDriveMotorPosition() { + return getDriveMotorPosition(false); + } + + public Angle getAngleMotorPosition(boolean refresh) { + if (refresh) { + _anglePositionSignal.refresh(); + _angleVelocitySignal.refresh(); + } + return Rotations.of(BaseStatusSignal.getLatencyCompensatedValue(_anglePositionSignal, _angleVelocitySignal).magnitude()); + } + + public Angle getAngleMotorPosition() { + return getAngleMotorPosition(false); + } + + public BaseStatusSignal[] getSignals(){ + return new BaseStatusSignal[] {_anglePositionSignal, _angleVelocitySignal, _drivePositionSignal, _driveVelocitySignal, _encoderSignal}; + } +} \ No newline at end of file diff --git a/src/main/java/frc/lib/Util.java b/src/main/java/frc/lib/Util.java new file mode 100644 index 0000000..4a7b1ea --- /dev/null +++ b/src/main/java/frc/lib/Util.java @@ -0,0 +1,208 @@ +package frc.team696.lib; + +import java.io.IOException; +import java.net.NetworkInterface; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Enumeration; +import java.util.HashMap; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Function; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.team696.lib.Logging.PLog; + +/** + * Useful functions which don't fit anywhere specific + */ +public class Util { + + /** + * Linear Interpolates between two numbers + * + * @param t Value from [0,1] + * @param min Lower value --> 0 + * @param max Upper value --> 1 + * @return Interpolated value between min and max + */ + public static double lerp(double t, double min, double max) { + return (max - min) * t + min; + } + + /** + * @param val Value to be clamped + * @param min Minimum Value + * @param max Maximum Value + * @return Value Clamped between min and max + */ + public static double clamp(double val, double min, double max) { + return Math.max(Math.min(max, val), min); + } + + public static double map(double x, double in_min, double in_max, double out_min, double out_max){ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + } + + /** + * @return Current Alliance through driverstation or FMS. Defaults to Blue. + */ + public static Alliance getAlliance() { + if (DriverStation.getAlliance().isPresent()) { + return DriverStation.getAlliance().get(); + } else { + return Alliance.Blue; + } + } + + /** + * @param wheelRPS Wheel Velocity: (in Rotations per Second) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Velocity: (in Meters per Second) + */ + public static double RPSToMPS(double wheelRPS, double circumference){ + double wheelMPS = wheelRPS * circumference; + return wheelMPS; + } + + /** + * @param wheelMPS Wheel Velocity: (in Meters per Second) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Velocity: (in Rotations per Second) + */ + public static double MPSToRPS(double wheelMPS, double circumference){ + double wheelRPS = wheelMPS / circumference; + return wheelRPS; + } + + /** + * @param wheelRotations Wheel Position: (in Rotations) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Distance: (in Meters) + */ + public static double rotationsToMeters(double wheelRotations, double circumference){ + double wheelMeters = wheelRotations * circumference; + return wheelMeters; + } + + /** + * @param wheelMeters Wheel Distance: (in Meters) + * @param circumference Wheel Circumference: (in Meters) + * @return Wheel Position: (in Rotations) + */ + public static double metersToRotations(double wheelMeters, double circumference){ + double wheelRotations = wheelMeters / circumference; + return wheelRotations; + } + + /** + * + * @return List of connected MAC Addresses + * @throws IOException + */ + public static List getMacAddresses() throws IOException { + List macAddresses = new ArrayList<>(); + + Enumeration networkInterfaces = NetworkInterface.getNetworkInterfaces(); + NetworkInterface networkInterface; + while (networkInterfaces.hasMoreElements()) { + networkInterface = networkInterfaces.nextElement(); + + byte[] address = networkInterface.getHardwareAddress(); + if (address == null) { + continue; + } + + macAddresses.add(address); + } + return macAddresses; + } + + public static String macToString(byte[] address) { + StringBuilder builder = new StringBuilder(); + for (int i = 0; i < address.length; i++) { + if (i != 0) { + builder.append(':'); + } + builder.append(String.format("%02X", address[i])); + } + return builder.toString(); + } + + /** Returns the current detected robot based on map of robot name to mac address + *

    + *

      + *
    • -1 is simulation
      • + *
    • 0 is unknown
      • + *
    • 1 is first item in map... etc.
      • + *
    + *

    + * verbose will print to console, which robot connected, or the MAC if unknown + * + *

    + * ex: (RobotInit()): Util.setRobotType(new LinkedHashMap<>() { }, true); + */ + public static int setRobotType(LinkedHashMap nameToMac, boolean verbose) { + if (RobotBase.isSimulation()) { + if (verbose) { + PLog.info("Robot", "Simulation Detected"); + } + return -1; + } + + List macAddresses; + try { + macAddresses = Util.getMacAddresses(); + } catch (IOException e) { + PLog.fatalException("Robot", "Mac Address Attempt Unsuccessful", e); + macAddresses = List.of(); + return 0; + } + + for (byte[] macAddress : macAddresses) { + int i = 1; + for (Map.Entry robotEntry : nameToMac.entrySet()) { + if (Arrays.compare(robotEntry.getValue(), macAddress) == 0) { + if (verbose) { + PLog.info("Robot", String.format("%s Detected", robotEntry.getKey())); + } + return i; + } + i++; + } + } + + if (verbose) { + PLog.info("Robot", "Unknown Robot Detected"); + for (byte[] macAddress : macAddresses) { + PLog.info(" ", Util.macToString(macAddress)); + } + } + return 0; + } + public static Map transformMap(Map originalMap, Function transformer) { + Map resultMap = new HashMap<>(); + for (Map.Entry entry : originalMap.entrySet()) { + resultMap.put(entry.getKey(), transformer.apply(entry.getValue())); + } + return resultMap; + } + + public static Map> transformNestedMap( + Map> originalMap, Function transformer) { + Map> resultMap = new HashMap<>(); + + for (Map.Entry> outerEntry : originalMap.entrySet()) { + Map innerResultMap = new HashMap<>(); + for (Map.Entry innerEntry : outerEntry.getValue().entrySet()) { + innerResultMap.put(innerEntry.getKey(), transformer.apply(innerEntry.getValue())); + } + resultMap.put(outerEntry.getKey(), innerResultMap); + } + + return resultMap; + } +} diff --git a/src/main/java/frc/robot/BotConstants.java b/src/main/java/frc/robot/BotConstants.java index 4831a02..a2c2a48 100644 --- a/src/main/java/frc/robot/BotConstants.java +++ b/src/main/java/frc/robot/BotConstants.java @@ -7,100 +7,114 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import frc.robot.util.TriggerNTDouble; - /** * Constants for robot mechanisms unrelated to the drive train */ public class BotConstants { - // The CAN Bus attached directly to the RoboRIO, used for devices controlling - // non-drivetrain mechanisms - public static CANBus rioBus; - // Used for drivetrain devices (motors, encoders, IMU, etc.). Supports CAN FD. - public static CANBus canivoreBus; + // The CAN Bus attached directly to the RoboRIO, used for devices controlling + // non-drivetrain mechanisms + public static CANBus rioBus; + // Used for drivetrain devices (motors, encoders, IMU, etc.). Supports CAN FD. + public static CANBus canivoreBus; + static { + rioBus = new CANBus("rio"); + canivoreBus = new CANBus("cv"); + } + + public static class Elevator { + public static int masterID = 11; + public static int slaveId = 10; + public static TalonFXConfiguration cfg; static { - rioBus = new CANBus("rio"); - canivoreBus = new CANBus("cv"); + cfg = new TalonFXConfiguration(); + cfg.Slot0.kP = 16.;//6.; + cfg.Slot0.kG = 0.05; + // cfg.Slot0.kS = 0.02; + cfg.Slot0.GravityType = GravityTypeValue.Elevator_Static; + cfg.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + + cfg.MotionMagic.MotionMagicCruiseVelocity = 100.; + cfg.MotionMagic.MotionMagicAcceleration = 120.; + //cfg.MotionMagic.MotionMagicJerk= 500; + cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; + cfg.CurrentLimits.StatorCurrentLimit = 120.; + cfg.CurrentLimits.StatorCurrentLimitEnable = true; + } - public static class Elevator { - public static int masterID = 11; - public static int slaveId = 10; - public static TalonFXConfiguration cfg; - static { - cfg = new TalonFXConfiguration(); - cfg.Slot0.kP = 6.; - //cfg.Slot0.kG = 0.05; - //cfg.Slot0.kS = 0.02; - cfg.Slot0.GravityType = GravityTypeValue.Elevator_Static; - cfg.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; - - cfg.MotionMagic.MotionMagicCruiseVelocity = 100.; - cfg.MotionMagic.MotionMagicAcceleration = 75.; - cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - - } + } + public static class Arm { + public static int masterID = 13; + public static TalonFXConfiguration cfg = new TalonFXConfiguration(); + static { + cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; + cfg.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + cfg.Slot0.kP = 8.; + cfg.CurrentLimits.StatorCurrentLimitEnable = true; + cfg.CurrentLimits.StatorCurrentLimit = 120.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 80.; + cfg.MotionMagic.MotionMagicAcceleration = 65.; + + cfg.Slot0.kG = 0; } + } - public static class Arm { - public static int masterID = 13; - public static int slaveID=16; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; - cfg.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - cfg.Slot0.kP = 8.; - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 80.; - cfg.MotionMagic.MotionMagicAcceleration = 80.; - - cfg.Slot0.kG = 0; - } + public static class Wrist { + public static int motorID = 14; + public static TalonFXConfiguration cfg = new TalonFXConfiguration(); + static { + cfg.Slot0.kP = 32.; + cfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; + cfg.MotionMagic.MotionMagicAcceleration = 80.; + cfg.MotionMagic.MotionMagicCruiseVelocity = 40.; + cfg.CurrentLimits.StatorCurrentLimitEnable = false; + cfg.CurrentLimits.SupplyCurrentLimitEnable = false; + cfg.CurrentLimits.StatorCurrentLimit = 120.; } - public static class Wrist { - public static int motorID = 14; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.Slot0.kP = 32.; - - cfg.MotionMagic.MotionMagicAcceleration = 16.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 10.; - cfg.CurrentLimits.StatorCurrentLimitEnable = false; - cfg.CurrentLimits.SupplyCurrentLimitEnable=false; - cfg.CurrentLimits.StatorCurrentLimit = 80.; - } + } + public static class EndEffector { + public static int motorID = 15; + public static TalonFXConfiguration cfg = new TalonFXConfiguration(); + static { + cfg.CurrentLimits.StatorCurrentLimitEnable = true; + cfg.CurrentLimits.StatorCurrentLimit = 80.; } + } - public static class EndEffector { - public static int motorID = 15; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - } + public static class GroundCoral { + public static int angleId = 17; + public static int rollerId = 18; + public static TalonFXConfiguration angleCfg = new TalonFXConfiguration(); + public static TalonFXConfiguration rollerCfg = new TalonFXConfiguration(); + static { + angleCfg.Slot0.kP = 1.; + angleCfg.CurrentLimits.StatorCurrentLimit = 120; + angleCfg.CurrentLimits.StatorCurrentLimitEnable = true; + angleCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + angleCfg.MotionMagic.MotionMagicCruiseVelocity = 60; + angleCfg.MotionMagic.MotionMagicAcceleration = 30; + + rollerCfg.CurrentLimits.StatorCurrentLimit = 120; + rollerCfg.CurrentLimits.StatorCurrentLimitEnable = true; } - public static class ClimberIntake { - public static int masterID = 0; - public static int slaveID = 1; - public static TalonFXConfiguration cfg = new TalonFXConfiguration(); - static { - cfg.MotorOutput.NeutralMode = NeutralModeValue.Coast; - cfg.CurrentLimits.StatorCurrentLimitEnable = true; - cfg.CurrentLimits.StatorCurrentLimit = 120.; - cfg.MotionMagic.MotionMagicCruiseVelocity = 20.; - cfg.MotionMagic.MotionMagicAcceleration = 15.; - cfg.MotionMagic.MotionMagicJerk = 30.; - } + } + + public static class Climber{ + public static int motorID=19; + public static TalonFXConfiguration cfg=new TalonFXConfiguration(); + static{ + cfg.Slot0.kP=1; + cfg.CurrentLimits.StatorCurrentLimitEnable=false; + } + } } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 536f0f3..dfd3494 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025BuildSeason"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 36; - public static final String GIT_SHA = "93c25fb9e2162579fff06051c4e0eaa98931c0f9"; - public static final String GIT_DATE = "2025-03-01 18:57:22 PST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2025-03-03 14:02:14 PST"; - public static final long BUILD_UNIX_TIME = 1741039334068L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 73; + public static final String GIT_SHA = "23d68b0cc9b2b5f702ed325f4ee68414f572fd28"; + public static final String GIT_DATE = "2025-10-06 18:41:54 PDT"; + public static final String GIT_BRANCH = "postLA"; + public static final String BUILD_DATE = "2025-10-13 14:32:18 PDT"; + public static final long BUILD_UNIX_TIME = 1760391138186L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/HumanControls.java b/src/main/java/frc/robot/HumanControls.java index de1e7f8..02f4934 100644 --- a/src/main/java/frc/robot/HumanControls.java +++ b/src/main/java/frc/robot/HumanControls.java @@ -4,67 +4,44 @@ package frc.robot; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.team696.lib.Swerve.Commands.TeleopSwerve; -/** Add your docs here. */ public final class HumanControls { - /*public final class OperatorPanel2024{ - public static final Joystick operatorPanel = new Joystick(1); - public static final Joystick operatorPanelB = new Joystick(2); + public final class OperatorPanel2025{ + public static final Joystick OperatorPanel=new Joystick(1); + + public static final JoystickButton L1=new JoystickButton(OperatorPanel, 9); + public static final JoystickButton L2=new JoystickButton(OperatorPanel, 10); + public static final JoystickButton L3=new JoystickButton(OperatorPanel, 11); + public static final JoystickButton L4=new JoystickButton(OperatorPanel, 12); + //public static final JoystickButton F1=new JoystickButton(null, 0); + //public static final JoystickButton F2=null; + //public static final JoystickButton F3=null; + public static final JoystickButton releaseCoral=new JoystickButton(OperatorPanel, 7); + public static final JoystickButton pickupAlgae=new JoystickButton(OperatorPanel, 8); + public static final JoystickButton gyro=new JoystickButton(OperatorPanel, 13); + public static final JoystickButton Climb1=new JoystickButton(OperatorPanel, 4); + public static final JoystickButton Climb2=new JoystickButton(OperatorPanel, 3); + public static final JoystickButton SouceCoral=new JoystickButton(OperatorPanel, 5); + public static final JoystickButton GroundCoral=new JoystickButton(OperatorPanel, 6); + public static final JoystickButton Barge=new JoystickButton(OperatorPanel,2); + public static final JoystickButton Processor=new JoystickButton(OperatorPanel, 1); - public static final JoystickButton Shoot = new JoystickButton(operatorPanel, 1); - public static final JoystickButton Amp = new JoystickButton(operatorPanel, 2); - public static final JoystickButton ExtraA = new JoystickButton(operatorPanel, 3); - public static final JoystickButton Trap = new JoystickButton(operatorPanel, 4); - public static final JoystickButton ExtraB = new JoystickButton(operatorPanel, 6); - public static final JoystickButton Ground = new JoystickButton(operatorPanel, 7); - public static final JoystickButton Source = new JoystickButton(operatorPanel, 8); - public static final JoystickButton Rollers = new JoystickButton(operatorPanel, 9); - public static final JoystickButton Drop = new JoystickButton(operatorPanel, 10); - public static final JoystickButton ExtraC = new JoystickButton(operatorPanel,11); + public static final JoystickButton leftOrRight=new JoystickButton(OperatorPanel, 14); + public static final JoystickButton unlabedSwitch=new JoystickButton(OperatorPanel, 15); + public static final JoystickButton deepOrSwitch=new JoystickButton(OperatorPanel, 16); - public static final JoystickButton Climb = new JoystickButton(operatorPanelB, 2); - public static final JoystickButton OhShit = new JoystickButton(operatorPanelB, 3); - public static final JoystickButton Rightest = new JoystickButton(operatorPanelB, 4); - public static final JoystickButton Right = new JoystickButton(operatorPanelB, 5); - public static final JoystickButton Left = new JoystickButton(operatorPanelB, 6); - public static final JoystickButton Gyro = new JoystickButton(operatorPanelB, 7); - public static final JoystickButton Leftest = new JoystickButton(operatorPanelB, 8); - }*/ - public final class OperatorPanel2025{ - public static final JoystickButton L1=null; - public static final JoystickButton L2=null; - public static final JoystickButton L3=null; - public static final JoystickButton L4=null; - public static final JoystickButton F1=null; - public static final JoystickButton F2=null; - public static final JoystickButton F3=null; - public static final JoystickButton releaseCoral=null; - public static final JoystickButton pickupAlgae=null; - public static final JoystickButton gyro=null; - public static final JoystickButton Climb1=null; - public static final JoystickButton Climb2=null; - public static final JoystickButton SouceCoral=null; - public static final JoystickButton GroundCoral=null; - public static final JoystickButton Barge=null; - public static final JoystickButton Processor=null; - public static final JoystickButton leftOrRight=null; // Use one of the switches on the driver station for this - public static final BooleanSupplier manualOverride=null; + //public static final BooleanSupplier manualOverride=new JoystickButton(null, 0); } public final class DriverPanel{ public static final Joystick DriverPanel=new Joystick(0); - public static final Joystick OperatorPanelA=new Joystick(1); - public static final Joystick OperatorPanelB=new Joystick(2); - public static final Joystick OperatorPanelC=new Joystick(3); public static final JoystickButton resetGyro=new JoystickButton(DriverPanel, 1); public static final JoystickButton OtherButton=new JoystickButton(DriverPanel,2); @@ -92,8 +69,4 @@ public final class SingleXboxController{ public static final Trigger RT = controller.rightTrigger(0.5); } - public final class SinglePS4Controller{ - - } - } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b43d13e..8b96040 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,51 +8,49 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathfindingCommand; import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.net.WebServer; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.AutoMoveSuperStructure; import frc.robot.commands.MoveSuperStructure; import frc.robot.commands.PIDtoNearest; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wrist; import frc.robot.util.GameInfo; -import frc.robot.util.GameInfo.CoralScoringPosition; -import frc.team696.lib.Camera.LimelightHelpers; +import frc.robot.util.GameInfo.ReefSide; import frc.team696.lib.Logging.BackupLogger; +import frc.team696.lib.Swerve.SwerveConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.EndEffector; -import frc.robot.HumanControls.OperatorPanel2025; public class Robot extends TimedRobot { private Command m_autonomousCommand; - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); - private double MaxRotationalRate = RotationsPerSecond.of(3).in(RadiansPerSecond); + private double MaxSpeed = SwerveConstants.MAX_VELOCITY.in(MetersPerSecond);// aTunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxRotationalRate = RotationsPerSecond.of(/*10*/7).in(RadiansPerSecond); private SwerveTelemetry m_SwerveTelemetry = new SwerveTelemetry(MaxSpeed); - private void configureBinds() { - OperatorPanel2025.gyro.onTrue(new InstantCommand(() -> Swerve.get().seedFieldCentric())); - OperatorPanel2025.releaseCoral.onTrue(new InstantCommand()); - } + private ProfiledPIDController thetaController = new ProfiledPIDController(1. / 200., 0, 0., + new TrapezoidProfile.Constraints(360, 480)); private void logBuildInfo() { BackupLogger.addToQueue("BuildConstants/ProjectName", BuildConstants.MAVEN_NAME); @@ -78,23 +76,6 @@ public double applyDeadband(double x, double deadband) { return Math.abs(x) < deadband ? 0 : x; } - public void putCommandButtons() { - SmartDashboard.putData("pathfindToMiddle", new PrintCommand("s").andThen( - AutoBuilder.pathfindToPose(new Pose2d(7, 3, Rotation2d.fromDegrees(12)), new PathConstraints(1, 1, Math.PI, Math.PI)))); - SmartDashboard.putData("ScoreNet", new MoveSuperStructure(GameInfo.Net, 0)); - SmartDashboard.putData("ScoreL4", new MoveSuperStructure(GameInfo.L4,0.6)); - SmartDashboard.putData("ScoreL3", new MoveSuperStructure(GameInfo.L3, 0.6)); - SmartDashboard.putData("ScoreL2", new MoveSuperStructure(GameInfo.L2, 0.6)); - SmartDashboard.putData("ScoreL1", new MoveSuperStructure(GameInfo.L1, 0.6)); - SmartDashboard.putData("Ground", new MoveSuperStructure(GameInfo.ground, 0)); - SmartDashboard.putData("Spin Rollers Forward", EndEffector.get().spin(0.6)); - SmartDashboard.putData("Spin Rollers Reverse", EndEffector.get().spin(-0.6)); - SmartDashboard.putData("IntakeSource", new MoveSuperStructure(GameInfo.Intake, 0.0)); - SmartDashboard.putData("Climb Up", new MoveSuperStructure(GameInfo.ClimbUp, 0)); - SmartDashboard.putData("Climb Down", new MoveSuperStructure(GameInfo.ClimbDown, 0)); - - } - public void putSwerveSysIDCalibrationButtons() { SmartDashboard.putData("CTRESwerveCalibrationc/DynamicForward", Swerve.get().sysIdDynamic(SysIdRoutine.Direction.kForward)); @@ -108,83 +89,183 @@ public void putSwerveSysIDCalibrationButtons() { private final SendableChooser autoChooser; - public Robot() { - - // For remote layout downloading - WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); - - SmartDashboard.putData(CommandScheduler.getInstance()); - SmartDashboard.putData(Elevator.get()); - // SmartDashboard.putData(ClimberIntake.get()); - SmartDashboard.putData(Arm.get()); - SmartDashboard.putData(EndEffector.get()); - SmartDashboard.putData(Wrist.get()); - - Swerve.get().registerTelemetry(m_SwerveTelemetry::telemeterize); + private Command OldTimesAuto(){ + SequentialCommandGroup commandGroup=new SequentialCommandGroup(); + PathConstraints constraints=new PathConstraints(2,1, 15, 10); + for(var index:GameInfo.getScoringPoses().entrySet()){ + Pose2d goalPoseLeft=index.getValue().get(GameInfo.ReefSide.Left); + Pose2d goalPoseRight=index.getValue().get(GameInfo.ReefSide.Right); + + commandGroup.addCommands( + AutoBuilder.pathfindToPose(goalPoseLeft, constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy(), + AutoBuilder.pathfindToPose(new Pose2d(1.397, 7.55, Rotation2d.fromDegrees(35)), constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy(), + AutoBuilder.pathfindToPose(goalPoseRight, constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy(), + AutoBuilder.pathfindToPose(new Pose2d(1.397, 7.55, Rotation2d.fromDegrees(35)), constraints), + new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy() + ); + } + return commandGroup; + } + public Robot() { + // TODO: strip out groundcoral system + thetaController.enableContinuousInput(-180, 180); + Arm.get(); + Elevator.get(); + EndEffector.get(); + Swerve.get(); + Wrist.get(); DriverStation.silenceJoystickConnectionWarning(true); - configureDriverStationBinds(); - - // Log Build information logBuildInfo(); - - // TODO: Restore TeleopSwerve + SignalLogger.start(); + configureDriverStationBinds(); Swerve.get().setDefaultCommand(Swerve.get().applyRequest( () -> Swerve.fcDriveReq.withVelocityX( - applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.1) * MaxSpeed) - .withVelocityY(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.1) * MaxSpeed) + Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) + .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) .withRotationalRate( - applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.1) * MaxRotationalRate))); - SmartDashboard.putData("Reset Gyro", Commands.runOnce(() -> Swerve.get().seedFieldCentric())); - - NamedCommands.registerCommand("PIDtoNearest", new PIDtoNearest(true)); - NamedCommands.registerCommand("ScoreL4", - Elevator.get().positionCommand(GameInfo.L4)); - NamedCommands.registerCommand("ScoreL3", - Arm.get().Position(GameInfo.L3).alongWith(Elevator.get().positionCommand(GameInfo.L3))); - NamedCommands.registerCommand("ScoreL2", Elevator.get().positionCommand(GameInfo.L2)); - NamedCommands.registerCommand("ScoreL1", Elevator.get().positionCommand(GameInfo.L1)); - putCommandButtons(); - NamedCommands.registerCommand("hello", new InstantCommand( - () -> SmartDashboard.putBoolean("auto", true))); - - // TODO: decide whether or not this will be a temporary fix + Math.pow(applyDeadband(HumanControls.DriverPanel.rightJoyX.getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.rightJoyX.getAsDouble()) * MaxRotationalRate))); + + /* + * HumanControls.DriverPanel.OtherButton.whileTrue(Swerve.get().applyRequest( + * () -> Swerve.fcDriveReq.withVelocityX( + * Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), + * 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) + * .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX. + * getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) + * + * .withRotationalRate( + * (thetaController.calculate(Swerve.get().getPose().getRotation().getDegrees(), + * Swerve.get().goalRotation.get().getDegrees())) + * MaxRotationalRate)) + * .alongWith( + * Commands.startEnd(() -> + * thetaController.reset(Swerve.get().getPose().getRotation().getDegrees()), () + * -> { + * }))); + */ + HumanControls.DriverPanel.OtherButton.whileTrue(Swerve.get().applyRequest( + () -> Swerve.fcDriveReq.withVelocityX( + Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyY.getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyY.getAsDouble()) * MaxSpeed) + .withVelocityY(Math.pow(applyDeadband(HumanControls.DriverPanel.leftJoyX.getAsDouble(), 0.09), 2) + * Math.signum(HumanControls.DriverPanel.leftJoyX.getAsDouble()) * MaxSpeed) + + .withRotationalRate( + (thetaController.calculate(Swerve.get().getPose().getRotation().getDegrees(), + Swerve.get().getGoalRotation().getDegrees())) + * MaxRotationalRate)) + .alongWith( + Commands.startEnd(() -> thetaController.reset(Swerve.get().getPose().getRotation().getDegrees()), () -> { + }))); + NamedCommands.registerCommand("L4", new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6, 0.0).asProxy()); + + NamedCommands.registerCommand("L1", new AutoMoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back),-0.6,0.0,true).asProxy()); + + NamedCommands.registerCommand("Intake", new AutoMoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), .6, .1, true).asProxy()); + NamedCommands.registerCommand("AfterIntake", + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), 0.15, + false, 0.1).asProxy()); + NamedCommands.registerCommand("Barge", new AutoMoveSuperStructure(GameInfo.Net, 1., 0).asProxy()); + NamedCommands.registerCommand("L3Algae", new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.) + .until(() -> EndEffector.get().isStalling()).asProxy()); + NamedCommands.registerCommand("L2Algae", + new MoveSuperStructure(GameInfo.L2AlgaeLow, -0.8, false, -1.).withTimeout(2).asProxy()); + NamedCommands.registerCommand("AlgaeUp", new MoveSuperStructure(GameInfo.algaeUp, -0.3).asProxy()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); + SmartDashboard.putData("L1",new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("L2", new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L2).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("L3",new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("L4",new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Front), 0)); + SmartDashboard.putData("Old times Auto", OldTimesAuto()); + // Warmup Commands for PathPlanner - PathfindingCommand.warmupCommand().schedule(); + // PathfindingCommand.warmupCommand().schedule(); + + Elevator.get().setDefaultCommand(Elevator.get().positionCommand(() -> { + return 0; + })); + Wrist.get().setDefaultCommand(Wrist.get().Position(-0.3)); + Arm.get().setDefaultCommand(Arm.get().Position(() -> .7)); + EndEffector.get().setDefaultCommand(EndEffector.get().spin(() -> EndEffector.get().idlePower)); } private void configureDriverStationBinds() { - // HumanControls.DriverPanel.resetGyro.onTrue(Commands.runOnce(()->Swerve.get().seedFieldCentric())); - // HumanControls.OperatorPanel2025.L1.whileTrue(Elevator.get().positionCommand(GameInfo.L1)); - // HumanControls.OperatorPanel2025.L2.whileTrue(Elevator.get().positionCommand(GameInfo.L2)); - // HumanControls.OperatorPanel2025.L3.whileTrue(Elevator.get().positionCommand(GameInfo.L3)); - // HumanControls.OperatorPanel2025.L4.whileTrue(Elevator.get().positionCommand(GameInfo.L4)); - - HumanControls.DriverPanel.OtherButton.whileTrue(new MoveSuperStructure(GameInfo.ground, 0.6)); - HumanControls.DriverPanel.resetGyro.whileTrue(new MoveSuperStructure(GameInfo.L3, 0.6)); + HumanControls.DriverPanel.resetGyro.whileTrue(new PIDtoNearest(false)); + HumanControls.OperatorPanel2025.gyro.onTrue(new InstantCommand(() -> Swerve.get().seedFieldCentric())); + HumanControls.OperatorPanel2025.releaseCoral.whileTrue( + new InstantCommand(() -> { + EndEffector.get().idlePower = -0.6; + })); + + + + HumanControls.OperatorPanel2025.L1.whileTrue( + new ConditionalCommand( + new MoveSuperStructure(GameInfo.ground, -0.8, false, -.8), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L1).get(GameInfo.RobotSide.Back), -0.15), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); + + HumanControls.OperatorPanel2025.L2.whileTrue( + new ConditionalCommand( + new MoveSuperStructure(GameInfo.L2Algae, -0.8, false, -0.8), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L2).get(GameInfo.RobotSide.Back), -0.6), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); + + HumanControls.OperatorPanel2025.L3.whileTrue( + new ConditionalCommand( + new MoveSuperStructure(GameInfo.L3Algae, -0.8, false, -1.), + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L3).get(GameInfo.RobotSide.Back), -0.6), + HumanControls.OperatorPanel2025.pickupAlgae::getAsBoolean) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); + + HumanControls.OperatorPanel2025.L4.whileTrue( + new MoveSuperStructure(GameInfo.RobotState.get(GameInfo.Position.L4).get(GameInfo.RobotSide.Back), -0.6) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceHexFace, Swerve.get()::FaceSource))); // + + HumanControls.OperatorPanel2025.Barge.whileTrue( + new MoveSuperStructure(GameInfo.Net, 1.) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceNet, Swerve.get()::FaceSource))); + + HumanControls.OperatorPanel2025.SouceCoral.whileTrue((new MoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.6, false, 0.1)) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceSource, Swerve.get()::FaceHexFace))); + HumanControls.OperatorPanel2025.SouceCoral.onFalse(new MoveSuperStructure( + GameInfo.RobotState.get(GameInfo.Position.Intake).get(GameInfo.RobotSide.Front), 0.15, false, 0.1)); + HumanControls.OperatorPanel2025.Climb1.whileTrue(new PIDtoNearest(false)); + HumanControls.OperatorPanel2025.Processor.whileTrue(new MoveSuperStructure(GameInfo.Processor, 0.6) + .deadlineFor(Swerve.get().setGoalRotation(Swerve.get()::FaceProcessor, Swerve.get()::FaceSource))); + /* + * HumanControls.OperatorPanel2025.releaseCoral.and(HumanControls. + * OperatorPanel2025.pickupAlgae) + * .whileTrue(new PrintCommand("trebuchet")); + */ + } @Override public void robotPeriodic() { - long start = RobotController.getTime(); CommandScheduler.getInstance().run(); - long elapsed = RobotController.getTime() - start; - BackupLogger.addToQueue("SchedulerTimeMS", elapsed); // Scheduler Time in Microseconds, anything over 20,000 should - // trigger the watchdog - updateVision("limelight-corner"); + m_SwerveTelemetry.telemeterize(Swerve.get().getState()); BackupLogger.logSystemInformation(); - - /* - * int i = 0; - * for (SwerveModule mod : - * CommandSwerveDrivetrain.get().getModules()) { - * SmartDashboard.putNumber("Encoder" + ++i, - * mod.getEncoder().getPosition().getValueAsDouble()); - * } - */ } @Override @@ -226,23 +307,6 @@ public void teleopInit() { } } - public void updateVision(String limelightName) { - double heading = Swerve.get().getState().Pose.getRotation().getDegrees(); - - LimelightHelpers.SetRobotOrientation(limelightName, heading, 0.0, 0.0, 0.0, 0.0, 0.0); - var measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - if (measurement != null) { - if (measurement.tagCount > 0 && measurement.rawFiducials[0].ambiguity < 0.5 - && measurement.rawFiducials[0].distToCamera < 5) { - // Experimental - double trustMetric = (Math.pow(measurement.rawFiducials[0].distToCamera, 2) - * measurement.rawFiducials[0].ambiguity / 35); - Swerve.get().setVisionMeasurementStdDevs(VecBuilder.fill(trustMetric, trustMetric, trustMetric)); - Swerve.get().addVisionMeasurement(measurement.pose, Utils.fpgaToCurrentTime(measurement.timestampSeconds)); - } - } - } - @Override public void teleopPeriodic() { diff --git a/src/main/java/frc/robot/SwerveTelemetry.java b/src/main/java/frc/robot/SwerveTelemetry.java index 6422fe1..257fff8 100644 --- a/src/main/java/frc/robot/SwerveTelemetry.java +++ b/src/main/java/frc/robot/SwerveTelemetry.java @@ -21,104 +21,111 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class SwerveTelemetry { - private final double MaxSpeed; + private final double MaxSpeed; - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public SwerveTelemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public SwerveTelemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + } - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = driveStateTable + .getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = driveStateTable + .getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = driveStateTable + .getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = driveStateTable + .getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + /** + * Accept the swerve drive state and telemeterize it to SmartDashboard and + * SignalLogger. + */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); - /* Telemeterize the module states to a Mechanism2d */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + /* Telemeterize the module states to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); } + } } diff --git a/src/main/java/frc/robot/TODO.md b/src/main/java/frc/robot/TODO.md new file mode 100644 index 0000000..f757eb3 --- /dev/null +++ b/src/main/java/frc/robot/TODO.md @@ -0,0 +1,11 @@ +- [x] Ground Intake Rollers Too fast intaking and way too fast spitting +- [x] Incorrect Position for ground intake when idling +- [x?] Elevator goes down when ready for intaking +- [] Middle Auto +- [] Left Auto +- [] Right Auto +- [???] Dead zones in auto align? +- [foundation layed] Picking up algae it returns too fast both reef and ground - Rotate Intake To Scoring Position?? +- [todo there] Processor +- [x] shoots too hard for l1 + diff --git a/src/main/java/frc/robot/TunerConstants.java b/src/main/java/frc/robot/TunerConstants.java index ae17c2f..2742841 100644 --- a/src/main/java/frc/robot/TunerConstants.java +++ b/src/main/java/frc/robot/TunerConstants.java @@ -19,268 +19,270 @@ // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(1.79).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("cv", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.06); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 0; - - private static final double kDriveGearRatio = 5.357142857142857; - private static final double kSteerGearRatio = 18.75; - private static final Distance kWheelRadius = Inches.of(3.36 / 2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 0; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 0; - private static final int kFrontLeftEncoderId = 0; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.229736328125); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(9); - private static final Distance kFrontLeftYPos = Inches.of(9); - - // Front Right - private static final int kFrontRightDriveMotorId = 3; - private static final int kFrontRightSteerMotorId = 2; - private static final int kFrontRightEncoderId = 1; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.415283203125); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(9); - private static final Distance kFrontRightYPos = Inches.of(-9); - - // Back Left - private static final int kBackLeftDriveMotorId = 5; - private static final int kBackLeftSteerMotorId = 4; - private static final int kBackLeftEncoderId = 2; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.448974609375); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-9); - private static final Distance kBackLeftYPos = Inches.of(9); - - // Back Right - private static final int kBackRightDriveMotorId = 7; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 3; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.455078125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-9); - private static final Distance kBackRightYPos = Inches.of(-9); - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with + // the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.79).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.18).withKI(0).withKD(0) + .withKS(0.1309).withKV(0.11584); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these + // cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API + // documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a + // relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("cv", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.06); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 0; + + private static final double kDriveGearRatio = 5.357142857142857; + private static final double kSteerGearRatio = 18.75; + private static final Distance kWheelRadius = Inches.of(3.2 / 2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 0; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 0; + private static final int kFrontLeftEncoderId = 0; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15771484375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(9); + private static final Distance kFrontLeftYPos = Inches.of(9); + + // Front Right + private static final int kFrontRightDriveMotorId = 3; + private static final int kFrontRightSteerMotorId = 2; + private static final int kFrontRightEncoderId = 1; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.415283203125); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(9); + private static final Distance kFrontRightYPos = Inches.of(-9); + + // Back Left + private static final int kBackLeftDriveMotorId = 5; + private static final int kBackLeftSteerMotorId = 4; + private static final int kBackLeftEncoderId = 2; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.448974609375); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-9); + private static final Distance kBackLeftYPos = Inches.of(9); + + // Back Right + private static final int kBackRightDriveMotorId = 7; + private static final int kBackRightSteerMotorId = 6; + private static final int kBackRightEncoderId = 3; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.455078125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-9); + private static final Distance kBackRightYPos = Inches.of(-9); + + public static final SwerveModuleConstants FrontLeft = ConstantCreator + .createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted); + public static final SwerveModuleConstants FrontRight = ConstantCreator + .createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants BackLeft = ConstantCreator + .createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted); + public static final SwerveModuleConstants BackRight = ConstantCreator + .createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ + public static Swerve createDrivetrain() { + return new Swerve( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + } + + /** + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected + * device types. + */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

    + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module */ - public static Swerve createDrivetrain() { - return new Swerve( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules); } + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

    + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules); + } /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

    + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz + * on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters + * and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters + * and radians + * @param modules Constants for each specific module */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

    - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

    - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

    - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules); } + } } diff --git a/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java new file mode 100644 index 0000000..f2d7213 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoMoveSuperStructure.java @@ -0,0 +1,88 @@ + +package frc.robot.commands; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Wrist; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.EndEffector; +import frc.robot.util.GameInfo.CoralScoringPosition; + +public class AutoMoveSuperStructure extends Command { + + CoralScoringPosition position; + + double runRollers = 0; + + double postRollerState = 0; + + double readyToShoot; + + boolean waitForStall = false; + + public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, + double postRollerState, boolean waitForStall) { + this.position = position; + + this.runRollers = runRollers; + + this.postRollerState = postRollerState; + + this.waitForStall = waitForStall; + + addRequirements(Arm.get(), Elevator.get(), Wrist.get(), EndEffector.get()); + } + + public AutoMoveSuperStructure(CoralScoringPosition position, double runRollers, double postRollerState) { + this(position, runRollers, postRollerState, false); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + readyToShoot = 999999999; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Wrist.get().goToPosition(position); + Elevator.get().goToPosition(position); + if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 + && Math.abs(Elevator.get().getPosition() - position.height) < .5) { + + Arm.get().goToPosition(position); + if (Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5) { + + EndEffector.get().run(runRollers); + if (readyToShoot > 99999) { + readyToShoot = Timer.getFPGATimestamp(); + } + } else { + EndEffector.get().run(EndEffector.get().idlePower); + } + } else { + EndEffector.get().run(EndEffector.get().idlePower); + + Arm.get().goToPosition(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Arm.get().stop(); + Wrist.get().stop(); + Elevator.get().stop(); + EndEffector.get().idlePower = postRollerState; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (Timer.getFPGATimestamp() - readyToShoot > 0.4) + && (!waitForStall || EndEffector.get().isStalling()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/MoveSuperStructure.java b/src/main/java/frc/robot/commands/MoveSuperStructure.java index 211f6c1..9e56a46 100644 --- a/src/main/java/frc/robot/commands/MoveSuperStructure.java +++ b/src/main/java/frc/robot/commands/MoveSuperStructure.java @@ -7,6 +7,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Wrist; +import frc.robot.HumanControls; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.EndEffector; @@ -18,29 +19,54 @@ public class MoveSuperStructure extends Command { double runRollers = 0; - public MoveSuperStructure(CoralScoringPosition position, double runRollers) { + double postRollerState = 0; + + boolean requirePress = true; + + public MoveSuperStructure(CoralScoringPosition position, double runRollers, boolean requirePress, + double postRollerState) { this.position = position; this.runRollers = runRollers; - addRequirements(Arm.get(), Elevator.get(), Wrist.get()); + this.postRollerState = postRollerState; + + this.requirePress = requirePress; + + addRequirements(Arm.get(), Elevator.get(), Wrist.get(), EndEffector.get()); + } + + public MoveSuperStructure(CoralScoringPosition position, double runRollers) { + this(position, runRollers, true, 0.); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Arm.get().goToPosition(position); Wrist.get().goToPosition(position); Elevator.get().goToPosition(position); - //if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < 2 && Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < 2 && Math.abs(Elevator.get().getPosition() - position.height) < 2 ) - // EndEffector.get().run(runRollers); - //else - // EndEffector.get().stop(); + if (Math.abs(Wrist.get().getPosition() - position.wristRot.in(Units.Rotation)) < .5 + && Math.abs(Elevator.get().getPosition() - position.height) < .5) { + + Arm.get().goToPosition(position); + if (Math.abs(Arm.get().getPosition() - position.armRot.in(Units.Rotation)) < .5 + && (!requirePress || HumanControls.OperatorPanel2025.releaseCoral.getAsBoolean())) { + + EndEffector.get().run(runRollers); + } else { + EndEffector.get().run(EndEffector.get().idlePower); + } + } else { + Arm.get().goToPosition(0); + EndEffector.get().run(EndEffector.get().idlePower); + } } // Called once the command ends or is interrupted. @@ -49,7 +75,7 @@ public void end(boolean interrupted) { Arm.get().stop(); Wrist.get().stop(); Elevator.get().stop(); - //EndEffector.get().stop(); + EndEffector.get().idlePower = postRollerState; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/PIDtoNearest.java b/src/main/java/frc/robot/commands/PIDtoNearest.java index 1184c10..5728b8d 100644 --- a/src/main/java/frc/robot/commands/PIDtoNearest.java +++ b/src/main/java/frc/robot/commands/PIDtoNearest.java @@ -7,16 +7,20 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.HumanControls; import frc.robot.subsystems.Swerve; import frc.robot.util.GameInfo; -import frc.robot.util.PoseUtil; +import frc.robot.util.GameInfo.ReefSide; import frc.team696.lib.Logging.BackupLogger; /** - * PIDToPosition but it takes goes to the nearest scoring pose defined in GameInfo. + * PIDToPosition but it takes goes to the nearest scoring pose defined in + * GameInfo. + * * @see PIDToPosition * @see GameInfo */ @@ -25,40 +29,84 @@ public class PIDtoNearest extends Command { private Pose2d goalPose; private boolean ignoreLR; - private double calculateWithTolerance(ProfiledPIDController controller, double measurement, double goal){ - double tolerance=controller.getPositionTolerance(); - double error=goal-measurement; - return Math.abs(error) velocitySignal; - StatusSignal positionSignal; - StatusSignal voltageSignal; - StatusSignal currentSignal; MotionMagicVoltage positionRequest = new MotionMagicVoltage(0); VoltageOut voltageRequest = new VoltageOut(0); - double ntpos = 0; + boolean slowMode = false; + + // to + // use + // this + // because + // CTR SHITTYTRONICS IS + // GATEKEEPING BASIC + // FUNCTIONALITY + // + ProfiledPIDController slowPidController = new ProfiledPIDController(1., 0, 0, + new TrapezoidProfile.Constraints(60., 35.)); /** Creates a new Arm. */ private Arm() { master.getConfigurator().apply(BotConstants.Arm.cfg); - slave.getConfigurator().apply(BotConstants.Arm.cfg); - velocitySignal = master.getVelocity(); - positionSignal = master.getPosition(); - voltageSignal = master.getMotorVoltage(); - currentSignal = master.getStatorCurrent(); - - slave.setControl(new Follower(master.getDeviceID(), false)); zeroArm(); - // this.setDefaultCommand(Position(()->0)); - this.setDefaultCommand(ArmWithNTPosition()); - new TriggerNTDouble("testing/armAngle", ntpos, (ev) -> ntpos = ev); - SmartDashboard.putData("ArmPlus", Spin(0.1)); - SmartDashboard.putData("ArmMinus", Spin(-0.1)); - SmartDashboard.putData("Zero Arm", this.runOnce(() -> zeroArm()).ignoringDisable(true)); + slowPidController.reset(0); } public void stop() { @@ -87,44 +68,39 @@ public void zeroArm() { resetArmPosition(0); } + public void goToPosition(double position) { + if (!slowMode) { + master.setControl(positionRequest.withPosition(position)); + } else { + master.setControl(voltageRequest.withOutput(slowPidController.calculate(getPosition(), position))); + } + } + public void goToPosition(GameInfo.CoralScoringPosition position) { - master.setControl(positionRequest.withPosition(position.armRot.in(Rotations))); + goToPosition(position.armRot.in(Rotations)); + } + + public void goToPosition(DoubleSupplier position) { + goToPosition(position.getAsDouble()); } public Command Position(DoubleSupplier position) { - return this.runEnd(() -> master.setControl(positionRequest.withPosition(position.getAsDouble())), - () -> master.set(0)); + return this.runEnd(() -> goToPosition(position), + () -> stop()); } public Command Position(GameInfo.CoralScoringPosition position) { - return this.startEnd(() -> master.setControl(positionRequest.withPosition(position.armRot.in(Rotations))), - () -> master.set(0)); + return this.startEnd(() -> goToPosition(position), + () -> stop()); } public double getPosition() { return master.getPosition().getValueAsDouble(); } - public Command ArmWithNTPosition() { - return this.runEnd(() -> master.setControl(positionRequest.withPosition(ntpos)), () -> master.stopMotor()); - } - - /** - * Spins the arm at a certain fraction of the motors - * - * @param speed [-1, 1] - * @return the command that spins the arm - */ - public Command Spin(double speed) { - return this.runEnd(() -> master.setControl(voltageRequest.withOutput(speed * 12)), () -> master.set(0)); - } - @Override public void periodic() { - // This method will be called once per scheduler run - BackupLogger.addToQueue("Arm/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); - BackupLogger.addToQueue("Arm/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); - BackupLogger.addToQueue("Arm/VoltageVolts", voltageSignal.refresh().getValue().in(Volts)); - BackupLogger.addToQueue("Arm/PositionRot", positionSignal.refresh().getValue().in(Rotations)); + slowPidController.calculate(getPosition()); + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 0e9ef7f..6b0beea 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -10,106 +10,120 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix6.controls.MotionMagicVoltage; import edu.wpi.first.units.measure.Voltage; -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; import frc.robot.BotConstants; import frc.robot.util.GameInfo; import frc.team696.lib.HardwareDevices.TalonFactory; -import frc.team696.lib.Logging.BackupLogger; public class Elevator extends SubsystemBase { - private static Elevator m_Elevator=null; - public static synchronized Elevator get(){ - if(m_Elevator==null){ - m_Elevator=new Elevator(); + private static Elevator m_Elevator = null; + + public static synchronized Elevator get() { + if (m_Elevator == null) { + m_Elevator = new Elevator(); } return m_Elevator; } + private TalonFactory master, slave; private MotionMagicVoltage positionReq; public SysIdRoutine identificationRoutine; private Elevator() { - master=new TalonFactory(BotConstants.Elevator.masterID, BotConstants.rioBus, BotConstants.Elevator.cfg, "Elevator Master"); - slave=new TalonFactory(BotConstants.Elevator.slaveId,BotConstants.rioBus, BotConstants.Elevator.cfg, "Elevator Slave"); + master = new TalonFactory(BotConstants.Elevator.masterID, BotConstants.rioBus, BotConstants.Elevator.cfg, + "Elevator Master"); + slave = new TalonFactory(BotConstants.Elevator.slaveId, BotConstants.rioBus, BotConstants.Elevator.cfg, + "Elevator Slave"); slave.Follow(master, true); - - positionReq=new MotionMagicVoltage(0).withSlot(0); + + positionReq = new MotionMagicVoltage(0).withSlot(0); zero(); - identificationRoutine=new SysIdRoutine(new SysIdRoutine.Config(Volts.per(Second).of(0.5), Volts.of(0.4),Seconds.of(3)), - new SysIdRoutine.Mechanism(this::DriveVoltage, (log)->{ - log.motor(master.getName()).voltage(master.get().getMotorVoltage().getValue()) - .linearPosition(Meters.of(master.getPosition())) - .linearVelocity(MetersPerSecond.of(master.getVelocity())); - }, this)); - this.setDefaultCommand(positionCommand(0)); - SmartDashboard.putData("Elevator duty cycle up", runDutyCycle()); + identificationRoutine = new SysIdRoutine( + new SysIdRoutine.Config(Volts.per(Second).of(0.5), Volts.of(0.4), Seconds.of(3)), + new SysIdRoutine.Mechanism(this::DriveVoltage, (log) -> { + log.motor(master.getName()).voltage(master.get().getMotorVoltage().getValue()) + .linearPosition(Meters.of(master.getPosition())) + .linearVelocity(MetersPerSecond.of(master.getVelocity())); + }, this)); } public void stop() { master.stop(); } + /** - * * Use ONLY for SysID. Sets the elevator motors to a specific voltage */ - public void DriveVoltage(Voltage v){ + public void DriveVoltage(Voltage v) { master.VoltageOut(v); } + public void goToPosition(double position) { + + master.get().setControl(positionReq.withPosition(position)); + } + public void goToPosition(GameInfo.CoralScoringPosition position) { - master.get().setControl(positionReq.withPosition(position.height)); - } + goToPosition(position.height); + } public double getPosition() { return master.getPosition(); } + /** * * Moves to and holds a position + * * @param position The scoring position to hold - * @return the command which holds the elevator at position (requires this subsystem) + * @return the command which holds the elevator at position (requires this + * subsystem) */ - public Command positionCommand(GameInfo.CoralScoringPosition position){ - return this.runEnd(()->master.get().setControl(positionReq.withPosition(position.height)), ()->master.get().set(0)); + public Command positionCommand(GameInfo.CoralScoringPosition position) { + return this.runEnd(() -> goToPosition(position), () -> master.get().set(0)); } - public Command positionCommandRun(GameInfo.CoralScoringPosition position){ - return this.runEnd(()->master.setControl(positionReq.withPosition(position.height)), ()->master.VoltageOut(Volts.of(0))); + + public Command positionCommand(double position) { + return this.runEnd(() -> goToPosition(position), () -> master.VoltageOut(Volts.of(0))); } - public Command positionCommand(double position){ - return this.startEnd(()->master.setControl(positionReq.withPosition(position)), ()->master.VoltageOut(Volts.of(0))); + + public Command positionCommand(DoubleSupplier position) { + return this.runEnd(() -> goToPosition(position.getAsDouble()), () -> stop()); } + /** * Holds the current position - * @return a command that holds the elevator at the position it was in at schedluing time + * + * @return a command that holds the elevator at the position it was in at + * schedluing time */ - public Command holdPosition(){ - return this.startEnd(()->master.setControl(positionReq.withPosition(master.getPosition())), ()->master.VoltageOut(Volts.of(0))); + public Command holdPosition() { + return this.startEnd(() -> goToPosition(master.getPosition()), () -> master.VoltageOut(Volts.of(0))); } - public Command runDutyCycle(){ - return this.startEnd(()->master.PercentOutput(.4), ()->master.stop()); - } - public void zero(){ + + public void zero() { resetPosition(0); } - public void resetPosition(double newPosition){ + + public void resetPosition(double newPosition) { master.setPosition(newPosition); slave.setPosition(newPosition); } + @Override public void periodic() { - BackupLogger.addToQueue("Elevator/MasterCurrent", master.getCurrent()); - BackupLogger.addToQueue("Elevator/SlaveCurrent", slave.getCurrent()); - BackupLogger.addToQueue("Elevator/MasterPosition", master.getPosition()); - BackupLogger.addToQueue("Elevator/SlavePosition", slave.getPosition()); - BackupLogger.addToQueue("Elevator/SlaveVelocity", slave.getVelocity()); - BackupLogger.addToQueue("Elevator/MasterVelocity", master.getVelocity()); - + /*if(Math.abs(Swerve.get().getPigeon2().getRoll().getValue().in(Degrees))>15||Math.abs(Swerve.get().getPigeon2().getPitch().getValue().in(Degrees))>15){ + if(this.getCurrentCommand()!=null) + this.getCurrentCommand().cancel(); + goToPosition(0); + }*/ } } diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java index 0a221a9..eda5a47 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector.java @@ -5,10 +5,7 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; - import java.util.function.DoubleSupplier; import com.ctre.phoenix6.StatusSignal; @@ -20,11 +17,9 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; -import frc.team696.lib.Logging.BackupLogger; /** * This class represents the Coral scoring mechanism attached to the arm of the @@ -52,6 +47,8 @@ public static final synchronized EndEffector get() { StatusSignal voltageSignal; StatusSignal currentSignal; + public double idlePower = 0; + public EndEffector() { motor.getConfigurator().apply(BotConstants.EndEffector.cfg); velocitySignal = motor.getVelocity(); @@ -76,11 +73,23 @@ public void run(double output) { * @return the command */ public Command spin(double power) { - return this.startEnd(() -> motor.setControl(VoltageRequest.withOutput(power * 12)), () -> motor.set(0)); + return this.startEnd(() -> run(power), () -> motor.set(0)); + } + + public boolean isStalling() { + + return motor.getStatorCurrent().getValue().in(Amps) >= 75 && velocitySignal.getValue().in(RotationsPerSecond) < 5; } + /** + * spins the roller at a fraction of it's power power ? + * + * @param power [-1,1] A function returning the fraction of the power that the + * roller can exert at 12V. Polled every tick. + * @return the command + */ public Command spin(DoubleSupplier power) { - return this.runEnd(() -> motor.setControl(VoltageRequest.withOutput(power.getAsDouble() * 12)), () -> motor.set(0)); + return this.runEnd(() -> run(power.getAsDouble()), () -> motor.set(0)); } public Command spinVelocity(double velocity) { @@ -89,10 +98,22 @@ public Command spinVelocity(double velocity) { @Override public void periodic() { + + // if (isStalling() && + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity == 10. ) { + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 6.; + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 6.; + // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); + // // idlePower = -0.8; + // } else if (!isStalling()) { + // if (BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity < 16.) { + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicCruiseVelocity = 10.; + // BotConstants.Wrist.cfg.MotionMagic.MotionMagicAcceleration = 16.; + // //Wrist.get().motor.getConfigurator().apply(BotConstants.Wrist.cfg.MotionMagic); + // } + // //idlePower = 0.; + // } + // This method will be called once per scheduler run - BackupLogger.addToQueue("EndEffector/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); - BackupLogger.addToQueue("EndEffector/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); - BackupLogger.addToQueue("EndEffector/VoltageVolts", voltageSignal.refresh().getValue().in(Volts)); - BackupLogger.addToQueue("EndEffector/PositionRot", positionSignal.refresh().getValue().in(Rotations)); } } diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java deleted file mode 100644 index ddc229c..0000000 --- a/src/main/java/frc/robot/subsystems/LED.java +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.ctre.phoenix.led.CANdle; -import com.ctre.phoenix.led.CANdle.LEDStripType; -import com.ctre.phoenix.led.CANdle.VBatOutputMode; -import com.ctre.phoenix.led.CANdleConfiguration; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class LED extends SubsystemBase { - private static LED m_LED = null; - private final int _ledOffset = 8; - private final int _numLed = 36 + 8; - - public static synchronized LED get() { - if (m_LED == null) { - m_LED = new LED(); - } - return m_LED; - } - - CANdle candle = new CANdle(0); - - public double getVbat() { - return candle.getBusVoltage(); - } - - public double get5V() { - return candle.get5VRailVoltage(); - } - - public double getCurrent() { - return candle.getCurrent(); - } - - public double getTemperature() { - return candle.getTemperature(); - } - - private void setColor(int r, int g, int b) { - candle.clearAnimation(0); - - candle.setLEDs(r, g, b, 255, _ledOffset, _numLed); - } - - private Command Color(int r, int g, int b) { - return this.startEnd(() -> setColor(r, g, b), () -> setColor(0, 0, 0)).ignoringDisable(true); - } - - /** Creates a new LED. */ - private LED() { - CANdleConfiguration _candleConfiguration = new CANdleConfiguration(); - _candleConfiguration.statusLedOffWhenActive = true; - _candleConfiguration.disableWhenLOS = false; - _candleConfiguration.stripType = LEDStripType.RGB; - _candleConfiguration.brightnessScalar = 1; - _candleConfiguration.vBatOutputMode = VBatOutputMode.On; - _candleConfiguration.enableOptimizations = true; - _candleConfiguration.v5Enabled = true; - candle.configAllSettings(_candleConfiguration); - this.setDefaultCommand(Color(255, 0, 0).ignoringDisable(true)); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index c927697..9d58e7b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -2,12 +2,10 @@ import static edu.wpi.first.units.Units.*; -import java.util.List; import java.util.function.Supplier; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.ApplyChassisSpeeds; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -17,9 +15,10 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -31,78 +30,103 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.TunerConstants; import frc.robot.TunerConstants.TunerSwerveDrivetrain; -import frc.team696.lib.Logging.BackupLogger; +import frc.robot.util.GameInfo; +import frc.team696.lib.Util; +import frc.team696.lib.Camera.BaseCam; +import frc.team696.lib.Camera.LimeLightCam; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements * Subsystem so it can easily be used in command-based projects. */ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - private static Swerve m_CommandSwerveDrivetrain=null; - public static synchronized Swerve get(){ - if(m_CommandSwerveDrivetrain==null){ - m_CommandSwerveDrivetrain=TunerConstants.createDrivetrain(); - } - return m_CommandSwerveDrivetrain; + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + private static Swerve m_CommandSwerveDrivetrain = null; + + public LimeLightCam CamA = new LimeLightCam("limelight"); + public LimeLightCam CamB = new LimeLightCam("limelight-left"); + + Rotation2d yawOffset = new Rotation2d(0); + + public Supplier goalRotation = () -> new Rotation2d(); + + public Command setGoalRotation(Supplier during, Supplier after) { + return Commands.startEnd(() -> { + if (during != null) { + goalRotation = during; + } + }, () -> { + if (after != null) { + goalRotation = after; + } + }); + + } + + public Rotation2d getGoalRotation(){ + Pose2d pose=getPose(); + // the second condition converts the blue reef pos to a red reef pos before checking if the robot is close enough + if(distTo(GameInfo.blueReef)<2.3||distTo((new Translation2d(GameInfo.fieldLengthMeters.in(Meters) - GameInfo.blueReef.getX(), + GameInfo.blueReef.getY())))<2.3){ + return FaceHexFace(); + }if(pose.getX()>6&&pose.getX()<9.8){ + return FaceNet(); } + return FaceSource(); + } - /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ - private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; - /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; + public void updateYawOffset() { + yawOffset = getPose().getRotation().minus(getPigeon2().getRotation2d()); + } - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + public static synchronized Swerve get() { + if (m_CommandSwerveDrivetrain == null) { + m_CommandSwerveDrivetrain = TunerConstants.createDrivetrain(); + } + return m_CommandSwerveDrivetrain; + } - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - Seconds.of(10), // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - Seconds.of(10), // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + /* + * SysId routine for characterizing translation. This is used to find PID gains + * for the drive motors. + */ + @SuppressWarnings("unused") + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + Seconds.of(10), // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this)); /* * SysId routine for characterizing rotation. * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. */ + @SuppressWarnings("unused") private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( new SysIdRoutine.Config( /* This is in radians per second², but SysId only supports "volts per second" */ @@ -124,236 +148,333 @@ public static synchronized Swerve get(){ this ) ); + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineRotation;//m_sysIdRoutineTranslation; - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

    - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public Swerve( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

    + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public Swerve( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); } + configureAutoBuilder(); + } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

    - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public Swerve( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

    + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public Swerve( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); } + configureAutoBuilder(); + } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

    - * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]ᵀ, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public Swerve( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - configureAutoBuilder(); + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

    + * This constructs the underlying hardware devices, so users should not + * construct + * the devices themselves. If they need the devices, they can access them + * through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve + * drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz + * on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters + * and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation + * in the form [x, y, theta]ᵀ, with units in + * meters + * and radians + * @param modules Constants for each specific module + */ + public Swerve( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); + if (Utils.isSimulation()) { + startSimThread(); } + configureAutoBuilder(); + } - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } + /** + * Returns a command that applies the specified control request to this swerve + * drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(12, 0, 0), - // PID constants for rotation - new PIDConstants(8, 0, 0) - ), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - } - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - public static SwerveRequest.ApplyRobotSpeeds rcDriveReq=new SwerveRequest.ApplyRobotSpeeds().withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage).withSteerRequestType(SteerRequestType.Position); - public static SwerveRequest.FieldCentric fcDriveReq=new SwerveRequest.FieldCentric().withForwardPerspective(ForwardPerspectiveValue.BlueAlliance).withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage).withSteerRequestType(SteerRequestType.Position); - public void Drive(ChassisSpeeds c){ - setControl(rcDriveReq.withSpeeds(c)); - } - public void Drive(ChassisSpeeds c, boolean fieldRelative){ - if(fieldRelative){ - this.setControl(fcDriveReq.withVelocityX(c.vxMetersPerSecond).withVelocityY(c.vyMetersPerSecond).withRotationalRate(c.omegaRadiansPerSecond)); - }else{ - Drive(c); - } - } + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { - setOperatorPerspectiveForward( - allianceColor == Alliance.Red - ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); - m_hasAppliedOperatorPerspective = true; - }); - } + private void configureAutoBuilder() { + try { + var config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + () -> getState().Pose, // Supplier of current robot pose + this::resetPose, // Consumer for seeding pose against auto + () -> getState().Speeds, // Supplier of current robot speeds + // Consumer of ChassisSpeeds and feedforwards to drive the robot + (speeds, feedforwards) -> setControl( + m_pathApplyRobotSpeeds.withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), + new PPHolonomicDriveController( + // PID constants for translation + new PIDConstants(12, 0, 0.00 ), + // PID constants for rotation + new PIDConstants(8, 0, 0.00)), + config, + // Assume the path needs to be flipped for Red vs Blue, this is normally the + // case + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this // Subsystem for requirements + ); + } catch (Exception ex) { + DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); } + } - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; + public static SwerveRequest.ApplyRobotSpeeds rcDriveReq = new SwerveRequest.ApplyRobotSpeeds() + .withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.Position); + public static SwerveRequest.FieldCentric fcDriveReq = new SwerveRequest.FieldCentric() + .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective) + .withDriveRequestType(com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType.OpenLoopVoltage) + .withSteerRequestType(SteerRequestType.Position); - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - public Pose2d getPose(){ - return getState().Pose; - } - /** - * - * @param position Position for distance from - * @return distance from position argument - */ - public double distTo(Translation2d position) { - return getPose().getTranslation().getDistance(position); - } + public void Drive(ChassisSpeeds c) { + setControl(rcDriveReq.withSpeeds(c)); + } - /** - * - * @param position Position for distance from - * @return distance from position argument - */ - public double distTo(Pose2d position) { - return distTo(position.getTranslation()); + public void Drive(ChassisSpeeds c, boolean fieldRelative) { + if (fieldRelative) { + setControl(rcDriveReq.withSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(c, getState().Pose.getRotation()))); + /*this.setControl(fcDriveReq.withVelocityX(c.vxMetersPerSecond).withVelocityY(c.vyMetersPerSecond) + .withRotationalRate(c.omegaRadiansPerSecond));*/ + } else { + Drive(c); } + } - /** - * - * @param position Position for angle to - * @return Angle to Position + @Override + public void periodic() { + if (DriverStation.isDisabled()) + updateYawOffset(); + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply + * it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts + * mid-match. + * Otherwise, only check and apply the operator perspective if the DS is + * disabled. + * This ensures driving behavior doesn't change until an explicit disable event + * occurs during testing. */ - public Rotation2d angleTo(Translation2d position) { - Translation2d delta = getPose().getTranslation().minus(position); - Rotation2d rot = Rotation2d.fromRadians(Math.atan2(delta.getY(), delta.getX())); - return rot; + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { + setOperatorPerspectiveForward( + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation); + m_hasAppliedOperatorPerspective = true; + }); } - /** - * - * @param position Position for angle to - * @return Angle to Position - */ - public Rotation2d angleTo(Pose2d position) { - return angleTo(position.getTranslation()); + BaseCam.acceptEstimate estimate = (Estimate) -> { + if (Estimate.distToTag > 3.5) + return false; + + if (Estimate.ambiguity > 0.6) + return false; // Too Ambiguous, Ignore + if (getState().Speeds.omegaRadiansPerSecond > 2.5) + return false; // Rotating too fast, ignore + + if (Estimate.distToTag < 1) { + setVisionMeasurementStdDevs(VecBuilder.fill(0.001, 0.001, 0.001)); + } else { + setVisionMeasurementStdDevs( + VecBuilder.fill(Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.01, + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.01, + Estimate.ambiguity * Math.pow(Estimate.distToTag, 2)*0.01)); + } + return true; + }; + CamA.SetRobotOrientation(yawOffset); + CamB.SetRobotOrientation(yawOffset); + + CamA.addVisionEstimate(this::addVisionMeasurement, estimate); + CamB.addVisionEstimate(this::addVisionMeasurement, estimate); + + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + public Pose2d getPose() { + return getState().Pose; + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Translation2d position) { + return getPose().getTranslation().getDistance(position); + } + + /** + * + * @param position Position for distance from + * @return distance from position argument + */ + public double distTo(Pose2d position) { + return distTo(position.getTranslation()); + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Translation2d position) { + Translation2d delta = getPose().getTranslation().minus(position); + Rotation2d rot = Rotation2d.fromRadians(Math.atan2(delta.getY(), delta.getX())); + return rot; + } + + /** + * + * @param position Position for angle to + * @return Angle to Position + */ + public Rotation2d angleTo(Pose2d position) { + return angleTo(position.getTranslation()); + } + + public Rotation2d FaceHexFace(Translation2d pose) { + Translation2d reefPosition = Util.getAlliance() == Alliance.Blue ? GameInfo.blueReef + : (new Translation2d(GameInfo.fieldLengthMeters.in(Meters) - GameInfo.blueReef.getX(), + GameInfo.blueReef.getY())); + + Rotation2d angleToReef = pose.minus(reefPosition).getAngle(); + Rotation2d hexAngleToReef = Rotation2d + .fromDegrees( + MathUtil.inputModulus( + ((int) ((angleToReef.getDegrees() + Math.signum(angleToReef.getDegrees()) * 30) / 60)) * 60. - 90., + -180., 180)); + return hexAngleToReef; + } + + public Rotation2d FaceHexFace() { + return FaceHexFace(Swerve.get().getPose().getTranslation()); + } + public Rotation2d FaceNet() { + return Util.getAlliance() == Alliance.Blue ? Rotation2d.fromDegrees(-90) : Rotation2d.fromDegrees(90); + } + + public Rotation2d FaceSource() { + if (Util.getAlliance() == Alliance.Blue) { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(36); + } else { + return Rotation2d.fromDegrees(140); + } + } else { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(-36); + } else { + return Rotation2d.fromDegrees(-140); + } } + } + public Rotation2d FaceProcessor() { + if (getPose().getY() > GameInfo.fieldWidthMeters.in(Meters) / 2) { + return Rotation2d.fromDegrees(180); + } else { + return Rotation2d.fromDegrees(0); + } + } } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index 70e2143..3d72929 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -4,26 +4,17 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.BotConstants; import frc.robot.util.GameInfo.CoralScoringPosition; -import frc.team696.lib.Logging.BackupLogger; public class Wrist extends SubsystemBase { private static Wrist m_Wrist = null; @@ -37,11 +28,6 @@ public static final synchronized Wrist get() { private TalonFX motor = new TalonFX(BotConstants.Wrist.motorID, BotConstants.rioBus); - StatusSignal velocitySignal; - StatusSignal positionSignal; - StatusSignal voltageSignal; - StatusSignal currentSignal; - MotionMagicVoltage WristPoistionRequest = new MotionMagicVoltage(0); VoltageOut WristVoltageRequest = new VoltageOut(0); @@ -49,12 +35,6 @@ public static final synchronized Wrist get() { private Wrist() { motor.getConfigurator().apply(BotConstants.Wrist.cfg); zero(); - velocitySignal = motor.getVelocity(); - positionSignal = motor.getPosition(); - voltageSignal = motor.getMotorVoltage(); - currentSignal = motor.getStatorCurrent(); - this.setDefaultCommand(Position(0)); - SmartDashboard.putData("Zero Wrist", this.runOnce(() -> zero()).ignoringDisable(true)); } public void resetPosition(double newPosition) { @@ -70,23 +50,25 @@ public void stop() { } public double getPosition() { - return positionSignal.getValueAsDouble(); + return motor.getPosition().getValueAsDouble(); } - public Command Position(double position){ - return this.startEnd(()->motor.setControl(WristPoistionRequest.withPosition(position)), ()->motor.set(0.0)); + + public Command Position(double position) { + return this.runEnd(() -> goToPosition(position), () -> motor.set(0.0)); } - public void goToPosition(CoralScoringPosition position){ - motor.setControl(WristPoistionRequest.withPosition(position.wristRot.in(Rotation))); + public void goToPosition(double position) { + motor.setControl(WristPoistionRequest.withPosition(position)); + } + public void goToPosition(CoralScoringPosition position) { + goToPosition(position.wristRot.in(Rotation)); + } @Override public void periodic() { + SmartDashboard.putNumber("Wrist/Position", getPosition()); // This method will be called once per scheduler run - BackupLogger.addToQueue("Wrist/VelocityRpsSquared", velocitySignal.refresh().getValue().in(RotationsPerSecond)); - BackupLogger.addToQueue("Wrist/CurrentAmps", currentSignal.refresh().getValue().in(Amps)); - BackupLogger.addToQueue("Wrist/VoltageVolts", voltageSignal.refresh().getValue().in(Volts)); - BackupLogger.addToQueue("Wrist/PositionRot", positionSignal.refresh().getValue().in(Rotation)); } } diff --git a/src/main/java/frc/robot/util/DynamicNTBoolean.java b/src/main/java/frc/robot/util/DynamicNTBoolean.java deleted file mode 100644 index a6753d4..0000000 --- a/src/main/java/frc/robot/util/DynamicNTBoolean.java +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.HashMap; -import java.util.Map; -import java.util.function.DoubleConsumer; - -import edu.wpi.first.networktables.BooleanEntry; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.util.function.BooleanConsumer; - -/** Add your docs here. */ -public class DynamicNTBoolean { - String name; - boolean val; - BooleanEntry ntEntry; - BooleanConsumer update; - public static Map avail=new HashMap(); - public static void periodic(){ - for(DynamicNTBoolean v:avail.values()){ - boolean prev=v.val; - v.val=v.ntEntry.get(false); - if(v.val!=prev){ - v.update.accept(v.val); - } - - } - } - public void Register(){ - if(avail.containsKey(name)){ - throw new IllegalArgumentException("DynamicNTBoolean with name "+name+" already exists"); - } - avail.put(name, this); - } - public DynamicNTBoolean(String name, boolean val, BooleanConsumer update){ - this.name=name; - this.val=val; - this.update=update; - this.ntEntry=NetworkTableInstance.getDefault().getBooleanTopic(name).getEntry(false); - this.ntEntry.set(val); - } -} diff --git a/src/main/java/frc/robot/util/DynamicNTDouble.java b/src/main/java/frc/robot/util/DynamicNTDouble.java deleted file mode 100644 index fc0db4c..0000000 --- a/src/main/java/frc/robot/util/DynamicNTDouble.java +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.HashMap; -import java.util.Map; -import java.util.function.DoubleConsumer; - -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -/** Add your docs here. */ -public class DynamicNTDouble { - String name; - double val; - DoubleEntry ntEntry; - DoubleConsumer update; - public static Map avail=new HashMap(); - public static void periodic(){ - for(DynamicNTDouble v:avail.values()){ - double prev=v.val; - v.val=v.ntEntry.get(0); - if(v.val!=prev){ - v.update.accept(v.val); - } - - } - } - public void Register(){ - if(avail.containsKey(name)){ - throw new IllegalArgumentException("DynamicNTDouble with name "+name+" already exists"); - } - avail.put(name, this); - } - public DynamicNTDouble(String name, double val, DoubleConsumer update){ - this.name=name; - this.val=val; - this.update=update; - this.ntEntry=NetworkTableInstance.getDefault().getDoubleTopic(name).getEntry(0); - this.ntEntry.set(val); - } -} diff --git a/src/main/java/frc/robot/util/DynamicNTVal.java b/src/main/java/frc/robot/util/DynamicNTVal.java deleted file mode 100644 index 7b3b34c..0000000 --- a/src/main/java/frc/robot/util/DynamicNTVal.java +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.HashMap; -import java.util.Map; -import java.util.function.Consumer; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; - -/** Add your docs here. */ -public class DynamicNTVal { - String name; - T val; - T prev; - NetworkTableEntry ntEntry; - Consumer update; - public static Map avail=new HashMap(); - public static void periodic(){ - for(DynamicNTVal v:avail.values()){ - v.prev=v.val; - v.val=v.MagicPull(); - if(!v.val.equals(v.prev)){ - v.update.accept(v.val); - } - - } - } - public void Register(){ - if(avail.containsKey(name)){ - throw new IllegalArgumentException("DynamicNTVal with name "+name+" already exists"); - } - avail.put(name, this); - } - public T MagicPull(){ - switch(ntEntry.getType()){ - case kDouble: - return (T)Double.valueOf(ntEntry.getDouble(0)); - case kBoolean: - return (T)Boolean.valueOf(ntEntry.getBoolean(false)); - case kString: - return (T)ntEntry.getString(""); - default: - throw new IllegalArgumentException("DynamicNTVal with name "+name+" has invalid type"); - } - } - // this is a magic function that allows you to put any type of value into the network table - public void MagicPut(T val){ - if(val instanceof Double) - ntEntry.setDouble((double)val); - else if(val instanceof Boolean) - ntEntry.setBoolean((boolean)val); - else if(val instanceof String) - ntEntry.setString((String)val); - else - throw new IllegalArgumentException("DynamicNTVal with name "+name+" has invalid type"); - - } - public DynamicNTVal(String name, T val, Consumer update){ - this.name=name; - this.val=val; - this.update=update; - this.ntEntry=NetworkTableInstance.getDefault().getEntry(name); - MagicPut(val); - } -} diff --git a/src/main/java/frc/robot/util/GameInfo.java b/src/main/java/frc/robot/util/GameInfo.java index baa3932..af79482 100644 --- a/src/main/java/frc/robot/util/GameInfo.java +++ b/src/main/java/frc/robot/util/GameInfo.java @@ -4,123 +4,167 @@ package frc.robot.util; -import static edu.wpi.first.units.Units.Degree; +import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.Rotation; -import java.util.Arrays; -import java.util.stream.Stream; +import java.util.Map; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.HumanControls; +import frc.team696.lib.Util; /** * A class contianing all the positions needed to move the superstructure into a * scoring position */ public class GameInfo { - public static class CoralScoringPosition { - /** - * Creates a new CoralScoringPosition - * - * @param height The height of the elevator (zero represents the elevator's - * lowest position), positive=up - * @param armRot The rotation of the arm - * @param wristRot The rotation of the wrist - */ - public CoralScoringPosition(double height, double armRot, double wristRot) { - this.height = height; - this.armRot = Rotation.of(armRot); - this.wristRot = Rotation.of(wristRot); - } - - public double height; - /* As measured from the vertical normal (arm facing up) */ - public Angle armRot; - public Angle wristRot; + public static class CoralScoringPosition { + /** + * Creates a new CoralScoringPosition + * + * @param height The height of the elevator (zero represents the elevator's + * lowest position), positive=up + * @param armRot The rotation of the arm + * @param wristRot The rotation of the wrist + */ + public CoralScoringPosition(double height, double armRot, double wristRot) { + this.height = height; + this.armRot = Rotation.of(armRot); + this.wristRot = Rotation.of(wristRot); } - public static CoralScoringPosition L1, L2, L3, L4, Net, Intake, ground, ClimbUp, ClimbDown; - - public static class FieldSide { - public Pose2d[] left, right, both; - } - - public static FieldSide red, blue; - - public static FieldSide getFieldSide() { - return DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? blue : red; - } - - public static Pose2d[] getScoringPoses() { - return HumanControls.OperatorPanel2025.leftOrRight.getAsBoolean() ? getFieldSide().left : getFieldSide().right; - } - - static { - blue = new FieldSide(); - // TODO: determine the real scoring poses - blue.left = new Pose2d[] { - new Pose2d(3.539, 4.912, Rotation2d.fromDegrees(-60 + 180)), - new Pose2d(3.179, 3.752, Rotation2d.fromDegrees(0 + 180)), - new Pose2d(4.056, 2.689, Rotation2d.fromDegrees(60 + 180)), - new Pose2d(5.450, 2.992, Rotation2d.fromDegrees(120 + 180)), - new Pose2d(5.850, 4.376, Rotation2d.fromDegrees(180 + 180)), - new Pose2d(4.924, 5.351, Rotation2d.fromDegrees(-120 + 180)) - }; - blue.right = new Pose2d[] { - new Pose2d(4.046, 5.322, Rotation2d.fromDegrees(-60 + 180)), - new Pose2d(3.188, 4.473, Rotation2d.fromDegrees(0 + 180)), - new Pose2d(3.481, 3.050, Rotation2d.fromDegrees(60 + 180)), - new Pose2d(4.836, 2.679, Rotation2d.fromDegrees(120 + 180)), - new Pose2d(5.821, 3.762, Rotation2d.fromDegrees(180 + 180)), - new Pose2d(5.489, 4.932, Rotation2d.fromDegrees(-120 + 180)) - }; - blue.both = Stream.concat(Arrays.stream(blue.left), Arrays.stream(blue.right)) - .toArray(size -> new Pose2d[size]); - - // The red poses are mirrored from the blue scoring poses - red = new FieldSide(); - red.left = Arrays.stream(blue.left).map(pose -> PoseUtil.mirrorPoseX(pose)).toArray(size -> new Pose2d[size]); - - red.right = Arrays.stream(blue.right).map(pose -> PoseUtil.mirrorPoseX(pose)).toArray(size -> new Pose2d[size]); - - red.both = Stream.concat(Arrays.stream(red.left), Arrays.stream(red.right)).toArray(size -> new Pose2d[size]); - - L1 = new CoralScoringPosition(0., 1.75, 0.45); - L2 = new CoralScoringPosition(8., 1.75, 0.45); - L3 = new CoralScoringPosition(30., 1.75, .45); - L4 = new CoralScoringPosition(67., 0.7, .65); - Net = new CoralScoringPosition(67., 0., 2.5); - ClimbUp=new CoralScoringPosition(27, -8., 0); - ClimbDown=new CoralScoringPosition(2, -8., 0); - Intake = new CoralScoringPosition(0, -1.75, 3.6); - ground=new CoralScoringPosition(5., 8.2, 4.3); - /** - * this puts the values on networktables so scoring positions can be quickly - * changed - * comment this out once the scoring positions are finalized - - new TriggerNTDouble("testing/L1/height", L1.height, height -> L1.height = height); - new TriggerNTDouble("testing/L2/height", L2.height, height -> L2.height = height); - new TriggerNTDouble("testing/L3/height", L3.height, height -> L3.height = height); - new TriggerNTDouble("testing/L4/height", L4.height, height -> L4.height = height); - new TriggerNTDouble("testing/ground/height", ground.height, height -> ground.height = height); - - new TriggerNTDouble("testing/L1/armRot", L1.armRot.in(Rotation), rot -> L1.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L2/armRot", L2.armRot.in(Rotation), rot -> L2.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L3/armRot", L3.armRot.in(Rotation), rot -> L3.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L4/armRot", L4.armRot.in(Rotation), rot -> L4.armRot = Rotation.of(rot)); - new TriggerNTDouble("testing/ground/armRot", ground.armRot.in(Rotation), rot -> ground.armRot = Rotation.of(rot)); - - new TriggerNTDouble("testing/L1/wristRot", L1.wristRot.in(Rotation), rot -> L1.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L2/wristRot", L2.wristRot.in(Rotation), rot -> L2.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L3/wristRot", L3.wristRot.in(Rotation), rot -> L3.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/L4/wristRot", L4.wristRot.in(Rotation), rot -> L4.wristRot = Rotation.of(rot)); - new TriggerNTDouble("testing/ground/wristRot", ground.wristRot.in(Rotation), rot -> ground.wristRot = Rotation.of(rot)); - */ - } + public double height; + // Rotations of the Motor, Im too lazy to change off angle now + public Angle armRot; + public Angle wristRot; + } + + public static CoralScoringPosition Net, ground, Processor, ClimbUp, ClimbDown, L2Algae, L2AlgaeLow, L3Algae, algaeUp; + + /* Looking at the Index Dead On */ + public enum ReefSide { + Right, + Left + } + + /* Labels For Blue Side Of Field, Relative To Center Of Hex */ + public enum Index { + One, // -X + Two, // -X, +Y + Three, // +X, +Y + Four, // +X + Five, // +X, -Y + Six // -X, -Y + } + + public final static Distance fieldLengthMeters = Feet.of(57.53); + public final static Distance fieldWidthMeters = Feet.of(26.75); + + /** + * Mirrors a Translation2D across the midpoint of the field x-ais + * @param starting The Translation2d to mirror + * @return The mirrored translation (X-Axis) + */ + public static Translation2d mirrorTranslation(Translation2d starting) { + return new Translation2d(17.55- starting.getX(), starting.getY()); + } + + /** + * Mirrors a Translation2D across the midpoint of the field x-ais and of the field y-axis + * @param starting The Translation2d to mirror + * @return The mirrored translation + */ + public static Translation2d mirrorTranslationXY(Translation2d starting) { + return new Translation2d(17.55- starting.getX(), 8.05-starting.getY()); + } + + /** + * Returns the relevant collection of scoring poses according to the alliance the robot is currently on + * @return The collection of scoring poses + */ + public static Map> getScoringPoses(){ + return (Util.getAlliance()==Alliance.Red)?ScoringPosesRed:ScoringPosesBlue; + } + + public final static Translation2d blueReef = new Translation2d(4.5, 4.); + + public final static Map> ScoringPosesBlue, ScoringPosesRed; + + public enum Position { + L1, + L2, + L3, + L4, + Intake + } + + public enum RobotSide { + Front, + Back + } + + public final static Map> RobotState; + + public final static double wristOffset = 1.15; + static { + ScoringPosesBlue = Map.of( + Index.One, Map.of( + ReefSide.Right, new Pose2d(3.29, 3.76, Rotation2d.fromDegrees(90)), + ReefSide.Left, new Pose2d(3.3, 4.10, Rotation2d.fromDegrees(90))), + + Index.Two, Map.of( + ReefSide.Right, new Pose2d(3.66, 4.96, Rotation2d.fromDegrees(30)), + ReefSide.Left, new Pose2d(4.02, 5.1, Rotation2d.fromDegrees(30))), + + Index.Three, Map.of( + ReefSide.Right, new Pose2d(4.87, 5.21, Rotation2d.fromDegrees(-30)), + ReefSide.Left, new Pose2d(5.16, 5.02, Rotation2d.fromDegrees(-30))), + + Index.Four, Map.of( + ReefSide.Right, new Pose2d(5.66, 4.28, Rotation2d.fromDegrees(-90)), + ReefSide.Left, new Pose2d(5.68, 3.91, Rotation2d.fromDegrees(-90))), + + Index.Five, Map.of( + ReefSide.Right, new Pose2d(5.30, 3.10, Rotation2d.fromDegrees(-150)), + ReefSide.Left, new Pose2d(5, 2.94, Rotation2d.fromDegrees(-150))), + + Index.Six, Map.of( + ReefSide.Right, new Pose2d(4.09, 2.87, Rotation2d.fromDegrees(150)), + ReefSide.Left, new Pose2d(3.78, 3.05, Rotation2d.fromDegrees(150)))); + + ScoringPosesRed = Util.transformNestedMap(ScoringPosesBlue, (p2d) -> { + return new Pose2d(mirrorTranslationXY(p2d.getTranslation()), + p2d.getRotation().rotateBy(Rotation2d.fromDegrees(180))); + }); + RobotState = Map.of( + Position.L1, Map.of( + RobotSide.Front, new CoralScoringPosition(0., 1.75, 1.1 - wristOffset), + RobotSide.Back, new CoralScoringPosition(0, -1., -20)), + Position.L2, Map.of( + RobotSide.Front, new CoralScoringPosition(14., 1.75, 1.56 - wristOffset), + RobotSide.Back, new CoralScoringPosition(3., -1., -20)), + Position.L3, Map.of( + RobotSide.Front, new CoralScoringPosition(33., 1.75, 1.1 - wristOffset), + RobotSide.Back, new CoralScoringPosition(25., -1., -20.5)), + Position.L4, Map.of( + RobotSide.Front, new CoralScoringPosition(67., 0.7, 1.6 - wristOffset), + RobotSide.Back, new CoralScoringPosition(64, -1.45, -22.6)), + Position.Intake, Map.of( + RobotSide.Front, new CoralScoringPosition(6., 1., -4.5), + RobotSide.Back, new CoralScoringPosition(0, 0, 0.3 - wristOffset))); + L2Algae = new CoralScoringPosition(17., -3., 1. - wristOffset); + L2AlgaeLow = new CoralScoringPosition(8., -3., 1. - wristOffset); + L3Algae = new CoralScoringPosition(38., -3., 1. - wristOffset); + Net = new CoralScoringPosition(67., 0.4, 12); + ClimbUp = new CoralScoringPosition(27, 0, 0 - wristOffset); + ClimbDown = new CoralScoringPosition(2, 0, 0 - wristOffset); + ground = new CoralScoringPosition(5., -6.3, -2 - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION + Processor = new CoralScoringPosition(0, -5., 1.3 - wristOffset); // TODO: UPDATE THE WRIST COMPONENT OF THIS POSITION + algaeUp=new CoralScoringPosition(0, 0, 3); + } } diff --git a/src/main/java/frc/robot/util/PoseUtil.java b/src/main/java/frc/robot/util/PoseUtil.java deleted file mode 100644 index 66a344a..0000000 --- a/src/main/java/frc/robot/util/PoseUtil.java +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.List; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; - -/** Add your docs here. */ -public class PoseUtil { - - /** - * Finds the closest Pose2d to a reference Pose2d from a list of Pose2ds. - * "Closest" is determined by the Euclidean distance between the translation components (x, y) of the poses. - * Rotation is not considered in the distance calculation. - * - * @param poseList The list of Pose2ds to search within. Must not be null or empty. - * @param referencePose The Pose2d to find the closest Pose2d to. Must not be null. - * @return The Pose2d from the list that is closest to the referencePose, or null if the list is null or empty. - * @throws IllegalArgumentException if poseList or referencePose is null. - * @throws IllegalStateException if poseList is empty. - */ - public static Pose2d findClosestPose(List poseList, Pose2d referencePose) { - if (poseList == null) { - throw new IllegalArgumentException("poseList cannot be null"); - } - if (referencePose == null) { - throw new IllegalArgumentException("referencePose cannot be null"); - } - if (poseList.isEmpty()) { - return null; // Or throw an exception: throw new IllegalStateException("poseList cannot be empty"); - } - - Pose2d closestPose = null; - double minDistance = Double.MAX_VALUE; // Initialize with a very large value - - for (Pose2d pose : poseList) { - // Calculate the squared Euclidean distance between the translations - Translation2d poseTranslation = pose.getTranslation(); - Translation2d referenceTranslation = referencePose.getTranslation(); - double distance = referenceTranslation.getDistance(poseTranslation); - - if (distance < minDistance) { - minDistance = distance; - closestPose = pose; - } - } - - return closestPose; - } - /** - * Finds the closest Pose2d to a reference Pose2d from an array of Pose2ds. - * "Closest" is determined by the Euclidean distance between the translation components (x, y) of the poses. - * Rotation is not considered in the distance calculation. - * - * @param poseList The array of Pose2ds to search within. Must not be null or empty. - * @param referencePose The Pose2d to find the closest Pose2d to. Must not be null. - * @return The Pose2d from the array that is closest to the referencePose, or null if the array is null or empty. - * @throws IllegalArgumentException if poseList or referencePose is null. - * @throws IllegalStateException if poseList is empty. - */ - public static Pose2d findClosestPose(Pose2d[] poseList, Pose2d referencePose) { - if (poseList == null) { - throw new IllegalArgumentException("poseList cannot be null"); - } - if (referencePose == null) { - throw new IllegalArgumentException("referencePose cannot be null"); - } - if (poseList.length==0) { - return null; // Or throw an exception: throw new IllegalStateException("poseList cannot be empty"); - } - - Pose2d closestPose = null; - double minDistance = Double.MAX_VALUE; // Initialize with a very large value - - for (Pose2d pose : poseList) { - // Calculate the squared Euclidean distance between the translations - Translation2d poseTranslation = pose.getTranslation(); - Translation2d referenceTranslation = referencePose.getTranslation(); - double distance = referenceTranslation.getDistance(poseTranslation); - - if (distance < minDistance) { - minDistance = distance; - closestPose = pose; - } - } - - return closestPose; - } - /** - * Mirrors the pose across the alliance divider line - * @param pose the pose to mirror - * @return the mirrored pose - */ - public static Pose2d mirrorPoseX(Pose2d pose){ - double fieldX = 17.5387; // Field length in meters; TODO: get the real number - pose=new Pose2d(fieldX-pose.getX(), pose.getY(), pose.getRotation()); - pose.rotateBy(Rotation2d.fromRotations(0)); - return pose; - } -} diff --git a/src/main/java/frc/robot/util/TriggerNTDouble.java b/src/main/java/frc/robot/util/TriggerNTDouble.java deleted file mode 100644 index 17ecf56..0000000 --- a/src/main/java/frc/robot/util/TriggerNTDouble.java +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.util; - -import java.util.function.DoubleConsumer; - -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** Add your docs here. */ -public class TriggerNTDouble { - String name; - double val; - Trigger trigger; - DoubleEntry ntEntry; - DoubleConsumer update; - - private boolean hasChanged() { - double prev = val; - val = ntEntry.getAsDouble(); - return prev != val; - } - - public TriggerNTDouble(String name, double val, DoubleConsumer update) { - this.name = name; - this.val = val; - this.update = update; - this.ntEntry = NetworkTableInstance.getDefault().getDoubleTopic(name).getEntry(0); - this.ntEntry.set(val); - trigger = new Trigger(() -> hasChanged()); - trigger.onTrue(new InstantCommand(() -> update.accept(this.val)).ignoringDisable(true)); - } -} diff --git a/src/main/java/frc/team696 b/src/main/java/frc/team696 deleted file mode 160000 index a86a539..0000000 --- a/src/main/java/frc/team696 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a86a539b21530af6334ce673a082747c2039a4a8 diff --git a/vendordeps/PathplannerLib-2025.2.4.json b/vendordeps/PathplannerLib-2025.2.6.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.4.json rename to vendordeps/PathplannerLib-2025.2.6.json index 24add57..95ba203 100644 --- a/vendordeps/PathplannerLib-2025.2.4.json +++ b/vendordeps/PathplannerLib-2025.2.6.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.4.json", + "fileName": "PathplannerLib-2025.2.6.json", "name": "PathplannerLib", - "version": "2025.2.4", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.4" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.4", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,