From 7331221230919d106d5ed60828693b9455f6f07e Mon Sep 17 00:00:00 2001 From: James Ding Date: Sat, 2 Mar 2024 23:42:58 -0800 Subject: [PATCH] Add drive constants --- advantage-scope-layout.json | 172 ++ simgui.json | 29 + .../pathplanner/autos/Example Auto.auto | 25 + src/main/deploy/pathplanner/navgrid.json | 1633 +++++++++++++++++ .../pathplanner/paths/Amp Placement Path.path | 49 + .../pathplanner/paths/Example Path.path | 65 + .../paths/Speaker Placement Path.path | 52 + src/main/java/org/rambots/BuildConstants.java | 10 +- src/main/java/org/rambots/RobotContainer.kt | 2 +- .../org/rambots/subsystems/drive/Drive.java | 17 +- .../subsystems/drive/DriveConstants.java | 12 +- .../subsystems/drive/GyroIOPigeon2.java | 78 - .../subsystems/drive/ModuleIOSparkMax.java | 182 -- 13 files changed, 2045 insertions(+), 281 deletions(-) create mode 100644 advantage-scope-layout.json create mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Amp Placement Path.path create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/paths/Speaker Placement Path.path delete mode 100644 src/main/java/org/rambots/subsystems/drive/GyroIOPigeon2.java delete mode 100644 src/main/java/org/rambots/subsystems/drive/ModuleIOSparkMax.java diff --git a/advantage-scope-layout.json b/advantage-scope-layout.json new file mode 100644 index 0000000..87e4280 --- /dev/null +++ b/advantage-scope-layout.json @@ -0,0 +1,172 @@ +{ + "hubs": [ + { + "x": 1912, + "y": -7, + "width": 1936, + "height": 1048, + "state": { + "sidebar": { + "width": 411, + "expanded": [ + "/AdvantageKit/AprilTagVision/Inst0/estimatedPose", + "/AdvantageKit/RealOutputs/AprilTagVision/Inst0", + "/AdvantageKit/RealOutputs/AprilTagVision/TagPoses", + "/AdvantageKit/RealOutputs/AprilTagVision/TagPoses/5", + "/SmartDashboard/VisionSystemSim-main/Sim Field", + "/SmartDashboard/VisionSystemSim-main/Sim Field/Robot", + "/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/3", + "/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized/3/angle", + "/PathPlanner/HotReload", + "/CameraPublisher", + "/SmartDashboard/Auto Choices/options", + "/AdvantageKit", + "/AdvantageKit/RealOutputs/Drive", + "/AdvantageKit/RealOutputs/Odometry", + "/SmartDashboard", + "/AdvantageKit/RealOutputs/SwerveStates/Setpoints", + "/AdvantageKit/Drive", + "/AdvantageKit/Drive/Gyro", + "/AdvantageKit/Drive/Gyro/YawPosition", + "/AdvantageKit/Drive/Module3", + "/AdvantageKit/RealOutputs" + ] + }, + "tabs": { + "selected": 2, + "tabs": [ + { + "type": 0, + "path": "../docs/tabs/3D-FIELD.md" + }, + { + "type": 9, + "uuid": "0dlg2hne50yuivilq6ny4jhky9jm7dvb", + "fields": [ + { + "key": "NT:/AdvantageKit/RealOutputs/SwerveStates/Setpoints", + "sourceTypeIndex": 1, + "sourceType": "SwerveModuleState[]" + }, + null, + null + ], + "listFields": [], + "options": { + "maxSpeed": 4.5, + "rotationUnits": "radians", + "arrangement": "0,1,2,3", + "sizeLeftRight": 0.65, + "sizeFrontBack": 0.65, + "forwardDirection": "up" + }, + "configHidden": false, + "visualizer": null, + "title": "Swerve" + }, + { + "type": 6, + "uuid": "19hz2501vbin3h8zp49utke8z70shieg", + "fields": [], + "listFields": [ + [], + [ + { + "type": "Robot", + "key": "NT:/AdvantageKit/RealOutputs/Odometry/PoseEstimation", + "sourceTypeIndex": 1, + "sourceType": "Pose2d" + } + ] + ], + "options": { + "field": "2024 Field", + "alliance": "blue", + "robot": "2024 KitBot", + "unitDistance": "meters", + "unitRotation": "degrees" + }, + "configHidden": false, + "visualizer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + 7.960180612671198, + 6.660526637776816, + 5.938596988110903 + ], + "cameraTarget": [ + 2.631471835702966, + -0.10382561190326713, + -0.4995247538744311 + ] + }, + "title": "3D Field" + }, + { + "type": 5, + "uuid": "6p06uj3msyaf732qgz6y105odkfzcsmr", + "fields": [], + "listFields": [ + [ + { + "type": "Robot", + "key": "NT:/AdvantageKit/RealOutputs/Odometry/PoseEstimation", + "sourceTypeIndex": 1, + "sourceType": "Pose2d" + } + ] + ], + "options": { + "game": "2024 Field", + "unitDistance": "meters", + "unitRotation": "degrees", + "origin": "right", + "size": 0.75, + "allianceBumpers": "auto", + "allianceOrigin": "blue", + "orientation": "blue left, red right" + }, + "configHidden": false, + "visualizer": null, + "title": "Odometry" + }, + { + "type": 1, + "legendHeight": 0.3, + "legends": { + "left": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + }, + "discrete": { + "fields": [] + }, + "right": { + "lockedRange": null, + "unitConversion": { + "type": null, + "factor": 1 + }, + "fields": [] + } + }, + "title": "Line Graph" + }, + { + "type": 3, + "field": "NT:/AdvantageKit/RealOutputs/Console", + "title": "Console" + } + ] + } + } + } + ], + "satellites": [], + "version": "3.2.0" +} diff --git a/simgui.json b/simgui.json index 18ae6d0..402cac5 100644 --- a/simgui.json +++ b/simgui.json @@ -1,12 +1,41 @@ { + "HALProvider": { + "PowerDistributions": { + "window": { + "visible": true + } + }, + "RoboRIO": { + "window": { + "visible": true + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/SmartDashboard/Auto Choices": "String Chooser", "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/Auto Choices": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "FMSInfo": { + "open": true + } } }, "NetworkTables Info": { + "Server": { + "open": true + }, "visible": true } } diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..efecee1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..690f5db --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1,1633 @@ +{ + "field_size": { + "x": 16.54, + "y": 8.21 + }, + "nodeSizeMeters": 0.3, + "grid": [ + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true, + true + ], + [ + true, + true, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + false, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ], + [ + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true, + true + ] + ] +} diff --git a/src/main/deploy/pathplanner/paths/Amp Placement Path.path b/src/main/deploy/pathplanner/paths/Amp Placement Path.path new file mode 100644 index 0000000..5b74233 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp Placement Path.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.888049050119454, + "y": 6.856133569918385 + }, + "prevControl": null, + "nextControl": { + "x": 2.526699430623062, + "y": 6.856133569918385 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8230185926564035, + "y": 7.246010790953966 + }, + "prevControl": { + "x": 2.0702578059960404, + "y": 6.818096767866134 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ 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 new file mode 100644 index 0000000..77a1d45 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.013282910175676, + "y": 6.5274984191074985 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.166769560390973, + "y": 5.0964860911203305 + }, + "prevControl": { + "x": 4.166769560390973, + "y": 6.0964860911203305 + }, + "nextControl": { + "x": 6.166769560390973, + "y": 4.0964860911203305 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.0, + "y": 1.0 + }, + "prevControl": { + "x": 6.75, + "y": 2.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.331208, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Speaker Placement Path.path b/src/main/deploy/pathplanner/paths/Speaker Placement Path.path new file mode 100644 index 0000000..89f6481 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Speaker Placement Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.515656283981608, + "y": 4.9352750662796705 + }, + "prevControl": null, + "nextControl": { + "x": 3.5414668324864382, + "y": 4.905017895903413 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.515656283981608, + "y": 4.9352750662796705 + }, + "prevControl": { + "x": 3.497409785517246, + "y": 4.934389260549541 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 174.9868862449643, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java index c357a5d..8f83f66 100644 --- a/src/main/java/org/rambots/BuildConstants.java +++ b/src/main/java/org/rambots/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "frc-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 4; - public static final String GIT_SHA = "08332d209d5099e406b9c6e408f1753c0f948987"; - public static final String GIT_DATE = "2024-03-02 21:49:43 PST"; + public static final int GIT_REVISION = 5; + public static final String GIT_SHA = "5e8a73aaa4496c06ad1a4e91d3936537282b250a"; + public static final String GIT_DATE = "2024-03-02 21:55:19 PST"; public static final String GIT_BRANCH = "james/swerve"; - public static final String BUILD_DATE = "2024-03-02 21:54:52 PST"; - public static final long BUILD_UNIX_TIME = 1709445292254L; + public static final String BUILD_DATE = "2024-03-02 23:37:18 PST"; + public static final long BUILD_UNIX_TIME = 1709451438465L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index 0475535..d77b3a5 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -65,7 +65,7 @@ object RobotContainer { Constants.Mode.REAL -> { // Real robot, instantiate hardware IO implementations drive = Drive( - GyroIOPigeon2(true), + GyroIONavX2(), ModuleIOTalonFX(moduleConfigs[0]), ModuleIOTalonFX(moduleConfigs[1]), ModuleIOTalonFX(moduleConfigs[2]), diff --git a/src/main/java/org/rambots/subsystems/drive/Drive.java b/src/main/java/org/rambots/subsystems/drive/Drive.java index e970a62..d77e751 100644 --- a/src/main/java/org/rambots/subsystems/drive/Drive.java +++ b/src/main/java/org/rambots/subsystems/drive/Drive.java @@ -62,7 +62,6 @@ public class Drive extends SubsystemBase { private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SysIdRoutine sysId; - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); private Rotation2d rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[]{ @@ -168,15 +167,15 @@ public void periodic() { module.stop(); } } + // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{}); + Logger.recordOutput("SwerveStates/Setpoints"); + Logger.recordOutput("SwerveStates/SetpointsOptimized"); } // Update odometry - double[] sampleTimestamps = - modules[0].getOdometryTimestamps(); // All signals are sampled together + double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together int sampleCount = sampleTimestamps.length; for (int i = 0; i < sampleCount; i++) { // Read wheel positions and deltas from each module @@ -206,6 +205,11 @@ public void periodic() { poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); odometryDrive.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); } + + // Logging + Logger.recordOutput("SwerveStates/Measured", getModuleStates()); + Logger.recordOutput("Odometry/PoseEstimation", getPose()); + Logger.recordOutput("Odometry/Drive", getDrive()); } /** @@ -273,7 +277,6 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { /** * Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { @@ -296,7 +299,6 @@ private SwerveModulePosition[] getModulePositions() { /** * Returns the current pose estimation. */ - @AutoLogOutput(key = "Odometry/PoseEstimation") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } @@ -313,7 +315,6 @@ public void setPose(Pose2d pose) { /** * Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Drive") public Pose2d getDrive() { return odometryDrive.getEstimatedPosition(); } diff --git a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java index f8557cc..7c354cb 100644 --- a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java +++ b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java @@ -33,7 +33,6 @@ public final class DriveConstants { switch (Constants.getRobot()) { default -> 0.01; }; - public static final int gyroID = 13; // Turn to "" for no canbus name public static final String canbus = "*"; public static final ModuleConstants moduleConstants = @@ -71,11 +70,11 @@ public final class DriveConstants { switch (Constants.getRobot()) { default -> new DrivetrainConfig( Units.inchesToMeters(2.0), - Units.inchesToMeters(26.0), - Units.inchesToMeters(26.0), - Units.feetToMeters(12.16), + Units.inchesToMeters(21.251), + Units.inchesToMeters(21.251), + Units.feetToMeters(14.21), Units.feetToMeters(21.32), - 7.93, + 12.93, 29.89); }; public static final Translation2d[] moduleTranslations = @@ -89,8 +88,7 @@ public final class DriveConstants { new Translation2d( -drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0) }; - public static final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics(moduleTranslations); + public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); public static ModuleConfig[] moduleConfigs = switch (Constants.getRobot()) { case COMPBOT -> new ModuleConfig[]{ diff --git a/src/main/java/org/rambots/subsystems/drive/GyroIOPigeon2.java b/src/main/java/org/rambots/subsystems/drive/GyroIOPigeon2.java deleted file mode 100644 index d42ae28..0000000 --- a/src/main/java/org/rambots/subsystems/drive/GyroIOPigeon2.java +++ /dev/null @@ -1,78 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.drive; - -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.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; - -import java.util.OptionalDouble; -import java.util.Queue; - -import static org.rambots.subsystems.drive.DriveConstants.*; - -/** - * IO implementation for Pigeon2 - */ -public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(gyroID, canbus); - private final StatusSignal yaw = pigeon.getYaw(); - private final Queue yawPositionQueue; - private final Queue yawTimestampQueue; - private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); - - public GyroIOPigeon2(boolean phoenixDrive) { - pigeon.getConfigurator().apply(new Pigeon2Configuration()); - pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(odometryFrequency); - yawVelocity.setUpdateFrequency(100.0); - pigeon.optimizeBusUtilization(); - if (phoenixDrive) { - yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw()); - } else { - yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - boolean valid = yaw.refresh().getStatus().isOK(); - if (valid) { - return OptionalDouble.of(yaw.getValueAsDouble()); - } else { - return OptionalDouble.empty(); - } - }); - } - } - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); - inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); - - inputs.odometryYawTimestamps = - yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryYawPositions = - yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new); - yawTimestampQueue.clear(); - yawPositionQueue.clear(); - } -} diff --git a/src/main/java/org/rambots/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/org/rambots/subsystems/drive/ModuleIOSparkMax.java deleted file mode 100644 index 84f660f..0000000 --- a/src/main/java/org/rambots/subsystems/drive/ModuleIOSparkMax.java +++ /dev/null @@ -1,182 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package org.rambots.subsystems.drive; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.RobotController; - -import java.util.OptionalDouble; -import java.util.Queue; - -import static org.rambots.subsystems.drive.DriveConstants.moduleConstants; -import static org.rambots.subsystems.drive.DriveConstants.odometryFrequency; - -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO - * or NEO 550), and analog absolute encoder connected to the RIO - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOSparkMax implements ModuleIO { - private final CANSparkMax driveSparkMax; - private final CANSparkMax turnSparkMax; - - private final RelativeEncoder driveEncoder; - private final RelativeEncoder turnRelativeEncoder; - private final AnalogInput turnAbsoluteEncoder; - private final Queue timestampQueue; - private final Queue drivePositionQueue; - private final Queue turnPositionQueue; - - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(DriveConstants.ModuleConfig config) { - // Init motor & encoder objects - driveSparkMax = new CANSparkMax(config.driveID(), MotorType.kBrushless); - turnSparkMax = new CANSparkMax(config.turnID(), MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); - absoluteEncoderOffset = config.absoluteEncoderOffset(); // MUST BE CALIBRATED - - driveSparkMax.restoreFactoryDefaults(); - turnSparkMax.restoreFactoryDefaults(); - - driveSparkMax.setCANTimeout(250); - turnSparkMax.setCANTimeout(250); - - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(config.turnMotorInverted()); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); - - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); - - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - turnSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); - timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = driveEncoder.getPosition(); - if (driveSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - turnPositionQueue = - SparkMaxOdometryThread.getInstance() - .registerSignal( - () -> { - double value = turnRelativeEncoder.getPosition(); - if (driveSparkMax.getLastError() == REVLibError.kOk) { - return OptionalDouble.of(value); - } else { - return OptionalDouble.empty(); - } - }); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / moduleConstants.driveReduction(); - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) - / moduleConstants.driveReduction(); - inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); - inputs.driveCurrentAmps = new double[]{driveSparkMax.getOutputCurrent()}; - - inputs.turnAbsolutePosition = - new Rotation2d( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations( - turnRelativeEncoder.getPosition() / moduleConstants.turnReduction()); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / moduleConstants.turnReduction(); - inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); - inputs.turnCurrentAmps = new double[]{turnSparkMax.getOutputCurrent()}; - - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble( - (Double value) -> - Units.rotationsToRadians(value) / moduleConstants.driveReduction()) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map( - (Double value) -> Rotation2d.fromRotations(value / moduleConstants.turnReduction())) - .toArray(Rotation2d[]::new); - timestampQueue.clear(); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); - } - - @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - driveSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); - } -}