From 759f4ef4b25c0941a9a41dca9786ae443e91e7b9 Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Sat, 4 Jan 2025 23:06:42 -0500 Subject: [PATCH] trying to get simple trajectory following --- simgui-ds.json | 5 - src/main/deploy/choreo/Simple Trajectory.traj | 100 ++++++++++++++++++ src/main/deploy/choreo/config.chor | 78 ++++++++++++++ src/main/java/frc/robot/Robot.java | 33 ++---- src/main/java/frc/robot/commands/Autos.java | 8 +- 5 files changed, 195 insertions(+), 29 deletions(-) create mode 100644 src/main/deploy/choreo/Simple Trajectory.traj create mode 100644 src/main/deploy/choreo/config.chor diff --git a/simgui-ds.json b/simgui-ds.json index ea056b5..61cc581 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "FMS": { - "window": { - "visible": false - } - }, "Keyboard 0 Settings": { "window": { "visible": true diff --git a/src/main/deploy/choreo/Simple Trajectory.traj b/src/main/deploy/choreo/Simple Trajectory.traj new file mode 100644 index 0000000..1b7323e --- /dev/null +++ b/src/main/deploy/choreo/Simple Trajectory.traj @@ -0,0 +1,100 @@ +{ + "name":"Simple Trajectory", + "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}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}], + "targetDt":0.05 + }, + "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}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.4498,2.55899], + "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]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/config.chor b/src/main/deploy/choreo/config.chor new file mode 100644 index 0000000..eb64ea5 --- /dev/null +++ b/src/main/deploy/choreo/config.chor @@ -0,0 +1,78 @@ +{ + "name":"config", + "version":1, + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"11 in", + "val":0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "backLeft":{ + "x":{ + "exp":"-11 in", + "val":-0.2794 + }, + "y":{ + "exp":"11 in", + "val":0.2794 + } + }, + "mass":{ + "exp":"150 lbs", + "val":68.0388555 + }, + "inertia":{ + "exp":"6 kg m ^ 2", + "val":6.0 + }, + "gearing":{ + "exp":"6.5", + "val":6.5 + }, + "radius":{ + "exp":"2 in", + "val":0.0508 + }, + "vmax":{ + "exp":"6000 RPM", + "val":628.3185307179587 + }, + "tmax":{ + "exp":"1.2 N * m", + "val":1.2 + }, + "cof":{ + "exp":"1.5", + "val":1.5 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[] +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d342869..0bc79ba 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,7 +6,9 @@ import static edu.wpi.first.units.Units.*; import static edu.wpi.first.wpilibj2.command.Commands.*; +import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.*; +import choreo.auto.AutoChooser; import com.ctre.phoenix6.SignalLogger; import dev.doglog.DogLog; import edu.wpi.first.epilogue.Epilogue; @@ -20,7 +22,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; 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.button.CommandXboxController; import frc.lib.FaultLogger; @@ -48,8 +49,7 @@ public class Robot extends TimedRobot { private final Swerve _swerve = TunerConstants.createDrivetrain(); private final Autos _autos = new Autos(_swerve); - - private Command _autonomousCommand = none(); + private final AutoChooser _autoChooser = new AutoChooser(); private final NetworkTableInstance _ntInst; @@ -95,6 +95,13 @@ public Robot(NetworkTableInstance ntInst) { SmartDashboard.putData(new WheelRadiusCharacterization(_swerve)); SmartDashboard.putData(runOnce(FaultLogger::clear).withName("Clear Faults")); + // set up autos + _autoChooser.addCmd("Simple Trajectory", _autos::simpleTrajectory); + + SmartDashboard.putData("Auto Chooser", _autoChooser); + + autonomous().whileTrue(_autoChooser.selectedCommandScheduler()); + addPeriodic(FaultLogger::update, 1); } @@ -152,26 +159,6 @@ public void robotPeriodic() { } } - /** This autonomous runs the autonomous command. */ - @Override - public void autonomousInit() { - // schedule the autonomous command (example) - if (_autonomousCommand != null) { - _autonomousCommand.schedule(); - } - } - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (_autonomousCommand != null) { - _autonomousCommand.cancel(); - } - } - @Override public void testInit() { // Cancels all running commands at the start of test mode. diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index b4d8c87..2262c04 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -4,8 +4,11 @@ package frc.robot.commands; +import static edu.wpi.first.wpilibj2.command.Commands.*; + import choreo.auto.AutoFactory; import choreo.auto.AutoFactory.AutoBindings; +import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Swerve; public class Autos { @@ -27,5 +30,8 @@ public Autos(Swerve swerve) { ); } - // TODO: add a simple path example + public Command simpleTrajectory() { + return sequence( + _factory.resetOdometry("Simple Trajectory"), _factory.trajectoryCmd("Simple Trajectory")); + } }