From 53e772901d7405d031f8958b0bb9fe55e80b2cde Mon Sep 17 00:00:00 2001 From: Noah-It-All Date: Thu, 22 Jan 2026 20:24:32 -0500 Subject: [PATCH 1/4] auto made --- src/main/deploy/choreo/CollectFuel.traj | 79 ++++++++++++ src/main/deploy/choreo/FaceFuel.traj | 101 +++++++++++++++ src/main/deploy/choreo/FuelToShot.traj | 119 ++++++++++++++++++ src/main/deploy/choreo/StartCollectShoot.chor | 84 +++++++++++++ src/main/java/frc/robot/RobotContainer.java | 5 +- .../robot/commands/autos/AutoConstants.java | 8 ++ .../commands/autos/StartCollectShoot.java | 54 ++++++++ .../subsystems/drive/DriveConstants.java | 5 +- 8 files changed, 452 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/choreo/CollectFuel.traj create mode 100644 src/main/deploy/choreo/FaceFuel.traj create mode 100644 src/main/deploy/choreo/FuelToShot.traj create mode 100644 src/main/deploy/choreo/StartCollectShoot.chor create mode 100644 src/main/java/frc/robot/commands/autos/AutoConstants.java create mode 100644 src/main/java/frc/robot/commands/autos/StartCollectShoot.java diff --git a/src/main/deploy/choreo/CollectFuel.traj b/src/main/deploy/choreo/CollectFuel.traj new file mode 100644 index 0000000..a0b9cf7 --- /dev/null +++ b/src/main/deploy/choreo/CollectFuel.traj @@ -0,0 +1,79 @@ +{ + "name":"CollectFuel", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.80876998901367, "y":3.5819289684295654, "heading":1.5707963267948966, "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.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.01609], + "samples":[ + {"t":0.0, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":9.01899, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.41051,153.41051,153.41051,153.41051]}, + {"t":0.04839, "x":7.80877, "y":1.27152, "heading":1.5708, "vx":0.0, "vy":0.43639, "omega":0.0, "ax":0.0, "ay":9.01813, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.39574,153.39574,153.39574,153.39574]}, + {"t":0.09677, "x":7.80877, "y":1.30319, "heading":1.5708, "vx":0.0, "vy":0.87273, "omega":0.0, "ax":0.0, "ay":9.01704, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.37727,153.37727,153.37727,153.37727]}, + {"t":0.14516, "x":7.80877, "y":1.35598, "heading":1.5708, "vx":0.0, "vy":1.30902, "omega":0.0, "ax":0.0, "ay":9.01564, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.35352,153.35352,153.35352,153.35352]}, + {"t":0.19354, "x":7.80877, "y":1.42987, "heading":1.5708, "vx":0.0, "vy":1.74525, "omega":0.0, "ax":0.0, "ay":9.01378, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.32184,153.32184,153.32184,153.32184]}, + {"t":0.24193, "x":7.80877, "y":1.52486, "heading":1.5708, "vx":0.0, "vy":2.18138, "omega":0.0, "ax":0.0, "ay":9.01117, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.2775,153.2775,153.2775,153.2775]}, + {"t":0.29031, "x":7.80877, "y":1.64096, "heading":1.5708, "vx":0.0, "vy":2.61739, "omega":0.0, "ax":0.0, "ay":9.00726, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.21097,153.21097,153.21097,153.21097]}, + {"t":0.3387, "x":7.80877, "y":1.77814, "heading":1.5708, "vx":0.0, "vy":3.05321, "omega":0.0, "ax":0.0, "ay":9.00074, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.10008,153.10008,153.10008,153.10008]}, + {"t":0.38708, "x":7.80877, "y":1.93641, "heading":1.5708, "vx":0.0, "vy":3.48871, "omega":0.0, "ax":0.0, "ay":8.98771, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[152.87834,152.87834,152.87834,152.87834]}, + {"t":0.43547, "x":7.80877, "y":2.11573, "heading":1.5708, "vx":0.0, "vy":3.92358, "omega":0.0, "ax":0.0, "ay":8.94867, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[152.21427,152.21427,152.21427,152.21427]}, + {"t":0.48385, "x":7.80877, "y":2.31605, "heading":1.5708, "vx":0.0, "vy":4.35657, "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.53224, "x":7.80877, "y":2.52684, "heading":1.5708, "vx":0.0, "vy":4.35657, "omega":0.0, "ax":0.0, "ay":-8.94867, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-152.21427,-152.21427,-152.21427,-152.21427]}, + {"t":0.58062, "x":7.80877, "y":2.72716, "heading":1.5708, "vx":0.0, "vy":3.92358, "omega":0.0, "ax":0.0, "ay":-8.98771, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-152.87834,-152.87834,-152.87834,-152.87834]}, + {"t":0.62901, "x":7.80877, "y":2.90648, "heading":1.5708, "vx":0.0, "vy":3.48871, "omega":0.0, "ax":0.0, "ay":-9.00074, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.10008,-153.10008,-153.10008,-153.10008]}, + {"t":0.67739, "x":7.80877, "y":3.06475, "heading":1.5708, "vx":0.0, "vy":3.05321, "omega":0.0, "ax":0.0, "ay":-9.00726, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.21097,-153.21097,-153.21097,-153.21097]}, + {"t":0.72578, "x":7.80877, "y":3.20194, "heading":1.5708, "vx":0.0, "vy":2.61739, "omega":0.0, "ax":0.0, "ay":-9.01117, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.2775,-153.2775,-153.2775,-153.2775]}, + {"t":0.77416, "x":7.80877, "y":3.31803, "heading":1.5708, "vx":0.0, "vy":2.18138, "omega":0.0, "ax":0.0, "ay":-9.01378, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.32184,-153.32184,-153.32184,-153.32184]}, + {"t":0.82255, "x":7.80877, "y":3.41303, "heading":1.5708, "vx":0.0, "vy":1.74525, "omega":0.0, "ax":0.0, "ay":-9.01564, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.35352,-153.35352,-153.35352,-153.35352]}, + {"t":0.87093, "x":7.80877, "y":3.48692, "heading":1.5708, "vx":0.0, "vy":1.30902, "omega":0.0, "ax":0.0, "ay":-9.01704, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.37727,-153.37727,-153.37727,-153.37727]}, + {"t":0.91932, "x":7.80877, "y":3.5397, "heading":1.5708, "vx":0.0, "vy":0.87273, "omega":0.0, "ax":0.0, "ay":-9.01813, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.39574,-153.39574,-153.39574,-153.39574]}, + {"t":0.9677, "x":7.80877, "y":3.57137, "heading":1.5708, "vx":0.0, "vy":0.43639, "omega":0.0, "ax":0.0, "ay":-9.01899, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.41051,-153.41051,-153.41051,-153.41051]}, + {"t":1.01609, "x":7.80877, "y":3.58193, "heading":1.5708, "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/FaceFuel.traj b/src/main/deploy/choreo/FaceFuel.traj new file mode 100644 index 0000000..036bd2b --- /dev/null +++ b/src/main/deploy/choreo/FaceFuel.traj @@ -0,0 +1,101 @@ +{ + "name":"FaceFuel", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":4.420536518096924, "y":0.8504827618598938, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.690505504608154, "y":0.6633973717689514, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.420536518096924 m", "val":4.420536518096924}, "y":{"exp":"0.8504827618598938 m", "val":0.8504827618598938}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.690505504608154 m", "val":6.690505504608154}, "y":{"exp":"0.6633973717689514 m", "val":0.6633973717689514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.47972,0.85838], + "samples":[ + {"t":0.0, "x":4.42054, "y":0.85048, "heading":0.0, "vx":4.57021, "vy":-1.19002, "omega":-0.53764, "ax":5.35546, "ay":-1.52639, "alpha":16.06544, "fx":[75.43527,49.63132,113.35305,125.95971], "fy":[43.53688,-102.91841,-64.38465,19.91253]}, + {"t":0.02284, "x":4.52634, "y":0.8229, "heading":-0.01228, "vx":4.69255, "vy":-1.22488, "omega":-0.17064, "ax":1.47303, "ay":-0.92868, "alpha":4.92878, "fx":[12.44825,11.64662,37.2907,38.8378], "fy":[-2.2446,-29.86888,-28.62529,-2.44774]}, + {"t":0.04569, "x":4.63392, "y":0.79468, "heading":-0.01618, "vx":4.7262, "vy":-1.2461, "omega":-0.05805, "ax":0.039, "ay":-0.13689, "alpha":0.21771, "fx":[0.08842,0.06949,1.23824,1.25743], "fy":[-1.73485,-2.90328,-2.92199,-1.75365]}, + {"t":0.06853, "x":4.74189, "y":0.76617, "heading":-0.01751, "vx":4.72709, "vy":-1.24923, "omega":-0.05308, "ax":0.069, "ay":0.2581, "alpha":-0.00021, "fx":[1.17417,1.17429,1.17318,1.17306], "fy":[4.38961,4.39072,4.39084,4.38973]}, + {"t":0.09138, "x":4.8499, "y":0.7377, "heading":-0.01872, "vx":4.72867, "vy":-1.24333, "omega":-0.05308, "ax":0.19405, "ay":0.76177, "alpha":-0.02004, "fx":[3.35414,3.35576,3.24746,3.24582], "fy":[12.90291,13.00977,13.01215,12.90529]}, + {"t":0.11422, "x":4.95797, "y":0.7095, "heading":-0.01993, "vx":4.7331, "vy":-1.22593, "omega":-0.05354, "ax":0.39878, "ay":1.60605, "alpha":-0.04278, "fx":[6.90075,6.90193,6.66545,6.66415], "fy":[27.20299,27.42572,27.43388,27.21113]}, + {"t":0.13706, "x":5.0662, "y":0.68191, "heading":-0.02115, "vx":4.74221, "vy":-1.18924, "omega":-0.05452, "ax":0.70337, "ay":2.98388, "alpha":-0.08731, "fx":[12.22975,12.21604,11.69907,11.71176], "fy":[50.52859,50.94831,50.98136,50.5616]}, + {"t":0.15991, "x":5.17471, "y":0.65553, "heading":-0.0224, "vx":4.75828, "vy":-1.12108, "omega":-0.05651, "ax":1.01244, "ay":4.71998, "alpha":-0.16679, "fx":[17.82833,17.74531,16.61757,16.69394], "fy":[79.89571,80.5555,80.6749,80.01597]}, + {"t":0.18275, "x":5.28367, "y":0.63115, "heading":-0.02369, "vx":4.78141, "vy":-1.01325, "omega":-0.06032, "ax":1.14526, "ay":6.23057, "alpha":-0.29592, "fx":[20.76313,20.52399,18.21407,18.42122], "fy":[105.40399,106.25802,106.55302,105.70597]}, + {"t":0.2056, "x":5.3932, "y":0.60963, "heading":-0.02507, "vx":4.80757, "vy":-0.87092, "omega":-0.06708, "ax":1.05042, "ay":7.24413, "alpha":-0.49754, "fx":[20.27471,19.82604,15.51934,15.84905], "fy":[122.46523,123.45224,123.96315,123.00147]}, + {"t":0.22844, "x":5.5033, "y":0.59162, "heading":-0.0266, "vx":4.83157, "vy":-0.70544, "omega":-0.07845, "ax":0.80213, "ay":7.8633, "alpha":-0.74099, "fx":[17.42561,16.84046,10.01803,10.29159], "fy":[132.9315,133.93112,134.54889,133.59825]}, + {"t":0.25128, "x":5.61388, "y":0.57756, "heading":-0.02839, "vx":4.84989, "vy":-0.52581, "omega":-0.09537, "ax":0.50479, "ay":8.23677, "alpha":-0.87328, "fx":[13.11492,12.6426,4.29612,4.29151], "fy":[139.45007,140.25902,140.73913,139.97221]}, + {"t":0.27413, "x":5.7248, "y":0.5677, "heading":-0.03057, "vx":4.86142, "vy":-0.33765, "omega":-0.11532, "ax":0.2192, "ay":8.46698, "alpha":-0.74668, "fx":[7.56344,7.42682,0.07779,-0.15427], "fy":[143.68018,144.1644,144.35762,143.88144]}, + {"t":0.29697, "x":5.83591, "y":0.56219, "heading":-0.03321, "vx":4.86643, "vy":-0.14423, "omega":-0.13238, "ax":-0.07257, "ay":8.61211, "alpha":-0.44188, "fx":[0.96926,1.08941,-3.37141,-3.62459], "fy":[146.39842,146.60998,146.5837,146.366]}, + {"t":0.31982, "x":5.94706, "y":0.56114, "heading":-0.03623, "vx":4.86477, "vy":0.05251, "omega":-0.14248, "ax":-0.39818, "ay":8.69922, "alpha":-0.22205, "fx":[-5.71077,-5.54324,-7.81807,-8.01945], "fy":[147.97595,148.06482,147.96876,147.87573]}, + {"t":0.34266, "x":6.05809, "y":0.56461, "heading":-0.03949, "vx":4.85568, "vy":0.25123, "omega":-0.14755, "ax":-0.86975, "ay":8.73163, "alpha":0.26162, "fx":[-15.94524,-16.33648,-13.62033,-13.27495], "fy":[148.45334,148.33417,148.59717,148.70547]}, + {"t":0.3655, "x":6.16879, "y":0.57263, "heading":-0.04286, "vx":4.83581, "vy":0.4507, "omega":-0.14157, "ax":-3.19518, "ay":7.9198, "alpha":7.27882, "fx":[-69.86865,-102.66531,-30.07179,-14.79031], "fy":[133.9292,109.36516,145.58133,149.97826]}, + {"t":0.38835, "x":6.27842, "y":0.58499, "heading":-0.04609, "vx":4.76282, "vy":0.63162, "omega":0.02471, "ax":-4.85901, "ay":6.42534, "alpha":11.73322, "fx":[-97.59403,-145.21307,-73.02321,-14.77121], "fy":[116.62634,42.97129,126.7252,150.84986]}, + {"t":0.41119, "x":6.38596, "y":0.6011, "heading":-0.04553, "vx":4.65182, "vy":0.7784, "omega":0.29274, "ax":-6.04111, "ay":4.68691, "alpha":14.47425, "fx":[-108.23307,-151.61383,-135.92965,-15.25396], "fy":[107.4611,12.93573,47.22444,151.27065]}, + {"t":0.43404, "x":6.49065, "y":0.6201, "heading":-0.03884, "vx":4.51381, "vy":0.88547, "omega":0.62339, "ax":-6.16757, "ay":2.92907, "alpha":19.8463, "fx":[-113.96529,-152.51791,-136.60226,-16.5492], "fy":[101.72439,-2.01965,-51.84171,151.42766]}, + {"t":0.45688, "x":6.59215, "y":0.6411, "heading":-0.0246, "vx":4.37292, "vy":0.95238, "omega":1.07676, "ax":-5.90468, "ay":2.09151, "alpha":23.18554, "fx":[-117.79156,-152.31693,-113.65192,-17.98697], "fy":[97.50426,-11.40979,-95.25948,151.46925]}, + {"t":0.47972, "x":6.69051, "y":0.6634, "heading":0.0, "vx":4.23804, "vy":1.00016, "omega":1.60641, "ax":-6.63098, "ay":3.11333, "alpha":17.02397, "fx":[-119.98775,-152.17065,-144.49155,-34.51419], "fy":[94.16214,-4.89741,-25.4041,147.96647]}, + {"t":0.49429, "x":6.75152, "y":0.67829, "heading":0.02339, "vx":4.14147, "vy":1.0455, "omega":1.85434, "ax":-6.64819, "ay":3.07387, "alpha":16.89704, "fx":[-121.09847,-151.98856,-143.83319,-35.41505], "fy":[92.63348,-7.29036,-23.877,147.67661]}, + {"t":0.50885, "x":6.81113, "y":0.69385, "heading":0.0504, "vx":4.04464, "vy":1.09027, "omega":2.10042, "ax":-6.62524, "ay":3.01492, "alpha":16.97382, "fx":[-122.0057,-151.71842,-142.21501,-34.8348], "fy":[91.33771,-10.31201,-23.64093,147.74665]}, + {"t":0.52342, "x":6.86933, "y":0.71004, "heading":0.08099, "vx":3.94816, "vy":1.13417, "omega":2.34762, "ax":-6.57944, "ay":2.97462, "alpha":17.0577, "fx":[-122.87178,-151.34367,-139.68325,-33.75871], "fy":[90.0675,-13.70746,-21.90526,147.93503]}, + {"t":0.53798, "x":6.92613, "y":0.72688, "heading":0.11518, "vx":3.85234, "vy":1.17749, "omega":2.59604, "ax":-6.50216, "ay":3.00384, "alpha":17.03873, "fx":[-123.74636,-150.84577,-135.27116,-32.53593], "fy":[88.75554,-17.39857,-15.13343,148.15446]}, + {"t":0.55254, "x":6.98155, "y":0.74434, "heading":0.15299, "vx":3.75764, "vy":1.22124, "omega":2.84418, "ax":-6.36357, "ay":3.29949, "alpha":16.52947, "fx":[-124.80606,-150.25128,-125.65036,-32.26218], "fy":[87.14282,-21.01777,10.2076,148.16057]}, + {"t":0.56711, "x":7.0356, "y":0.76248, "heading":0.19441, "vx":3.66497, "vy":1.26929, "omega":3.08491, "ax":-6.16221, "ay":3.98889, "alpha":15.35585, "fx":[-126.54549,-149.76679,-106.94908,-36.00845], "fy":[84.44998,-23.27354,63.00802,147.21505]}, + {"t":0.58167, "x":7.08832, "y":0.78139, "heading":0.23934, "vx":3.57523, "vy":1.32739, "omega":3.30854, "ax":-6.0638, "ay":4.35322, "alpha":14.82281, "fx":[-128.67261,-149.43706,-91.98057,-42.48356], "fy":[80.99411,-24.28039,94.09621,145.37826]}, + {"t":0.59623, "x":7.13974, "y":0.80118, "heading":0.28752, "vx":3.48692, "vy":1.39078, "omega":3.52441, "ax":-6.03211, "ay":4.48803, "alpha":14.71054, "fx":[-130.69193,-149.15618,-81.58951,-48.98034], "fy":[77.50009,-24.85971,109.49781,143.22232]}, + {"t":0.6108, "x":7.18989, "y":0.82191, "heading":0.33885, "vx":3.39907, "vy":1.45615, "omega":3.73865, "ax":-6.02714, "ay":4.53616, "alpha":14.76291, "fx":[-132.52803,-148.91461,-73.59421,-55.04314], "fy":[74.11254,-25.10395,118.71226,140.91446]}, + {"t":0.62536, "x":7.23875, "y":0.8436, "heading":0.39329, "vx":3.31129, "vy":1.52221, "omega":3.95365, "ax":-6.18514, "ay":4.47774, "alpha":14.36653, "fx":[-134.44838,-148.86506,-75.06554,-62.45096], "fy":[70.33354,-24.10605,120.74159,137.69122]}, + {"t":0.63992, "x":7.28632, "y":0.86624, "heading":0.45087, "vx":3.22121, "vy":1.58742, "omega":4.16288, "ax":-7.18615, "ay":3.83841, "alpha":10.87935, "fx":[-138.36111,-149.8786,-115.15772,-85.5401], "fy":[61.75281,-14.72657,89.78752,124.3474]}, + {"t":0.65449, "x":7.33247, "y":0.88977, "heading":0.5115, "vx":3.11656, "vy":1.64332, "omega":4.32132, "ax":-7.68969, "ay":3.26823, "alpha":9.0586, "fx":[-141.82518,-149.89715,-128.75053,-102.72492], "fy":[52.57219,-11.62757,71.25051,110.17136]}, + {"t":0.66905, "x":7.37704, "y":0.91405, "heading":0.57443, "vx":3.00457, "vy":1.69092, "omega":4.45324, "ax":-7.83989, "ay":2.88865, "alpha":8.88256, "fx":[-144.14376,-148.96824,-131.23028,-109.07457], "fy":[44.97594,-17.43449,65.54684,103.45235]}, + {"t":0.68361, "x":7.41997, "y":0.93898, "heading":0.63929, "vx":2.89039, "vy":1.73299, "omega":4.5826, "ax":-7.93332, "ay":2.51475, "alpha":8.96028, "fx":[-146.02179,-147.44665,-132.48984,-113.81559], "fy":[37.23594,-24.89296,61.0952,97.66276]}, + {"t":0.69818, "x":7.46122, "y":0.96448, "heading":0.70603, "vx":2.77486, "vy":1.76961, "omega":4.7131, "ax":-7.99878, "ay":2.12454, "alpha":9.10587, "fx":[-147.47556,-145.29364,-133.34029,-118.11828], "fy":[29.12458,-32.90271,56.63331,91.69635]}, + {"t":0.71274, "x":7.50078, "y":0.99048, "heading":0.77467, "vx":2.65837, "vy":1.80055, "omega":4.84571, "ax":-8.03432, "ay":1.71779, "alpha":9.30198, "fx":[-148.43132,-142.40318,-133.7564,-122.05517], "fy":[20.63953,-41.2738,52.08297,85.42746]}, + {"t":0.7273, "x":7.53864, "y":1.01688, "heading":0.84524, "vx":2.54136, "vy":1.82557, "omega":4.98118, "ax":-8.03471, "ay":1.29661, "alpha":9.54262, "fx":[-148.79304,-138.65966,-133.62173,-125.59795], "fy":[11.81218,-49.84062,47.44976,78.79839]}, + {"t":0.74187, "x":7.5748, "y":1.04361, "heading":0.91778, "vx":2.42434, "vy":1.84445, "omega":5.12015, "ax":-7.99278, "ay":0.86507, "alpha":9.81886, "fx":[-148.43795,-133.95536,-132.75493,-128.67133], "fy":[2.71411,-58.35361,42.74799,71.74977]}, + {"t":0.75643, "x":7.60926, "y":1.07056, "heading":0.99235, "vx":2.30794, "vy":1.85705, "omega":5.26315, "ax":-7.89849, "ay":0.43096, "alpha":10.11318, "fx":[-147.2016,-128.2123,-130.86531,-131.12474], "fy":[-6.50589,-66.40117,38.00921,64.22006]}, + {"t":0.77099, "x":7.64204, "y":1.09765, "heading":1.069, "vx":2.19291, "vy":1.86332, "omega":5.41043, "ax":-7.73644, "ay":0.01022, "alpha":10.38784, "fx":[-144.84188,-121.42259,-127.45776,-132.65642], "fy":[-15.5348,-73.24572,33.31745,56.15833]}, + {"t":0.78556, "x":7.67315, "y":1.12479, "heading":1.14779, "vx":2.08024, "vy":1.86347, "omega":5.56172, "ax":-7.47906, "ay":-0.36048, "alpha":10.55065, "fx":[-140.92935,-113.72067,-121.61754,-132.59923], "fy":[-23.64911,-77.36692,28.91772,47.57163]}, + {"t":0.80012, "x":7.70266, "y":1.15189, "heading":1.22879, "vx":1.97132, "vy":1.85822, "omega":5.71537, "ax":-7.06049, "ay":-0.57625, "alpha":10.32425, "fx":[-134.3853,-105.43687,-111.42515,-129.14035], "fy":[-28.7881,-74.76219,25.5977,38.74501]}, + {"t":0.81468, "x":7.73062, "y":1.17889, "heading":1.31202, "vx":1.8685, "vy":1.84983, "omega":5.86573, "ax":-6.20395, "ay":-0.19504, "alpha":8.45115, "fx":[-120.02569,-95.38333,-91.82213,-114.87832], "fy":[-21.92642,-49.24136,26.23648,31.6612]}, + {"t":0.82925, "x":7.75717, "y":1.20581, "heading":1.39745, "vx":1.77815, "vy":1.84699, "omega":5.98881, "ax":-1.5998, "ay":3.03532, "alpha":-5.13844, "fx":[-9.18836,-37.0062,-46.23629,-16.41769], "fy":[64.14144,64.65288,39.85526,37.87]}, + {"t":0.84381, "x":7.7829, "y":1.23303, "heading":1.48467, "vx":1.75485, "vy":1.8912, "omega":5.91397, "ax":2.9877, "ay":3.68303, "alpha":-17.20252, "fx":[89.1827,6.44688,-5.82658,113.47703], "fy":[89.07098,118.6037,35.81823,7.09638]}, + {"t":0.85838, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":1.79836, "vy":1.94483, "omega":5.66344, "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/FuelToShot.traj b/src/main/deploy/choreo/FuelToShot.traj new file mode 100644 index 0000000..74e9b72 --- /dev/null +++ b/src/main/deploy/choreo/FuelToShot.traj @@ -0,0 +1,119 @@ +{ + "name":"FuelToShot", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":7.80876998901367, "y":3.5819289684295654, "heading":1.5707963267948966, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.657511234283447, "y":2.5716681480407715, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.3354413509368896, "y":3.0580899715423584, "heading":3.7524578917878086, "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.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.657511234283447 m", "val":4.657511234283447}, "y":{"exp":"2.5716681480407715 m", "val":2.5716681480407715}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.3354413509368896 m", "val":3.3354413509368896}, "y":{"exp":"3.0580899715423584 m", "val":3.0580899715423584}, "heading":{"exp":"215 deg", "val":3.7524578917878086}, "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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.97983,1.55968], + "samples":[ + {"t":0.0, "x":7.80877, "y":3.58193, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.6936, "ay":-3.51677, "alpha":10.04437, "fx":[-130.15898,-88.89209,-151.40065,-153.01204], "fy":[-81.15222,-124.89633,-22.94009,-10.28804]}, + {"t":0.02882, "x":7.80558, "y":3.58047, "heading":1.5708, "vx":-0.22172, "vy":-0.10135, "omega":0.28946, "ax":-7.7102, "ay":-3.53963, "alpha":9.80757, "fx":[-130.28049,-90.28188,-151.10441,-152.92663], "fy":[-80.93624,-123.87635,-24.7011,-11.31874]}, + {"t":0.05764, "x":7.79598, "y":3.57608, "heading":1.57914, "vx":-0.44392, "vy":-0.20336, "omega":0.5721, "ax":-7.72544, "ay":-3.56164, "alpha":9.58937, "fx":[-130.20168,-91.71596,-150.91391,-152.79855], "fy":[-81.03905,-122.79516,-25.72732,-12.76813]}, + {"t":0.08646, "x":7.77998, "y":3.56874, "heading":1.59563, "vx":-0.66655, "vy":-0.306, "omega":0.84846, "ax":-7.74028, "ay":-3.5839, "alpha":9.37463, "fx":[-129.94384,-93.26599,-150.8142,-152.6156], "fy":[-81.42409,-121.59392,-26.19208,-14.63447]}, + {"t":0.11527, "x":7.75756, "y":3.55843, "heading":1.62008, "vx":-0.88962, "vy":-0.40928, "omega":1.11862, "ax":-7.75572, "ay":-3.60773, "alpha":9.14555, "fx":[-129.53146,-95.02357,-150.77393,-152.3611], "fy":[-82.04605,-120.19122,-26.30443,-16.92388]}, + {"t":0.14409, "x":7.7287, "y":3.54514, "heading":1.65231, "vx":-1.11312, "vy":-0.51325, "omega":1.38218, "ax":-7.77296, "ay":-3.63438, "alpha":8.88008, "fx":[-128.99613,-97.10226,-150.7515,-152.0131], "fy":[-82.84674,-118.4765,-26.30579,-19.65034]}, + {"t":0.17291, "x":7.6934, "y":3.52884, "heading":1.69215, "vx":-1.33713, "vy":-0.61799, "omega":1.63809, "ax":-7.79351, "ay":-3.66488, "alpha":8.55023, "fx":[-128.3823,-99.63751,-150.69864,-151.54307], "fy":[-83.74856,-116.3013,-26.46853,-22.83607]}, + {"t":0.20173, "x":7.65162, "y":3.50951, "heading":1.73935, "vx":-1.56173, "vy":-0.7236, "omega":1.8845, "ax":-7.81922, "ay":-3.69968, "alpha":8.12089, "fx":[-127.75608,-102.78149,-150.55946,-150.91376], "fy":[-84.64408,-113.46911,-27.09613,-26.51298]}, + {"t":0.23055, "x":7.60337, "y":3.48712, "heading":1.79366, "vx":-1.78707, "vy":-0.83022, "omega":2.11853, "ax":-7.85204, "ay":-3.73843, "alpha":7.54942, "fx":[-127.21897,-106.68682,-150.2625,-150.07565], "fy":[-85.37794,-109.72662,-28.52732,-30.72634]}, + {"t":0.25937, "x":7.54861, "y":3.46164, "heading":1.85472, "vx":-2.01335, "vy":-0.93796, "omega":2.3361, "ax":-7.89348, "ay":-3.77976, "alpha":6.78634, "fx":[-126.93045,-111.47098,-149.70179,-148.96009], "fy":[-85.71374,-104.76446,-31.14815,-35.54386]}, + {"t":0.28819, "x":7.48731, "y":3.43304, "heading":1.92204, "vx":-2.24083, "vy":-1.04689, "omega":2.53167, "ax":-7.94346, "ay":-3.82148, "alpha":5.7757, "fx":[-127.14646,-117.15383,-148.69919,-147.46456], "fy":[-85.26745,-98.24475,-35.4197,-41.07734]}, + {"t":0.317, "x":7.41943, "y":3.40128, "heading":1.995, "vx":-2.46975, "vy":-1.15702, "omega":2.69812, "ax":-7.99846, "ay":-3.8612, "alpha":4.4496, "fx":[-128.2876,-123.57209,-146.92973,-145.41662], "fy":[-83.35985,-89.88326,-41.93289,-47.53538]}, + {"t":0.34582, "x":7.34494, "y":3.36634, "heading":2.07275, "vx":-2.70025, "vy":-1.26829, "omega":2.82635, "ax":-8.04837, "ay":-3.89667, "alpha":2.69798, "fx":[-131.05709,-130.30796,-143.76434,-142.47229], "fy":[-78.64893,-79.61002,-51.50389,-55.36188]}, + {"t":0.37464, "x":7.26378, "y":3.32817, "heading":2.1542, "vx":-2.9322, "vy":-1.38059, "omega":2.9041, "ax":-8.06854, "ay":-3.92171, "alpha":0.25819, "fx":[-136.57829,-136.71267,-137.91629,-137.76672], "fy":[-68.08908,-67.78094,-65.30236,-65.65621]}, + {"t":0.40346, "x":7.17593, "y":3.28675, "heading":2.2379, "vx":-3.16472, "vy":-1.49361, "omega":2.91154, "ax":-7.9776, "ay":-3.90405, "alpha":-3.65444, "fx":[-145.80313,-142.09478,-126.64047,-128.24841], "fy":[-43.67842,-55.3093,-84.84427,-81.79487]}, + {"t":0.43228, "x":7.08141, "y":3.24209, "heading":2.3218, "vx":-3.39462, "vy":-1.60611, "omega":2.80622, "ax":-7.27827, "ay":-3.78453, "alpha":-11.73741, "fx":[-150.85755,-145.99941,-104.36734,-93.98054], "fy":[14.54999,-43.56808,-110.81748,-117.65928]}, + {"t":0.4611, "x":6.98056, "y":3.19423, "heading":2.40267, "vx":-3.60437, "vy":-1.71518, "omega":2.46797, "ax":-5.89335, "ay":-3.71453, "alpha":-19.63874, "fx":[-139.70958,-146.70529,-85.92334,-28.63827], "fy":[57.54382,-40.3294,-125.27404,-144.67263]}, + {"t":0.48992, "x":6.87424, "y":3.14326, "heading":2.4738, "vx":-3.77421, "vy":-1.82223, "omega":1.90201, "ax":-5.90736, "ay":-3.71124, "alpha":-18.95585, "fx":[-139.30361,-145.43621,-82.4816,-34.70892], "fy":[56.93394,-43.15418,-126.84503,-139.44293]}, + {"t":0.51873, "x":6.76302, "y":3.0892, "heading":2.52861, "vx":-3.94445, "vy":-1.92918, "omega":1.35573, "ax":-5.92284, "ay":-3.59371, "alpha":-18.31018, "fx":[-137.98948,-144.04611,-79.55219,-41.39554], "fy":[56.80953,-44.71211,-127.31451,-129.29505]}, + {"t":0.54755, "x":6.64689, "y":3.03211, "heading":2.56768, "vx":-4.11514, "vy":-2.03274, "omega":0.82805, "ax":-5.93622, "ay":-3.13679, "alpha":-17.52844, "fx":[-134.08386,-141.69436,-77.62037,-50.49484], "fy":[56.77378,-43.85045,-124.57201,-101.77499]}, + {"t":0.57637, "x":6.52583, "y":2.97223, "heading":2.59154, "vx":-4.28621, "vy":-2.12314, "omega":0.32291, "ax":-4.64913, "ay":0.24833, "alpha":-10.60684, "fx":[-86.88141,-107.07445,-74.99846,-47.3669], "fy":[48.70468,-6.31834,-47.56007,22.0697]}, + {"t":0.60519, "x":6.40038, "y":2.91115, "heading":2.60085, "vx":-4.42019, "vy":-2.11599, "omega":0.01723, "ax":-1.27182, "ay":2.64722, "alpha":-0.08014, "fx":[-21.68753,-21.95414,-21.57935,-21.31249], "fy":[45.29782,44.93531,44.75876,45.12199]}, + {"t":0.63401, "x":6.27246, "y":2.85127, "heading":2.60135, "vx":-4.45685, "vy":-2.0397, "omega":0.01492, "ax":-1.79874, "ay":4.07116, "alpha":-0.00131, "fx":[-30.59662,-30.60192,-30.59556,-30.59026], "fy":[69.25313,69.24744,69.24551,69.2512]}, + {"t":0.66283, "x":6.14328, "y":2.79418, "heading":2.60178, "vx":-4.50868, "vy":-1.92237, "omega":0.01489, "ax":-2.40441, "ay":5.95358, "alpha":0.00236, "fx":[-40.89848,-40.88578,-40.89831,-40.91102], "fy":[101.26393,101.27313,101.27345,101.26425]}, + {"t":0.69165, "x":6.01234, "y":2.74125, "heading":2.60221, "vx":-4.57798, "vy":-1.7508, "omega":0.01495, "ax":-2.55912, "ay":7.18304, "alpha":0.00862, "fx":[-43.53177,-43.47695,-43.52788,-43.5827], "fy":[122.17086,122.19933,122.19205,122.16357]}, + {"t":0.72046, "x":5.87935, "y":2.69378, "heading":2.60264, "vx":-4.65173, "vy":-1.54379, "omega":0.0152, "ax":-2.39129, "ay":7.86152, "alpha":0.01783, "fx":[-40.67763,-40.55673,-40.67281,-40.79367], "fy":[133.70933,133.75759,133.73502,133.6867]}, + {"t":0.74928, "x":5.7443, "y":2.65255, "heading":2.60308, "vx":-4.72064, "vy":-1.31724, "omega":0.01572, "ax":-2.06413, "ay":8.26281, "alpha":0.04795, "fx":[-35.10482,-34.77648,-35.1159,-35.44409], "fy":[140.52896,140.63128,140.5674,140.46442]}, + {"t":0.7781, "x":5.6074, "y":2.61802, "heading":2.60353, "vx":-4.78013, "vy":-1.07911, "omega":0.0171, "ax":-1.56861, "ay":8.53798, "alpha":0.33783, "fx":[-26.50395,-24.2608,-26.85732,-29.10415], "fy":[145.17269,145.67054,145.30313,144.7678]}, + {"t":0.80692, "x":5.46899, "y":2.59047, "heading":2.60402, "vx":-4.82533, "vy":-0.83306, "omega":0.02684, "ax":-0.38595, "ay":8.74393, "alpha":2.28139, "fx":[-2.73729,9.53713,-9.69311,-23.36662], "fy":[148.82934,149.17702,149.46441,147.45627]}, + {"t":0.83574, "x":5.32977, "y":2.57009, "heading":2.60479, "vx":-4.83645, "vy":-0.58107, "omega":0.09258, "ax":1.31637, "ay":8.58332, "alpha":5.48238, "fx":[42.53098,54.68035,10.14774,-17.79509], "fy":[143.39331,140.60363,150.77573,149.22672]}, + {"t":0.86456, "x":5.19094, "y":2.55691, "heading":2.60746, "vx":-4.79852, "vy":-0.33371, "omega":0.25058, "ax":2.46988, "ay":8.2309, "alpha":7.47844, "fx":[77.33111,77.67208,22.72291,-9.67856], "fy":[129.00388,130.27977,150.10142,150.63598]}, + {"t":0.89338, "x":5.05368, "y":2.55071, "heading":2.61468, "vx":-4.72734, "vy":-0.09651, "omega":0.46609, "ax":3.14945, "ay":7.94062, "alpha":8.49559, "fx":[96.69237,88.69928,30.44796,-1.5548], "fy":[116.11669,123.61214,149.13378,151.40787]}, + {"t":0.92219, "x":4.91875, "y":2.55123, "heading":2.62812, "vx":-4.63658, "vy":0.13233, "omega":0.71092, "ax":3.54982, "ay":7.74499, "alpha":8.99984, "fx":[107.24597,94.34079,34.98608,4.95293], "fy":[107.15322,119.73399,148.3975,151.67554]}, + {"t":0.95101, "x":4.78661, "y":2.55826, "heading":2.6486, "vx":-4.53428, "vy":0.35553, "omega":0.97029, "ax":3.79135, "ay":7.62162, "alpha":9.24356, "fx":[113.34029,97.19646,37.43086,9.99144], "fy":[101.22028,117.68403,147.98171,151.6801]}, + {"t":0.97983, "x":4.65751, "y":2.57167, "heading":2.67657, "vx":-4.42501, "vy":0.57517, "omega":1.23667, "ax":4.2214, "ay":7.23872, "alpha":10.47527, "fx":[126.70908,104.13722,41.09032,15.28254], "fy":[83.44697,111.41965,146.84396,150.80336]}, + {"t":1.00303, "x":4.55601, "y":2.58696, "heading":2.70525, "vx":-4.3271, "vy":0.74306, "omega":1.47964, "ax":4.77157, "ay":6.5771, "alpha":12.60323, "fx":[142.63058,114.26805,46.00676,21.74687], "fy":[51.6953,101.00095,145.3138,149.48859]}, + {"t":1.02622, "x":4.45693, "y":2.60596, "heading":2.73957, "vx":-4.21643, "vy":0.89561, "omega":1.77195, "ax":5.15767, "ay":5.88243, "alpha":14.57376, "fx":[150.73136,122.66761,49.25717,28.26588], "fy":[17.97027,90.63673,144.17739,147.44972]}, + {"t":1.04941, "x":4.36053, "y":2.62831, "heading":2.78067, "vx":-4.09681, "vy":1.03205, "omega":2.10998, "ax":6.2956, "ay":4.98944, "alpha":12.48901, "fx":[151.96753,130.37838,63.02833,82.97146], "fy":[-2.15085,79.14145,138.58636,123.89859]}, + {"t":1.07261, "x":4.2672, "y":2.65359, "heading":2.8296, "vx":-3.95079, "vy":1.14777, "omega":2.39964, "ax":7.85098, "ay":3.27467, "alpha":8.62329, "fx":[152.14046,139.05076,100.70041,142.27995], "fy":[-3.36145,62.58218,113.98846,49.59565]}, + {"t":1.0958, "x":4.17768, "y":2.6811, "heading":2.88526, "vx":-3.76869, "vy":1.22373, "omega":2.59965, "ax":8.57521, "ay":1.84536, "alpha":5.62484, "fx":[152.27019,146.44101,133.74192,150.99434], "fy":[-3.93398,42.43311,72.45439,14.60263]}, + {"t":1.119, "x":4.09257, "y":2.70998, "heading":2.94556, "vx":-3.5698, "vy":1.26653, "omega":2.73011, "ax":8.89874, "ay":0.62349, "alpha":2.56348, "fx":[152.34596,151.25514,149.65064,152.20829], "fy":[-4.95498,19.2054,28.17653,-0.00569]}, + {"t":1.14219, "x":4.01217, "y":2.73952, "heading":3.00888, "vx":-3.3634, "vy":1.28099, "omega":2.78957, "ax":8.95838, "ay":-0.35592, "alpha":0.12107, "fx":[152.35534,152.40748,152.40318,152.35161], "fy":[-6.75369,-5.47453,-5.34903,-6.63889]}, + {"t":1.16538, "x":3.93657, "y":2.76913, "heading":3.07358, "vx":-3.15562, "vy":1.27273, "omega":2.79238, "ax":8.88675, "ay":-1.10894, "alpha":-1.76699, "fx":[152.268,149.74407,150.25726,152.37469], "fy":[-9.50005,-29.23734,-27.10562,-9.60785]}, + {"t":1.18858, "x":3.86577, "y":2.79836, "heading":3.13835, "vx":-2.9495, "vy":1.24701, "omega":2.7514, "ax":8.75967, "ay":-1.68515, "alpha":-3.27007, "fx":[152.03042,144.22711,147.31371,152.42675], "fy":[-13.28184,-50.02643,-40.6703,-10.67714]}, + {"t":1.21177, "x":3.79971, "y":2.82683, "heading":-3.08102, "vx":-2.74633, "vy":1.20793, "omega":2.67555, "ax":8.61627, "ay":-2.1283, "alpha":-4.45378, "fx":[151.56525,137.32915,144.83256,152.51414], "fy":[-18.12784,-66.87868,-49.10848,-10.69214]}, + {"t":1.23496, "x":3.73833, "y":2.85427, "heading":-3.01897, "vx":-2.54649, "vy":1.15856, "omega":2.57225, "ax":8.47658, "ay":-2.47416, "alpha":-5.35339, "fx":[150.7761,130.30728,143.03316,152.62012], "fy":[-24.00686,-79.86837,-54.35555,-10.10828]}, + {"t":1.25816, "x":3.68155, "y":2.88048, "heading":-2.95931, "vx":-2.34988, "vy":1.10118, "omega":2.44808, "ax":8.34865, "ay":-2.75133, "alpha":-6.01859, "fx":[149.55887,123.90832,141.8365,152.72918], "fy":[-30.82135,-89.60971,-57.57371,-9.19245]}, + {"t":1.28135, "x":3.62929, "y":2.90528, "heading":-2.90252, "vx":-2.15625, "vy":1.03736, "omega":2.30849, "ax":8.23343, "ay":-2.98096, "alpha":-6.5122, "fx":[147.81902,118.4376,141.10527,152.8311], "fy":[-38.40527,-96.83368,-59.47203,-8.11037]}, + {"t":1.30455, "x":3.58149, "y":2.92853, "heading":-2.84898, "vx":-1.96528, "vy":0.96822, "omega":2.15745, "ax":8.12855, "ay":-3.17733, "alpha":-6.89681, "fx":[145.49214,113.93265,140.71224,152.92043], "fy":[-46.53076,-102.1839,-60.49818,-6.9692]}, + {"t":1.32774, "x":3.5381, "y":2.95014, "heading":-2.79894, "vx":-1.77675, "vy":0.89453, "omega":1.99748, "ax":8.03095, "ay":-3.34918, "alpha":-7.2246, "fx":[142.56309,110.30341,140.55499,152.99513], "fy":[-54.92559,-106.163,-60.94587,-5.84022]}, + {"t":1.35093, "x":3.49905, "y":2.96998, "heading":-2.75261, "vx":-1.59048, "vy":0.81685, "omega":1.82992, "ax":7.93815, "ay":-3.50141, "alpha":-7.5326, "fx":[139.07752,107.41469,140.5549,153.05537], "fy":[-63.30052,-109.14393,-61.01541,-4.77179]}, + {"t":1.37413, "x":3.46429, "y":2.98799, "heading":-2.71017, "vx":-1.40636, "vy":0.73564, "omega":1.65521, "ax":7.84879, "ay":-3.63654, "alpha":-7.84202, "fx":[135.1411,105.12639,140.65252,153.10264], "fy":[-71.38147,-111.39879,-60.84882,-3.79677]}, + {"t":1.39732, "x":3.43379, "y":3.00407, "heading":-2.67178, "vx":-1.22432, "vy":0.65129, "omega":1.47332, "ax":7.76259, "ay":-3.75598, "alpha":-8.1604, "fx":[130.90595,103.31008,140.80288,153.13905], "fy":[-78.93868,-113.12645,-60.55063,-2.93702]}, + {"t":1.42052, "x":3.40748, "y":3.01817, "heading":-2.63761, "vx":-1.04427, "vy":0.56418, "omega":1.28405, "ax":7.68004, "ay":-3.86083, "alpha":-8.48527, "fx":[126.54816,101.85399,140.97172,153.16696], "fy":[-85.80579,-114.47403,-60.20052,-2.20603]}, + {"t":1.44371, "x":3.38532, "y":3.03021, "heading":-2.60783, "vx":-0.86614, "vy":0.47463, "omega":1.08724, "ax":7.60195, "ay":-3.95229, "alpha":-8.80826, "fx":[122.24363,100.66316,141.13263,153.18858], "fy":[-91.885,-115.55228,-59.86105,-1.61077]}, + {"t":1.4669, "x":3.36728, "y":3.04016, "heading":-2.58261, "vx":-0.68982, "vy":0.38296, "omega":0.88294, "ax":7.52919, "ay":-4.03183, "alpha":-9.11872, "fx":[118.14887,99.65775,141.26496,153.20584], "fy":[-97.13999,-116.44596,-59.58244,-1.15304]}, + {"t":1.4901, "x":3.3533, "y":3.04796, "heading":-2.56213, "vx":-0.51519, "vy":0.28945, "omega":0.67144, "ax":7.46241, "ay":-4.10117, "alpha":-9.4063, "fx":[114.39015,98.77106,141.3523,153.22029], "fy":[-101.58107,-117.22097,-59.40562,-0.83093]}, + {"t":1.51329, "x":3.34336, "y":3.05357, "heading":-2.54656, "vx":-0.34211, "vy":0.19432, "omega":0.45328, "ax":7.40199, "ay":-4.16205, "alpha":-9.66237, "fx":[111.06109,97.94758,141.3813,153.23301], "fy":[-105.24789,-117.92895,-59.36414,-0.64021]}, + {"t":1.53649, "x":3.33742, "y":3.05696, "heading":-2.53604, "vx":-0.17043, "vy":0.09779, "omega":0.22917, "ax":7.34806, "ay":-4.2162, "alpha":-9.8805, "fx":[108.22654,97.14143,141.34079,153.2447], "fy":[-108.19366,-118.61043,-59.48542,-0.57592]}, + {"t":1.55968, "x":3.33544, "y":3.05809, "heading":-2.53073, "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/StartCollectShoot.chor b/src/main/deploy/choreo/StartCollectShoot.chor new file mode 100644 index 0000000..9147e2c --- /dev/null +++ b/src/main/deploy/choreo/StartCollectShoot.chor @@ -0,0 +1,84 @@ +{ + "name":"StartCollectShoot", + "version":2, + "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":[], + "codegen":{ + "root":null, + "genVars":true, + "genTrajData":true, + "useChoreoLib":true + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c13ce1a..9f43d0c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.commands.DriveCommands; import frc.robot.commands.autos.DriveTesting; +import frc.robot.commands.autos.StartCollectShoot; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; @@ -138,7 +139,9 @@ public RobotContainer() { autoChooser.addRoutine( "Drive Straight Test", () -> DriveTesting.getRoutine(autoFactory, drive)); - + autoChooser.addRoutine( + "Start, Collect, and Shoot", + () -> StartCollectShoot.getRoutine(autoFactory, drive, intakeAndShooter, feederAndIndexer)); RobotModeTriggers.autonomous() .whileTrue( Commands.sequence( diff --git a/src/main/java/frc/robot/commands/autos/AutoConstants.java b/src/main/java/frc/robot/commands/autos/AutoConstants.java new file mode 100644 index 0000000..2681adc --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/AutoConstants.java @@ -0,0 +1,8 @@ +package frc.robot.commands.autos; + +import frc.robot.util.LoggedTunableNumber; + +public class AutoConstants { + public static final LoggedTunableNumber launchLength = + new LoggedTunableNumber("Auto/Launch Length", 10); +} diff --git a/src/main/java/frc/robot/commands/autos/StartCollectShoot.java b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java new file mode 100644 index 0000000..baecf7c --- /dev/null +++ b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java @@ -0,0 +1,54 @@ +package frc.robot.commands.autos; + +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants; +import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncher; +import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncherConstants; + +public class StartCollectShoot { + public static AutoRoutine getRoutine( + AutoFactory factory, + Drive driveSubsystem, + IntakeAndLauncher intakeAndShooter, + Feeder feederAndIndexer) { + AutoRoutine routine = factory.newRoutine("Start Collect Shoot"); + + AutoTrajectory faceFuel = routine.trajectory("FaceFuel"); + AutoTrajectory collectFuel = routine.trajectory("CollectFuel"); + AutoTrajectory fuelToShot = routine.trajectory("FuelToShot"); + + routine + .active() + .onTrue( + Commands.sequence( + new InstantCommand(() -> System.out.println("Going to fuel")), + faceFuel.resetOdometry(), + faceFuel.cmd())); + + faceFuel + .done() + .onTrue( + Commands.parallel( + intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.intakeVoltage.get()), + collectFuel.cmd())); + collectFuel.done().onTrue(fuelToShot.cmd()); + fuelToShot + .done() + .onTrue( + Commands.race( + Commands.parallel( + intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.launchVoltage.get()), + Commands.sequence( + Commands.waitSeconds(IntakeAndLauncherConstants.launchWarmUpTime.get()), + feederAndIndexer.voltageCmd(FeederConstants.launchVoltage.get()))), + Commands.waitSeconds(AutoConstants.launchLength.get()))); + + return routine; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 78369c9..13aba9b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -13,6 +13,7 @@ public final class AutoConstants { public static final LoggedTunableGains headingTrajectoryController = new LoggedTunableGains("headingTraj", new ControlGains().p(3).d(0.0)); } - public final static LoggedTunableNumber speedScalar = - new LoggedTunableNumber("Drive/Drive Speed Scalar", 1); + + public static final LoggedTunableNumber speedScalar = + new LoggedTunableNumber("Drive/Drive Speed Scalar", 1); } From b456c08a831f5655a35099e00a8b9ffd377782d5 Mon Sep 17 00:00:00 2001 From: noah-it-all Date: Fri, 23 Jan 2026 13:30:44 -0500 Subject: [PATCH 2/4] Add auto tunable constants and enhance logging in StartCollectShoot command --- src/main/java/frc/robot/RobotContainer.java | 4 +++ .../robot/commands/autos/AutoConstants.java | 12 ++++++- .../commands/autos/StartCollectShoot.java | 36 ++++++++++++------- 3 files changed, 39 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9f43d0c..4676e1a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.commands.DriveCommands; +import frc.robot.commands.autos.AutoConstants; import frc.robot.commands.autos.DriveTesting; import frc.robot.commands.autos.StartCollectShoot; import frc.robot.generated.TunerConstants; @@ -171,6 +172,9 @@ public RobotContainer() { // Send to elastic SmartDashboard.putData("Auto Chooser", autoChooser); + // Initialize auto tunable numbers so they appear on dashboard + AutoConstants.initialize(); + // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/autos/AutoConstants.java b/src/main/java/frc/robot/commands/autos/AutoConstants.java index 2681adc..4c98ca7 100644 --- a/src/main/java/frc/robot/commands/autos/AutoConstants.java +++ b/src/main/java/frc/robot/commands/autos/AutoConstants.java @@ -3,6 +3,16 @@ import frc.robot.util.LoggedTunableNumber; public class AutoConstants { - public static final LoggedTunableNumber launchLength = + public static final LoggedTunableNumber launchDuration = new LoggedTunableNumber("Auto/Launch Length", 10); + public static final LoggedTunableNumber autoSpeed = + new LoggedTunableNumber("Auto/Auto Speed", 1.0); + + /** + * Initialize all auto constants by accessing them once so they appear on dashboard + */ + public static void initialize() { + autoSpeed.get(); + launchDuration.get(); + } } diff --git a/src/main/java/frc/robot/commands/autos/StartCollectShoot.java b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java index baecf7c..4d27600 100644 --- a/src/main/java/frc/robot/commands/autos/StartCollectShoot.java +++ b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java @@ -27,27 +27,39 @@ public static AutoRoutine getRoutine( .active() .onTrue( Commands.sequence( - new InstantCommand(() -> System.out.println("Going to fuel")), + new InstantCommand(() -> Commands.print("Going to fuel")), faceFuel.resetOdometry(), faceFuel.cmd())); faceFuel .done() .onTrue( - Commands.parallel( - intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.intakeVoltage.get()), - collectFuel.cmd())); - collectFuel.done().onTrue(fuelToShot.cmd()); + Commands.sequence( + new InstantCommand(() -> Commands.print("Starting intake and collecting fuel")), + Commands.parallel( + intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.intakeVoltage.get()), + collectFuel.cmd()))); + collectFuel + .done() + .onTrue( + Commands.sequence( + new InstantCommand(() -> Commands.print("Moving to shooting position")), + fuelToShot.cmd())); fuelToShot .done() .onTrue( - Commands.race( - Commands.parallel( - intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.launchVoltage.get()), - Commands.sequence( - Commands.waitSeconds(IntakeAndLauncherConstants.launchWarmUpTime.get()), - feederAndIndexer.voltageCmd(FeederConstants.launchVoltage.get()))), - Commands.waitSeconds(AutoConstants.launchLength.get()))); + Commands.sequence( + new InstantCommand(() -> Commands.print("Starting launch sequence")), + Commands.race( + Commands.parallel( + intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.launchVoltage.get()), + Commands.sequence( + Commands.waitSeconds(IntakeAndLauncherConstants.launchWarmUpTime.get()), + Commands.sequence( + new InstantCommand( + () -> Commands.print("Feeding fuel to launcher")), + feederAndIndexer.voltageCmd(FeederConstants.launchVoltage.get())))), + Commands.waitSeconds(AutoConstants.launchDuration.get())))); return routine; } From 4c21f85815f79e123ae60145fc39e090f8468c61 Mon Sep 17 00:00:00 2001 From: Noah H Date: Fri, 23 Jan 2026 20:14:46 -0500 Subject: [PATCH 3/4] modify trajectories and add traj logging --- src/main/deploy/choreo/CollectFuel.traj | 84 +++++++++++++------ src/main/java/frc/robot/RobotContainer.java | 12 ++- .../robot/commands/autos/AutoConstants.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 7 +- .../java/frc/robot/util/AllianceFlipUtil.java | 70 ++++++++++++++++ 5 files changed, 145 insertions(+), 32 deletions(-) create mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java diff --git a/src/main/deploy/choreo/CollectFuel.traj b/src/main/deploy/choreo/CollectFuel.traj index a0b9cf7..609c047 100644 --- a/src/main/deploy/choreo/CollectFuel.traj +++ b/src/main/deploy/choreo/CollectFuel.traj @@ -3,22 +3,24 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":7.80876998901367, "y":3.5819289684295654, "heading":1.5707963267948966, "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.541, "h":8.0692}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + {"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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -49,30 +51,58 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.01609], + "waypoints":[0.0,2.43628], "samples":[ - {"t":0.0, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":9.01899, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.41051,153.41051,153.41051,153.41051]}, - {"t":0.04839, "x":7.80877, "y":1.27152, "heading":1.5708, "vx":0.0, "vy":0.43639, "omega":0.0, "ax":0.0, "ay":9.01813, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.39574,153.39574,153.39574,153.39574]}, - {"t":0.09677, "x":7.80877, "y":1.30319, "heading":1.5708, "vx":0.0, "vy":0.87273, "omega":0.0, "ax":0.0, "ay":9.01704, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.37727,153.37727,153.37727,153.37727]}, - {"t":0.14516, "x":7.80877, "y":1.35598, "heading":1.5708, "vx":0.0, "vy":1.30902, "omega":0.0, "ax":0.0, "ay":9.01564, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.35352,153.35352,153.35352,153.35352]}, - {"t":0.19354, "x":7.80877, "y":1.42987, "heading":1.5708, "vx":0.0, "vy":1.74525, "omega":0.0, "ax":0.0, "ay":9.01378, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.32184,153.32184,153.32184,153.32184]}, - {"t":0.24193, "x":7.80877, "y":1.52486, "heading":1.5708, "vx":0.0, "vy":2.18138, "omega":0.0, "ax":0.0, "ay":9.01117, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.2775,153.2775,153.2775,153.2775]}, - {"t":0.29031, "x":7.80877, "y":1.64096, "heading":1.5708, "vx":0.0, "vy":2.61739, "omega":0.0, "ax":0.0, "ay":9.00726, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.21097,153.21097,153.21097,153.21097]}, - {"t":0.3387, "x":7.80877, "y":1.77814, "heading":1.5708, "vx":0.0, "vy":3.05321, "omega":0.0, "ax":0.0, "ay":9.00074, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.10008,153.10008,153.10008,153.10008]}, - {"t":0.38708, "x":7.80877, "y":1.93641, "heading":1.5708, "vx":0.0, "vy":3.48871, "omega":0.0, "ax":0.0, "ay":8.98771, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[152.87834,152.87834,152.87834,152.87834]}, - {"t":0.43547, "x":7.80877, "y":2.11573, "heading":1.5708, "vx":0.0, "vy":3.92358, "omega":0.0, "ax":0.0, "ay":8.94867, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[152.21427,152.21427,152.21427,152.21427]}, - {"t":0.48385, "x":7.80877, "y":2.31605, "heading":1.5708, "vx":0.0, "vy":4.35657, "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.53224, "x":7.80877, "y":2.52684, "heading":1.5708, "vx":0.0, "vy":4.35657, "omega":0.0, "ax":0.0, "ay":-8.94867, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-152.21427,-152.21427,-152.21427,-152.21427]}, - {"t":0.58062, "x":7.80877, "y":2.72716, "heading":1.5708, "vx":0.0, "vy":3.92358, "omega":0.0, "ax":0.0, "ay":-8.98771, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-152.87834,-152.87834,-152.87834,-152.87834]}, - {"t":0.62901, "x":7.80877, "y":2.90648, "heading":1.5708, "vx":0.0, "vy":3.48871, "omega":0.0, "ax":0.0, "ay":-9.00074, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.10008,-153.10008,-153.10008,-153.10008]}, - {"t":0.67739, "x":7.80877, "y":3.06475, "heading":1.5708, "vx":0.0, "vy":3.05321, "omega":0.0, "ax":0.0, "ay":-9.00726, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.21097,-153.21097,-153.21097,-153.21097]}, - {"t":0.72578, "x":7.80877, "y":3.20194, "heading":1.5708, "vx":0.0, "vy":2.61739, "omega":0.0, "ax":0.0, "ay":-9.01117, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.2775,-153.2775,-153.2775,-153.2775]}, - {"t":0.77416, "x":7.80877, "y":3.31803, "heading":1.5708, "vx":0.0, "vy":2.18138, "omega":0.0, "ax":0.0, "ay":-9.01378, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.32184,-153.32184,-153.32184,-153.32184]}, - {"t":0.82255, "x":7.80877, "y":3.41303, "heading":1.5708, "vx":0.0, "vy":1.74525, "omega":0.0, "ax":0.0, "ay":-9.01564, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.35352,-153.35352,-153.35352,-153.35352]}, - {"t":0.87093, "x":7.80877, "y":3.48692, "heading":1.5708, "vx":0.0, "vy":1.30902, "omega":0.0, "ax":0.0, "ay":-9.01704, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.37727,-153.37727,-153.37727,-153.37727]}, - {"t":0.91932, "x":7.80877, "y":3.5397, "heading":1.5708, "vx":0.0, "vy":0.87273, "omega":0.0, "ax":0.0, "ay":-9.01813, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.39574,-153.39574,-153.39574,-153.39574]}, - {"t":0.9677, "x":7.80877, "y":3.57137, "heading":1.5708, "vx":0.0, "vy":0.43639, "omega":0.0, "ax":0.0, "ay":-9.01899, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.41051,-153.41051,-153.41051,-153.41051]}, - {"t":1.01609, "x":7.80877, "y":3.58193, "heading":1.5708, "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":7.80877, "y":1.26096, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":9.01865, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.4046,153.4046,153.4046,153.4046]}, + {"t":0.04972, "x":7.80877, "y":1.27211, "heading":1.5708, "vx":0.0, "vy":0.44841, "omega":0.0, "ax":0.0, "ay":9.01049, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.26592,153.26592,153.26592,153.26592]}, + {"t":0.09944, "x":7.80877, "y":1.30554, "heading":1.5708, "vx":0.0, "vy":0.89641, "omega":0.0, "ax":0.0, "ay":2.07942, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[35.37032,35.37032,35.37032,35.37032]}, + {"t":0.14916, "x":7.80877, "y":1.35268, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00006,0.00006,0.00006,0.00006]}, + {"t":0.19888, "x":7.80877, "y":1.40239, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.2486, "x":7.80877, "y":1.4521, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.29832, "x":7.80877, "y":1.50181, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.34804, "x":7.80877, "y":1.55152, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.39776, "x":7.80877, "y":1.60123, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.44748, "x":7.80877, "y":1.65094, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.4972, "x":7.80877, "y":1.70065, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.54692, "x":7.80877, "y":1.75036, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.59664, "x":7.80877, "y":1.80007, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.64636, "x":7.80877, "y":1.84978, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.69608, "x":7.80877, "y":1.89949, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.7458, "x":7.80877, "y":1.9492, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.79552, "x":7.80877, "y":1.99891, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.84524, "x":7.80877, "y":2.04862, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.89496, "x":7.80877, "y":2.09833, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.94468, "x":7.80877, "y":2.14804, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.9944, "x":7.80877, "y":2.19775, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.04412, "x":7.80877, "y":2.24746, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.09384, "x":7.80877, "y":2.29717, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.14356, "x":7.80877, "y":2.34688, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.19328, "x":7.80877, "y":2.39659, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.243, "x":7.80877, "y":2.4463, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.29272, "x":7.80877, "y":2.49601, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.34244, "x":7.80877, "y":2.54572, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.39216, "x":7.80877, "y":2.59543, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.44188, "x":7.80877, "y":2.64514, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.4916, "x":7.80877, "y":2.69485, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.54132, "x":7.80877, "y":2.74456, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.59104, "x":7.80877, "y":2.79427, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.64076, "x":7.80877, "y":2.84398, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.69048, "x":7.80877, "y":2.89369, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.7402, "x":7.80877, "y":2.9434, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.78992, "x":7.80877, "y":2.99311, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.83964, "x":7.80877, "y":3.04282, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.88936, "x":7.80877, "y":3.09253, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.93908, "x":7.80877, "y":3.14224, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.9888, "x":7.80877, "y":3.19195, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.03852, "x":7.80877, "y":3.24166, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.08824, "x":7.80877, "y":3.29137, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.13796, "x":7.80877, "y":3.34108, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.18768, "x":7.80877, "y":3.39079, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.2374, "x":7.80877, "y":3.4405, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00006,-0.00006,-0.00006,-0.00006]}, + {"t":2.28712, "x":7.80877, "y":3.49021, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":-2.07942, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-35.37032,-35.37032,-35.37032,-35.37032]}, + {"t":2.33684, "x":7.80877, "y":3.53735, "heading":1.5708, "vx":0.0, "vy":0.89641, "omega":0.0, "ax":0.0, "ay":-9.01049, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.26592,-153.26592,-153.26592,-153.26592]}, + {"t":2.38656, "x":7.80877, "y":3.57078, "heading":1.5708, "vx":0.0, "vy":0.44841, "omega":0.0, "ax":0.0, "ay":-9.01865, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.4046,-153.4046,-153.4046,-153.4046]}, + {"t":2.43628, "x":7.80877, "y":3.58193, "heading":1.5708, "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/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4676e1a..c7a6944 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,7 +35,10 @@ import frc.robot.subsystems.feeder.FeederConstants; import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncher; import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncherConstants; +import frc.robot.util.AllianceFlipUtil; import frc.robot.util.CANHealthLogger; +import java.util.Arrays; +import org.littletonrobotics.junction.Logger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -132,7 +135,14 @@ public RobotContainer() { drive::setPose, // Function that resets the current robot pose to the provided Pose2d drive::followTrajectory, // The drive subsystem trajectory follower true, // If alliance flipping should be enabled - drive // The drive subsystem + drive, + (sample, isStart) -> { + Logger.recordOutput( + "TrajectoryFollowing/ActiveTrajectory", + Arrays.stream(sample.getPoses()) + .map(AllianceFlipUtil::apply) + .toArray(Pose2d[]::new)); + } // The drive subsystem ); // Set up auto routines diff --git a/src/main/java/frc/robot/commands/autos/AutoConstants.java b/src/main/java/frc/robot/commands/autos/AutoConstants.java index 4c98ca7..39bb6e2 100644 --- a/src/main/java/frc/robot/commands/autos/AutoConstants.java +++ b/src/main/java/frc/robot/commands/autos/AutoConstants.java @@ -8,9 +8,7 @@ public class AutoConstants { public static final LoggedTunableNumber autoSpeed = new LoggedTunableNumber("Auto/Auto Speed", 1.0); - /** - * Initialize all auto constants by accessing them once so they appear on dashboard - */ + /** Initialize all auto constants by accessing them once so they appear on dashboard */ public static void initialize() { autoSpeed.get(); launchDuration.get(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5eff19f..7b6bca7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -362,11 +362,16 @@ public static Translation2d[] getModuleTranslations() { public void followTrajectory(SwerveSample sample) { // Get the current pose of the robot Pose2d pose = getPose(); - + Pose2d samplePose2d = + new Pose2d( + new Translation2d(sample.x, sample.y), new Rotation2d(Radians.of(sample.heading))); + Logger.recordOutput("TrajectoryFollowing/pose2d", samplePose2d); Logger.recordOutput("TrajectoryFollowing/posex", pose.getX()); Logger.recordOutput("TrajectoryFollowing/samplex", sample.x); + Logger.recordOutput("TrajectoryFollowing/posey", pose.getY()); Logger.recordOutput("TrajectoryFollowing/sampley", sample.y); + Logger.recordOutput( "TrajectoryFollowing/heading", pose.getRotation().getRadians() + Math.PI * 2); Logger.recordOutput( diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..f01b082 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,70 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public class AllianceFlipUtil { + public static double fieldWidth = Units.feetToMeters(26.0) + Units.inchesToMeters(5.0); + public static double fieldLength = Units.feetToMeters(57.0) + Units.inchesToMeters(6.875); + + public static double applyX(double x) { + return shouldFlip() ? fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Rotation2d dontApply(Rotation2d rotation) { + return rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Pose2d flip(Pose2d pose) { + return new Pose2d( + new Translation2d(fieldLength - pose.getX(), fieldWidth - pose.getY()), + pose.getRotation().rotateBy(Rotation2d.kPi)); + } + + public static Translation2d flip(Translation2d pose) { + return new Translation2d(fieldLength - pose.getX(), fieldWidth - pose.getY()); + } + + public static Rotation2d flip(Rotation2d pose) { + return pose.rotateBy(Rotation2d.kPi); + } + + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } + + public static Command apply(Command ifFlip, Command otherwise) { + return Commands.either(ifFlip, otherwise, AllianceFlipUtil::shouldFlip); + } +} From bf5ca02401ffaf1a86047d16ac863c203e282e92 Mon Sep 17 00:00:00 2001 From: Noah H Date: Sat, 24 Jan 2026 13:45:09 -0500 Subject: [PATCH 4/4] W.I.P. auto --- src/main/deploy/choreo/CollectFuel.traj | 146 ++++++++++-------- src/main/deploy/choreo/FaceFuel.traj | 98 +++++------- src/main/deploy/choreo/FuelToShot.traj | 119 -------------- src/main/deploy/choreo/FuelToShotB.traj | 92 +++++++++++ src/main/deploy/choreo/FuelToShotNB.traj | 146 ++++++++++++++++++ src/main/deploy/choreo/StartCollectShoot.chor | 20 +-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../commands/autos/StartCollectShoot.java | 20 +-- .../prototypeLauncher/PrototypeLauncher.java | 67 ++++++++ 9 files changed, 451 insertions(+), 259 deletions(-) delete mode 100644 src/main/deploy/choreo/FuelToShot.traj create mode 100644 src/main/deploy/choreo/FuelToShotB.traj create mode 100644 src/main/deploy/choreo/FuelToShotNB.traj create mode 100644 src/main/java/frc/robot/subsystems/prototypeLauncher/PrototypeLauncher.java diff --git a/src/main/deploy/choreo/CollectFuel.traj b/src/main/deploy/choreo/CollectFuel.traj index 609c047..77233bf 100644 --- a/src/main/deploy/choreo/CollectFuel.traj +++ b/src/main/deploy/choreo/CollectFuel.traj @@ -3,24 +3,24 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.80876998901367, "y":3.5819289684295654, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.666309833526611, "y":1.2865098714828491, "heading":1.5707963267948966, "intervals":71, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.685800075531006, "y":3.391429901123047, "heading":1.5707963267948966, "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.541, "h":8.0692}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":0.6}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"7.666309833526611 m", "val":7.666309833526611}, "y":{"exp":"1.2865098714828491 m", "val":1.2865098714828491}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":71, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.685800075531006 m", "val":7.685800075531006}, "y":{"exp":"3.391429901123047 m", "val":3.391429901123047}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.6 m / s", "val":0.6}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,73 +36,95 @@ "x":-0.2794, "y":0.2794 }, - "mass":68.0388555, + "mass":43.54486752, "inertia":6.0, "gearing":6.5, - "radius":0.0508, + "radius":0.0497078, "vmax":628.3185307179587, "tmax":1.2, "cof":1.5, "bumper":{ - "front":0.4064, - "side":0.4064, - "back":0.4064 + "front":0.55245, + "side":0.3302, + "back":0.33654999999999996 }, "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,2.43628], + "waypoints":[0.0,3.55917], "samples":[ - {"t":0.0, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":9.01865, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.4046,153.4046,153.4046,153.4046]}, - {"t":0.04972, "x":7.80877, "y":1.27211, "heading":1.5708, "vx":0.0, "vy":0.44841, "omega":0.0, "ax":0.0, "ay":9.01049, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[153.26592,153.26592,153.26592,153.26592]}, - {"t":0.09944, "x":7.80877, "y":1.30554, "heading":1.5708, "vx":0.0, "vy":0.89641, "omega":0.0, "ax":0.0, "ay":2.07942, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[35.37032,35.37032,35.37032,35.37032]}, - {"t":0.14916, "x":7.80877, "y":1.35268, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00006,0.00006,0.00006,0.00006]}, - {"t":0.19888, "x":7.80877, "y":1.40239, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.2486, "x":7.80877, "y":1.4521, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.29832, "x":7.80877, "y":1.50181, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.34804, "x":7.80877, "y":1.55152, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.39776, "x":7.80877, "y":1.60123, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.44748, "x":7.80877, "y":1.65094, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.4972, "x":7.80877, "y":1.70065, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.54692, "x":7.80877, "y":1.75036, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.59664, "x":7.80877, "y":1.80007, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.64636, "x":7.80877, "y":1.84978, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.69608, "x":7.80877, "y":1.89949, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.7458, "x":7.80877, "y":1.9492, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.79552, "x":7.80877, "y":1.99891, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.84524, "x":7.80877, "y":2.04862, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.89496, "x":7.80877, "y":2.09833, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.94468, "x":7.80877, "y":2.14804, "heading":1.5708, "vx":0.0, "vy":0.9998, "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.9944, "x":7.80877, "y":2.19775, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.04412, "x":7.80877, "y":2.24746, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.09384, "x":7.80877, "y":2.29717, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.14356, "x":7.80877, "y":2.34688, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.19328, "x":7.80877, "y":2.39659, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.243, "x":7.80877, "y":2.4463, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.29272, "x":7.80877, "y":2.49601, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.34244, "x":7.80877, "y":2.54572, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.39216, "x":7.80877, "y":2.59543, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.44188, "x":7.80877, "y":2.64514, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.4916, "x":7.80877, "y":2.69485, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.54132, "x":7.80877, "y":2.74456, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.59104, "x":7.80877, "y":2.79427, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.64076, "x":7.80877, "y":2.84398, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.69048, "x":7.80877, "y":2.89369, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.7402, "x":7.80877, "y":2.9434, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.78992, "x":7.80877, "y":2.99311, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.83964, "x":7.80877, "y":3.04282, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.88936, "x":7.80877, "y":3.09253, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.93908, "x":7.80877, "y":3.14224, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":1.9888, "x":7.80877, "y":3.19195, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.03852, "x":7.80877, "y":3.24166, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.08824, "x":7.80877, "y":3.29137, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.13796, "x":7.80877, "y":3.34108, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.18768, "x":7.80877, "y":3.39079, "heading":1.5708, "vx":0.0, "vy":0.9998, "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":2.2374, "x":7.80877, "y":3.4405, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00006,-0.00006,-0.00006,-0.00006]}, - {"t":2.28712, "x":7.80877, "y":3.49021, "heading":1.5708, "vx":0.0, "vy":0.9998, "omega":0.0, "ax":0.0, "ay":-2.07942, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-35.37032,-35.37032,-35.37032,-35.37032]}, - {"t":2.33684, "x":7.80877, "y":3.53735, "heading":1.5708, "vx":0.0, "vy":0.89641, "omega":0.0, "ax":0.0, "ay":-9.01049, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.26592,-153.26592,-153.26592,-153.26592]}, - {"t":2.38656, "x":7.80877, "y":3.57078, "heading":1.5708, "vx":0.0, "vy":0.44841, "omega":0.0, "ax":0.0, "ay":-9.01865, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-153.4046,-153.4046,-153.4046,-153.4046]}, - {"t":2.43628, "x":7.80877, "y":3.58193, "heading":1.5708, "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":7.66631, "y":1.28651, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.1108, "ay":11.96621, "alpha":0.0, "fx":[1.20619,1.20619,1.20619,1.20619], "fy":[130.26674,130.26674,130.26674,130.26674]}, + {"t":0.05013, "x":7.66645, "y":1.30155, "heading":1.5708, "vx":0.00555, "vy":0.59986, "omega":0.0, "ax":0.0, "ay":0.00001, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00009,0.00009,0.00009,0.00009]}, + {"t":0.10026, "x":7.66673, "y":1.33162, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.15039, "x":7.66701, "y":1.36169, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.20052, "x":7.66728, "y":1.39176, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.25065, "x":7.66756, "y":1.42183, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.30077, "x":7.66784, "y":1.4519, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.3509, "x":7.66812, "y":1.48197, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.40103, "x":7.6684, "y":1.51204, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.45116, "x":7.66868, "y":1.54211, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.50129, "x":7.66895, "y":1.57218, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.55142, "x":7.66923, "y":1.60225, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.60155, "x":7.66951, "y":1.63232, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.65168, "x":7.66979, "y":1.66239, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.70181, "x":7.67007, "y":1.69246, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.75194, "x":7.67035, "y":1.72253, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.80207, "x":7.67063, "y":1.7526, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.8522, "x":7.6709, "y":1.78267, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.90232, "x":7.67118, "y":1.81274, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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.95245, "x":7.67146, "y":1.84281, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.00258, "x":7.67174, "y":1.87288, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.05271, "x":7.67202, "y":1.90295, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.10284, "x":7.6723, "y":1.93302, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.15297, "x":7.67257, "y":1.96309, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.2031, "x":7.67285, "y":1.99316, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.25323, "x":7.67313, "y":2.02323, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.30336, "x":7.67341, "y":2.0533, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.35349, "x":7.67369, "y":2.08337, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.40362, "x":7.67397, "y":2.11344, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.45375, "x":7.67425, "y":2.14351, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.50387, "x":7.67452, "y":2.17358, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.554, "x":7.6748, "y":2.20365, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.60413, "x":7.67508, "y":2.23372, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.65426, "x":7.67536, "y":2.26379, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.70439, "x":7.67564, "y":2.29386, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.75452, "x":7.67592, "y":2.32393, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.80465, "x":7.67619, "y":2.35401, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.85478, "x":7.67647, "y":2.38408, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.90491, "x":7.67675, "y":2.41415, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":1.95504, "x":7.67703, "y":2.44422, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.00517, "x":7.67731, "y":2.47429, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.0553, "x":7.67759, "y":2.50436, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.10542, "x":7.67786, "y":2.53443, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.15555, "x":7.67814, "y":2.5645, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.20568, "x":7.67842, "y":2.59457, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.25581, "x":7.6787, "y":2.62464, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.30594, "x":7.67898, "y":2.65471, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.35607, "x":7.67926, "y":2.68478, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.4062, "x":7.67954, "y":2.71485, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.45633, "x":7.67981, "y":2.74492, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.50646, "x":7.68009, "y":2.77499, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.55659, "x":7.68037, "y":2.80506, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.60672, "x":7.68065, "y":2.83513, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.65685, "x":7.68093, "y":2.8652, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.70697, "x":7.68121, "y":2.89527, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.7571, "x":7.68148, "y":2.92534, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.80723, "x":7.68176, "y":2.95541, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.85736, "x":7.68204, "y":2.98548, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.90749, "x":7.68232, "y":3.01555, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":2.95762, "x":7.6826, "y":3.04562, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.00775, "x":7.68288, "y":3.07569, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.05788, "x":7.68315, "y":3.10576, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.10801, "x":7.68343, "y":3.13583, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.15814, "x":7.68371, "y":3.1659, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.20827, "x":7.68399, "y":3.19597, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.25839, "x":7.68427, "y":3.22604, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.30852, "x":7.68455, "y":3.25611, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.35865, "x":7.68483, "y":3.28618, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.40878, "x":7.6851, "y":3.31625, "heading":1.5708, "vx":0.00555, "vy":0.59986, "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":3.45891, "x":7.68538, "y":3.34632, "heading":1.5708, "vx":0.00555, "vy":0.59986, "omega":0.0, "ax":0.0, "ay":-0.00001, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00009,-0.00009,-0.00009,-0.00009]}, + {"t":3.50904, "x":7.68566, "y":3.37639, "heading":1.5708, "vx":0.00555, "vy":0.59986, "omega":0.0, "ax":-0.1108, "ay":-11.96621, "alpha":0.0, "fx":[-1.20619,-1.20619,-1.20619,-1.20619], "fy":[-130.26674,-130.26674,-130.26674,-130.26674]}, + {"t":3.55917, "x":7.6858, "y":3.39143, "heading":1.5708, "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/FaceFuel.traj b/src/main/deploy/choreo/FaceFuel.traj index 036bd2b..e6d3a8b 100644 --- a/src/main/deploy/choreo/FaceFuel.traj +++ b/src/main/deploy/choreo/FaceFuel.traj @@ -3,16 +3,16 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":4.420536518096924, "y":0.8504827618598938, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.690505504608154, "y":0.6633973717689514, "heading":0.0, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.415289402008057, "y":0.5481976866722107, "heading":1.5707963267948966, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.453956127166748, "y":0.5831669569015503, "heading":1.5707963267948966, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":7.80876998901367, "y":1.2609648704528809, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"4.420536518096924 m", "val":4.420536518096924}, "y":{"exp":"0.8504827618598938 m", "val":0.8504827618598938}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.690505504608154 m", "val":6.690505504608154}, "y":{"exp":"0.6633973717689514 m", "val":0.6633973717689514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.415289402008057 m", "val":4.415289402008057}, "y":{"exp":"0.5481976866722107 m", "val":0.5481976866722107}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.453956127166748 m", "val":6.453956127166748}, "y":{"exp":"0.5831669569015503 m", "val":0.5831669569015503}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"1.2609648704528809 m", "val":1.2609648704528809}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[], "targetDt":{ @@ -30,71 +30,53 @@ "x":-0.2794, "y":0.2794 }, - "mass":68.0388555, + "mass":43.54486752, "inertia":6.0, "gearing":6.5, - "radius":0.0508, + "radius":0.0497078, "vmax":628.3185307179587, "tmax":1.2, "cof":1.5, "bumper":{ - "front":0.4064, - "side":0.4064, - "back":0.4064 + "front":0.55245, + "side":0.3302, + "back":0.33654999999999996 }, "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.47972,0.85838], + "waypoints":[0.0,0.42591,0.74254], "samples":[ - {"t":0.0, "x":4.42054, "y":0.85048, "heading":0.0, "vx":4.57021, "vy":-1.19002, "omega":-0.53764, "ax":5.35546, "ay":-1.52639, "alpha":16.06544, "fx":[75.43527,49.63132,113.35305,125.95971], "fy":[43.53688,-102.91841,-64.38465,19.91253]}, - {"t":0.02284, "x":4.52634, "y":0.8229, "heading":-0.01228, "vx":4.69255, "vy":-1.22488, "omega":-0.17064, "ax":1.47303, "ay":-0.92868, "alpha":4.92878, "fx":[12.44825,11.64662,37.2907,38.8378], "fy":[-2.2446,-29.86888,-28.62529,-2.44774]}, - {"t":0.04569, "x":4.63392, "y":0.79468, "heading":-0.01618, "vx":4.7262, "vy":-1.2461, "omega":-0.05805, "ax":0.039, "ay":-0.13689, "alpha":0.21771, "fx":[0.08842,0.06949,1.23824,1.25743], "fy":[-1.73485,-2.90328,-2.92199,-1.75365]}, - {"t":0.06853, "x":4.74189, "y":0.76617, "heading":-0.01751, "vx":4.72709, "vy":-1.24923, "omega":-0.05308, "ax":0.069, "ay":0.2581, "alpha":-0.00021, "fx":[1.17417,1.17429,1.17318,1.17306], "fy":[4.38961,4.39072,4.39084,4.38973]}, - {"t":0.09138, "x":4.8499, "y":0.7377, "heading":-0.01872, "vx":4.72867, "vy":-1.24333, "omega":-0.05308, "ax":0.19405, "ay":0.76177, "alpha":-0.02004, "fx":[3.35414,3.35576,3.24746,3.24582], "fy":[12.90291,13.00977,13.01215,12.90529]}, - {"t":0.11422, "x":4.95797, "y":0.7095, "heading":-0.01993, "vx":4.7331, "vy":-1.22593, "omega":-0.05354, "ax":0.39878, "ay":1.60605, "alpha":-0.04278, "fx":[6.90075,6.90193,6.66545,6.66415], "fy":[27.20299,27.42572,27.43388,27.21113]}, - {"t":0.13706, "x":5.0662, "y":0.68191, "heading":-0.02115, "vx":4.74221, "vy":-1.18924, "omega":-0.05452, "ax":0.70337, "ay":2.98388, "alpha":-0.08731, "fx":[12.22975,12.21604,11.69907,11.71176], "fy":[50.52859,50.94831,50.98136,50.5616]}, - {"t":0.15991, "x":5.17471, "y":0.65553, "heading":-0.0224, "vx":4.75828, "vy":-1.12108, "omega":-0.05651, "ax":1.01244, "ay":4.71998, "alpha":-0.16679, "fx":[17.82833,17.74531,16.61757,16.69394], "fy":[79.89571,80.5555,80.6749,80.01597]}, - {"t":0.18275, "x":5.28367, "y":0.63115, "heading":-0.02369, "vx":4.78141, "vy":-1.01325, "omega":-0.06032, "ax":1.14526, "ay":6.23057, "alpha":-0.29592, "fx":[20.76313,20.52399,18.21407,18.42122], "fy":[105.40399,106.25802,106.55302,105.70597]}, - {"t":0.2056, "x":5.3932, "y":0.60963, "heading":-0.02507, "vx":4.80757, "vy":-0.87092, "omega":-0.06708, "ax":1.05042, "ay":7.24413, "alpha":-0.49754, "fx":[20.27471,19.82604,15.51934,15.84905], "fy":[122.46523,123.45224,123.96315,123.00147]}, - {"t":0.22844, "x":5.5033, "y":0.59162, "heading":-0.0266, "vx":4.83157, "vy":-0.70544, "omega":-0.07845, "ax":0.80213, "ay":7.8633, "alpha":-0.74099, "fx":[17.42561,16.84046,10.01803,10.29159], "fy":[132.9315,133.93112,134.54889,133.59825]}, - {"t":0.25128, "x":5.61388, "y":0.57756, "heading":-0.02839, "vx":4.84989, "vy":-0.52581, "omega":-0.09537, "ax":0.50479, "ay":8.23677, "alpha":-0.87328, "fx":[13.11492,12.6426,4.29612,4.29151], "fy":[139.45007,140.25902,140.73913,139.97221]}, - {"t":0.27413, "x":5.7248, "y":0.5677, "heading":-0.03057, "vx":4.86142, "vy":-0.33765, "omega":-0.11532, "ax":0.2192, "ay":8.46698, "alpha":-0.74668, "fx":[7.56344,7.42682,0.07779,-0.15427], "fy":[143.68018,144.1644,144.35762,143.88144]}, - {"t":0.29697, "x":5.83591, "y":0.56219, "heading":-0.03321, "vx":4.86643, "vy":-0.14423, "omega":-0.13238, "ax":-0.07257, "ay":8.61211, "alpha":-0.44188, "fx":[0.96926,1.08941,-3.37141,-3.62459], "fy":[146.39842,146.60998,146.5837,146.366]}, - {"t":0.31982, "x":5.94706, "y":0.56114, "heading":-0.03623, "vx":4.86477, "vy":0.05251, "omega":-0.14248, "ax":-0.39818, "ay":8.69922, "alpha":-0.22205, "fx":[-5.71077,-5.54324,-7.81807,-8.01945], "fy":[147.97595,148.06482,147.96876,147.87573]}, - {"t":0.34266, "x":6.05809, "y":0.56461, "heading":-0.03949, "vx":4.85568, "vy":0.25123, "omega":-0.14755, "ax":-0.86975, "ay":8.73163, "alpha":0.26162, "fx":[-15.94524,-16.33648,-13.62033,-13.27495], "fy":[148.45334,148.33417,148.59717,148.70547]}, - {"t":0.3655, "x":6.16879, "y":0.57263, "heading":-0.04286, "vx":4.83581, "vy":0.4507, "omega":-0.14157, "ax":-3.19518, "ay":7.9198, "alpha":7.27882, "fx":[-69.86865,-102.66531,-30.07179,-14.79031], "fy":[133.9292,109.36516,145.58133,149.97826]}, - {"t":0.38835, "x":6.27842, "y":0.58499, "heading":-0.04609, "vx":4.76282, "vy":0.63162, "omega":0.02471, "ax":-4.85901, "ay":6.42534, "alpha":11.73322, "fx":[-97.59403,-145.21307,-73.02321,-14.77121], "fy":[116.62634,42.97129,126.7252,150.84986]}, - {"t":0.41119, "x":6.38596, "y":0.6011, "heading":-0.04553, "vx":4.65182, "vy":0.7784, "omega":0.29274, "ax":-6.04111, "ay":4.68691, "alpha":14.47425, "fx":[-108.23307,-151.61383,-135.92965,-15.25396], "fy":[107.4611,12.93573,47.22444,151.27065]}, - {"t":0.43404, "x":6.49065, "y":0.6201, "heading":-0.03884, "vx":4.51381, "vy":0.88547, "omega":0.62339, "ax":-6.16757, "ay":2.92907, "alpha":19.8463, "fx":[-113.96529,-152.51791,-136.60226,-16.5492], "fy":[101.72439,-2.01965,-51.84171,151.42766]}, - {"t":0.45688, "x":6.59215, "y":0.6411, "heading":-0.0246, "vx":4.37292, "vy":0.95238, "omega":1.07676, "ax":-5.90468, "ay":2.09151, "alpha":23.18554, "fx":[-117.79156,-152.31693,-113.65192,-17.98697], "fy":[97.50426,-11.40979,-95.25948,151.46925]}, - {"t":0.47972, "x":6.69051, "y":0.6634, "heading":0.0, "vx":4.23804, "vy":1.00016, "omega":1.60641, "ax":-6.63098, "ay":3.11333, "alpha":17.02397, "fx":[-119.98775,-152.17065,-144.49155,-34.51419], "fy":[94.16214,-4.89741,-25.4041,147.96647]}, - {"t":0.49429, "x":6.75152, "y":0.67829, "heading":0.02339, "vx":4.14147, "vy":1.0455, "omega":1.85434, "ax":-6.64819, "ay":3.07387, "alpha":16.89704, "fx":[-121.09847,-151.98856,-143.83319,-35.41505], "fy":[92.63348,-7.29036,-23.877,147.67661]}, - {"t":0.50885, "x":6.81113, "y":0.69385, "heading":0.0504, "vx":4.04464, "vy":1.09027, "omega":2.10042, "ax":-6.62524, "ay":3.01492, "alpha":16.97382, "fx":[-122.0057,-151.71842,-142.21501,-34.8348], "fy":[91.33771,-10.31201,-23.64093,147.74665]}, - {"t":0.52342, "x":6.86933, "y":0.71004, "heading":0.08099, "vx":3.94816, "vy":1.13417, "omega":2.34762, "ax":-6.57944, "ay":2.97462, "alpha":17.0577, "fx":[-122.87178,-151.34367,-139.68325,-33.75871], "fy":[90.0675,-13.70746,-21.90526,147.93503]}, - {"t":0.53798, "x":6.92613, "y":0.72688, "heading":0.11518, "vx":3.85234, "vy":1.17749, "omega":2.59604, "ax":-6.50216, "ay":3.00384, "alpha":17.03873, "fx":[-123.74636,-150.84577,-135.27116,-32.53593], "fy":[88.75554,-17.39857,-15.13343,148.15446]}, - {"t":0.55254, "x":6.98155, "y":0.74434, "heading":0.15299, "vx":3.75764, "vy":1.22124, "omega":2.84418, "ax":-6.36357, "ay":3.29949, "alpha":16.52947, "fx":[-124.80606,-150.25128,-125.65036,-32.26218], "fy":[87.14282,-21.01777,10.2076,148.16057]}, - {"t":0.56711, "x":7.0356, "y":0.76248, "heading":0.19441, "vx":3.66497, "vy":1.26929, "omega":3.08491, "ax":-6.16221, "ay":3.98889, "alpha":15.35585, "fx":[-126.54549,-149.76679,-106.94908,-36.00845], "fy":[84.44998,-23.27354,63.00802,147.21505]}, - {"t":0.58167, "x":7.08832, "y":0.78139, "heading":0.23934, "vx":3.57523, "vy":1.32739, "omega":3.30854, "ax":-6.0638, "ay":4.35322, "alpha":14.82281, "fx":[-128.67261,-149.43706,-91.98057,-42.48356], "fy":[80.99411,-24.28039,94.09621,145.37826]}, - {"t":0.59623, "x":7.13974, "y":0.80118, "heading":0.28752, "vx":3.48692, "vy":1.39078, "omega":3.52441, "ax":-6.03211, "ay":4.48803, "alpha":14.71054, "fx":[-130.69193,-149.15618,-81.58951,-48.98034], "fy":[77.50009,-24.85971,109.49781,143.22232]}, - {"t":0.6108, "x":7.18989, "y":0.82191, "heading":0.33885, "vx":3.39907, "vy":1.45615, "omega":3.73865, "ax":-6.02714, "ay":4.53616, "alpha":14.76291, "fx":[-132.52803,-148.91461,-73.59421,-55.04314], "fy":[74.11254,-25.10395,118.71226,140.91446]}, - {"t":0.62536, "x":7.23875, "y":0.8436, "heading":0.39329, "vx":3.31129, "vy":1.52221, "omega":3.95365, "ax":-6.18514, "ay":4.47774, "alpha":14.36653, "fx":[-134.44838,-148.86506,-75.06554,-62.45096], "fy":[70.33354,-24.10605,120.74159,137.69122]}, - {"t":0.63992, "x":7.28632, "y":0.86624, "heading":0.45087, "vx":3.22121, "vy":1.58742, "omega":4.16288, "ax":-7.18615, "ay":3.83841, "alpha":10.87935, "fx":[-138.36111,-149.8786,-115.15772,-85.5401], "fy":[61.75281,-14.72657,89.78752,124.3474]}, - {"t":0.65449, "x":7.33247, "y":0.88977, "heading":0.5115, "vx":3.11656, "vy":1.64332, "omega":4.32132, "ax":-7.68969, "ay":3.26823, "alpha":9.0586, "fx":[-141.82518,-149.89715,-128.75053,-102.72492], "fy":[52.57219,-11.62757,71.25051,110.17136]}, - {"t":0.66905, "x":7.37704, "y":0.91405, "heading":0.57443, "vx":3.00457, "vy":1.69092, "omega":4.45324, "ax":-7.83989, "ay":2.88865, "alpha":8.88256, "fx":[-144.14376,-148.96824,-131.23028,-109.07457], "fy":[44.97594,-17.43449,65.54684,103.45235]}, - {"t":0.68361, "x":7.41997, "y":0.93898, "heading":0.63929, "vx":2.89039, "vy":1.73299, "omega":4.5826, "ax":-7.93332, "ay":2.51475, "alpha":8.96028, "fx":[-146.02179,-147.44665,-132.48984,-113.81559], "fy":[37.23594,-24.89296,61.0952,97.66276]}, - {"t":0.69818, "x":7.46122, "y":0.96448, "heading":0.70603, "vx":2.77486, "vy":1.76961, "omega":4.7131, "ax":-7.99878, "ay":2.12454, "alpha":9.10587, "fx":[-147.47556,-145.29364,-133.34029,-118.11828], "fy":[29.12458,-32.90271,56.63331,91.69635]}, - {"t":0.71274, "x":7.50078, "y":0.99048, "heading":0.77467, "vx":2.65837, "vy":1.80055, "omega":4.84571, "ax":-8.03432, "ay":1.71779, "alpha":9.30198, "fx":[-148.43132,-142.40318,-133.7564,-122.05517], "fy":[20.63953,-41.2738,52.08297,85.42746]}, - {"t":0.7273, "x":7.53864, "y":1.01688, "heading":0.84524, "vx":2.54136, "vy":1.82557, "omega":4.98118, "ax":-8.03471, "ay":1.29661, "alpha":9.54262, "fx":[-148.79304,-138.65966,-133.62173,-125.59795], "fy":[11.81218,-49.84062,47.44976,78.79839]}, - {"t":0.74187, "x":7.5748, "y":1.04361, "heading":0.91778, "vx":2.42434, "vy":1.84445, "omega":5.12015, "ax":-7.99278, "ay":0.86507, "alpha":9.81886, "fx":[-148.43795,-133.95536,-132.75493,-128.67133], "fy":[2.71411,-58.35361,42.74799,71.74977]}, - {"t":0.75643, "x":7.60926, "y":1.07056, "heading":0.99235, "vx":2.30794, "vy":1.85705, "omega":5.26315, "ax":-7.89849, "ay":0.43096, "alpha":10.11318, "fx":[-147.2016,-128.2123,-130.86531,-131.12474], "fy":[-6.50589,-66.40117,38.00921,64.22006]}, - {"t":0.77099, "x":7.64204, "y":1.09765, "heading":1.069, "vx":2.19291, "vy":1.86332, "omega":5.41043, "ax":-7.73644, "ay":0.01022, "alpha":10.38784, "fx":[-144.84188,-121.42259,-127.45776,-132.65642], "fy":[-15.5348,-73.24572,33.31745,56.15833]}, - {"t":0.78556, "x":7.67315, "y":1.12479, "heading":1.14779, "vx":2.08024, "vy":1.86347, "omega":5.56172, "ax":-7.47906, "ay":-0.36048, "alpha":10.55065, "fx":[-140.92935,-113.72067,-121.61754,-132.59923], "fy":[-23.64911,-77.36692,28.91772,47.57163]}, - {"t":0.80012, "x":7.70266, "y":1.15189, "heading":1.22879, "vx":1.97132, "vy":1.85822, "omega":5.71537, "ax":-7.06049, "ay":-0.57625, "alpha":10.32425, "fx":[-134.3853,-105.43687,-111.42515,-129.14035], "fy":[-28.7881,-74.76219,25.5977,38.74501]}, - {"t":0.81468, "x":7.73062, "y":1.17889, "heading":1.31202, "vx":1.8685, "vy":1.84983, "omega":5.86573, "ax":-6.20395, "ay":-0.19504, "alpha":8.45115, "fx":[-120.02569,-95.38333,-91.82213,-114.87832], "fy":[-21.92642,-49.24136,26.23648,31.6612]}, - {"t":0.82925, "x":7.75717, "y":1.20581, "heading":1.39745, "vx":1.77815, "vy":1.84699, "omega":5.98881, "ax":-1.5998, "ay":3.03532, "alpha":-5.13844, "fx":[-9.18836,-37.0062,-46.23629,-16.41769], "fy":[64.14144,64.65288,39.85526,37.87]}, - {"t":0.84381, "x":7.7829, "y":1.23303, "heading":1.48467, "vx":1.75485, "vy":1.8912, "omega":5.91397, "ax":2.9877, "ay":3.68303, "alpha":-17.20252, "fx":[89.1827,6.44688,-5.82658,113.47703], "fy":[89.07098,118.6037,35.81823,7.09638]}, - {"t":0.85838, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":1.79836, "vy":1.94483, "omega":5.66344, "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":4.41529, "y":0.5482, "heading":1.5708, "vx":4.79013, "vy":-0.06628, "omega":0.0, "ax":0.26942, "ay":-0.00373, "alpha":0.0, "fx":[2.93297,2.93297,2.93297,2.93297], "fy":[-0.04058,-0.04058,-0.04058,-0.04058]}, + {"t":0.02662, "x":4.54289, "y":0.54643, "heading":1.5708, "vx":4.7973, "vy":-0.06638, "omega":0.0, "ax":0.00019, "ay":0.0, "alpha":0.0, "fx":[0.00207,0.00207,0.00207,0.00207], "fy":[0.0,0.0,0.0,0.0]}, + {"t":0.05324, "x":4.67059, "y":0.54467, "heading":1.5708, "vx":4.79731, "vy":-0.06638, "omega":0.0, "ax":0.0, "ay":0.00001, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00012,0.00012,0.00012,0.00012]}, + {"t":0.07986, "x":4.79829, "y":0.5429, "heading":1.5708, "vx":4.79731, "vy":-0.06638, "omega":0.0, "ax":0.0, "ay":0.00004, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.00045,0.00045,0.00045,0.00045]}, + {"t":0.10648, "x":4.92599, "y":0.54113, "heading":1.5708, "vx":4.79731, "vy":-0.06637, "omega":0.0, "ax":0.0, "ay":0.00016, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[0.00173,0.00173,0.00173,0.00173]}, + {"t":0.1331, "x":5.05369, "y":0.53936, "heading":1.5708, "vx":4.79731, "vy":-0.06637, "omega":0.0, "ax":0.00001, "ay":0.00062, "alpha":0.0, "fx":[0.00009,0.00009,0.00009,0.00009], "fy":[0.0067,0.0067,0.0067,0.0067]}, + {"t":0.15971, "x":5.18139, "y":0.5376, "heading":1.5708, "vx":4.79731, "vy":-0.06635, "omega":0.0, "ax":0.00003, "ay":0.00238, "alpha":0.0, "fx":[0.00036,0.00036,0.00036,0.00036], "fy":[0.02593,0.02593,0.02593,0.02593]}, + {"t":0.18633, "x":5.30909, "y":0.53583, "heading":1.5708, "vx":4.79731, "vy":-0.06629, "omega":0.0, "ax":0.00013, "ay":0.00921, "alpha":0.0, "fx":[0.00138,0.00138,0.00138,0.00138], "fy":[0.1003,0.1003,0.1003,0.1003]}, + {"t":0.21295, "x":5.43679, "y":0.53407, "heading":1.5708, "vx":4.79731, "vy":-0.06605, "omega":0.0, "ax":0.00049, "ay":0.03564, "alpha":0.0, "fx":[0.0053,0.0053,0.0053,0.0053], "fy":[0.38797,0.38797,0.38797,0.38797]}, + {"t":0.23957, "x":5.56449, "y":0.53233, "heading":1.5708, "vx":4.79732, "vy":-0.0651, "omega":0.0, "ax":0.00182, "ay":0.13785, "alpha":0.0, "fx":[0.01978,0.01978,0.01978,0.01978], "fy":[1.50068,1.50068,1.50068,1.50068]}, + {"t":0.26619, "x":5.6922, "y":0.53064, "heading":1.5708, "vx":4.79737, "vy":-0.06143, "omega":0.0, "ax":0.00602, "ay":0.53256, "alpha":0.0, "fx":[0.06556,0.06556,0.06556,0.06556], "fy":[5.79755,5.79755,5.79755,5.79755]}, + {"t":0.29281, "x":5.8199, "y":0.5292, "heading":1.5708, "vx":4.79753, "vy":-0.04725, "omega":0.0, "ax":0.00845, "ay":2.02098, "alpha":0.0, "fx":[0.09194,0.09194,0.09194,0.09194], "fy":[22.00085,22.00085,22.00085,22.00085]}, + {"t":0.31943, "x":5.94761, "y":0.52865, "heading":1.5708, "vx":4.79776, "vy":0.00655, "omega":0.0, "ax":-0.12162, "ay":6.35423, "alpha":0.0, "fx":[-1.32402,-1.32402,-1.32402,-1.32402], "fy":[69.17354,69.17354,69.17354,69.17354]}, + {"t":0.34605, "x":6.07528, "y":0.53108, "heading":1.5708, "vx":4.79452, "vy":0.17569, "omega":0.0, "ax":-0.75059, "ay":11.0708, "alpha":0.0, "fx":[-8.17108,-8.17108,-8.17108,-8.17108], "fy":[120.51913,120.51913,120.51913,120.51913]}, + {"t":0.37267, "x":6.20264, "y":0.53968, "heading":1.5708, "vx":4.77454, "vy":0.47038, "omega":0.0, "ax":-1.74827, "ay":12.89084, "alpha":0.0, "fx":[-19.03208,-19.03208,-19.03208,-19.03208], "fy":[140.33251,140.33251,140.33251,140.33251]}, + {"t":0.39929, "x":6.32911, "y":0.55677, "heading":1.5708, "vx":4.728, "vy":0.81353, "omega":0.0, "ax":-2.85248, "ay":13.39303, "alpha":0.0, "fx":[-31.05271,-31.05271,-31.05271,-31.05271], "fy":[145.79942,145.79942,145.79942,145.79942]}, + {"t":0.42591, "x":6.45396, "y":0.58317, "heading":1.5708, "vx":4.65207, "vy":1.17004, "omega":0.0, "ax":-3.7937, "ay":13.11371, "alpha":0.0, "fx":[-41.29899,-41.29899,-41.29899,-41.29899], "fy":[142.7587,142.7587,142.7587,142.7587]}, + {"t":0.45026, "x":6.56614, "y":0.61555, "heading":1.5708, "vx":4.55967, "vy":1.48944, "omega":0.0, "ax":-4.45079, "ay":12.25822, "alpha":0.0, "fx":[-48.45231,-48.45231,-48.45231,-48.45231], "fy":[133.44564,133.44564,133.44564,133.44564]}, + {"t":0.47462, "x":6.67588, "y":0.65547, "heading":1.5708, "vx":4.45127, "vy":1.78801, "omega":0.0, "ax":-4.61403, "ay":10.5889, "alpha":0.0, "fx":[-50.22934,-50.22934,-50.22934,-50.22934], "fy":[115.27303,115.27303,115.27303,115.27303]}, + {"t":0.49898, "x":6.78292, "y":0.70216, "heading":1.5708, "vx":4.33888, "vy":2.04592, "omega":0.0, "ax":-3.53358, "ay":7.12125, "alpha":0.0, "fx":[-38.46737,-38.46737,-38.46737,-38.46737], "fy":[77.5235,77.5235,77.5235,77.5235]}, + {"t":0.52333, "x":6.88756, "y":0.7541, "heading":1.5708, "vx":4.25282, "vy":2.21936, "omega":0.0, "ax":-1.5681, "ay":2.94447, "alpha":0.0, "fx":[-17.07068,-17.07068,-17.07068,-17.07068], "fy":[32.05412,32.05412,32.05412,32.05412]}, + {"t":0.54769, "x":6.99067, "y":0.80903, "heading":1.5708, "vx":4.21462, "vy":2.29108, "omega":0.0, "ax":-0.50675, "ay":0.92636, "alpha":0.0, "fx":[-5.51661,-5.51661,-5.51661,-5.51661], "fy":[10.0846,10.0846,10.0846,10.0846]}, + {"t":0.57204, "x":7.09318, "y":0.86511, "heading":1.5708, "vx":4.20228, "vy":2.31364, "omega":0.0, "ax":-0.15399, "ay":0.27917, "alpha":0.0, "fx":[-1.67637,-1.67637,-1.67637,-1.67637], "fy":[3.03906,3.03906,3.03906,3.03906]}, + {"t":0.5964, "x":7.19548, "y":0.92154, "heading":1.5708, "vx":4.19853, "vy":2.32044, "omega":0.0, "ax":-0.04637, "ay":0.08385, "alpha":0.0, "fx":[-0.50478,-0.50478,-0.50478,-0.50478], "fy":[0.91281,0.91281,0.91281,0.91281]}, + {"t":0.62076, "x":7.29773, "y":0.97809, "heading":1.5708, "vx":4.1974, "vy":2.32249, "omega":0.0, "ax":-0.01394, "ay":0.02518, "alpha":0.0, "fx":[-0.15172,-0.15172,-0.15172,-0.15172], "fy":[0.27416,0.27416,0.27416,0.27416]}, + {"t":0.64511, "x":7.39996, "y":1.03466, "heading":1.5708, "vx":4.19706, "vy":2.3231, "omega":0.0, "ax":-0.00419, "ay":0.00756, "alpha":0.0, "fx":[-0.04558,-0.04558,-0.04558,-0.04558], "fy":[0.08234,0.08234,0.08234,0.08234]}, + {"t":0.66947, "x":7.50219, "y":1.09125, "heading":1.5708, "vx":4.19696, "vy":2.32328, "omega":0.0, "ax":-0.00126, "ay":0.00227, "alpha":0.0, "fx":[-0.01366,-0.01366,-0.01366,-0.01366], "fy":[0.02468,0.02468,0.02468,0.02468]}, + {"t":0.69383, "x":7.60441, "y":1.14783, "heading":1.5708, "vx":4.19693, "vy":2.32334, "omega":0.0, "ax":-0.00065, "ay":0.00051, "alpha":0.0, "fx":[-0.00708,-0.00708,-0.00708,-0.00708], "fy":[0.00552,0.00552,0.00552,0.00552]}, + {"t":0.71818, "x":7.70663, "y":1.20442, "heading":1.5708, "vx":4.19691, "vy":2.32335, "omega":0.0, "ax":-0.28119, "ay":-0.15548, "alpha":0.0, "fx":[-3.06114,-3.06114,-3.06114,-3.06114], "fy":[-1.69256,-1.69256,-1.69256,-1.69256]}, + {"t":0.74254, "x":7.80877, "y":1.26096, "heading":1.5708, "vx":4.19007, "vy":2.31956, "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/FuelToShot.traj b/src/main/deploy/choreo/FuelToShot.traj deleted file mode 100644 index 74e9b72..0000000 --- a/src/main/deploy/choreo/FuelToShot.traj +++ /dev/null @@ -1,119 +0,0 @@ -{ - "name":"FuelToShot", - "version":3, - "snapshot":{ - "waypoints":[ - {"x":7.80876998901367, "y":3.5819289684295654, "heading":1.5707963267948966, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.657511234283447, "y":2.5716681480407715, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":3.3354413509368896, "y":3.0580899715423584, "heading":3.7524578917878086, "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.541, "h":8.0692}}, "enabled":false}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.657511234283447 m", "val":4.657511234283447}, "y":{"exp":"2.5716681480407715 m", "val":2.5716681480407715}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"3.3354413509368896 m", "val":3.3354413509368896}, "y":{"exp":"3.0580899715423584 m", "val":3.0580899715423584}, "heading":{"exp":"215 deg", "val":3.7524578917878086}, "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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "config":{ - "frontLeft":{ - "x":0.2794, - "y":0.2794 - }, - "backLeft":{ - "x":-0.2794, - "y":0.2794 - }, - "mass":68.0388555, - "inertia":6.0, - "gearing":6.5, - "radius":0.0508, - "vmax":628.3185307179587, - "tmax":1.2, - "cof":1.5, - "bumper":{ - "front":0.4064, - "side":0.4064, - "back":0.4064 - }, - "differentialTrackWidth":0.5588 - }, - "sampleType":"Swerve", - "waypoints":[0.0,0.97983,1.55968], - "samples":[ - {"t":0.0, "x":7.80877, "y":3.58193, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.6936, "ay":-3.51677, "alpha":10.04437, "fx":[-130.15898,-88.89209,-151.40065,-153.01204], "fy":[-81.15222,-124.89633,-22.94009,-10.28804]}, - {"t":0.02882, "x":7.80558, "y":3.58047, "heading":1.5708, "vx":-0.22172, "vy":-0.10135, "omega":0.28946, "ax":-7.7102, "ay":-3.53963, "alpha":9.80757, "fx":[-130.28049,-90.28188,-151.10441,-152.92663], "fy":[-80.93624,-123.87635,-24.7011,-11.31874]}, - {"t":0.05764, "x":7.79598, "y":3.57608, "heading":1.57914, "vx":-0.44392, "vy":-0.20336, "omega":0.5721, "ax":-7.72544, "ay":-3.56164, "alpha":9.58937, "fx":[-130.20168,-91.71596,-150.91391,-152.79855], "fy":[-81.03905,-122.79516,-25.72732,-12.76813]}, - {"t":0.08646, "x":7.77998, "y":3.56874, "heading":1.59563, "vx":-0.66655, "vy":-0.306, "omega":0.84846, "ax":-7.74028, "ay":-3.5839, "alpha":9.37463, "fx":[-129.94384,-93.26599,-150.8142,-152.6156], "fy":[-81.42409,-121.59392,-26.19208,-14.63447]}, - {"t":0.11527, "x":7.75756, "y":3.55843, "heading":1.62008, "vx":-0.88962, "vy":-0.40928, "omega":1.11862, "ax":-7.75572, "ay":-3.60773, "alpha":9.14555, "fx":[-129.53146,-95.02357,-150.77393,-152.3611], "fy":[-82.04605,-120.19122,-26.30443,-16.92388]}, - {"t":0.14409, "x":7.7287, "y":3.54514, "heading":1.65231, "vx":-1.11312, "vy":-0.51325, "omega":1.38218, "ax":-7.77296, "ay":-3.63438, "alpha":8.88008, "fx":[-128.99613,-97.10226,-150.7515,-152.0131], "fy":[-82.84674,-118.4765,-26.30579,-19.65034]}, - {"t":0.17291, "x":7.6934, "y":3.52884, "heading":1.69215, "vx":-1.33713, "vy":-0.61799, "omega":1.63809, "ax":-7.79351, "ay":-3.66488, "alpha":8.55023, "fx":[-128.3823,-99.63751,-150.69864,-151.54307], "fy":[-83.74856,-116.3013,-26.46853,-22.83607]}, - {"t":0.20173, "x":7.65162, "y":3.50951, "heading":1.73935, "vx":-1.56173, "vy":-0.7236, "omega":1.8845, "ax":-7.81922, "ay":-3.69968, "alpha":8.12089, "fx":[-127.75608,-102.78149,-150.55946,-150.91376], "fy":[-84.64408,-113.46911,-27.09613,-26.51298]}, - {"t":0.23055, "x":7.60337, "y":3.48712, "heading":1.79366, "vx":-1.78707, "vy":-0.83022, "omega":2.11853, "ax":-7.85204, "ay":-3.73843, "alpha":7.54942, "fx":[-127.21897,-106.68682,-150.2625,-150.07565], "fy":[-85.37794,-109.72662,-28.52732,-30.72634]}, - {"t":0.25937, "x":7.54861, "y":3.46164, "heading":1.85472, "vx":-2.01335, "vy":-0.93796, "omega":2.3361, "ax":-7.89348, "ay":-3.77976, "alpha":6.78634, "fx":[-126.93045,-111.47098,-149.70179,-148.96009], "fy":[-85.71374,-104.76446,-31.14815,-35.54386]}, - {"t":0.28819, "x":7.48731, "y":3.43304, "heading":1.92204, "vx":-2.24083, "vy":-1.04689, "omega":2.53167, "ax":-7.94346, "ay":-3.82148, "alpha":5.7757, "fx":[-127.14646,-117.15383,-148.69919,-147.46456], "fy":[-85.26745,-98.24475,-35.4197,-41.07734]}, - {"t":0.317, "x":7.41943, "y":3.40128, "heading":1.995, "vx":-2.46975, "vy":-1.15702, "omega":2.69812, "ax":-7.99846, "ay":-3.8612, "alpha":4.4496, "fx":[-128.2876,-123.57209,-146.92973,-145.41662], "fy":[-83.35985,-89.88326,-41.93289,-47.53538]}, - {"t":0.34582, "x":7.34494, "y":3.36634, "heading":2.07275, "vx":-2.70025, "vy":-1.26829, "omega":2.82635, "ax":-8.04837, "ay":-3.89667, "alpha":2.69798, "fx":[-131.05709,-130.30796,-143.76434,-142.47229], "fy":[-78.64893,-79.61002,-51.50389,-55.36188]}, - {"t":0.37464, "x":7.26378, "y":3.32817, "heading":2.1542, "vx":-2.9322, "vy":-1.38059, "omega":2.9041, "ax":-8.06854, "ay":-3.92171, "alpha":0.25819, "fx":[-136.57829,-136.71267,-137.91629,-137.76672], "fy":[-68.08908,-67.78094,-65.30236,-65.65621]}, - {"t":0.40346, "x":7.17593, "y":3.28675, "heading":2.2379, "vx":-3.16472, "vy":-1.49361, "omega":2.91154, "ax":-7.9776, "ay":-3.90405, "alpha":-3.65444, "fx":[-145.80313,-142.09478,-126.64047,-128.24841], "fy":[-43.67842,-55.3093,-84.84427,-81.79487]}, - {"t":0.43228, "x":7.08141, "y":3.24209, "heading":2.3218, "vx":-3.39462, "vy":-1.60611, "omega":2.80622, "ax":-7.27827, "ay":-3.78453, "alpha":-11.73741, "fx":[-150.85755,-145.99941,-104.36734,-93.98054], "fy":[14.54999,-43.56808,-110.81748,-117.65928]}, - {"t":0.4611, "x":6.98056, "y":3.19423, "heading":2.40267, "vx":-3.60437, "vy":-1.71518, "omega":2.46797, "ax":-5.89335, "ay":-3.71453, "alpha":-19.63874, "fx":[-139.70958,-146.70529,-85.92334,-28.63827], "fy":[57.54382,-40.3294,-125.27404,-144.67263]}, - {"t":0.48992, "x":6.87424, "y":3.14326, "heading":2.4738, "vx":-3.77421, "vy":-1.82223, "omega":1.90201, "ax":-5.90736, "ay":-3.71124, "alpha":-18.95585, "fx":[-139.30361,-145.43621,-82.4816,-34.70892], "fy":[56.93394,-43.15418,-126.84503,-139.44293]}, - {"t":0.51873, "x":6.76302, "y":3.0892, "heading":2.52861, "vx":-3.94445, "vy":-1.92918, "omega":1.35573, "ax":-5.92284, "ay":-3.59371, "alpha":-18.31018, "fx":[-137.98948,-144.04611,-79.55219,-41.39554], "fy":[56.80953,-44.71211,-127.31451,-129.29505]}, - {"t":0.54755, "x":6.64689, "y":3.03211, "heading":2.56768, "vx":-4.11514, "vy":-2.03274, "omega":0.82805, "ax":-5.93622, "ay":-3.13679, "alpha":-17.52844, "fx":[-134.08386,-141.69436,-77.62037,-50.49484], "fy":[56.77378,-43.85045,-124.57201,-101.77499]}, - {"t":0.57637, "x":6.52583, "y":2.97223, "heading":2.59154, "vx":-4.28621, "vy":-2.12314, "omega":0.32291, "ax":-4.64913, "ay":0.24833, "alpha":-10.60684, "fx":[-86.88141,-107.07445,-74.99846,-47.3669], "fy":[48.70468,-6.31834,-47.56007,22.0697]}, - {"t":0.60519, "x":6.40038, "y":2.91115, "heading":2.60085, "vx":-4.42019, "vy":-2.11599, "omega":0.01723, "ax":-1.27182, "ay":2.64722, "alpha":-0.08014, "fx":[-21.68753,-21.95414,-21.57935,-21.31249], "fy":[45.29782,44.93531,44.75876,45.12199]}, - {"t":0.63401, "x":6.27246, "y":2.85127, "heading":2.60135, "vx":-4.45685, "vy":-2.0397, "omega":0.01492, "ax":-1.79874, "ay":4.07116, "alpha":-0.00131, "fx":[-30.59662,-30.60192,-30.59556,-30.59026], "fy":[69.25313,69.24744,69.24551,69.2512]}, - {"t":0.66283, "x":6.14328, "y":2.79418, "heading":2.60178, "vx":-4.50868, "vy":-1.92237, "omega":0.01489, "ax":-2.40441, "ay":5.95358, "alpha":0.00236, "fx":[-40.89848,-40.88578,-40.89831,-40.91102], "fy":[101.26393,101.27313,101.27345,101.26425]}, - {"t":0.69165, "x":6.01234, "y":2.74125, "heading":2.60221, "vx":-4.57798, "vy":-1.7508, "omega":0.01495, "ax":-2.55912, "ay":7.18304, "alpha":0.00862, "fx":[-43.53177,-43.47695,-43.52788,-43.5827], "fy":[122.17086,122.19933,122.19205,122.16357]}, - {"t":0.72046, "x":5.87935, "y":2.69378, "heading":2.60264, "vx":-4.65173, "vy":-1.54379, "omega":0.0152, "ax":-2.39129, "ay":7.86152, "alpha":0.01783, "fx":[-40.67763,-40.55673,-40.67281,-40.79367], "fy":[133.70933,133.75759,133.73502,133.6867]}, - {"t":0.74928, "x":5.7443, "y":2.65255, "heading":2.60308, "vx":-4.72064, "vy":-1.31724, "omega":0.01572, "ax":-2.06413, "ay":8.26281, "alpha":0.04795, "fx":[-35.10482,-34.77648,-35.1159,-35.44409], "fy":[140.52896,140.63128,140.5674,140.46442]}, - {"t":0.7781, "x":5.6074, "y":2.61802, "heading":2.60353, "vx":-4.78013, "vy":-1.07911, "omega":0.0171, "ax":-1.56861, "ay":8.53798, "alpha":0.33783, "fx":[-26.50395,-24.2608,-26.85732,-29.10415], "fy":[145.17269,145.67054,145.30313,144.7678]}, - {"t":0.80692, "x":5.46899, "y":2.59047, "heading":2.60402, "vx":-4.82533, "vy":-0.83306, "omega":0.02684, "ax":-0.38595, "ay":8.74393, "alpha":2.28139, "fx":[-2.73729,9.53713,-9.69311,-23.36662], "fy":[148.82934,149.17702,149.46441,147.45627]}, - {"t":0.83574, "x":5.32977, "y":2.57009, "heading":2.60479, "vx":-4.83645, "vy":-0.58107, "omega":0.09258, "ax":1.31637, "ay":8.58332, "alpha":5.48238, "fx":[42.53098,54.68035,10.14774,-17.79509], "fy":[143.39331,140.60363,150.77573,149.22672]}, - {"t":0.86456, "x":5.19094, "y":2.55691, "heading":2.60746, "vx":-4.79852, "vy":-0.33371, "omega":0.25058, "ax":2.46988, "ay":8.2309, "alpha":7.47844, "fx":[77.33111,77.67208,22.72291,-9.67856], "fy":[129.00388,130.27977,150.10142,150.63598]}, - {"t":0.89338, "x":5.05368, "y":2.55071, "heading":2.61468, "vx":-4.72734, "vy":-0.09651, "omega":0.46609, "ax":3.14945, "ay":7.94062, "alpha":8.49559, "fx":[96.69237,88.69928,30.44796,-1.5548], "fy":[116.11669,123.61214,149.13378,151.40787]}, - {"t":0.92219, "x":4.91875, "y":2.55123, "heading":2.62812, "vx":-4.63658, "vy":0.13233, "omega":0.71092, "ax":3.54982, "ay":7.74499, "alpha":8.99984, "fx":[107.24597,94.34079,34.98608,4.95293], "fy":[107.15322,119.73399,148.3975,151.67554]}, - {"t":0.95101, "x":4.78661, "y":2.55826, "heading":2.6486, "vx":-4.53428, "vy":0.35553, "omega":0.97029, "ax":3.79135, "ay":7.62162, "alpha":9.24356, "fx":[113.34029,97.19646,37.43086,9.99144], "fy":[101.22028,117.68403,147.98171,151.6801]}, - {"t":0.97983, "x":4.65751, "y":2.57167, "heading":2.67657, "vx":-4.42501, "vy":0.57517, "omega":1.23667, "ax":4.2214, "ay":7.23872, "alpha":10.47527, "fx":[126.70908,104.13722,41.09032,15.28254], "fy":[83.44697,111.41965,146.84396,150.80336]}, - {"t":1.00303, "x":4.55601, "y":2.58696, "heading":2.70525, "vx":-4.3271, "vy":0.74306, "omega":1.47964, "ax":4.77157, "ay":6.5771, "alpha":12.60323, "fx":[142.63058,114.26805,46.00676,21.74687], "fy":[51.6953,101.00095,145.3138,149.48859]}, - {"t":1.02622, "x":4.45693, "y":2.60596, "heading":2.73957, "vx":-4.21643, "vy":0.89561, "omega":1.77195, "ax":5.15767, "ay":5.88243, "alpha":14.57376, "fx":[150.73136,122.66761,49.25717,28.26588], "fy":[17.97027,90.63673,144.17739,147.44972]}, - {"t":1.04941, "x":4.36053, "y":2.62831, "heading":2.78067, "vx":-4.09681, "vy":1.03205, "omega":2.10998, "ax":6.2956, "ay":4.98944, "alpha":12.48901, "fx":[151.96753,130.37838,63.02833,82.97146], "fy":[-2.15085,79.14145,138.58636,123.89859]}, - {"t":1.07261, "x":4.2672, "y":2.65359, "heading":2.8296, "vx":-3.95079, "vy":1.14777, "omega":2.39964, "ax":7.85098, "ay":3.27467, "alpha":8.62329, "fx":[152.14046,139.05076,100.70041,142.27995], "fy":[-3.36145,62.58218,113.98846,49.59565]}, - {"t":1.0958, "x":4.17768, "y":2.6811, "heading":2.88526, "vx":-3.76869, "vy":1.22373, "omega":2.59965, "ax":8.57521, "ay":1.84536, "alpha":5.62484, "fx":[152.27019,146.44101,133.74192,150.99434], "fy":[-3.93398,42.43311,72.45439,14.60263]}, - {"t":1.119, "x":4.09257, "y":2.70998, "heading":2.94556, "vx":-3.5698, "vy":1.26653, "omega":2.73011, "ax":8.89874, "ay":0.62349, "alpha":2.56348, "fx":[152.34596,151.25514,149.65064,152.20829], "fy":[-4.95498,19.2054,28.17653,-0.00569]}, - {"t":1.14219, "x":4.01217, "y":2.73952, "heading":3.00888, "vx":-3.3634, "vy":1.28099, "omega":2.78957, "ax":8.95838, "ay":-0.35592, "alpha":0.12107, "fx":[152.35534,152.40748,152.40318,152.35161], "fy":[-6.75369,-5.47453,-5.34903,-6.63889]}, - {"t":1.16538, "x":3.93657, "y":2.76913, "heading":3.07358, "vx":-3.15562, "vy":1.27273, "omega":2.79238, "ax":8.88675, "ay":-1.10894, "alpha":-1.76699, "fx":[152.268,149.74407,150.25726,152.37469], "fy":[-9.50005,-29.23734,-27.10562,-9.60785]}, - {"t":1.18858, "x":3.86577, "y":2.79836, "heading":3.13835, "vx":-2.9495, "vy":1.24701, "omega":2.7514, "ax":8.75967, "ay":-1.68515, "alpha":-3.27007, "fx":[152.03042,144.22711,147.31371,152.42675], "fy":[-13.28184,-50.02643,-40.6703,-10.67714]}, - {"t":1.21177, "x":3.79971, "y":2.82683, "heading":-3.08102, "vx":-2.74633, "vy":1.20793, "omega":2.67555, "ax":8.61627, "ay":-2.1283, "alpha":-4.45378, "fx":[151.56525,137.32915,144.83256,152.51414], "fy":[-18.12784,-66.87868,-49.10848,-10.69214]}, - {"t":1.23496, "x":3.73833, "y":2.85427, "heading":-3.01897, "vx":-2.54649, "vy":1.15856, "omega":2.57225, "ax":8.47658, "ay":-2.47416, "alpha":-5.35339, "fx":[150.7761,130.30728,143.03316,152.62012], "fy":[-24.00686,-79.86837,-54.35555,-10.10828]}, - {"t":1.25816, "x":3.68155, "y":2.88048, "heading":-2.95931, "vx":-2.34988, "vy":1.10118, "omega":2.44808, "ax":8.34865, "ay":-2.75133, "alpha":-6.01859, "fx":[149.55887,123.90832,141.8365,152.72918], "fy":[-30.82135,-89.60971,-57.57371,-9.19245]}, - {"t":1.28135, "x":3.62929, "y":2.90528, "heading":-2.90252, "vx":-2.15625, "vy":1.03736, "omega":2.30849, "ax":8.23343, "ay":-2.98096, "alpha":-6.5122, "fx":[147.81902,118.4376,141.10527,152.8311], "fy":[-38.40527,-96.83368,-59.47203,-8.11037]}, - {"t":1.30455, "x":3.58149, "y":2.92853, "heading":-2.84898, "vx":-1.96528, "vy":0.96822, "omega":2.15745, "ax":8.12855, "ay":-3.17733, "alpha":-6.89681, "fx":[145.49214,113.93265,140.71224,152.92043], "fy":[-46.53076,-102.1839,-60.49818,-6.9692]}, - {"t":1.32774, "x":3.5381, "y":2.95014, "heading":-2.79894, "vx":-1.77675, "vy":0.89453, "omega":1.99748, "ax":8.03095, "ay":-3.34918, "alpha":-7.2246, "fx":[142.56309,110.30341,140.55499,152.99513], "fy":[-54.92559,-106.163,-60.94587,-5.84022]}, - {"t":1.35093, "x":3.49905, "y":2.96998, "heading":-2.75261, "vx":-1.59048, "vy":0.81685, "omega":1.82992, "ax":7.93815, "ay":-3.50141, "alpha":-7.5326, "fx":[139.07752,107.41469,140.5549,153.05537], "fy":[-63.30052,-109.14393,-61.01541,-4.77179]}, - {"t":1.37413, "x":3.46429, "y":2.98799, "heading":-2.71017, "vx":-1.40636, "vy":0.73564, "omega":1.65521, "ax":7.84879, "ay":-3.63654, "alpha":-7.84202, "fx":[135.1411,105.12639,140.65252,153.10264], "fy":[-71.38147,-111.39879,-60.84882,-3.79677]}, - {"t":1.39732, "x":3.43379, "y":3.00407, "heading":-2.67178, "vx":-1.22432, "vy":0.65129, "omega":1.47332, "ax":7.76259, "ay":-3.75598, "alpha":-8.1604, "fx":[130.90595,103.31008,140.80288,153.13905], "fy":[-78.93868,-113.12645,-60.55063,-2.93702]}, - {"t":1.42052, "x":3.40748, "y":3.01817, "heading":-2.63761, "vx":-1.04427, "vy":0.56418, "omega":1.28405, "ax":7.68004, "ay":-3.86083, "alpha":-8.48527, "fx":[126.54816,101.85399,140.97172,153.16696], "fy":[-85.80579,-114.47403,-60.20052,-2.20603]}, - {"t":1.44371, "x":3.38532, "y":3.03021, "heading":-2.60783, "vx":-0.86614, "vy":0.47463, "omega":1.08724, "ax":7.60195, "ay":-3.95229, "alpha":-8.80826, "fx":[122.24363,100.66316,141.13263,153.18858], "fy":[-91.885,-115.55228,-59.86105,-1.61077]}, - {"t":1.4669, "x":3.36728, "y":3.04016, "heading":-2.58261, "vx":-0.68982, "vy":0.38296, "omega":0.88294, "ax":7.52919, "ay":-4.03183, "alpha":-9.11872, "fx":[118.14887,99.65775,141.26496,153.20584], "fy":[-97.13999,-116.44596,-59.58244,-1.15304]}, - {"t":1.4901, "x":3.3533, "y":3.04796, "heading":-2.56213, "vx":-0.51519, "vy":0.28945, "omega":0.67144, "ax":7.46241, "ay":-4.10117, "alpha":-9.4063, "fx":[114.39015,98.77106,141.3523,153.22029], "fy":[-101.58107,-117.22097,-59.40562,-0.83093]}, - {"t":1.51329, "x":3.34336, "y":3.05357, "heading":-2.54656, "vx":-0.34211, "vy":0.19432, "omega":0.45328, "ax":7.40199, "ay":-4.16205, "alpha":-9.66237, "fx":[111.06109,97.94758,141.3813,153.23301], "fy":[-105.24789,-117.92895,-59.36414,-0.64021]}, - {"t":1.53649, "x":3.33742, "y":3.05696, "heading":-2.53604, "vx":-0.17043, "vy":0.09779, "omega":0.22917, "ax":7.34806, "ay":-4.2162, "alpha":-9.8805, "fx":[108.22654,97.14143,141.34079,153.2447], "fy":[-108.19366,-118.61043,-59.48542,-0.57592]}, - {"t":1.55968, "x":3.33544, "y":3.05809, "heading":-2.53073, "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/FuelToShotB.traj b/src/main/deploy/choreo/FuelToShotB.traj new file mode 100644 index 0000000..b064aa0 --- /dev/null +++ b/src/main/deploy/choreo/FuelToShotB.traj @@ -0,0 +1,92 @@ +{ + "name":"FuelToShotB", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":7.685800075531006, "y":3.391429901123047, "heading":1.5707963267948966, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.5568366050720215, "y":2.3671467304229736, "heading":1.5707963267948966, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.662621021270752, "y":2.4298689365386963, "heading":1.5707963267948966, "intervals":15, "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.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.685800075531006 m", "val":7.685800075531006}, "y":{"exp":"3.391429901123047 m", "val":3.391429901123047}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.5568366050720215 m", "val":5.5568366050720215}, "y":{"exp":"2.3671467304229736 m", "val":2.3671467304229736}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.662621021270752 m", "val":3.662621021270752}, "y":{"exp":"2.4298689365386963 m", "val":2.4298689365386963}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":15, "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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":43.54486752, + "inertia":6.0, + "gearing":6.5, + "radius":0.0497078, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.55245, + "side":0.3302, + "back":0.33654999999999996 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.66067,1.22358], + "samples":[ + {"t":0.0, "x":7.6858, "y":3.39143, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.78999, "ay":-6.61306, "alpha":0.0, "fx":[-139.23463,-139.23463,-139.23463,-139.23463], "fy":[-71.99122,-71.99122,-71.99122,-71.99122]}, + {"t":0.03886, "x":7.67614, "y":3.38644, "heading":1.5708, "vx":-0.49706, "vy":-0.257, "omega":0.0, "ax":-12.78799, "ay":-6.61202, "alpha":0.0, "fx":[-139.2128,-139.2128,-139.2128,-139.2128], "fy":[-71.97993,-71.97993,-71.97993,-71.97993]}, + {"t":0.07773, "x":7.64717, "y":3.37146, "heading":1.5708, "vx":-0.99403, "vy":-0.51396, "omega":0.0, "ax":-12.78531, "ay":-6.61064, "alpha":0.0, "fx":[-139.18369,-139.18369,-139.18369,-139.18369], "fy":[-71.96488,-71.96488,-71.96488,-71.96488]}, + {"t":0.11659, "x":7.59888, "y":3.34649, "heading":1.5708, "vx":-1.49091, "vy":-0.77087, "omega":0.0, "ax":-12.78157, "ay":-6.60871, "alpha":0.0, "fx":[-139.14295,-139.14295,-139.14295,-139.14295], "fy":[-71.94382,-71.94382,-71.94382,-71.94382]}, + {"t":0.15545, "x":7.53129, "y":3.31154, "heading":1.5708, "vx":-1.98763, "vy":-1.02771, "omega":0.0, "ax":-12.77596, "ay":-6.60581, "alpha":0.0, "fx":[-139.08187,-139.08187,-139.08187,-139.08187], "fy":[-71.91224,-71.91224,-71.91224,-71.91224]}, + {"t":0.19431, "x":7.4444, "y":3.26661, "heading":1.5708, "vx":-2.48414, "vy":-1.28443, "omega":0.0, "ax":-12.76662, "ay":-6.60098, "alpha":0.0, "fx":[-138.98017,-138.98017,-138.98017,-138.98017], "fy":[-71.85965,-71.85965,-71.85965,-71.85965]}, + {"t":0.23318, "x":7.33821, "y":3.21171, "heading":1.5708, "vx":-2.98029, "vy":-1.54096, "omega":0.0, "ax":-12.74798, "ay":-6.59134, "alpha":0.0, "fx":[-138.77729,-138.77729,-138.77729,-138.77729], "fy":[-71.75475,-71.75475,-71.75475,-71.75475]}, + {"t":0.27204, "x":7.21277, "y":3.14685, "heading":1.5708, "vx":-3.47571, "vy":-1.79712, "omega":0.0, "ax":-12.69257, "ay":-6.56269, "alpha":0.0, "fx":[-138.17405,-138.17405,-138.17405,-138.17405], "fy":[-71.44284,-71.44284,-71.44284,-71.44284]}, + {"t":0.3109, "x":7.0681, "y":3.07205, "heading":1.5708, "vx":-3.96898, "vy":-2.05216, "omega":0.0, "ax":-7.58536, "ay":-3.92199, "alpha":0.0, "fx":[-82.5759,-82.5759,-82.5759,-82.5759], "fy":[-42.6956,-42.6956,-42.6956,-42.6956]}, + {"t":0.34977, "x":6.90813, "y":2.98934, "heading":1.5708, "vx":-4.26377, "vy":-2.20458, "omega":0.0, "ax":-0.0019, "ay":-0.00079, "alpha":0.0, "fx":[-0.02067,-0.02067,-0.02067,-0.02067], "fy":[-0.0086,-0.0086,-0.0086,-0.0086]}, + {"t":0.38863, "x":6.74243, "y":2.90366, "heading":1.5708, "vx":-4.26384, "vy":-2.20461, "omega":0.0, "ax":-0.00067, "ay":0.00129, "alpha":0.0, "fx":[-0.00724,-0.00724,-0.00724,-0.00724], "fy":[0.014,0.014,0.014,0.014]}, + {"t":0.42749, "x":6.57672, "y":2.81798, "heading":1.5708, "vx":-4.26387, "vy":-2.20456, "omega":0.0, "ax":-0.00568, "ay":0.01099, "alpha":0.0, "fx":[-0.06183,-0.06183,-0.06183,-0.06183], "fy":[0.11959,0.11959,0.11959,0.11959]}, + {"t":0.46635, "x":6.41101, "y":2.73232, "heading":1.5708, "vx":-4.26409, "vy":-2.20413, "omega":0.0, "ax":-0.04848, "ay":0.09388, "alpha":0.0, "fx":[-0.52772,-0.52772,-0.52772,-0.52772], "fy":[1.02201,1.02201,1.02201,1.02201]}, + {"t":0.50522, "x":6.24526, "y":2.64673, "heading":1.5708, "vx":-4.26597, "vy":-2.20049, "omega":0.0, "ax":-0.4093, "ay":0.8007, "alpha":0.0, "fx":[-4.45571,-4.45571,-4.45571,-4.45571], "fy":[8.71662,8.71662,8.71662,8.71662]}, + {"t":0.54408, "x":6.07916, "y":2.56182, "heading":1.5708, "vx":-4.28188, "vy":-2.16937, "omega":0.0, "ax":-2.68101, "ay":5.64281, "alpha":0.0, "fx":[-29.18604,-29.18604,-29.18604,-29.18604], "fy":[61.42881,61.42881,61.42881,61.42881]}, + {"t":0.58294, "x":5.91073, "y":2.48177, "heading":1.5708, "vx":-4.38607, "vy":-1.95007, "omega":0.0, "ax":-4.54761, "ay":11.83879, "alpha":0.0, "fx":[-49.50624,-49.50624,-49.50624,-49.50624], "fy":[128.8796,128.8796,128.8796,128.8796]}, + {"t":0.62181, "x":5.73684, "y":2.41492, "heading":1.5708, "vx":-4.5628, "vy":-1.48998, "omega":0.0, "ax":-3.55354, "ay":13.41151, "alpha":0.0, "fx":[-38.68464,-38.68464,-38.68464,-38.68464], "fy":[146.00065,146.00065,146.00065,146.00065]}, + {"t":0.66067, "x":5.55684, "y":2.36715, "heading":1.5708, "vx":-4.70091, "vy":-0.96877, "omega":0.0, "ax":-2.06163, "ay":13.71001, "alpha":0.0, "fx":[-22.44339,-22.44339,-22.44339,-22.44339], "fy":[149.25012,149.25012,149.25012,149.25012]}, + {"t":0.6982, "x":5.37897, "y":2.34045, "heading":1.5708, "vx":-4.77827, "vy":-0.45427, "omega":0.0, "ax":-0.57589, "ay":12.73117, "alpha":0.0, "fx":[-6.2693,-6.2693,-6.2693,-6.2693], "fy":[138.59423,138.59423,138.59423,138.59423]}, + {"t":0.73572, "x":5.19925, "y":2.33236, "heading":1.5708, "vx":-4.79988, "vy":0.02349, "omega":0.0, "ax":0.21638, "ay":6.85002, "alpha":0.0, "fx":[2.35551,2.35551,2.35551,2.35551], "fy":[74.57077,74.57077,74.57077,74.57077]}, + {"t":0.77325, "x":5.01928, "y":2.33807, "heading":1.5708, "vx":-4.79176, "vy":0.28056, "omega":0.0, "ax":0.06962, "ay":1.10757, "alpha":0.0, "fx":[0.75794,0.75794,0.75794,0.75794], "fy":[12.05729,12.05729,12.05729,12.05729]}, + {"t":0.81078, "x":4.8395, "y":2.34938, "heading":1.5708, "vx":-4.78915, "vy":0.32212, "omega":0.0, "ax":0.00949, "ay":0.13989, "alpha":0.0, "fx":[0.10328,0.10328,0.10328,0.10328], "fy":[1.52292,1.52292,1.52292,1.52292]}, + {"t":0.8483, "x":4.65979, "y":2.36156, "heading":1.5708, "vx":-4.7888, "vy":0.32737, "omega":0.0, "ax":0.01354, "ay":0.01728, "alpha":0.0, "fx":[0.14744,0.14744,0.14744,0.14744], "fy":[0.1881,0.1881,0.1881,0.1881]}, + {"t":0.88583, "x":4.48008, "y":2.37386, "heading":1.5708, "vx":-4.78829, "vy":0.32802, "omega":0.0, "ax":12.89673, "ay":-0.88216, "alpha":0.0, "fx":[140.39664,140.39664,140.39664,140.39664], "fy":[-9.60341,-9.60341,-9.60341,-9.60341]}, + {"t":0.92336, "x":4.30947, "y":2.38555, "heading":1.5708, "vx":-4.30431, "vy":0.29491, "omega":0.0, "ax":14.25737, "ay":-0.97677, "alpha":0.0, "fx":[155.20884,155.20884,155.20884,155.20884], "fy":[-10.63335,-10.63335,-10.63335,-10.63335]}, + {"t":0.96089, "x":4.15798, "y":2.39593, "heading":1.5708, "vx":-3.76927, "vy":0.25826, "omega":0.0, "ax":14.31647, "ay":-0.98088, "alpha":0.0, "fx":[155.85224,155.85224,155.85224,155.85224], "fy":[-10.67812,-10.67812,-10.67812,-10.67812]}, + {"t":0.99841, "x":4.02661, "y":2.40493, "heading":1.5708, "vx":-3.23201, "vy":0.22145, "omega":0.0, "ax":14.33728, "ay":-0.98233, "alpha":0.0, "fx":[156.07871,156.07871,156.07871,156.07871], "fy":[-10.69388,-10.69388,-10.69388,-10.69388]}, + {"t":1.03594, "x":3.91542, "y":2.41255, "heading":1.5708, "vx":-2.69397, "vy":0.18458, "omega":0.0, "ax":14.34789, "ay":-0.98307, "alpha":0.0, "fx":[156.19427,156.19427,156.19427,156.19427], "fy":[-10.70192,-10.70192,-10.70192,-10.70192]}, + {"t":1.07347, "x":3.82443, "y":2.41878, "heading":1.5708, "vx":-2.15553, "vy":0.14769, "omega":0.0, "ax":14.35433, "ay":-0.98352, "alpha":0.0, "fx":[156.26434,156.26434,156.26434,156.26434], "fy":[-10.7068,-10.7068,-10.7068,-10.7068]}, + {"t":1.111, "x":3.75364, "y":2.42363, "heading":1.5708, "vx":-1.61685, "vy":0.11078, "omega":0.0, "ax":14.35865, "ay":-0.98382, "alpha":0.0, "fx":[156.31137,156.31137,156.31137,156.31137], "fy":[-10.71007,-10.71007,-10.71007,-10.71007]}, + {"t":1.14852, "x":3.70308, "y":2.4271, "heading":1.5708, "vx":-1.078, "vy":0.07386, "omega":0.0, "ax":14.36175, "ay":-0.98403, "alpha":0.0, "fx":[156.3451,156.3451,156.3451,156.3451], "fy":[-10.71242,-10.71242,-10.71242,-10.71242]}, + {"t":1.18605, "x":3.67274, "y":2.42918, "heading":1.5708, "vx":-0.53905, "vy":0.03693, "omega":0.0, "ax":14.36408, "ay":-0.9842, "alpha":0.0, "fx":[156.37049,156.37049,156.37049,156.37049], "fy":[-10.71418,-10.71418,-10.71418,-10.71418]}, + {"t":1.22358, "x":3.66262, "y":2.42987, "heading":1.5708, "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/FuelToShotNB.traj b/src/main/deploy/choreo/FuelToShotNB.traj new file mode 100644 index 0000000..1e08eb7 --- /dev/null +++ b/src/main/deploy/choreo/FuelToShotNB.traj @@ -0,0 +1,146 @@ +{ + "name":"FuelToShotNB", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":7.80876998901367, "y":3.5819289684295654, "heading":1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.857902526855469, "y":0.5263999700546265, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.289196491241455, "y":0.5263999700546265, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.3809244632720947, "y":1.7185068130493164, "heading":0.0, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.976977825164795, "y":3.2654073238372803, "heading":0.3490658503988659, "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.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.80876998901367 m", "val":7.80876998901367}, "y":{"exp":"3.5819289684295654 m", "val":3.5819289684295654}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.857902526855469 m", "val":5.857902526855469}, "y":{"exp":"0.5263999700546265 m", "val":0.5263999700546265}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.289196491241455 m", "val":3.289196491241455}, "y":{"exp":"0.5263999700546265 m", "val":0.5263999700546265}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.3809244632720947 m", "val":2.3809244632720947}, "y":{"exp":"1.7185068130493164 m", "val":1.7185068130493164}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.976977825164795 m", "val":2.976977825164795}, "y":{"exp":"3.2654073238372803 m", "val":3.2654073238372803}, "heading":{"exp":"20 deg", "val":0.3490658503988659}, "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.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":43.54486752, + "inertia":6.0, + "gearing":6.5, + "radius":0.0497078, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.55245, + "side":0.3302, + "back":0.33654999999999996 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.01461,1.56223,1.89106,2.4261], + "samples":[ + {"t":0.0, "x":7.80877, "y":3.58193, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.15299, "ay":-9.7871, "alpha":-23.18359, "fx":[97.8125,-154.72289,-96.04453,15.6582], "fy":[-121.40282,-24.87609,-123.92771,-155.97122]}, + {"t":0.03273, "x":7.80708, "y":3.57669, "heading":1.5708, "vx":-0.1032, "vy":-0.32032, "omega":-0.75878, "ax":-3.40106, "ay":-9.97657, "alpha":-22.28301, "fx":[88.33749,-154.46073,-96.17774,14.2021], "fy":[-128.27467,-26.26929,-123.79921,-156.08511]}, + {"t":0.06546, "x":7.80188, "y":3.56086, "heading":1.54596, "vx":-0.21451, "vy":-0.64685, "omega":-1.48809, "ax":-3.68214, "ay":-10.0436, "alpha":-21.80488, "fx":[81.17619,-154.49717,-97.86662,10.84917], "fy":[-132.81428,-25.77652,-122.4342,-156.32204]}, + {"t":0.09819, "x":7.79289, "y":3.53431, "heading":1.49726, "vx":-0.33502, "vy":-0.97557, "omega":-2.20174, "ax":-4.00424, "ay":-10.01064, "alpha":-21.64953, "fx":[75.84319,-154.76065,-101.00668,5.55994], "fy":[-135.86198,-23.68651,-119.80696,-156.55654]}, + {"t":0.13092, "x":7.77978, "y":3.49702, "heading":1.4252, "vx":-0.46608, "vy":-1.30321, "omega":-2.91031, "ax":-4.42347, "ay":-9.91427, "alpha":-21.57374, "fx":[69.89566,-155.11112,-105.50051,-1.9035], "fy":[-138.92659,-20.41866,-115.78989,-156.58033]}, + {"t":0.16365, "x":7.76216, "y":3.44905, "heading":1.32994, "vx":-0.61086, "vy":-1.62769, "omega":-3.61641, "ax":-5.03118, "ay":-9.79222, "alpha":-21.21622, "fx":[59.69731,-155.3327,-111.26205,-12.18458], "fy":[-143.41091,-16.86022,-110.12036,-156.00927]}, + {"t":0.19638, "x":7.73947, "y":3.39054, "heading":1.21158, "vx":-0.77552, "vy":-1.94819, "omega":-4.3108, "ax":-6.04428, "ay":-9.68942, "alpha":-19.7959, "fx":[37.41211,-155.02904,-118.27479,-27.30585], "fy":[-150.27953,-15.57687,-102.21821,-153.84987]}, + {"t":0.2291, "x":7.71085, "y":3.32158, "heading":1.07049, "vx":-0.97335, "vy":-2.26531, "omega":-4.9587, "ax":-8.5892, "ay":-9.41402, "alpha":-13.74425, "fx":[-35.41548,-152.03362,-127.24115,-59.32516], "fy":[-149.12791,-27.6762,-89.48589,-143.64216]}, + {"t":0.26183, "x":7.67439, "y":3.2424, "heading":0.9082, "vx":-1.25446, "vy":-2.57343, "omega":-5.40854, "ax":-10.76443, "ay":-0.11114, "alpha":17.22048, "fx":[-151.32296,-102.4152,-93.76069,-121.23684], "fy":[-14.41894,-109.39564,30.30118,88.67377]}, + {"t":0.29456, "x":7.62757, "y":3.15812, "heading":0.73118, "vx":-1.60678, "vy":-2.57706, "omega":-4.84493, "ax":-5.87942, "ay":-5.74121, "alpha":26.23896, "fx":[-138.56766,-57.66021,59.17966,-118.97037], "fy":[-52.25154,-136.68055,-125.03777,63.96966]}, + {"t":0.32729, "x":7.57183, "y":3.07069, "heading":0.57261, "vx":-1.7992, "vy":-2.76497, "omega":-3.98615, "ax":-3.25079, "ay":-7.29831, "alpha":23.64974, "fx":[-125.14414,-38.09541,72.1237,-50.43944], "fy":[-71.47857,-142.30362,-123.32007,19.29842]}, + {"t":0.36002, "x":7.5112, "y":2.97629, "heading":0.44215, "vx":-1.9056, "vy":-3.00384, "omega":-3.21211, "ax":-1.3849, "ay":-8.36111, "alpha":21.40697, "fx":[-122.07613,-41.93888,64.71567,38.99389], "fy":[-68.73998,-139.21223,-125.95247,-30.17895]}, + {"t":0.39275, "x":7.44809, "y":2.8735, "heading":0.33702, "vx":-1.95093, "vy":-3.27749, "omega":-2.51148, "ax":-1.85443, "ay":-8.15426, "alpha":20.43508, "fx":[-121.08952,-51.34048,51.71388,39.96504], "fy":[-58.15873,-132.09053,-127.15806,-37.66894]}, + {"t":0.42548, "x":7.38325, "y":2.76186, "heading":0.25482, "vx":-2.01162, "vy":-3.54437, "omega":-1.84265, "ax":-2.6192, "ay":-7.29342, "alpha":19.22711, "fx":[-115.90633,-59.20719,37.49497,23.56602], "fy":[-44.51328,-120.71175,-122.45908,-29.90702]}, + {"t":0.45821, "x":7.31601, "y":2.64195, "heading":0.19451, "vx":-2.09735, "vy":-3.78308, "omega":-1.21336, "ax":-2.92605, "ay":-5.81563, "alpha":16.42809, "fx":[-98.10526,-59.62831,22.85157,7.46758], "fy":[-30.07828,-101.4045,-104.70981,-17.0482]}, + {"t":0.49094, "x":7.24579, "y":2.51502, "heading":0.1548, "vx":-2.19311, "vy":-3.97342, "omega":-0.67568, "ax":-1.97145, "ay":-3.17744, "alpha":9.4905, "fx":[-53.66769,-40.58444,8.31777,0.08793], "fy":[-13.72808,-60.47467,-58.06595,-6.09246]}, + {"t":0.52367, "x":7.17296, "y":2.38327, "heading":0.13268, "vx":-2.25764, "vy":-4.07742, "omega":-0.36507, "ax":-0.54414, "ay":-0.7694, "alpha":2.38064, "fx":[-13.15687,-11.37012,1.26669,-0.43439], "fy":[-2.89209,-15.4976,-13.91135,-1.20226]}, + {"t":0.5564, "x":7.09878, "y":2.24941, "heading":0.12074, "vx":-2.27545, "vy":-4.1026, "omega":-0.28715, "ax":-0.0909, "ay":-0.13855, "alpha":0.41084, "fx":[-2.21751,-1.95129,0.23814,-0.0276], "fy":[-0.54634,-2.73559,-2.47043,-0.28067]}, + {"t":0.58913, "x":7.02425, "y":2.11506, "heading":0.11134, "vx":-2.27842, "vy":-4.10713, "omega":-0.2737, "ax":0.00166, "ay":-0.03272, "alpha":0.05856, "fx":[-0.1557,-0.12069,0.19174,0.15674], "fy":[-0.21748,-0.5299,-0.4949,-0.18247]}, + {"t":0.62186, "x":6.94968, "y":1.98062, "heading":0.10238, "vx":-2.27837, "vy":-4.1082, "omega":-0.27179, "ax":0.01936, "ay":-0.01595, "alpha":-0.00174, "fx":[0.21586,0.21491,0.20562,0.20656], "fy":[-0.1778,-0.1685,-0.16945,-0.17875]}, + {"t":0.65458, "x":6.87513, "y":1.84615, "heading":0.09348, "vx":-2.27773, "vy":-4.10873, "omega":-0.27185, "ax":0.02084, "ay":-0.01275, "alpha":-0.01148, "fx":[0.26043,0.25472,0.19334,0.19906], "fy":[-0.16658,-0.1052,-0.11092,-0.17229]}, + {"t":0.68731, "x":6.80059, "y":1.71167, "heading":0.08459, "vx":-2.27705, "vy":-4.10914, "omega":-0.27222, "ax":0.00994, "ay":-0.00868, "alpha":-0.00762, "fx":[0.13034,0.12688,0.08613,0.08958], "fy":[-0.11309,-0.07234,-0.0758,-0.11655]}, + {"t":0.72004, "x":6.72607, "y":1.57717, "heading":0.07568, "vx":-2.27673, "vy":-4.10943, "omega":-0.27247, "ax":-0.05364, "ay":0.0149, "alpha":0.01891, "fx":[-0.63844,-0.63075,-0.52951,-0.53721], "fy":[0.20893,0.10769,0.11539,0.21662]}, + {"t":0.75277, "x":6.65152, "y":1.44268, "heading":0.06676, "vx":-2.27848, "vy":-4.10894, "omega":-0.27185, "ax":-0.40034, "ay":0.16853, "alpha":0.11238, "fx":[-4.67892,-4.63913,-4.03749,-4.07735], "fy":[2.11549,1.51323,1.55383,2.15621]}, + {"t":0.7855, "x":6.57674, "y":1.30829, "heading":0.05786, "vx":-2.29158, "vy":-4.10342, "omega":-0.26817, "ax":-2.21897, "ay":1.15162, "alpha":0.26911, "fx":[-24.88867,-24.84316,-23.42262,-23.47037], "fy":[13.20825,11.74485,11.86347,13.33059]}, + {"t":0.81823, "x":6.50055, "y":1.17461, "heading":0.04908, "vx":-2.36421, "vy":-4.06573, "omega":-0.25937, "ax":-7.54018, "ay":4.82913, "alpha":-0.19204, "fx":[-81.80306,-81.4837,-82.36585,-82.68353], "fy":[52.19076,53.37074,52.94933,51.77281]}, + {"t":0.85096, "x":6.41913, "y":1.04413, "heading":0.0406, "vx":-2.61099, "vy":-3.90768, "omega":-0.26565, "ax":-10.17237, "ay":7.82549, "alpha":-0.63962, "fx":[-110.60226,-108.03483,-110.92324,-113.39401], "fy":[84.58259,88.5943,85.80781,81.7754]}, + {"t":0.88369, "x":6.32822, "y":0.92042, "heading":0.0319, "vx":-2.94393, "vy":-3.65156, "omega":-0.28659, "ax":-10.30802, "ay":9.14425, "alpha":0.06062, "fx":[-112.22221,-112.50765,-112.20885,-111.92248], "fy":[99.56874,99.21711,99.52409,99.87509]}, + {"t":0.91642, "x":6.22635, "y":0.80581, "heading":0.02252, "vx":-3.2813, "vy":-3.35227, "omega":-0.2846, "ax":-9.92032, "ay":9.979, "alpha":1.22903, "fx":[-108.38943,-114.37575,-107.75866,-101.45501], "fy":[108.71868,102.10345,108.74634,114.96564]}, + {"t":0.94915, "x":6.11364, "y":0.70143, "heading":0.01321, "vx":-3.60598, "vy":-3.02567, "omega":-0.24438, "ax":-9.36578, "ay":10.61478, "alpha":2.60444, "fx":[-103.52114,-116.2527,-100.88932,-87.16853], "fy":[115.32746,102.10628,116.9001,127.88517]}, + {"t":0.98188, "x":5.99061, "y":0.60809, "heading":0.00521, "vx":-3.91252, "vy":-2.67825, "omega":-0.15913, "ax":-8.68242, "ay":11.13731, "alpha":4.30315, "fx":[-98.48984,-119.33099,-91.12701,-69.12687], "fy":[120.59507,99.61066,125.4587,139.30804]}, + {"t":1.01461, "x":5.8579, "y":0.5264, "heading":0.0, "vx":-4.19669, "vy":-2.31374, "omega":-0.0183, "ax":-6.45005, "ay":12.72544, "alpha":0.59895, "fx":[-71.46373,-74.04793,-68.9245,-66.43038], "fy":[137.97718,136.54441,139.16671,140.43944]}, + {"t":1.04503, "x":5.72724, "y":0.4619, "heading":-0.00056, "vx":-4.39292, "vy":-1.92659, "omega":-0.00007, "ax":-5.13769, "ay":13.24346, "alpha":0.04673, "fx":[-56.06284,-56.22783,-55.79671,-55.6326], "fy":[144.12484,144.05333,144.21805,144.28867]}, + {"t":1.07545, "x":5.59121, "y":0.40941, "heading":-0.00056, "vx":-4.54923, "vy":-1.52367, "omega":0.00135, "ax":-3.88728, "ay":13.53872, "alpha":-0.00284, "fx":[-42.30786,-42.30012,-42.32773,-42.33548], "fy":[147.38785,147.39076,147.38303,147.38012]}, + {"t":1.10588, "x":5.45101, "y":0.36932, "heading":-0.00052, "vx":-4.66749, "vy":-1.11178, "omega":0.00126, "ax":-2.62159, "ay":13.58043, "alpha":-0.00352, "fx":[-28.52492,-28.51842,-28.5535,-28.56], "fy":[147.84132,147.84408,147.8376,147.83484]}, + {"t":1.1363, "x":5.3078, "y":0.34178, "heading":-0.00048, "vx":-4.74725, "vy":-0.69862, "omega":0.00115, "ax":-1.37698, "ay":13.08866, "alpha":-0.00201, "fx":[-14.9812,-14.97932,-14.99891,-15.0008], "fy":[142.4859,142.48789,142.48602,142.48403]}, + {"t":1.16672, "x":5.16273, "y":0.32659, "heading":-0.00044, "vx":-4.78914, "vy":-0.30041, "omega":0.00109, "ax":-0.30882, "ay":10.85171, "alpha":-0.00084, "fx":[-3.35848,-3.35833,-3.36537,-3.36552], "fy":[118.13318,118.13513,118.13499,118.13304]}, + {"t":1.19715, "x":5.01689, "y":0.32247, "heading":-0.00041, "vx":-4.79854, "vy":0.02973, "omega":0.00107, "ax":0.10319, "ay":4.82436, "alpha":-0.00017, "fx":[1.1239,1.1239,1.12285,1.12286], "fy":[52.51863,52.51946,52.51946,52.51863]}, + {"t":1.22757, "x":4.87095, "y":0.32561, "heading":-0.00038, "vx":-4.7954, "vy":0.17651, "omega":0.00106, "ax":0.04388, "ay":1.09059, "alpha":-0.00001, "fx":[0.47771,0.47771,0.47763,0.47763], "fy":[11.87237,11.87245,11.87245,11.87237]}, + {"t":1.25799, "x":4.72507, "y":0.33148, "heading":-0.00035, "vx":-4.79406, "vy":0.20969, "omega":0.00106, "ax":0.00991, "ay":0.2231, "alpha":0.0, "fx":[0.10793,0.10793,0.10793,0.10793], "fy":[2.42869,2.4287,2.4287,2.42869]}, + {"t":1.28842, "x":4.57923, "y":0.33796, "heading":-0.00031, "vx":-4.79376, "vy":0.21647, "omega":0.00106, "ax":0.00259, "ay":0.05721, "alpha":0.0, "fx":[0.02824,0.02824,0.02824,0.02824], "fy":[0.62282,0.62282,0.62282,0.62282]}, + {"t":1.31884, "x":4.43339, "y":0.34458, "heading":-0.00028, "vx":-4.79368, "vy":0.21821, "omega":0.00106, "ax":0.00333, "ay":0.07274, "alpha":0.0, "fx":[0.03623,0.03623,0.03623,0.03623], "fy":[0.79181,0.79181,0.79181,0.79181]}, + {"t":1.34926, "x":4.28755, "y":0.35125, "heading":-0.00025, "vx":-4.79358, "vy":0.22043, "omega":0.00106, "ax":0.01499, "ay":0.31878, "alpha":0.0, "fx":[0.16313,0.16313,0.16314,0.16314], "fy":[3.47028,3.47027,3.47027,3.47028]}, + {"t":1.37969, "x":4.14172, "y":0.3581, "heading":-0.00022, "vx":-4.79313, "vy":0.23012, "omega":0.00106, "ax":0.08257, "ay":1.55729, "alpha":0.00003, "fx":[0.89876,0.89876,0.89891,0.89891], "fy":[16.95302,16.95288,16.95288,16.95302]}, + {"t":1.41011, "x":3.99593, "y":0.36582, "heading":-0.00018, "vx":-4.79061, "vy":0.2775, "omega":0.00106, "ax":0.49046, "ay":6.27892, "alpha":0.00027, "fx":[5.33842,5.33847,5.34018,5.34014], "fy":[68.35427,68.3531,68.35306,68.35423]}, + {"t":1.44053, "x":3.85041, "y":0.37717, "heading":-0.00015, "vx":-4.77569, "vy":0.46853, "omega":0.00107, "ax":1.57267, "ay":11.57097, "alpha":0.00107, "fx":[17.11526,17.11626,17.1256,17.1246], "fy":[125.9657,125.96359,125.96259,125.96469]}, + {"t":1.47096, "x":3.70585, "y":0.39678, "heading":-0.00012, "vx":-4.72785, "vy":0.82056, "omega":0.0011, "ax":2.85266, "ay":13.07652, "alpha":0.00256, "fx":[31.03994,31.04487,31.06941,31.06447], "fy":[142.35778,142.35481,142.34987,142.35284]}, + {"t":1.50138, "x":3.56333, "y":0.4278, "heading":-0.00009, "vx":-4.64106, "vy":1.21839, "omega":0.00118, "ax":4.13357, "ay":13.28108, "alpha":0.01567, "fx":[44.90245,44.94682,45.09547,45.05098], "fy":[144.61277,144.5932,144.54871,144.56837]}, + {"t":1.5318, "x":3.42404, "y":0.47101, "heading":-0.00005, "vx":-4.5153, "vy":1.62245, "omega":0.00166, "ax":5.45168, "ay":13.0248, "alpha":0.3095, "fx":[57.3988,58.51672,61.31487,60.16211], "fy":[142.62017,142.09746,140.94173,141.50401]}, + {"t":1.56223, "x":3.2892, "y":0.5264, "heading":0.0, "vx":-4.34944, "vy":2.01871, "omega":0.01107, "ax":6.43052, "ay":12.51602, "alpha":0.25393, "fx":[68.42002,69.46837,71.59586,70.5321], "fy":[137.07667,136.48375,135.41369,136.03417]}, + {"t":1.58278, "x":3.20117, "y":0.57053, "heading":0.00023, "vx":-4.21728, "vy":2.27593, "omega":0.01629, "ax":7.08231, "ay":12.17366, "alpha":0.06537, "fx":[76.69585,76.98689,77.50367,77.21198], "fy":[132.76297,132.57847,132.28635,132.47267]}, + {"t":1.60333, "x":3.11599, "y":0.61988, "heading":0.00056, "vx":-4.07173, "vy":2.52613, "omega":0.01764, "ax":7.81394, "ay":11.74537, "alpha":0.04414, "fx":[84.79721,85.00668,85.33127,85.12169], "fy":[128.04181,127.89283,127.68318,127.83296]}, + {"t":1.62388, "x":3.03396, "y":0.67427, "heading":0.00092, "vx":-3.91114, "vy":2.76752, "omega":0.01854, "ax":8.53824, "ay":11.26728, "alpha":0.074, "fx":[92.51473,92.8832,93.38335,93.01513], "fy":[122.98847,122.69524,122.3266,122.62189]}, + {"t":1.64444, "x":2.95538, "y":0.73353, "heading":0.00131, "vx":-3.73566, "vy":2.99908, "omega":0.02006, "ax":9.24854, "ay":10.73407, "alpha":0.15392, "fx":[99.81363,100.60623,101.54792,100.7587], "fy":[117.60058,116.89511,116.1025,116.81565]}, + {"t":1.66499, "x":2.88056, "y":0.79744, "heading":0.00172, "vx":-3.54558, "vy":3.21969, "omega":0.02323, "ax":9.9532, "ay":10.12665, "alpha":0.34046, "fx":[106.52974,108.32018,110.16194,108.39879], "fy":[112.01894,110.23342,108.4482,110.2631]}, + {"t":1.68554, "x":2.80979, "y":0.86574, "heading":0.0022, "vx":-3.34103, "vy":3.42781, "omega":0.03022, "ax":10.66363, "ay":9.41349, "alpha":0.7119, "fx":[112.51864,116.3028,119.57339,115.95147], "fy":[106.44556,102.19397,98.46859,102.8012]}, + {"t":1.70609, "x":2.74338, "y":0.93818, "heading":0.00282, "vx":-3.12187, "vy":3.62127, "omega":0.04486, "ax":11.36182, "ay":8.58389, "alpha":1.26414, "fx":[117.85287,124.56712,129.21006,123.11897], "fy":[100.91226,92.32793,85.94541,94.59878]}, + {"t":1.72664, "x":2.68162, "y":1.01442, "heading":0.00374, "vx":-2.88836, "vy":3.79769, "omega":0.07084, "ax":11.9928, "ay":7.68901, "alpha":1.84775, "fx":[122.83171,132.45502,137.53977,129.39847], "fy":[95.15107,80.9914,72.40337,86.27096]}, + {"t":1.74719, "x":2.62479, "y":1.09409, "heading":0.00519, "vx":-2.64189, "vy":3.95571, "omega":0.10881, "ax":12.52028, "ay":6.80121, "alpha":2.30089, "fx":[127.72733,139.16946,143.68172,134.61547], "fy":[88.79728,69.23586,59.83911,78.28546]}, + {"t":1.76775, "x":2.57314, "y":1.17682, "heading":0.00743, "vx":-2.38457, "vy":4.09549, "omega":0.1561, "ax":12.94662, "ay":5.96178, "alpha":2.55102, "fx":[132.61768,144.37567,147.8336,138.9321], "fy":[81.61409,58.06378,49.23639,70.69057]}, + {"t":1.7883, "x":2.52686, "y":1.26225, "heading":0.01064, "vx":-2.1185, "vy":4.21801, "omega":0.20853, "ax":13.29095, "ay":5.17922, "alpha":2.591, "fx":[137.43077,148.17356,150.56274,142.58551], "fy":[73.50637,48.0604,40.6598,63.3019]}, + {"t":1.80885, "x":2.48613, "y":1.35004, "heading":0.01492, "vx":-1.84534, "vy":4.32446, "omega":0.26178, "ax":13.57288, "ay":4.44301, "alpha":2.43852, "fx":[142.03607,150.85316,152.36853,145.77134], "fy":[64.44134,39.40529,33.77803,55.8457]}, + {"t":1.8294, "x":2.45107, "y":1.43985, "heading":0.0203, "vx":-1.56639, "vy":4.41577, "omega":0.31189, "ax":13.80567, "ay":3.73757, "alpha":2.12245, "fx":[146.25431,152.71535,153.59489,148.60159], "fy":[54.48534,32.035,28.17225,48.05948]}, + {"t":1.84995, "x":2.4218, "y":1.53139, "heading":0.02671, "vx":-1.28266, "vy":4.49258, "omega":0.35551, "ax":13.99307, "ay":3.06098, "alpha":1.6943, "fx":[149.81841,153.99431,154.45509,151.05839], "fy":[44.06416,25.83023,23.48065,39.91471]}, + {"t":1.87051, "x":2.39839, "y":1.62437, "heading":0.03402, "vx":-0.99508, "vy":4.55549, "omega":0.39033, "ax":14.1334, "ay":2.4357, "alpha":1.23095, "fx":[152.48614,154.85498,155.07042,153.02568], "fy":[34.0567,20.71682,19.483,31.80588]}, + {"t":1.89106, "x":2.38092, "y":1.71851, "heading":0.04204, "vx":-0.70461, "vy":4.60555, "omega":0.41563, "ax":14.28315, "ay":1.5931, "alpha":0.50864, "fx":[155.15129,155.76923,155.81079,155.22643], "fy":[20.27354,14.78311,14.50429,19.81027]}, + {"t":1.92253, "x":2.36582, "y":1.86425, "heading":0.05512, "vx":-0.25507, "vy":4.65569, "omega":0.43164, "ax":14.36432, "ay":0.19336, "alpha":-0.22808, "fx":[156.39347,156.35979,156.35337,156.38599], "fy":[0.94613,3.36549,3.28267,0.82547]}, + {"t":1.954, "x":2.36491, "y":2.01087, "heading":0.06871, "vx":0.19701, "vy":4.66178, "omega":0.42446, "ax":14.29053, "ay":-1.33172, "alpha":-0.61928, "fx":[155.34965,155.91786,155.82404,155.18751], "fy":[-17.15714,-10.78094,-11.7079,-18.34353]}, + {"t":1.98548, "x":2.37819, "y":2.15693, "heading":0.08207, "vx":0.64678, "vy":4.61986, "omega":0.40497, "ax":14.04826, "ay":-2.88593, "alpha":-0.06982, "fx":[152.88411,153.02636,152.98143,152.83764], "fy":[-31.66469,-30.96461,-31.16771,-31.87052]}, + {"t":2.01695, "x":2.4055, "y":2.3009, "heading":0.09481, "vx":1.08892, "vy":4.52903, "omega":0.40277, "ax":13.47035, "ay":-4.79608, "alpha":1.68297, "fx":[148.31499,142.30524,145.55235,150.39189], "fy":[-47.83076,-63.65531,-56.10121,-41.25744]}, + {"t":2.04842, "x":2.44644, "y":2.44107, "heading":0.10749, "vx":1.51288, "vy":4.37809, "omega":0.45574, "ax":12.08762, "ay":-7.19993, "alpha":5.16824, "fx":[136.95424,108.96368,131.5554,148.88031], "fy":[-73.05356,-111.13924,-83.66163,-45.66553]}, + {"t":2.0799, "x":2.50005, "y":2.5753, "heading":0.12183, "vx":1.89331, "vy":4.15148, "omega":0.6184, "ax":8.0969, "ay":-11.14956, "alpha":7.65481, "fx":[70.73063,48.29477,101.35672,132.19627], "fy":[-137.50674,-147.95899,-118.36766,-81.67251]}, + {"t":2.11137, "x":2.56364, "y":2.70043, "heading":0.1413, "vx":2.14815, "vy":3.80057, "omega":0.85932, "ax":0.89085, "ay":-14.19566, "alpha":3.11877, "fx":[-9.61318,-3.71816,25.64814,26.47494], "fy":[-155.31368,-155.80731,-153.71223,-153.31509]}, + {"t":2.14284, "x":2.63169, "y":2.81302, "heading":0.16834, "vx":2.17618, "vy":3.35379, "omega":0.95748, "ax":-3.62855, "ay":-13.86913, "alpha":-0.18548, "fx":[-38.23777,-39.01716,-40.77148,-39.97844], "fy":[-151.31329,-151.10399,-150.64478,-150.86723]}, + {"t":2.17432, "x":2.69839, "y":2.9117, "heading":0.19848, "vx":2.06198, "vy":2.91729, "omega":0.95164, "ax":-5.8851, "ay":-13.06048, "alpha":-1.91336, "fx":[-51.14155,-61.80126,-77.12659,-66.19668], "fy":[-147.69176,-143.47577,-135.91873,-141.63046]}, + {"t":2.20579, "x":2.76037, "y":2.99705, "heading":0.22843, "vx":1.87676, "vy":2.50623, "omega":0.89142, "ax":-7.11879, "ay":-12.39985, "alpha":-2.88396, "fx":[-58.31158,-77.04385,-96.12076,-78.51076], "fy":[-145.15136,-136.03547,-123.40354,-135.35934]}, + {"t":2.23726, "x":2.81591, "y":3.06979, "heading":0.25648, "vx":1.65271, "vy":2.11597, "omega":0.80066, "ax":-7.87219, "ay":-11.90626, "alpha":-3.48811, "fx":[-62.86367,-87.74577,-106.89612,-85.28779], "fy":[-143.33108,-129.49666,-114.32921,-131.2997]}, + {"t":2.26873, "x":2.86403, "y":3.13049, "heading":0.28168, "vx":1.40495, "vy":1.74124, "omega":0.69087, "ax":-8.37323, "ay":-11.53437, "alpha":-3.90179, "fx":[-66.02582,-95.57483,-113.56588,-89.4448], "fy":[-141.96753,-123.91505,-107.80462,-128.57555]}, + {"t":2.30021, "x":2.9041, "y":3.17958, "heading":0.30343, "vx":1.14142, "vy":1.37822, "omega":0.56807, "ax":-8.72796, "ay":-11.24732, "alpha":-4.20585, "fx":[-68.36681,-101.48171,-118.00145,-92.20804], "fy":[-140.90389,-119.19378,-103.00487,-126.66044]}, + {"t":2.33168, "x":2.9357, "y":3.21738, "heading":0.32131, "vx":0.86672, "vy":1.02424, "omega":0.4357, "ax":-8.99113, "ay":-11.0203, "alpha":-4.44025, "fx":[-70.18104,-106.035,-121.12813,-94.17333], "fy":[-140.047,-115.21795,-99.36714,-125.24539]}, + {"t":2.36315, "x":2.95853, "y":3.24416, "heading":0.33502, "vx":0.58374, "vy":0.67739, "omega":0.29595, "ax":-9.19354, "ay":-10.8369, "alpha":-4.62646, "fx":[-71.63356,-109.59166,-123.44337,-95.66302], "fy":[-139.34005,-111.88714,-96.52204,-124.14203]}, + {"t":2.39463, "x":2.97235, "y":3.26011, "heading":0.34433, "vx":0.29439, "vy":0.33632, "omega":0.15034, "ax":-9.35378, "ay":-10.68603, "alpha":-4.77694, "fx":[-72.82298,-112.3854,-125.23438,-96.86631], "fy":[-138.74719,-109.12015,-94.22421,-123.23016]}, + {"t":2.4261, "x":2.97698, "y":3.26541, "heading":0.34907, "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/StartCollectShoot.chor b/src/main/deploy/choreo/StartCollectShoot.chor index 9147e2c..7fc9d15 100644 --- a/src/main/deploy/choreo/StartCollectShoot.chor +++ b/src/main/deploy/choreo/StartCollectShoot.chor @@ -28,8 +28,8 @@ } }, "mass":{ - "exp":"150 lbs", - "val":68.0388555 + "exp":"96 lbs", + "val":43.54486752 }, "inertia":{ "exp":"6 kg m ^ 2", @@ -40,8 +40,8 @@ "val":6.5 }, "radius":{ - "exp":"2 in", - "val":0.0508 + "exp":"1.957 in", + "val":0.0497078 }, "vmax":{ "exp":"6000 RPM", @@ -57,16 +57,16 @@ }, "bumper":{ "front":{ - "exp":"16 in", - "val":0.4064 + "exp":"21.75 in", + "val":0.55245 }, "side":{ - "exp":"16 in", - "val":0.4064 + "exp":"13 in", + "val":0.3302 }, "back":{ - "exp":"16 in", - "val":0.4064 + "exp":"13.25 in", + "val":0.33654999999999996 } }, "differentialTrackWidth":{ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7a6944..f82025d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -137,7 +137,7 @@ public RobotContainer() { true, // If alliance flipping should be enabled drive, (sample, isStart) -> { - Logger.recordOutput( + Logger.recordOutput( "TrajectoryFollowing/ActiveTrajectory", Arrays.stream(sample.getPoses()) .map(AllianceFlipUtil::apply) diff --git a/src/main/java/frc/robot/commands/autos/StartCollectShoot.java b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java index 4d27600..25540be 100644 --- a/src/main/java/frc/robot/commands/autos/StartCollectShoot.java +++ b/src/main/java/frc/robot/commands/autos/StartCollectShoot.java @@ -5,6 +5,7 @@ import choreo.auto.AutoTrajectory; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.FeederConstants; @@ -21,21 +22,19 @@ public static AutoRoutine getRoutine( AutoTrajectory faceFuel = routine.trajectory("FaceFuel"); AutoTrajectory collectFuel = routine.trajectory("CollectFuel"); - AutoTrajectory fuelToShot = routine.trajectory("FuelToShot"); + AutoTrajectory fuelToShotB = routine.trajectory("FuelToShotB"); routine .active() .onTrue( Commands.sequence( - new InstantCommand(() -> Commands.print("Going to fuel")), - faceFuel.resetOdometry(), - faceFuel.cmd())); + Commands.print("Going to fuel"), faceFuel.resetOdometry(), faceFuel.cmd())); faceFuel .done() .onTrue( Commands.sequence( - new InstantCommand(() -> Commands.print("Starting intake and collecting fuel")), + Commands.print("Starting intake and collecting fuel"), Commands.parallel( intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.intakeVoltage.get()), collectFuel.cmd()))); @@ -43,13 +42,16 @@ public static AutoRoutine getRoutine( .done() .onTrue( Commands.sequence( - new InstantCommand(() -> Commands.print("Moving to shooting position")), - fuelToShot.cmd())); - fuelToShot + Commands.print("Moving to shooting position"), + intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.intakeVoltage.get()), + new InstantCommand(() -> driveSubsystem.stop()), + new WaitCommand(2), + fuelToShotB.cmd())); + fuelToShotB .done() .onTrue( Commands.sequence( - new InstantCommand(() -> Commands.print("Starting launch sequence")), + Commands.print("Starting launch sequence"), Commands.race( Commands.parallel( intakeAndShooter.voltageCmd(IntakeAndLauncherConstants.launchVoltage.get()), diff --git a/src/main/java/frc/robot/subsystems/prototypeLauncher/PrototypeLauncher.java b/src/main/java/frc/robot/subsystems/prototypeLauncher/PrototypeLauncher.java new file mode 100644 index 0000000..3b57c97 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/prototypeLauncher/PrototypeLauncher.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems.prototypeLauncher; + +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.LoggedTunableNumber; +import org.littletonrobotics.junction.Logger; + +public class PrototypeLauncher extends SubsystemBase { + private TalonFX leftMotor = new TalonFX(2); + private TalonFX rightMotor = new TalonFX(1); + private double trackedvoltage = 0; + private AngularVelocity velocity = RotationsPerSecond.of(0); + private TalonFXConfiguration leftMotorConfig = new TalonFXConfiguration(); + private LoggedTunableNumber prototypeP = new LoggedTunableNumber("prototypeP", 0); + + @Override + public void periodic() { + if (prototypeP.hasChanged(hashCode())) { + this.setPID(prototypeP.get()); + } + Logger.recordOutput("Prototype/voltage", trackedvoltage); + Logger.recordOutput("Prototype/desiredVelocity", velocity); + Logger.recordOutput("Prototype/velocity", leftMotor.getVelocity().getValue()); + } + + public PrototypeLauncher() { + rightMotor.setControl(new Follower(leftMotor.getDeviceID(), MotorAlignmentValue.Opposed)); + this.setPID(prototypeP.get()); + } + + public void setPID(double P) { + leftMotorConfig.Slot0.kP = P; + leftMotorConfig.Slot0.kV = 0; + leftMotor.getConfigurator().apply(leftMotorConfig); + } + + public void setVoltagePrototype(double commandedVoltage) { + leftMotor.setVoltage(commandedVoltage); + trackedvoltage = commandedVoltage; + } + + public void setVelocityPrototype(AngularVelocity desiredVelocity) { + this.velocity = desiredVelocity; + leftMotor.setControl(new VelocityVoltage(desiredVelocity)); + } + + public Command voltageCmdPrototype(double desiredVoltage) { + return Commands.sequence( + Commands.print("LaunchPrototype set to: " + desiredVoltage), + Commands.runOnce(() -> this.setVoltagePrototype(desiredVoltage), this)); + } + + public Command velocityCmdPrototype(AngularVelocity velocity) { + return Commands.sequence( + Commands.print("LaunchPrototypeVelocity set to: " + velocity), + Commands.runOnce(() -> this.setVelocityPrototype(velocity), this)); + } +}