From ff05cbfc6ef848e306f499cff8bcef27cc970e37 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Sun, 5 Jan 2025 22:03:30 -0500 Subject: [PATCH] added logging to choreo paths and also copied choreo config as it is in the ctre example --- advantagescope-layout-telemetry.json | 181 ++++++++++++++---- src/main/deploy/choreo/Simple Trajectory.traj | 114 +++++------ src/main/deploy/choreo/config.chor | 40 ++-- src/main/java/frc/robot/commands/Autos.java | 12 +- .../commands/WheelRadiusCharacterization.java | 2 +- .../frc/robot/generated/TunerConstants.java | 22 +-- .../java/frc/robot/subsystems/Swerve.java | 4 +- .../WheelRadiusCharacterizationTest.java | 4 +- 8 files changed, 229 insertions(+), 150 deletions(-) diff --git a/advantagescope-layout-telemetry.json b/advantagescope-layout-telemetry.json index 2ce3f07..439f896 100644 --- a/advantagescope-layout-telemetry.json +++ b/advantagescope-layout-telemetry.json @@ -13,19 +13,20 @@ "/Robot/Swerve/Desired Module States/0", "/Robot/Swerve/Module States/0", "/Robot/Swerve/Pose/translation", - "/Robot", "/NT/Faults", "/NT/Faults/Active Faults/errors", "/NT/Faults/Active Faults", "/Robot/Faults", "/Robot/Faults/Counts", - "/Robot/Swerve", "/Robot/Swerve/left-arducam", - "/Robot/Swerve/left-arducam/Estimate" + "/Robot/Swerve/left-arducam/Estimate", + "/Robot/Swerve/blue-arducam/Estimate", + "/Robot", + "/Robot/Swerve" ] }, "tabs": { - "selected": 1, + "selected": 3, "tabs": [ { "type": 0, @@ -35,6 +36,66 @@ "renderer": "#/", "controlsHeight": 0 }, + { + "type": 9, + "title": "Swerve", + "controller": { + "sources": [ + { + "type": "chassisSpeeds", + "logKey": "NT:/Robot/Swerve/Speeds", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#ff0000" + } + }, + { + "type": "chassisSpeeds", + "logKey": "NT:/Robot/Swerve/Desired Speeds", + "logType": "ChassisSpeeds", + "visible": true, + "options": { + "color": "#0000ff" + } + }, + { + "type": "states", + "logKey": "NT:/Robot/Swerve/Module States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#ff0000", + "arrangement": "0,1,2,3" + } + }, + { + "type": "states", + "logKey": "NT:/Robot/Swerve/Desired Module States", + "logType": "SwerveModuleState[]", + "visible": true, + "options": { + "color": "#0000ff", + "arrangement": "0,1,2,3" + } + }, + { + "type": "rotation", + "logKey": "NT:/Robot/Swerve/Pose/rotation", + "logType": "Rotation2d", + "visible": true, + "options": {} + } + ], + "maxSpeed": 4.7, + "sizeX": 0.65, + "sizeY": 0.65, + "orientation": 1 + }, + "controllerUUID": "tj8b7ik3e8qxcapobn65d8jzeb8a406h", + "renderer": null, + "controlsHeight": 200 + }, { "type": 2, "title": "Odometry", @@ -61,7 +122,7 @@ "type": "ghost", "logKey": "NT:/Robot/Swerve/Accepted Estimates", "logType": "Pose3d[]", - "visible": true, + "visible": false, "options": { "color": "#00ff00" } @@ -70,13 +131,32 @@ "type": "ghost", "logKey": "NT:/Robot/Swerve/Rejected Estimates", "logType": "Pose3d[]", - "visible": true, + "visible": false, "options": { "color": "#ffff00" } + }, + { + "type": "trajectory", + "logKey": "NT:/Robot/Auto/Current Trajectory", + "logType": "Pose2d[]", + "visible": true, + "options": { + "color": "#ff00ff", + "size": "normal" + } + }, + { + "type": "ghost", + "logKey": "NT:/Robot/Auto/Current Trajectory Desired Pose", + "logType": "Pose2d", + "visible": true, + "options": { + "color": "#ff00ff" + } } ], - "game": "2024 Field", + "game": "2025 Field", "bumpers": "auto", "origin": "blue", "orientation": 0, @@ -87,63 +167,88 @@ "controlsHeight": 200 }, { - "type": 9, - "title": "Swerve", + "type": 3, + "title": "3D Field", "controller": { "sources": [ { - "type": "chassisSpeeds", - "logKey": "NT:/Robot/Swerve/Speeds", - "logType": "ChassisSpeeds", + "type": "trajectory", + "logKey": "NT:/Robot/Auto/Current Trajectory", + "logType": "Pose2d[]", "visible": true, "options": { - "color": "#ff0000" + "color": "#ff00ff", + "size": "normal" } }, { - "type": "chassisSpeeds", - "logKey": "NT:/Robot/Swerve/Desired Speeds", - "logType": "ChassisSpeeds", + "type": "ghost", + "logKey": "NT:/Robot/Auto/Current Trajectory Desired Pose", + "logType": "Pose2d", "visible": true, "options": { - "color": "#0000ff" + "model": "Crab Bot", + "color": "#ff00ff" } }, { - "type": "states", - "logKey": "NT:/Robot/Swerve/Module States", - "logType": "SwerveModuleState[]", + "type": "robot", + "logKey": "NT:/Robot/Swerve/Pose", + "logType": "Pose2d", "visible": true, "options": { - "color": "#ff0000", - "arrangement": "0,1,2,3" + "model": "Crab Bot" } }, { - "type": "states", - "logKey": "NT:/Robot/Swerve/Desired Module States", - "logType": "SwerveModuleState[]", + "type": "vision", + "logKey": "NT:/Robot/Swerve/Detected Tags", + "logType": "Pose3d[]", "visible": true, "options": { - "color": "#0000ff", - "arrangement": "0,1,2,3" + "color": "#00ff00", + "size": "normal" } }, { - "type": "rotation", - "logKey": "NT:/Robot/Swerve/Pose/rotation", - "logType": "Rotation2d", + "type": "ghost", + "logKey": "NT:/Robot/Swerve/Accepted Estimates", + "logType": "Pose3d[]", "visible": true, - "options": {} + "options": { + "model": "Crab Bot", + "color": "#00ff00" + } + }, + { + "type": "ghost", + "logKey": "NT:/Robot/Swerve/Rejected Estimates", + "logType": "Pose3d[]", + "visible": true, + "options": { + "model": "Crab Bot", + "color": "#ff0000" + } } ], - "maxSpeed": 4.7, - "sizeX": 0.65, - "sizeY": 0.65, - "orientation": 1 + "game": "2025 Field", + "origin": "blue" + }, + "controllerUUID": "s72u8uqxhizena798en69qzlt8fabtzk", + "renderer": { + "cameraIndex": -1, + "orbitFov": 50, + "cameraPosition": [ + -3.8929782247951725, + 3.65017916178649, + -5.250724325879612 + ], + "cameraTarget": [ + 0, + 0.5, + 0 + ] }, - "controllerUUID": "tj8b7ik3e8qxcapobn65d8jzeb8a406h", - "renderer": null, "controlsHeight": 200 } ] @@ -152,5 +257,5 @@ } ], "satellites": [], - "version": "4.0.0-beta-1" + "version": "4.1.0" } diff --git a/src/main/deploy/choreo/Simple Trajectory.traj b/src/main/deploy/choreo/Simple Trajectory.traj index 1b7323e..e704204 100644 --- a/src/main/deploy/choreo/Simple Trajectory.traj +++ b/src/main/deploy/choreo/Simple Trajectory.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":0.74199378490448, "y":4.257874488830566, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.480556011199951, "y":3.866609096527099, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":9.437779426574709, "y":1.0520118474960327, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":0.74199378490448, "y":4.257874488830566, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.147460699081421, "y":6.279630661010742, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.431147575378418, "y":6.279630661010742, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"0.74199378490448 m", "val":0.74199378490448}, "y":{"exp":"4.257874488830566 m", "val":4.257874488830566}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.480556011199951 m", "val":6.480556011199951}, "y":{"exp":"3.8666090965270996 m", "val":3.866609096527099}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"9.437779426574707 m", "val":9.437779426574709}, "y":{"exp":"1.0520118474960327 m", "val":1.0520118474960327}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"0.74199378490448 m", "val":0.74199378490448}, "y":{"exp":"4.257874488830566 m", "val":4.257874488830566}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.147460699081421 m", "val":3.147460699081421}, "y":{"exp":"6.279630661010742 m", "val":6.279630661010742}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.431147575378418 m", "val":5.431147575378418}, "y":{"exp":"6.279630661010742 m", "val":6.279630661010742}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -28,72 +28,44 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.4498,2.55899], + "waypoints":[0.0,0.91839,1.60031], "samples":[ - {"t":0.0, "x":0.74199, "y":4.25787, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.01372, "ay":-0.27427, "alpha":0.0, "fx":[153.32072,153.32072,153.32072,153.32072], "fy":[-4.66524,-4.66524,-4.66524,-4.66524]}, - {"t":0.04142, "x":0.74973, "y":4.25764, "heading":0.0, "vx":0.37337, "vy":-0.01136, "omega":0.0, "ax":9.01297, "ay":-0.27427, "alpha":0.0, "fx":[153.30796,153.30796,153.30796,153.30796], "fy":[-4.66522,-4.66522,-4.66522,-4.66522]}, - {"t":0.08285, "x":0.77293, "y":4.25693, "heading":0.0, "vx":0.74671, "vy":-0.02272, "omega":0.0, "ax":9.01208, "ay":-0.27426, "alpha":0.0, "fx":[153.29287,153.29287,153.29287,153.29287], "fy":[-4.66508,-4.66508,-4.66508,-4.66508]}, - {"t":0.12427, "x":0.81159, "y":4.25576, "heading":0.0, "vx":1.12002, "vy":-0.03408, "omega":0.0, "ax":9.01101, "ay":-0.27424, "alpha":0.0, "fx":[153.27474,153.27474,153.27474,153.27474], "fy":[-4.6648,-4.6648,-4.6648,-4.6648]}, - {"t":0.16569, "x":0.86571, "y":4.25411, "heading":0.0, "vx":1.49328, "vy":-0.04544, "omega":0.0, "ax":9.00971, "ay":-0.27422, "alpha":0.0, "fx":[153.25256,153.25256,153.25256,153.25256], "fy":[-4.66436,-4.66436,-4.66436,-4.66436]}, - {"t":0.20711, "x":0.9353, "y":4.25199, "heading":0.0, "vx":1.86649, "vy":-0.0568, "omega":0.0, "ax":9.00808, "ay":-0.27418, "alpha":0.0, "fx":[153.22478,153.22478,153.22478,153.22478], "fy":[-4.6637,-4.6637,-4.6637,-4.6637]}, - {"t":0.24854, "x":1.02034, "y":4.2494, "heading":0.0, "vx":2.23963, "vy":-0.06816, "omega":0.0, "ax":9.00597, "ay":-0.27412, "alpha":0.0, "fx":[153.18899,153.18899,153.18899,153.18899], "fy":[-4.66276,-4.66276,-4.66276,-4.66276]}, - {"t":0.28996, "x":1.12084, "y":4.24635, "heading":0.0, "vx":2.61268, "vy":-0.07951, "omega":0.0, "ax":9.00316, "ay":-0.27404, "alpha":0.0, "fx":[153.14114,153.14114,153.14114,153.14114], "fy":[-4.66142,-4.66142,-4.66142,-4.66142]}, - {"t":0.33138, "x":1.23679, "y":4.24282, "heading":0.0, "vx":2.98561, "vy":-0.09087, "omega":0.0, "ax":8.9992, "ay":-0.27393, "alpha":0.0, "fx":[153.07389,153.07389,153.07389,153.07389], "fy":[-4.65946,-4.65946,-4.65946,-4.65946]}, - {"t":0.3728, "x":1.36818, "y":4.23882, "heading":0.0, "vx":3.35838, "vy":-0.10221, "omega":0.0, "ax":8.99324, "ay":-0.27375, "alpha":0.0, "fx":[152.97245,152.97245,152.97245,152.97245], "fy":[-4.65643,-4.65643,-4.65643,-4.65643]}, - {"t":0.41423, "x":1.51501, "y":4.23435, "heading":0.0, "vx":3.73091, "vy":-0.11355, "omega":0.0, "ax":8.98322, "ay":-0.27345, "alpha":0.0, "fx":[152.80198,152.80198,152.80198,152.80198], "fy":[-4.65129,-4.65129,-4.65129,-4.65129]}, - {"t":0.45565, "x":1.67726, "y":4.22941, "heading":0.0, "vx":4.10302, "vy":-0.12488, "omega":0.0, "ax":8.96288, "ay":-0.27283, "alpha":0.0, "fx":[152.4561,152.4561,152.4561,152.4561], "fy":[-4.64081,-4.64081,-4.64081,-4.64081]}, - {"t":0.49707, "x":1.85491, "y":4.224, "heading":0.0, "vx":4.47429, "vy":-0.13618, "omega":0.0, "ax":8.89987, "ay":-0.27092, "alpha":0.0, "fx":[151.38424,151.38424,151.38424,151.38424], "fy":[-4.60826,-4.60826,-4.60826,-4.60826]}, - {"t":0.5385, "x":2.04788, "y":4.21813, "heading":0.0, "vx":4.84294, "vy":-0.1474, "omega":0.0, "ax":1.46168, "ay":-0.04476, "alpha":0.0, "fx":[24.86278,24.86278,24.86278,24.86278], "fy":[-0.76137,-0.76137,-0.76137,-0.76137]}, - {"t":0.57992, "x":2.24974, "y":4.21199, "heading":0.0, "vx":4.90349, "vy":-0.14926, "omega":0.0, "ax":0.00048, "ay":-0.00004, "alpha":0.0, "fx":[0.00815,0.00815,0.00815,0.00815], "fy":[-0.00061,-0.00061,-0.00061,-0.00061]}, - {"t":0.62134, "x":2.45286, "y":4.2058, "heading":0.0, "vx":4.90351, "vy":-0.14926, "omega":0.0, "ax":-0.00001, "ay":0.00004, "alpha":0.0, "fx":[-0.00014,-0.00014,-0.00014,-0.00014], "fy":[0.00069,0.00069,0.00069,0.00069]}, - {"t":0.66276, "x":2.65598, "y":4.19962, "heading":0.0, "vx":4.90351, "vy":-0.14926, "omega":0.0, "ax":-0.00001, "ay":0.00006, "alpha":0.0, "fx":[-0.00011,-0.00011,-0.00011,-0.00011], "fy":[0.00097,0.00097,0.00097,0.00097]}, - {"t":0.70419, "x":2.85909, "y":4.19344, "heading":0.0, "vx":4.90351, "vy":-0.14925, "omega":0.0, "ax":-0.00001, "ay":0.00006, "alpha":0.0, "fx":[-0.0001,-0.0001,-0.0001,-0.0001], "fy":[0.00105,0.00105,0.00105,0.00105]}, - {"t":0.74561, "x":3.06221, "y":4.18726, "heading":0.0, "vx":4.90351, "vy":-0.14925, "omega":0.0, "ax":-0.00001, "ay":0.00006, "alpha":0.0, "fx":[-0.00009,-0.00009,-0.00009,-0.00009], "fy":[0.00105,0.00105,0.00105,0.00105]}, - {"t":0.78703, "x":3.26533, "y":4.18107, "heading":0.0, "vx":4.90351, "vy":-0.14925, "omega":0.0, "ax":0.0, "ay":0.00006, "alpha":0.0, "fx":[-0.00008,-0.00008,-0.00008,-0.00008], "fy":[0.00099,0.00099,0.00099,0.00099]}, - {"t":0.82845, "x":3.46844, "y":4.17489, "heading":0.0, "vx":4.90351, "vy":-0.14925, "omega":0.0, "ax":0.0, "ay":0.00005, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[0.00085,0.00085,0.00085,0.00085]}, - {"t":0.86988, "x":3.67156, "y":4.16871, "heading":0.0, "vx":4.90351, "vy":-0.14924, "omega":0.0, "ax":0.0, "ay":0.00003, "alpha":0.0, "fx":[-0.00008,-0.00008,-0.00008,-0.00008], "fy":[0.00052,0.00052,0.00052,0.00052]}, - {"t":0.9113, "x":3.87468, "y":4.16253, "heading":0.0, "vx":4.90351, "vy":-0.14924, "omega":0.0, "ax":-0.00001, "ay":-0.00004, "alpha":0.0, "fx":[-0.0001,-0.0001,-0.0001,-0.0001], "fy":[-0.00065,-0.00065,-0.00065,-0.00065]}, - {"t":0.95272, "x":4.07779, "y":4.15635, "heading":0.0, "vx":4.90351, "vy":-0.14924, "omega":0.0, "ax":-0.00001, "ay":-0.00034, "alpha":0.0, "fx":[-0.00025,-0.00025,-0.00025,-0.00025], "fy":[-0.00571,-0.00571,-0.00571,-0.00571]}, - {"t":0.99415, "x":4.28091, "y":4.15016, "heading":0.0, "vx":4.90351, "vy":-0.14926, "omega":0.0, "ax":-0.00006, "ay":-0.00172, "alpha":0.0, "fx":[-0.00096,-0.00096,-0.00096,-0.00096], "fy":[-0.02928,-0.02928,-0.02928,-0.02928]}, - {"t":1.03557, "x":4.48403, "y":4.14398, "heading":0.0, "vx":4.9035, "vy":-0.14933, "omega":0.0, "ax":-0.00026, "ay":-0.0083, "alpha":0.0, "fx":[-0.00437,-0.00437,-0.00437,-0.00437], "fy":[-0.14116,-0.14116,-0.14116,-0.14116]}, - {"t":1.07699, "x":4.68714, "y":4.13779, "heading":0.0, "vx":4.90349, "vy":-0.14967, "omega":0.0, "ax":-0.00122, "ay":-0.03965, "alpha":0.0, "fx":[-0.02076,-0.02076,-0.02076,-0.02076], "fy":[-0.67449,-0.67449,-0.67449,-0.67449]}, - {"t":1.11841, "x":4.89026, "y":4.13155, "heading":0.0, "vx":4.90344, "vy":-0.15132, "omega":0.0, "ax":-0.00599, "ay":-0.1892, "alpha":0.0, "fx":[-0.10197,-0.10197,-0.10197,-0.10197], "fy":[-3.21828,-3.21828,-3.21828,-3.21828]}, - {"t":1.15984, "x":5.09337, "y":4.12512, "heading":0.0, "vx":4.90319, "vy":-0.15915, "omega":0.0, "ax":-0.03244, "ay":-0.8943, "alpha":0.0, "fx":[-0.5518,-0.5518,-0.5518,-0.5518], "fy":[-15.21175,-15.21175,-15.21175,-15.21175]}, - {"t":1.20126, "x":5.29644, "y":4.11776, "heading":0.0, "vx":4.90185, "vy":-0.1962, "omega":0.0, "ax":-0.19921, "ay":-3.59893, "alpha":0.0, "fx":[-3.38853,-3.38853,-3.38853,-3.38853], "fy":[-61.21669,-61.21669,-61.21669,-61.21669]}, - {"t":1.24268, "x":5.49932, "y":4.10655, "heading":0.0, "vx":4.8936, "vy":-0.34527, "omega":0.0, "ax":-0.71064, "ay":-7.0471, "alpha":0.0, "fx":[-12.08774,-12.08774,-12.08774,-12.08774], "fy":[-119.86912,-119.86912,-119.86912,-119.86912]}, - {"t":1.2841, "x":5.70141, "y":4.0862, "heading":0.0, "vx":4.86416, "vy":-0.63718, "omega":0.0, "ax":-1.36801, "ay":-8.18595, "alpha":0.0, "fx":[-23.26949,-23.26949,-23.26949,-23.26949], "fy":[-139.24072,-139.24072,-139.24072,-139.24072]}, - {"t":1.32553, "x":5.90173, "y":4.05278, "heading":0.0, "vx":4.8075, "vy":-0.97627, "omega":0.0, "ax":-2.04038, "ay":-8.43146, "alpha":0.0, "fx":[-34.70629,-34.70629,-34.70629,-34.70629], "fy":[-143.41676,-143.41676,-143.41676,-143.41676]}, - {"t":1.36695, "x":6.09912, "y":4.00511, "heading":0.0, "vx":4.72298, "vy":-1.32552, "omega":0.0, "ax":-2.70469, "ay":-8.39924, "alpha":0.0, "fx":[-46.006,-46.006,-46.006,-46.006], "fy":[-142.86865,-142.86865,-142.86865,-142.86865]}, - {"t":1.40837, "x":6.29244, "y":3.943, "heading":0.0, "vx":4.61094, "vy":-1.67344, "omega":0.0, "ax":-3.35355, "ay":-8.23962, "alpha":0.0, "fx":[-57.04286,-57.04286,-57.04286,-57.04286], "fy":[-140.15359,-140.15359,-140.15359,-140.15359]}, - {"t":1.4498, "x":6.48056, "y":3.86661, "heading":0.0, "vx":4.47203, "vy":-2.01475, "omega":0.0, "ax":-3.93742, "ay":-7.97169, "alpha":0.0, "fx":[-66.97431,-66.97431,-66.97431,-66.97431], "fy":[-135.59621,-135.59621,-135.59621,-135.59621]}, - {"t":1.48941, "x":6.65462, "y":3.78054, "heading":0.0, "vx":4.31605, "vy":-2.33054, "omega":0.0, "ax":-4.45963, "ay":-7.61017, "alpha":0.0, "fx":[-75.85711,-75.85711,-75.85711,-75.85711], "fy":[-129.44689,-129.44689,-129.44689,-129.44689]}, - {"t":1.52902, "x":6.8221, "y":3.68225, "heading":0.0, "vx":4.13939, "vy":-2.63201, "omega":0.0, "ax":-4.90789, "ay":-7.15937, "alpha":0.0, "fx":[-83.48176,-83.48176,-83.48176,-83.48176], "fy":[-121.77875,-121.77875,-121.77875,-121.77875]}, - {"t":1.56864, "x":6.98223, "y":3.57237, "heading":0.0, "vx":3.94497, "vy":-2.91562, "omega":0.0, "ax":-5.18284, "ay":-6.54361, "alpha":0.0, "fx":[-88.15869,-88.15869,-88.15869,-88.15869], "fy":[-111.30502,-111.30502,-111.30502,-111.30502]}, - {"t":1.60825, "x":7.13443, "y":3.45173, "heading":0.0, "vx":3.73965, "vy":-3.17484, "omega":0.0, "ax":-4.92814, "ay":-5.46878, "alpha":0.0, "fx":[-83.82628,-83.82628,-83.82628,-83.82628], "fy":[-93.02242,-93.02242,-93.02242,-93.02242]}, - {"t":1.64787, "x":7.27871, "y":3.32167, "heading":0.0, "vx":3.54443, "vy":-3.39148, "omega":0.0, "ax":-3.10851, "ay":-3.13542, "alpha":0.0, "fx":[-52.87494,-52.87494,-52.87494,-52.87494], "fy":[-53.33252,-53.33252,-53.33252,-53.33252]}, - {"t":1.68748, "x":7.41668, "y":3.18486, "heading":0.0, "vx":3.42129, "vy":-3.51569, "omega":0.0, "ax":-0.93816, "ay":-0.90349, "alpha":0.0, "fx":[-15.95783,-15.95783,-15.95783,-15.95783], "fy":[-15.36802,-15.36802,-15.36802,-15.36802]}, - {"t":1.72709, "x":7.55148, "y":3.04488, "heading":0.0, "vx":3.38413, "vy":-3.55148, "omega":0.0, "ax":-0.21975, "ay":-0.20889, "alpha":0.0, "fx":[-3.7378,-3.7378,-3.7378,-3.7378], "fy":[-3.55309,-3.55309,-3.55309,-3.55309]}, - {"t":1.76671, "x":7.68536, "y":2.90403, "heading":0.0, "vx":3.37542, "vy":-3.55975, "omega":0.0, "ax":-0.05204, "ay":-0.04932, "alpha":0.0, "fx":[-0.88512,-0.88512,-0.88512,-0.88512], "fy":[-0.83888,-0.83888,-0.83888,-0.83888]}, - {"t":1.80632, "x":7.81903, "y":2.76298, "heading":0.0, "vx":3.37336, "vy":-3.56171, "omega":0.0, "ax":-0.01428, "ay":-0.01353, "alpha":0.0, "fx":[-0.24292,-0.24292,-0.24292,-0.24292], "fy":[-0.23013,-0.23013,-0.23013,-0.23013]}, - {"t":1.84594, "x":7.95266, "y":2.62187, "heading":0.0, "vx":3.37279, "vy":-3.56224, "omega":0.0, "ax":-0.00607, "ay":-0.00576, "alpha":0.0, "fx":[-0.10329,-0.10329,-0.10329,-0.10329], "fy":[-0.09791,-0.09791,-0.09791,-0.09791]}, - {"t":1.88555, "x":8.08626, "y":2.48075, "heading":0.0, "vx":3.37255, "vy":-3.56247, "omega":0.0, "ax":-0.00477, "ay":-0.00452, "alpha":0.0, "fx":[-0.08117,-0.08117,-0.08117,-0.08117], "fy":[-0.07697,-0.07697,-0.07697,-0.07697]}, - {"t":1.92516, "x":8.21986, "y":2.33963, "heading":0.0, "vx":3.37236, "vy":-3.56265, "omega":0.0, "ax":-0.00595, "ay":-0.00563, "alpha":0.0, "fx":[-0.10114,-0.10114,-0.10114,-0.10114], "fy":[-0.09582,-0.09582,-0.09582,-0.09582]}, - {"t":1.96478, "x":8.35345, "y":2.19849, "heading":0.0, "vx":3.37213, "vy":-3.56287, "omega":0.0, "ax":-0.01604, "ay":-0.00628, "alpha":0.0, "fx":[-0.27278,-0.27278,-0.27278,-0.27278], "fy":[-0.10682,-0.10682,-0.10682,-0.10682]}, - {"t":2.00439, "x":8.48702, "y":2.05735, "heading":0.0, "vx":3.37149, "vy":-3.56312, "omega":0.0, "ax":-4.7359, "ay":4.9752, "alpha":0.0, "fx":[-80.55625,-80.55625,-80.55625,-80.55625], "fy":[84.62666,84.62666,84.62666,84.62666]}, - {"t":2.04401, "x":8.61686, "y":1.9201, "heading":0.0, "vx":3.18388, "vy":-3.36603, "omega":0.0, "ax":-6.12337, "ay":6.46899, "alpha":0.0, "fx":[-104.15678,-104.15678,-104.15678,-104.15678], "fy":[110.03569,110.03569,110.03569,110.03569]}, - {"t":2.08362, "x":8.73818, "y":1.79183, "heading":0.0, "vx":2.94131, "vy":-3.10977, "omega":0.0, "ax":-6.16277, "ay":6.51195, "alpha":0.0, "fx":[-104.82694,-104.82694,-104.82694,-104.82694], "fy":[110.76645,110.76645,110.76645,110.76645]}, - {"t":2.12323, "x":8.84986, "y":1.67375, "heading":0.0, "vx":2.69718, "vy":-2.85181, "omega":0.0, "ax":-6.17617, "ay":6.52689, "alpha":0.0, "fx":[-105.05496,-105.05496,-105.05496,-105.05496], "fy":[111.02058,111.02058,111.02058,111.02058]}, - {"t":2.16285, "x":8.95186, "y":1.5659, "heading":0.0, "vx":2.45252, "vy":-2.59325, "omega":0.0, "ax":-6.18282, "ay":6.53457, "alpha":0.0, "fx":[-105.16803,-105.16803,-105.16803,-105.16803], "fy":[111.15118,111.15118,111.15118,111.15118]}, - {"t":2.20246, "x":9.04417, "y":1.4683, "heading":0.0, "vx":2.20759, "vy":-2.33439, "omega":0.0, "ax":-6.18672, "ay":6.53931, "alpha":0.0, "fx":[-105.23429,-105.23429,-105.23429,-105.23429], "fy":[111.23183,111.23183,111.23183,111.23183]}, - {"t":2.24208, "x":9.12676, "y":1.38096, "heading":0.0, "vx":1.96251, "vy":-2.07534, "omega":0.0, "ax":-6.18922, "ay":6.54258, "alpha":0.0, "fx":[-105.27681,-105.27681,-105.27681,-105.27681], "fy":[111.28747,111.28747,111.28747,111.28747]}, - {"t":2.28169, "x":9.19965, "y":1.30388, "heading":0.0, "vx":1.71733, "vy":-1.81616, "omega":0.0, "ax":-6.19091, "ay":6.54502, "alpha":0.0, "fx":[-105.30557,-105.30557,-105.30557,-105.30557], "fy":[111.32888,111.32888,111.32888,111.32888]}, - {"t":2.3213, "x":9.26282, "y":1.23707, "heading":0.0, "vx":1.47209, "vy":-1.55689, "omega":0.0, "ax":-6.19208, "ay":6.54694, "alpha":0.0, "fx":[-105.32558,-105.32558,-105.32558,-105.32558], "fy":[111.36149,111.36149,111.36149,111.36149]}, - {"t":2.36092, "x":9.31628, "y":1.18053, "heading":0.0, "vx":1.22679, "vy":-1.29754, "omega":0.0, "ax":-6.19291, "ay":6.54851, "alpha":0.0, "fx":[-105.33964,-105.33964,-105.33964,-105.33964], "fy":[111.38832,111.38832,111.38832,111.38832]}, - {"t":2.40053, "x":9.36002, "y":1.13427, "heading":0.0, "vx":0.98147, "vy":-1.03813, "omega":0.0, "ax":-6.19349, "ay":6.54986, "alpha":0.0, "fx":[-105.34944,-105.34944,-105.34944,-105.34944], "fy":[111.41118,111.41118,111.41118,111.41118]}, - {"t":2.44015, "x":9.39404, "y":1.09828, "heading":0.0, "vx":0.73612, "vy":-0.77866, "omega":0.0, "ax":-6.19388, "ay":6.55103, "alpha":0.0, "fx":[-105.35606,-105.35606,-105.35606,-105.35606], "fy":[111.43122,111.43122,111.43122,111.43122]}, - {"t":2.47976, "x":9.41834, "y":1.07258, "heading":0.0, "vx":0.49075, "vy":-0.51915, "omega":0.0, "ax":-6.19412, "ay":6.55209, "alpha":0.0, "fx":[-105.36024,-105.36024,-105.36024,-105.36024], "fy":[111.4492,111.4492,111.4492,111.4492]}, - {"t":2.51937, "x":9.43292, "y":1.05715, "heading":0.0, "vx":0.24538, "vy":-0.25959, "omega":0.0, "ax":-6.19425, "ay":6.55306, "alpha":0.0, "fx":[-105.3625,-105.3625,-105.3625,-105.3625], "fy":[111.46562,111.46562,111.46562,111.46562]}, - {"t":2.55899, "x":9.43778, "y":1.05201, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":0.74199, "y":4.25787, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":27.7987, "ay":23.78281, "alpha":0.0, "fx":[236.42393,236.42393,236.42393,236.42393], "fy":[202.26939,202.26939,202.26939,202.26939]}, + {"t":0.04592, "x":0.7713, "y":4.28295, "heading":0.0, "vx":1.2765, "vy":1.0921, "omega":0.0, "ax":27.76967, "ay":23.76244, "alpha":0.0, "fx":[236.17705,236.17705,236.17705,236.17705], "fy":[202.09616,202.09616,202.09616,202.09616]}, + {"t":0.09184, "x":0.8592, "y":4.35815, "heading":0.0, "vx":2.55168, "vy":2.18326, "omega":0.0, "ax":4.46725, "ay":3.84282, "alpha":0.0, "fx":[37.99334,37.99334,37.99334,37.99334], "fy":[32.68264,32.68264,32.68264,32.68264]}, + {"t":0.13776, "x":0.98108, "y":4.46246, "heading":0.0, "vx":2.75681, "vy":2.35972, "omega":0.0, "ax":-0.00671, "ay":0.0079, "alpha":0.0, "fx":[-0.05711,-0.05711,-0.05711,-0.05711], "fy":[0.06715,0.06715,0.06715,0.06715]}, + {"t":0.18368, "x":1.10766, "y":4.57082, "heading":0.0, "vx":2.7565, "vy":2.36008, "omega":0.0, "ax":-0.00591, "ay":0.00689, "alpha":0.0, "fx":[-0.05028,-0.05028,-0.05028,-0.05028], "fy":[0.05861,0.05861,0.05861,0.05861]}, + {"t":0.2296, "x":1.23423, "y":4.6792, "heading":0.0, "vx":2.75623, "vy":2.3604, "omega":0.0, "ax":-0.00533, "ay":0.00621, "alpha":0.0, "fx":[-0.04529,-0.04529,-0.04529,-0.04529], "fy":[0.05279,0.05279,0.05279,0.05279]}, + {"t":0.27552, "x":1.36079, "y":4.7876, "heading":0.0, "vx":2.75599, "vy":2.36068, "omega":0.0, "ax":-0.00489, "ay":0.0057, "alpha":0.0, "fx":[-0.04162,-0.04162,-0.04162,-0.04162], "fy":[0.04851,0.04851,0.04851,0.04851]}, + {"t":0.32144, "x":1.48734, "y":4.89601, "heading":0.0, "vx":2.75576, "vy":2.36094, "omega":0.0, "ax":-0.00457, "ay":0.00533, "alpha":0.0, "fx":[-0.03887,-0.03887,-0.03887,-0.03887], "fy":[0.04529,0.04529,0.04529,0.04529]}, + {"t":0.36736, "x":1.61388, "y":5.00442, "heading":0.0, "vx":2.75555, "vy":2.36119, "omega":0.0, "ax":-0.00432, "ay":0.00504, "alpha":0.0, "fx":[-0.03678,-0.03678,-0.03678,-0.03678], "fy":[0.04286,0.04286,0.04286,0.04286]}, + {"t":0.41328, "x":1.74041, "y":5.11285, "heading":0.0, "vx":2.75535, "vy":2.36142, "omega":0.0, "ax":-0.00414, "ay":0.00483, "alpha":0.0, "fx":[-0.03522,-0.03522,-0.03522,-0.03522], "fy":[0.04104,0.04104,0.04104,0.04104]}, + {"t":0.4592, "x":1.86693, "y":5.2213, "heading":0.0, "vx":2.75516, "vy":2.36164, "omega":0.0, "ax":-0.00401, "ay":0.00467, "alpha":0.0, "fx":[-0.03407,-0.03407,-0.03407,-0.03407], "fy":[0.0397,0.0397,0.0397,0.0397]}, + {"t":0.50512, "x":1.99344, "y":5.32975, "heading":0.0, "vx":2.75498, "vy":2.36186, "omega":0.0, "ax":-0.00391, "ay":0.00456, "alpha":0.0, "fx":[-0.03326,-0.03326,-0.03326,-0.03326], "fy":[0.03876,0.03876,0.03876,0.03876]}, + {"t":0.55103, "x":2.11994, "y":5.43821, "heading":0.0, "vx":2.7548, "vy":2.36207, "omega":0.0, "ax":-0.00385, "ay":0.00449, "alpha":0.0, "fx":[-0.03275,-0.03275,-0.03275,-0.03275], "fy":[0.03816,0.03816,0.03816,0.03816]}, + {"t":0.59695, "x":2.24644, "y":5.54668, "heading":0.0, "vx":2.75462, "vy":2.36227, "omega":0.0, "ax":-0.00382, "ay":0.00445, "alpha":0.0, "fx":[-0.0325,-0.0325,-0.0325,-0.0325], "fy":[0.03787,0.03787,0.03787,0.03787]}, + {"t":0.64287, "x":2.37293, "y":5.65515, "heading":0.0, "vx":2.75445, "vy":2.36248, "omega":0.0, "ax":-0.00382, "ay":0.00445, "alpha":0.0, "fx":[-0.03249,-0.03249,-0.03249,-0.03249], "fy":[0.03786,0.03786,0.03786,0.03786]}, + {"t":0.68879, "x":2.49941, "y":5.76364, "heading":0.0, "vx":2.75427, "vy":2.36268, "omega":0.0, "ax":-0.00385, "ay":0.00448, "alpha":0.0, "fx":[-0.03271,-0.03271,-0.03271,-0.03271], "fy":[0.03812,0.03812,0.03812,0.03812]}, + {"t":0.73471, "x":2.62588, "y":5.87214, "heading":0.0, "vx":2.75409, "vy":2.36289, "omega":0.0, "ax":-0.00388, "ay":0.00452, "alpha":0.0, "fx":[-0.03297,-0.03297,-0.03297,-0.03297], "fy":[0.03842,0.03842,0.03842,0.03842]}, + {"t":0.78063, "x":2.75234, "y":5.98065, "heading":0.0, "vx":2.75392, "vy":2.36309, "omega":0.0, "ax":-0.00124, "ay":0.00145, "alpha":0.0, "fx":[-0.01055,-0.01055,-0.01055,-0.01055], "fy":[0.0123,0.0123,0.0123,0.0123]}, + {"t":0.82655, "x":2.8788, "y":6.08916, "heading":0.0, "vx":2.75386, "vy":2.36316, "omega":0.0, "ax":0.33331, "ay":-0.39111, "alpha":0.0, "fx":[2.83476,2.83476,2.83476,2.83476], "fy":[-3.32636,-3.32636,-3.32636,-3.32636]}, + {"t":0.87247, "x":3.0056, "y":6.19727, "heading":0.0, "vx":2.76916, "vy":2.3452, "omega":0.0, "ax":13.94085, "ay":-24.02106, "alpha":0.0, "fx":[118.56496,118.56496,118.56496,118.56496], "fy":[-204.29571,-204.29571,-204.29571,-204.29571]}, + {"t":0.91839, "x":3.14746, "y":6.27963, "heading":0.0, "vx":3.40932, "vy":1.24216, "omega":0.0, "ax":4.82539, "ay":-27.84517, "alpha":0.0, "fx":[41.03923,41.03923,41.03923,41.03923], "fy":[-236.81922,-236.81922,-236.81922,-236.81922]}, + {"t":0.96385, "x":3.30744, "y":6.30733, "heading":0.0, "vx":3.62869, "vy":-0.02372, "omega":0.0, "ax":-0.00564, "ay":-0.56766, "alpha":0.0, "fx":[-0.04794,-0.04794,-0.04794,-0.04794], "fy":[-4.8279,-4.8279,-4.8279,-4.8279]}, + {"t":1.00931, "x":3.4724, "y":6.30566, "heading":0.0, "vx":3.62844, "vy":-0.04952, "omega":0.0, "ax":0.00001, "ay":0.00084, "alpha":0.0, "fx":[0.00007,0.00007,0.00007,0.00007], "fy":[0.00715,0.00715,0.00715,0.00715]}, + {"t":1.05478, "x":3.63735, "y":6.30341, "heading":0.0, "vx":3.62844, "vy":-0.04948, "omega":0.0, "ax":0.00007, "ay":0.00557, "alpha":0.0, "fx":[0.00062,0.00062,0.00062,0.00062], "fy":[0.04739,0.04739,0.04739,0.04739]}, + {"t":1.10024, "x":3.80231, "y":6.30117, "heading":0.0, "vx":3.62844, "vy":-0.04923, "omega":0.0, "ax":0.00007, "ay":0.0056, "alpha":0.0, "fx":[0.00062,0.00062,0.00062,0.00062], "fy":[0.04766,0.04766,0.04766,0.04766]}, + {"t":1.1457, "x":3.96726, "y":6.29894, "heading":0.0, "vx":3.62844, "vy":-0.04898, "omega":0.0, "ax":0.00007, "ay":0.0056, "alpha":0.0, "fx":[0.00062,0.00062,0.00062,0.00062], "fy":[0.0476,0.0476,0.0476,0.0476]}, + {"t":1.19116, "x":4.13221, "y":6.29671, "heading":0.0, "vx":3.62845, "vy":-0.04872, "omega":0.0, "ax":0.00007, "ay":0.00559, "alpha":0.0, "fx":[0.00062,0.00062,0.00062,0.00062], "fy":[0.04753,0.04753,0.04753,0.04753]}, + {"t":1.23662, "x":4.29717, "y":6.29451, "heading":0.0, "vx":3.62845, "vy":-0.04847, "omega":0.0, "ax":0.00007, "ay":0.00558, "alpha":0.0, "fx":[0.00062,0.00062,0.00062,0.00062], "fy":[0.04746,0.04746,0.04746,0.04746]}, + {"t":1.28208, "x":4.46212, "y":6.29231, "heading":0.0, "vx":3.62845, "vy":-0.04821, "omega":0.0, "ax":0.00007, "ay":0.00557, "alpha":0.0, "fx":[0.00061,0.00061,0.00061,0.00061], "fy":[0.04738,0.04738,0.04738,0.04738]}, + {"t":1.32754, "x":4.62708, "y":6.29012, "heading":0.0, "vx":3.62846, "vy":-0.04796, "omega":0.0, "ax":0.00007, "ay":0.00556, "alpha":0.0, "fx":[0.00061,0.00061,0.00061,0.00061], "fy":[0.04731,0.04731,0.04731,0.04731]}, + {"t":1.373, "x":4.79203, "y":6.28795, "heading":0.0, "vx":3.62846, "vy":-0.04771, "omega":0.0, "ax":0.00007, "ay":0.00555, "alpha":0.0, "fx":[0.00061,0.00061,0.00061,0.00061], "fy":[0.04724,0.04724,0.04724,0.04724]}, + {"t":1.41847, "x":4.95699, "y":6.28578, "heading":0.0, "vx":3.62846, "vy":-0.04746, "omega":0.0, "ax":0.00002, "ay":0.00555, "alpha":0.0, "fx":[0.00016,0.00016,0.00016,0.00016], "fy":[0.04724,0.04724,0.04724,0.04724]}, + {"t":1.46393, "x":5.12194, "y":6.28363, "heading":0.0, "vx":3.62846, "vy":-0.0472, "omega":0.0, "ax":-6.68998, "ay":0.09332, "alpha":0.0, "fx":[-56.89732,-56.89732,-56.89732,-56.89732], "fy":[0.79367,0.79367,0.79367,0.79367]}, + {"t":1.50939, "x":5.27998, "y":6.28158, "heading":0.0, "vx":3.32433, "vy":-0.04296, "omega":0.0, "ax":-36.54411, "ay":0.47291, "alpha":0.0, "fx":[-310.8024,-310.8024,-310.8024,-310.8024], "fy":[4.02199,4.02199,4.02199,4.02199]}, + {"t":1.55485, "x":5.39335, "y":6.28012, "heading":0.0, "vx":1.66298, "vy":-0.02146, "omega":0.0, "ax":-36.58017, "ay":0.47208, "alpha":0.0, "fx":[-311.10908,-311.10908,-311.10908,-311.10908], "fy":[4.01499,4.01499,4.01499,4.01499]}, + {"t":1.60031, "x":5.43115, "y":6.27963, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/config.chor b/src/main/deploy/choreo/config.chor index eb64ea5..da8c43b 100644 --- a/src/main/deploy/choreo/config.chor +++ b/src/main/deploy/choreo/config.chor @@ -9,47 +9,47 @@ "config":{ "frontLeft":{ "x":{ - "exp":"11 in", - "val":0.2794 + "exp":"10.5 in", + "val":0.2667 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"10.5 in", + "val":0.2667 } }, "backLeft":{ "x":{ - "exp":"-11 in", - "val":-0.2794 + "exp":"-10.5 in", + "val":-0.2667 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"10.5 in", + "val":0.2667 } }, "mass":{ - "exp":"150 lbs", - "val":68.0388555 + "exp":"75 lbs", + "val":34.01942775 }, "inertia":{ - "exp":"6 kg m ^ 2", - "val":6.0 + "exp":"6.883 kg m ^ 2", + "val":6.883 }, "gearing":{ - "exp":"6.5", - "val":6.5 + "exp":"7.363636364", + "val":7.363636364 }, "radius":{ - "exp":"2 in", - "val":0.0508 + "exp":"2.167 in", + "val":0.055041799999999995 }, "vmax":{ - "exp":"6000 RPM", - "val":628.3185307179587 + "exp":"485.89966375522135 rad / s", + "val":485.8996637552213 }, "tmax":{ - "exp":"1.2 N * m", - "val":1.2 + "exp":"2.327950310559006 N m", + "val":2.327950310559006 }, "cof":{ "exp":"1.5", diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 4a847e1..06c9c52 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -9,6 +9,7 @@ import choreo.auto.AutoFactory; import choreo.auto.AutoFactory.AutoBindings; import choreo.auto.AutoRoutine; +import dev.doglog.DogLog; import frc.robot.subsystems.Swerve; public class Autos { @@ -26,8 +27,15 @@ public Autos(Swerve swerve) { _swerve::followTrajectory, true, _swerve, - new AutoBindings() // TODO - ); + new AutoBindings(), // TODO + (traj, isActive) -> { + traj = traj.flipped(); + + DogLog.log("Auto/Current Trajectory", traj.getPoses()); + DogLog.log("Auto/Current Trajectory Name", traj.name()); + DogLog.log("Auto/Current Trajectory Duration", traj.getTotalTime()); + DogLog.log("Auto/Current Trajectory Is Active", isActive); + }); } public AutoRoutine simpleTrajectory() { diff --git a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java index 9ccd609..31398eb 100644 --- a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java @@ -48,7 +48,7 @@ private double[] getWheelDistancesRadians() { for (int i = 0; i < _swerve.getModules().length; i++) { // radians = (meters / radius) - distances[i] = positions[i].distanceMeters / TunerConstants.kWheelRadius.in(Meters); + distances[i] = positions[i].distanceMeters / TunerConstants.FrontLeft.WheelRadius; } return distances; diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 1f52241..eca4abf 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -1,6 +1,3 @@ -// 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.generated; import static edu.wpi.first.units.Units.*; @@ -31,8 +28,8 @@ public class TunerConstants { new Slot0Configs() .withKP(100) .withKI(0) - .withKD(2.0) - .withKS(0.2) + .withKD(0.5) + .withKS(0.1) .withKV(1.59) .withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); @@ -86,7 +83,7 @@ public class TunerConstants { private static final double kDriveGearRatio = 7.363636364; private static final double kSteerGearRatio = 12.8; - public static final Distance kWheelRadius = Inches.of(2.167); + private static final Distance kWheelRadius = Inches.of(2.167); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; @@ -94,11 +91,11 @@ public class TunerConstants { private static final int kPigeonId = 1; // These are only used for simulation - private static final double kSteerInertia = 0.01; - private static final double kDriveInertia = 0.01; + 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.25); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.25); + 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() @@ -216,10 +213,7 @@ public class TunerConstants { kBackRightSteerMotorInverted, kBackRightCANcoderInverted); - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ + /** Creates a Swerve instance. This should only be called once in your robot program,. */ public static Swerve createDrivetrain() { return new Swerve(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 13e3cc7..87e141a 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -390,7 +390,9 @@ public void followTrajectory(SwerveSample sample) { var desiredSpeeds = sample.getChassisSpeeds(); // TODO: add control effort from pose PID to the target speeds - // var pose = sample.getPose(); + var pose = sample.getPose(); + + DogLog.log("Auto/Current Trajectory Desired Pose", pose); setControl( _fieldSpeedsRequest diff --git a/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java b/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java index 424ae3e..cf8b963 100644 --- a/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java +++ b/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java @@ -39,8 +39,6 @@ public void wheelRadiusCharacterization() { // accept with a tolerance of 0.002 meters (i think that's good?) assertEquals( - TunerConstants.kWheelRadius.in(Meters), - _characterization.getWheelRadius().in(Meters), - 2e-3); + TunerConstants.FrontLeft.WheelRadius, _characterization.getWheelRadius().in(Meters), 2e-3); } }