From 5ea6ab19d223f16a301748d0394110d73092df5b Mon Sep 17 00:00:00 2001 From: moo Date: Fri, 1 May 2026 13:36:47 -0500 Subject: [PATCH] betta --- src/main/deploy/choreo/liberal.traj | 184 +++++++++--------- src/main/java/frc/robot/RobotContainer.java | 6 +- .../commands/auto_comm/ChoreoPathCommand.java | 42 +++- 3 files changed, 136 insertions(+), 96 deletions(-) diff --git a/src/main/deploy/choreo/liberal.traj b/src/main/deploy/choreo/liberal.traj index 867aba0..c1e9830 100644 --- a/src/main/deploy/choreo/liberal.traj +++ b/src/main/deploy/choreo/liberal.traj @@ -10,7 +10,7 @@ {"x":3.9274368286132817, "y":7.657727718353272, "heading":0.0, "intervals":19, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.9600324630737305, "y":6.425201892852783, "heading":-1.5493711192251445, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":5.909896373748779, "y":4.600945472717285, "heading":-0.7175417284567359, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.322737216949463, "y":4.6251654624938965, "heading":1.2444378111094314, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.322737216949463, "y":4.6251654624938965, "heading":1.3569870388467742, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":6.571913242340088, "y":6.780756950378418, "heading":-0.22347657213134992, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":3.003480911254883, "y":7.571948051452637, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -31,7 +31,7 @@ {"x":{"exp":"3.9274368286132812 m", "val":3.9274368286132817}, "y":{"exp":"7.6577277183532715 m", "val":7.657727718353272}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.9600324630737305 m", "val":5.9600324630737305}, "y":{"exp":"6.425201892852783 m", "val":6.425201892852783}, "heading":{"exp":"-1.5493711192251445 rad", "val":-1.5493711192251445}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.909896373748779 m", "val":5.909896373748779}, "y":{"exp":"4.600945472717285 m", "val":4.600945472717285}, "heading":{"exp":"-0.7175417284567359 rad", "val":-0.7175417284567359}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.322737216949463 m", "val":7.322737216949463}, "y":{"exp":"4.6251654624938965 m", "val":4.6251654624938965}, "heading":{"exp":"1.2444378111094314 rad", "val":1.2444378111094314}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.322737216949463 m", "val":7.322737216949463}, "y":{"exp":"4.6251654624938965 m", "val":4.6251654624938965}, "heading":{"exp":"1.3569870388467742 rad", "val":1.3569870388467742}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"6.571913242340088 m", "val":6.571913242340088}, "y":{"exp":"6.780756950378418 m", "val":6.780756950378418}, "heading":{"exp":"-0.22347657213134994 rad", "val":-0.22347657213134992}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"3.003480911254883 m", "val":3.003480911254883}, "y":{"exp":"7.571948051452637 m", "val":7.571948051452637}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ @@ -71,7 +71,7 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.34566,2.0759,3.01312,3.70343,4.69255,5.23987,5.91052,6.68203,7.75993], + "waypoints":[0.0,1.34566,2.0759,3.01312,3.70343,4.6926,5.23957,5.92852,6.71725,7.79538], "samples":[ {"t":0.0, "x":4.42074, "y":7.65268, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.66679, "ay":-1.35691, "alpha":-3.31345, "fx":[135.0025,136.02086,138.58749,138.48707], "fy":[-32.22091,-27.69131,-7.83953,-9.18403]}, {"t":0.05383, "x":4.43475, "y":7.65071, "heading":-1.5756, "vx":0.52033, "vy":-0.07304, "omega":-0.17835, "ax":9.66992, "ay":-1.33865, "alpha":-3.21926, "fx":[135.1498,136.08667,138.56656,138.47222], "fy":[-31.52253,-27.28952,-7.93971,-9.14826]}, @@ -148,99 +148,99 @@ {"t":3.53086, "x":4.07318, "y":7.65375, "heading":0.01041, "vx":-1.68888, "vy":0.04573, "omega":-0.13594, "ax":9.78401, "ay":-0.26357, "alpha":0.99276, "fx":[138.77118,138.58417,138.61094,138.77799], "fy":[-0.49318,-7.22385,-6.82369,-0.40338]}, {"t":3.58838, "x":3.99222, "y":7.65594, "heading":0.00423, "vx":-1.12605, "vy":0.03057, "omega":-0.07883, "ax":9.78665, "ay":-0.25816, "alpha":0.77613, "fx":[138.78875,138.64772,138.66339,138.79358], "fy":[-1.11849,-6.35729,-6.10508,-1.05638]}, {"t":3.64591, "x":3.94363, "y":7.65728, "heading":0.00098, "vx":-0.56306, "vy":0.01572, "omega":-0.03419, "ax":9.78799, "ay":-0.27325, "alpha":0.5943, "fx":[138.79549,138.6824,138.69244,138.79919], "fy":[-1.93239,-5.92789,-5.75842,-1.87447]}, - {"t":3.70343, "x":3.92744, "y":7.65773, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.74867, "ay":-0.86179, "alpha":-1.34707, "fx":[137.86103,138.59459,138.55892,137.72573], "fy":[-16.11664,-7.53538,-8.03597,-17.17486]}, - {"t":3.75549, "x":3.94065, "y":7.65656, "heading":-0.00183, "vx":0.5075, "vy":-0.04486, "omega":-0.07013, "ax":9.74467, "ay":-0.85348, "alpha":-1.72727, "fx":[137.7283,138.64339,138.60846,137.53315], "fy":[-17.06346,-6.16605,-6.67435,-18.48747]}, - {"t":3.80755, "x":3.98027, "y":7.65307, "heading":-0.00782, "vx":1.0148, "vy":-0.08929, "omega":-0.16005, "ax":9.73662, "ay":-0.8593, "alpha":-2.2205, "fx":[137.51411,138.67476,138.64281,137.22548], "fy":[-18.51645,-4.68672,-5.08919,-20.42927]}, - {"t":3.85961, "x":4.04629, "y":7.64725, "heading":-0.01916, "vx":1.52168, "vy":-0.13403, "omega":-0.27564, "ax":9.7217, "ay":-0.88205, "alpha":-2.89738, "fx":[137.1628,138.68188,138.65143,136.71482], "fy":[-20.69739,-2.98048,-3.02772,-23.30558]}, - {"t":3.91167, "x":4.13868, "y":7.63908, "heading":-0.03743, "vx":2.02778, "vy":-0.17995, "omega":-0.42648, "ax":9.69159, "ay":-0.93126, "alpha":-3.94303, "fx":[136.5236,138.64405,138.59152,135.74463], "fy":[-24.1816,-0.77942,0.12676,-27.9674]}, - {"t":3.96373, "x":4.25738, "y":7.62845, "heading":-0.06498, "vx":2.53231, "vy":-0.22843, "omega":-0.63175, "ax":9.61247, "ay":-1.03886, "alpha":-5.9849, "fx":[135.067,138.48044,138.22648,133.2442], "fy":[-30.69301,2.70411,6.45637,-37.37008]}, - {"t":4.01579, "x":4.40224, "y":7.61515, "heading":-0.10598, "vx":3.03272, "vy":-0.28251, "omega":-0.94331, "ax":9.11284, "ay":-1.37121, "alpha":-13.84791, "fx":[129.03421,137.54359,133.52686,116.58454], "fy":[-49.11028,11.82611,32.36206,-72.82407]}, - {"t":4.06784, "x":4.57246, "y":7.59859, "heading":-0.17385, "vx":3.50713, "vy":-0.35389, "omega":-1.66422, "ax":3.31814, "ay":-8.35951, "alpha":6.53317, "fx":[38.94663,22.57133,53.45218,73.16498], "fy":[-120.50548,-127.85394,-119.46181,-106.1552]}, - {"t":4.1199, "x":4.75954, "y":7.56884, "heading":-0.25163, "vx":3.67986, "vy":-0.78908, "omega":-1.32411, "ax":-3.09224, "ay":-7.52758, "alpha":-2.32487, "fx":[-37.68221,-37.91265,-50.49644,-49.23604], "fy":[-110.31226,-107.69709,-102.82665,-105.97068]}, - {"t":4.17196, "x":4.94692, "y":7.51756, "heading":-0.32371, "vx":3.51888, "vy":-1.18096, "omega":-1.44514, "ax":-2.53129, "ay":-4.85913, "alpha":-1.04434, "fx":[-34.05797,-33.54403,-37.75807,-38.16194], "fy":[-70.94616,-68.55544,-66.78278,-69.22373]}, - {"t":4.22402, "x":5.12667, "y":7.44949, "heading":-0.40036, "vx":3.38711, "vy":-1.43392, "omega":-1.49951, "ax":-5.38281, "ay":-3.88414, "alpha":-5.09053, "fx":[-67.91419,-68.48924,-85.15388,-83.64314], "fy":[-69.25944,-55.20532,-40.02968,-55.73276]}, - {"t":4.27608, "x":5.29571, "y":7.36958, "heading":-0.48532, "vx":3.10689, "vy":-1.63612, "omega":-1.76451, "ax":-7.86566, "ay":-2.95717, "alpha":-8.0511, "fx":[-99.18274,-107.33065,-121.64143,-117.82057], "fy":[-71.9503,-43.15328,-10.04529,-42.51984]}, - {"t":4.32814, "x":5.44679, "y":7.2804, "heading":-0.58809, "vx":2.69741, "vy":-1.79007, "omega":-2.18364, "ax":-8.72012, "ay":-1.10416, "alpha":-8.75252, "fx":[-116.37488,-122.51701,-126.64274,-128.88801], "fy":[-53.16398,-11.73876,20.87111,-18.57322]}, - {"t":4.3802, "x":5.5754, "y":7.18572, "heading":-0.71363, "vx":2.24345, "vy":-1.84755, "omega":-2.63929, "ax":-8.58395, "ay":1.62019, "alpha":-8.89315, "fx":[-125.62791,-117.16225,-115.31551,-128.59619], "fy":[-15.70873,35.45348,55.94733,16.17102]}, - {"t":4.43226, "x":5.68056, "y":7.09173, "heading":-0.86308, "vx":1.79658, "vy":-1.7632, "omega":-3.10226, "ax":-8.59333, "ay":-1.83178, "alpha":-5.62987, "fx":[-117.47721,-118.40299,-125.30767,-126.04575], "fy":[-47.23567,-33.55761,-2.22643,-20.84072]}, - {"t":4.48431, "x":5.76244, "y":6.99746, "heading":-1.03221, "vx":1.34922, "vy":-1.85856, "omega":-3.39534, "ax":-4.30399, "ay":-8.3097, "alpha":6.52077, "fx":[-68.6987,-84.75845,-57.01948,-33.55556], "fy":[-114.02518,-104.62921,-122.53525,-129.96258]}, - {"t":4.53637, "x":5.82685, "y":6.88944, "heading":-1.20013, "vx":1.12516, "vy":-2.29116, "omega":-3.05588, "ax":-3.39925, "ay":-8.76938, "alpha":9.92004, "fx":[-46.3456,-90.16593,-51.37012,-4.85243], "fy":[-128.36783,-103.68117,-127.80782,-137.35876]}, - {"t":4.58843, "x":5.88082, "y":6.75829, "heading":-1.34577, "vx":0.9482, "vy":-2.74768, "omega":-2.53945, "ax":-3.52737, "ay":-8.70841, "alpha":11.03092, "fx":[-38.25246,-97.2442,-58.60398,-5.89806], "fy":[-132.22798,-98.1109,-125.32298,-138.09693]}, - {"t":4.64049, "x":5.9254, "y":6.60345, "heading":-1.46302, "vx":0.76457, "vy":-3.20103, "omega":-1.9652, "ax":-3.8146, "ay":-8.5616, "alpha":11.77699, "fx":[-35.45335,-103.27428,-66.33748,-11.21887], "fy":[-133.52077,-92.18849,-121.66798,-138.05704]}, - {"t":4.69255, "x":5.96003, "y":6.4252, "heading":-1.54937, "vx":0.56599, "vy":-3.64674, "omega":-1.3521, "ax":-1.50997, "ay":-3.20838, "alpha":50.70562, "fx":[124.50229,-138.36318,-91.95239,20.19948], "fy":[54.07374,4.70973,-103.6958,-136.9997]}, - {"t":4.73164, "x":5.98101, "y":6.28018, "heading":-1.56348, "vx":0.50695, "vy":-3.77217, "omega":0.63022, "ax":-7.55717, "ay":-5.81858, "alpha":8.03461, "fx":[-113.00038,-127.19513,-106.74901,-81.53985], "fy":[-78.11315,-53.19814,-87.51511,-111.08149]}, - {"t":4.77074, "x":5.99505, "y":6.12826, "heading":-1.5327, "vx":0.21151, "vy":-3.99964, "omega":0.94433, "ax":-8.24651, "ay":-4.63247, "alpha":-8.72557, "fx":[-114.69102,-93.9802,-126.23078,-132.66728], "fy":[-75.84844,-99.70146,-51.72424,-35.38274]}, - {"t":4.80983, "x":5.99702, "y":5.96836, "heading":-1.50245, "vx":-0.11088, "vy":-4.18075, "omega":0.6032, "ax":-8.79703, "ay":-3.45479, "alpha":-7.30451, "fx":[-121.96227,-111.08582,-131.64369,-134.09164], "fy":[-61.46109,-78.10989,-31.36253,-24.95003]}, - {"t":4.84893, "x":5.98596, "y":5.80228, "heading":-1.48445, "vx":-0.4548, "vy":-4.31581, "omega":0.31764, "ax":-9.34934, "ay":1.20008, "alpha":-0.71963, "fx":[-132.92951,-132.65463,-132.09645,-132.41779], "fy":[14.33977,15.21886,19.74875,18.73606]}, - {"t":4.88802, "x":5.96103, "y":5.63447, "heading":-1.47259, "vx":-0.82031, "vy":-4.26889, "omega":0.2895, "ax":-2.5671, "ay":8.72196, "alpha":11.4388, "fx":[-6.00523,-58.08797,-80.572,-0.88686], "fy":[134.70354,121.94184,105.84047,132.04091]}, - {"t":4.92712, "x":5.927, "y":5.47424, "heading":-1.45253, "vx":-0.92067, "vy":-3.92791, "omega":0.7367, "ax":2.66911, "ay":8.75355, "alpha":12.73913, "fx":[54.89434,-3.69005,8.2174,91.91415], "fy":[125.42871,136.55879,134.18538,100.14502]}, - {"t":4.96621, "x":5.89305, "y":5.32737, "heading":-1.41399, "vx":-0.81632, "vy":-3.58569, "omega":1.23473, "ax":5.02135, "ay":7.78415, "alpha":11.8616, "fx":[76.74351,23.8946,69.15715,114.91073], "fy":[114.28373,135.20396,116.839,75.02729]}, - {"t":5.00531, "x":5.86497, "y":5.19314, "heading":-1.35666, "vx":-0.62001, "vy":-3.28138, "omega":1.69845, "ax":6.09122, "ay":7.12291, "alpha":10.83394, "fx":[85.37027,43.343,96.90612,119.74718], "fy":[108.41409,130.66742,96.64578,68.13477]}, - {"t":5.0444, "x":5.84539, "y":5.0703, "heading":-1.28198, "vx":-0.38188, "vy":-3.00291, "omega":2.122, "ax":6.73484, "ay":6.64996, "alpha":9.73483, "fx":[90.6863,59.95604,110.03449,121.182], "fy":[104.25402,124.18357,82.45965,66.14911]}, - {"t":5.0835, "x":5.83561, "y":4.95798, "heading":-1.19158, "vx":-0.11858, "vy":-2.74293, "omega":2.50258, "ax":7.2332, "ay":6.23478, "alpha":8.4115, "fx":[95.35016,75.62514,117.38338,121.75686], "fy":[100.17129,115.5121,72.36021,65.46243]}, - {"t":5.12259, "x":5.8365, "y":4.85551, "heading":-1.08731, "vx":0.1642, "vy":-2.49919, "omega":2.83142, "ax":7.66947, "ay":5.82561, "alpha":6.71466, "fx":[100.48516,90.87663,121.54777,121.94227], "fy":[95.14159,104.12395,65.66855,65.37245]}, - {"t":5.16168, "x":5.84878, "y":4.76226, "heading":-0.97149, "vx":0.46403, "vy":-2.27144, "omega":3.09393, "ax":8.05021, "ay":5.42096, "alpha":4.56918, "fx":[106.77264,104.75443,123.19513,121.71684], "fy":[88.12673,90.34615,62.92186,65.96876]}, - {"t":5.20078, "x":5.87307, "y":4.6776, "heading":-0.84704, "vx":0.77875, "vy":-2.05951, "omega":3.27256, "ax":8.34978, "ay":5.04937, "alpha":2.0394, "fx":[114.44445,115.40503,122.48627,121.08904], "fy":[77.99829,76.49314,64.56809,67.23478]}, - {"t":5.23987, "x":5.9099, "y":4.60095, "heading":-0.71754, "vx":1.10518, "vy":-1.8621, "omega":3.35229, "ax":8.38565, "ay":5.01401, "alpha":-0.30666, "fx":[119.52056,119.20105,118.21103,118.52567], "fy":[69.96715,70.52143,72.16511,71.63587]}, - {"t":5.28179, "x":5.96359, "y":4.5273, "heading":-0.5773, "vx":1.45667, "vy":-1.65194, "omega":3.33944, "ax":8.21708, "ay":5.24042, "alpha":-2.34878, "fx":[121.97881,118.58746,111.00663,114.32756], "fy":[65.28419,71.36025,82.62431,77.85812]}, - {"t":5.3237, "x":6.03186, "y":4.46266, "heading":-0.43939, "vx":1.80109, "vy":-1.43228, "omega":3.24099, "ax":8.0434, "ay":5.41066, "alpha":-4.33243, "fx":[124.67612,117.08223,103.07278,111.22178], "fy":[59.47513,73.51717,92.04326,81.74392]}, - {"t":5.36562, "x":6.11442, "y":4.40738, "heading":-0.30735, "vx":2.13823, "vy":-1.20549, "omega":3.05939, "ax":7.86976, "ay":5.52518, "alpha":-6.11074, "fx":[126.87128,114.76442,94.87573,109.69662], "fy":[53.75097,76.61266,100.0012,82.90759]}, - {"t":5.40754, "x":6.21096, "y":4.3617, "heading":-0.18448, "vx":2.4681, "vy":-0.97391, "omega":2.80326, "ax":7.66526, "ay":5.63077, "alpha":-7.51894, "fx":[127.6144,111.35407,86.59159,109.05278], "fy":[50.22777,80.61721,106.38506,82.02928]}, - {"t":5.44945, "x":6.32114, "y":4.32583, "heading":-0.07358, "vx":2.78939, "vy":-0.73789, "omega":2.4881, "ax":7.21168, "ay":5.98089, "alpha":-8.12763, "fx":[124.01122,104.4748,76.52131,103.8883], "fy":[54.90456,87.42707,111.93205,84.84732]}, - {"t":5.49137, "x":6.4444, "y":4.30015, "heading":0.02356, "vx":3.09167, "vy":-0.4872, "omega":2.14743, "ax":4.70168, "ay":7.8226, "alpha":-5.11217, "fx":[85.49401,71.45669,48.41239,61.21787], "fy":[98.21709,110.52715,121.52833,113.2616]}, - {"t":5.53328, "x":6.57812, "y":4.2866, "heading":0.10908, "vx":3.28874, "vy":-0.15931, "omega":1.93315, "ax":-5.45724, "ay":7.00231, "alpha":7.99669, "fx":[-85.36776,-103.5206,-68.15765,-52.37429], "fy":[98.76545,75.49662,104.56979,118.19243]}, - {"t":5.5752, "x":6.71117, "y":4.28608, "heading":0.19714, "vx":3.06, "vy":0.13419, "omega":2.26833, "ax":-8.10188, "ay":4.15089, "alpha":10.06729, "fx":[-117.83298,-132.19686,-119.16268,-90.17635], "fy":[65.13642,19.74804,52.31871,98.1483]}, - {"t":5.61711, "x":6.83231, "y":4.29535, "heading":0.30106, "vx":2.72041, "vy":0.30818, "omega":2.69031, "ax":-8.43906, "ay":3.81129, "alpha":9.70577, "fx":[-123.08221,-134.67953,-122.93014,-97.79497], "fy":[57.99188,13.57084,51.13279,93.40098]}, - {"t":5.65903, "x":6.93893, "y":4.31161, "heading":0.42235, "vx":2.36668, "vy":0.46793, "omega":3.09713, "ax":-8.18333, "ay":4.59876, "alpha":8.6683, "fx":[-121.51041,-133.17816,-113.29765,-96.00105], "fy":[62.70557,28.02037,73.24648,96.77275]}, - {"t":5.70094, "x":7.03094, "y":4.33527, "heading":0.55978, "vx":2.02367, "vy":0.66069, "omega":3.46046, "ax":-7.39591, "ay":6.0246, "alpha":6.3416, "fx":[-112.83285,-121.42146,-95.18207,-89.90484], "fy":[77.95148,62.65467,97.59824,103.38473]}, - {"t":5.74286, "x":7.10927, "y":4.36825, "heading":0.7104, "vx":1.71367, "vy":0.91321, "omega":3.72627, "ax":-6.14151, "ay":7.48092, "alpha":2.30815, "fx":[-92.77221,-93.01684,-80.69258,-81.73595], "fy":[101.46795,101.0199,111.11744,110.55562]}, - {"t":5.78477, "x":7.1757, "y":4.4131, "heading":0.86861, "vx":1.45625, "vy":1.22678, "omega":3.82302, "ax":-4.93121, "ay":8.3676, "alpha":-2.32235, "fx":[-61.14559,-66.22761,-77.95552,-74.26594], "fy":[123.55092,121.01818,113.77882,116.08679]}, - {"t":5.82669, "x":7.23241, "y":4.47187, "heading":1.02682, "vx":1.24955, "vy":1.57751, "omega":3.72568, "ax":-4.79076, "ay":7.4868, "alpha":-17.51407, "fx":[9.48675,-57.0679,-115.38827,-108.66215], "fy":[137.88545,126.15858,76.36791,84.08246]}, - {"t":5.8686, "x":7.28057, "y":4.54457, "heading":1.1676, "vx":1.04875, "vy":1.89132, "omega":2.99157, "ax":-2.04316, "ay":1.49983, "alpha":-55.26868, "fx":[87.49447,-69.24858,-138.65077,4.55967], "fy":[107.54195,120.22344,-4.30867,-138.41793]}, - {"t":5.91052, "x":7.32274, "y":4.62517, "heading":1.24444, "vx":0.96311, "vy":1.95419, "omega":0.67496, "ax":-2.167, "ay":1.16753, "alpha":-55.53185, "fx":[82.23533,-80.86859,-137.25056,13.01716], "fy":[111.62421,112.74585,-20.23127,-137.94108]}, - {"t":5.95338, "x":7.36203, "y":4.71, "heading":1.22236, "vx":0.87023, "vy":2.00423, "omega":-1.70522, "ax":-5.62061, "ay":1.97656, "alpha":-40.95633, "fx":[4.56373,-101.82637,-138.21329,-83.20724], "fy":[137.9378,93.94941,-9.64984,-110.16852]}, - {"t":5.99624, "x":7.39416, "y":4.79772, "heading":1.11165, "vx":0.62932, "vy":2.08895, "omega":-3.46067, "ax":-9.66588, "ay":0.6998, "alpha":3.92578, "fx":[-138.03455,-137.72442,-136.67958,-135.60741], "fy":[3.64973,-7.68459,17.95393,25.75871]}, - {"t":6.0391, "x":7.41226, "y":4.8879, "heading":0.96693, "vx":0.21502, "vy":2.11894, "omega":-3.2924, "ax":-9.63104, "ay":1.08037, "alpha":2.7737, "fx":[-137.26227,-137.6241,-136.18436,-135.00013], "fy":[11.94467,2.6971,19.39743,27.21704]}, - {"t":6.08197, "x":7.41263, "y":4.97971, "heading":0.82836, "vx":-0.19778, "vy":2.16525, "omega":-3.17352, "ax":-9.51879, "ay":1.64648, "alpha":2.2158, "fx":[-135.57123,-136.45798,-134.55906,-133.11804], "fy":[21.37947,13.2662,25.66608,33.04203]}, - {"t":6.12483, "x":7.39541, "y":5.07403, "heading":0.69437, "vx":-0.60577, "vy":2.23582, "omega":-3.07854, "ax":-9.13553, "ay":2.82109, "alpha":2.78463, "fx":[-130.73245,-132.92884,-128.60419,-125.71051], "fy":[37.82172,27.72222,42.7626,51.64647]}, - {"t":6.16769, "x":7.36105, "y":5.17245, "heading":0.56498, "vx":-0.99733, "vy":2.35673, "omega":-2.95919, "ax":-6.04959, "ay":6.79785, "alpha":7.61409, "fx":[-99.43355,-106.74013,-67.38901,-69.4431], "fy":[87.65003,74.58081,110.59941,112.60161]}, - {"t":6.21055, "x":7.31275, "y":5.27971, "heading":0.44514, "vx":-1.25662, "vy":2.6481, "omega":-2.63284, "ax":4.67621, "ay":7.5045, "alpha":7.52762, "fx":[37.07724,73.11053,91.33405,63.61482], "fy":[121.83232,100.20486,90.31467,113.14624]}, - {"t":6.25341, "x":7.26318, "y":5.4001, "heading":0.3392, "vx":-1.05619, "vy":2.96975, "omega":-2.31019, "ax":6.17086, "ay":3.50631, "alpha":2.42677, "fx":[82.47298,88.12084,92.20793,87.08013], "fy":[55.33293,44.08053,44.47676,54.91447]}, - {"t":6.29627, "x":7.22358, "y":5.53061, "heading":0.24241, "vx":-0.7917, "vy":3.12004, "omega":-2.20618, "ax":4.13495, "ay":-2.09776, "alpha":-2.97832, "fx":[63.11281,62.8727,54.01695,54.44537], "fy":[-32.69212,-22.36781,-26.66369,-37.21749]}, - {"t":6.33913, "x":7.19345, "y":5.66241, "heading":0.14512, "vx":-0.61447, "vy":3.03013, "omega":-2.33383, "ax":-4.39877, "ay":0.07198, "alpha":5.20468, "fx":[-70.06519,-68.29989,-54.51285,-56.52833], "fy":[9.20144,-10.24232,-8.25845,13.3808]}, - {"t":6.382, "x":7.16307, "y":5.79235, "heading":0.04987, "vx":-0.80301, "vy":3.03321, "omega":-2.11075, "ax":-6.56618, "ay":2.61495, "alpha":8.75813, "fx":[-98.54295,-106.09494,-89.81559,-77.84285], "fy":[49.19954,13.36564,21.11392,64.58599]}, - {"t":6.42486, "x":7.12262, "y":5.92476, "heading":-0.03256, "vx":-1.08445, "vy":3.14529, "omega":-1.73537, "ax":-7.16249, "ay":3.48775, "alpha":9.37519, "fx":[-103.82581,-116.87105,-103.69124,-81.71835], "fy":[62.55103,22.85696,31.5032,80.84101]}, - {"t":6.46772, "x":7.06956, "y":6.06278, "heading":-0.09833, "vx":-1.39144, "vy":3.29478, "omega":-1.33353, "ax":-8.01657, "ay":2.87091, "alpha":8.76428, "fx":[-113.26665,-124.73473,-118.0416,-98.48901], "fy":[56.6485,16.54376,17.81836,71.76744]}, - {"t":6.51058, "x":7.00256, "y":6.20664, "heading":-0.14743, "vx":-1.73504, "vy":3.41784, "omega":-0.95788, "ax":-8.94664, "ay":0.84192, "alpha":7.09773, "fx":[-126.84466,-130.6774,-127.10591,-122.63797], "fy":[31.39233,-5.00262,-13.90053,35.24673]}, - {"t":6.55344, "x":6.91997, "y":6.3539, "heading":-0.18197, "vx":-2.11851, "vy":3.45392, "omega":-0.65366, "ax":-9.23201, "ay":-1.37127, "alpha":5.41491, "fx":[-133.90615,-131.08775,-126.18935,-132.26262], "fy":[0.28625,-28.8144,-42.26644,-6.95529]}, - {"t":6.5963, "x":6.82069, "y":6.50068, "heading":-0.20501, "vx":-2.51421, "vy":3.39515, "omega":-0.42157, "ax":-9.09592, "ay":-2.79605, "alpha":4.73062, "fx":[-134.37425,-128.52881,-121.66974,-131.15726], "fy":[-20.57541,-45.04202,-60.02932,-32.8865]}, - {"t":6.63916, "x":6.70457, "y":6.64364, "heading":-0.21874, "vx":-2.90407, "vy":3.2753, "omega":-0.21881, "ax":-8.90952, "ay":-3.55422, "alpha":5.05074, "fx":[-133.85033,-126.04357,-116.54571,-128.72191], "fy":[-29.53177,-54.35353,-71.92299,-45.71272]}, - {"t":6.68203, "x":6.57191, "y":6.78076, "heading":-0.22348, "vx":-3.28595, "vy":3.12296, "omega":-0.00233, "ax":-6.16324, "ay":-7.45195, "alpha":0.33837, "fx":[-88.5079,-87.75092,-86.22072,-86.97033], "fy":[-104.66919,-105.33846,-106.57787,-105.93268]}, - {"t":6.73592, "x":6.38587, "y":6.93825, "heading":-0.22311, "vx":-3.61812, "vy":2.72134, "omega":0.01591, "ax":-5.28446, "ay":-7.91048, "alpha":0.00538, "fx":[-74.92446,-74.91467,-74.88752,-74.89731], "fy":[-112.11648,-112.12418,-112.14188,-112.13418]}, - {"t":6.78982, "x":6.18319, "y":7.07342, "heading":-0.22225, "vx":-3.90292, "vy":2.29501, "omega":0.0162, "ax":-4.03515, "ay":-7.81621, "alpha":0.05599, "fx":[-57.37339,-57.31411,-57.02152,-57.08025], "fy":[-110.68744,-110.75748,-110.89822,-110.82863]}, - {"t":6.84371, "x":5.96698, "y":7.18576, "heading":-0.22129, "vx":-4.1204, "vy":1.87375, "omega":0.01922, "ax":-1.58486, "ay":-3.79911, "alpha":0.06013, "fx":[-22.56494,-22.58965,-22.36532,-22.34035], "fy":[-53.73752,-53.90267,-53.96544,-53.80036]}, - {"t":6.89761, "x":5.74261, "y":7.28123, "heading":-0.22017, "vx":-4.20581, "vy":1.669, "omega":0.02246, "ax":-0.48465, "ay":-1.30158, "alpha":0.03511, "fx":[-6.9156,-6.93994,-6.82412,-6.79975], "fy":[-18.38051,-18.49222,-18.51879,-18.40708]}, - {"t":6.9515, "x":5.51524, "y":7.36929, "heading":-0.21891, "vx":-4.23193, "vy":1.59885, "omega":0.02435, "ax":-1.35225, "ay":-5.22668, "alpha":0.74456, "fx":[-20.58745,-20.88104,-17.77891,-17.4241], "fy":[-72.8616,-74.56279,-75.30484,-73.6187]}, - {"t":7.0054, "x":5.28519, "y":7.44787, "heading":-0.21651, "vx":-4.30481, "vy":1.31716, "omega":0.06448, "ax":1.71794, "ay":-6.97394, "alpha":6.34331, "fx":[14.42764,4.96294,32.24446,45.77036], "fy":[-94.79642,-105.77596,-103.85264,-90.99083]}, - {"t":7.05929, "x":5.05568, "y":7.50873, "heading":-0.20383, "vx":-4.21223, "vy":0.9413, "omega":0.40635, "ax":2.28329, "ay":-3.99103, "alpha":5.4666, "fx":[26.34314,19.3328,38.00763,45.77664], "fy":[-48.18754,-65.01208,-64.63125,-48.45657]}, - {"t":7.11319, "x":4.83198, "y":7.55366, "heading":-0.17399, "vx":-4.08917, "vy":0.7262, "omega":0.70097, "ax":-0.26526, "ay":-3.12312, "alpha":0.39377, "fx":[-4.35936,-4.58208,-3.16483,-2.93369], "fy":[-43.57924,-44.73406,-44.95893,-43.80582]}, - {"t":7.16708, "x":4.61121, "y":7.58827, "heading":-0.13563, "vx":-4.10346, "vy":0.55788, "omega":0.7222, "ax":-3.46794, "ay":-2.86024, "alpha":-5.60183, "fx":[-39.6168,-40.47623,-59.16241,-57.37343], "fy":[-53.16317,-34.28694,-28.16348,-46.55909]}, - {"t":7.22098, "x":4.38501, "y":7.61418, "heading":-0.10485, "vx":-4.29037, "vy":0.40373, "omega":0.42028, "ax":-4.52263, "ay":-6.42494, "alpha":-7.70004, "fx":[-42.94716,-52.59282,-87.18726,-73.70152], "fy":[-107.54344,-92.48796,-71.57943,-92.67716]}, - {"t":7.27487, "x":4.14722, "y":7.62661, "heading":-0.09338, "vx":-4.53412, "vy":0.05745, "omega":0.00529, "ax":6.0206, "ay":-5.18306, "alpha":6.43705, "fx":[85.28433,67.59323,87.23655,101.24799], "fy":[-66.64593,-91.74424,-79.91142,-55.57294]}, - {"t":7.32877, "x":3.91159, "y":7.62218, "heading":-0.08375, "vx":-4.20964, "vy":-0.22189, "omega":0.35222, "ax":9.73141, "ay":0.37687, "alpha":-0.37914, "fx":[137.99955,137.90888,137.87785,137.97561], "fy":[3.95979,6.41952,6.74596,4.24275]}, - {"t":7.38266, "x":3.69885, "y":7.61077, "heading":-0.06531, "vx":-3.68516, "vy":-0.20158, "omega":0.33178, "ax":9.75577, "ay":0.48559, "alpha":-0.72372, "fx":[138.41837,138.19673,138.13981,138.38811], "fy":[4.27294,8.9454,9.57307,4.741]}, - {"t":7.43656, "x":3.51441, "y":7.60061, "heading":-0.04848, "vx":-3.15937, "vy":-0.1754, "omega":0.29278, "ax":9.7637, "ay":0.52063, "alpha":-0.83052, "fx":[138.55663,138.28276,138.22288,138.52998], "fy":[4.42161,9.7813,10.44392,4.87242]}, - {"t":7.49045, "x":3.35831, "y":7.59191, "heading":-0.03391, "vx":-2.63316, "vy":-0.14735, "omega":0.24802, "ax":9.76771, "ay":0.53661, "alpha":-0.87982, "fx":[138.62528,138.32463,138.26732,138.60268], "fy":[4.50889,10.18865,10.82324,4.90426]}, - {"t":7.54435, "x":3.23058, "y":7.58475, "heading":-0.02182, "vx":-2.10673, "vy":-0.11842, "omega":0.2006, "ax":9.77019, "ay":0.54484, "alpha":-0.90783, "fx":[138.6665,138.34996,138.29654,138.64745], "fy":[4.55937,10.42243,11.01458,4.8956]}, - {"t":7.59824, "x":3.13123, "y":7.57916, "heading":-0.01233, "vx":-1.58016, "vy":-0.08906, "omega":0.15167, "ax":9.77191, "ay":0.54918, "alpha":-0.92604, "fx":[138.69426,138.36752,138.31785,138.6781], "fy":[4.58406,10.5667,11.11861,4.86874]}, - {"t":7.65214, "x":3.06026, "y":7.57515, "heading":-0.0055, "vx":-1.05351, "vy":-0.05946, "omega":0.10176, "ax":9.77319, "ay":0.55129, "alpha":-0.93908, "fx":[138.7145,138.38096,138.33428,138.70055], "fy":[4.58881,10.65684,11.17779,4.83433]}, - {"t":7.70603, "x":3.01768, "y":7.57275, "heading":-0.00138, "vx":-0.52678, "vy":-0.02975, "omega":0.05115, "ax":9.77419, "ay":0.55201, "alpha":-0.94907, "fx":[138.73018,138.39213,138.34735,138.71774], "fy":[4.57708,10.71012,11.21297,4.7982]}, - {"t":7.75993, "x":3.00348, "y":7.57195, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":3.70343, "x":3.92744, "y":7.65773, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.74871, "ay":-0.86137, "alpha":-1.34725, "fx":[137.86166,138.59496,138.55933,137.72643], "fy":[-16.11136,-7.52895,-8.02918,-17.16938]}, + {"t":3.75549, "x":3.94065, "y":7.65656, "heading":-0.00183, "vx":0.50753, "vy":-0.04484, "omega":-0.07014, "ax":9.74469, "ay":-0.85316, "alpha":-1.72753, "fx":[137.72876,138.64364,138.60875,137.53367], "fy":[-17.05986,-6.1608,-6.66873,-18.48379]}, + {"t":3.80756, "x":3.98028, "y":7.65307, "heading":-0.00782, "vx":1.01486, "vy":-0.08926, "omega":-0.16008, "ax":9.73663, "ay":-0.85913, "alpha":-2.22086, "fx":[137.51432,138.67491,138.64298,137.22568], "fy":[-18.51514,-4.6833,-5.08542,-20.42811]}, + {"t":3.85962, "x":4.04631, "y":7.64726, "heading":-0.01916, "vx":1.52176, "vy":-0.13399, "omega":-0.2757, "ax":9.72169, "ay":-0.88211, "alpha":-2.8978, "fx":[137.16253,138.68192,138.65148,136.71442], "fy":[-20.6994,-2.98013,-3.02723,-23.30818]}, + {"t":3.91168, "x":4.13871, "y":7.63909, "heading":-0.03744, "vx":2.02789, "vy":-0.17991, "omega":-0.42656, "ax":9.69154, "ay":-0.93175, "alpha":-3.94341, "fx":[136.52237,138.64408,138.5916,135.74289], "fy":[-24.18888,-0.78483,0.12062,-27.97621]}, + {"t":3.96374, "x":4.25742, "y":7.62846, "heading":-0.06499, "vx":2.53245, "vy":-0.22842, "omega":-0.63187, "ax":9.61234, "ay":-1.04027, "alpha":-5.9847, "fx":[135.06327,138.48093,138.22774,133.2385], "fy":[-30.71004,2.68572,6.43282,-37.39109]}, + {"t":4.0158, "x":4.40229, "y":7.61516, "heading":-0.106, "vx":3.03288, "vy":-0.28258, "omega":-0.94344, "ax":9.11278, "ay":-1.37587, "alpha":-13.83943, "fx":[129.01975,137.54963,133.55633,116.56029], "fy":[-49.14987,11.7609,32.24415,-72.86582]}, + {"t":4.06786, "x":4.57254, "y":7.59858, "heading":-0.17387, "vx":3.50731, "vy":-0.35421, "omega":-1.66394, "ax":3.32668, "ay":-8.35994, "alpha":6.54803, "fx":[39.07666,22.62495,53.56783,73.35001], "fy":[-120.531,-127.90019,-119.467,-106.1024]}, + {"t":4.11993, "x":4.75964, "y":7.56881, "heading":-0.25163, "vx":3.6805, "vy":-0.78944, "omega":-1.32304, "ax":-3.05188, "ay":-7.52814, "alpha":-2.26045, "fx":[-37.29511,-37.48189,-49.71268,-48.549], "fy":[-110.19647,-107.65004,-102.97689,-106.01493]}, + {"t":4.17199, "x":4.94712, "y":7.51751, "heading":-0.32357, "vx":3.52162, "vy":-1.18137, "omega":-1.44072, "ax":-2.50681, "ay":-4.85158, "alpha":-1.01458, "fx":[-33.76734,-33.26052,-37.35152,-37.75459], "fy":[-70.77641,-68.45122,-66.74009,-69.11221]}, + {"t":4.22405, "x":5.12706, "y":7.44943, "heading":-0.39995, "vx":3.39111, "vy":-1.43395, "omega":-1.49354, "ax":-5.39626, "ay":-3.86569, "alpha":-5.12996, "fx":[-68.05758,-68.65517,-85.38748,-83.8623], "fy":[-69.13804,-54.91303,-39.62467,-55.50531]}, + {"t":4.27611, "x":5.29629, "y":7.36954, "heading":-0.48466, "vx":3.11017, "vy":-1.63521, "omega":-1.76062, "ax":-7.87487, "ay":-2.94856, "alpha":-8.08004, "fx":[-99.27284,-107.50717,-121.77186,-117.94551], "fy":[-71.98205,-42.97509,-9.78354,-42.43998]}, + {"t":4.32817, "x":5.44754, "y":7.28041, "heading":-0.58727, "vx":2.70019, "vy":-1.78871, "omega":-2.18128, "ax":-8.72643, "ay":-1.10631, "alpha":-8.76893, "fx":[-116.42542,-122.65083,-126.7374,-128.96634], "fy":[-53.29783,-11.72535,20.92618,-18.62964]}, + {"t":4.38023, "x":5.57629, "y":7.18579, "heading":-0.71272, "vx":2.24588, "vy":-1.84631, "omega":-2.6378, "ax":-8.60535, "ay":1.57928, "alpha":-8.90067, "fx":[-125.7594,-117.63126,-115.702,-128.82274], "fy":[-16.41788,34.76326,55.54284,15.6556]}, + {"t":4.4323, "x":5.68156, "y":7.0918, "heading":-0.86211, "vx":1.79787, "vy":-1.76409, "omega":-3.10119, "ax":-8.61561, "ay":-1.84984, "alpha":-5.64283, "fx":[-117.71793,-118.76139,-125.6844,-126.33326], "fy":[-47.57345,-33.88366,-2.36824,-21.05878]}, + {"t":4.48436, "x":5.76348, "y":6.99746, "heading":-1.03121, "vx":1.34933, "vy":-1.8604, "omega":-3.39496, "ax":-4.37992, "ay":-8.278, "alpha":6.42167, "fx":[-69.88648,-85.29541,-57.96209,-35.19359], "fy":[-113.40782,-104.25295,-122.12352,-129.57024]}, + {"t":4.53642, "x":5.82779, "y":6.88938, "heading":-1.19925, "vx":1.1213, "vy":-2.29136, "omega":-3.06064, "ax":-3.43566, "ay":-8.75715, "alpha":9.89765, "fx":[-47.21099,-90.4593,-51.7161,-5.41198], "fy":[-128.06763,-103.43796,-127.67381,-137.34261]}, + {"t":4.58848, "x":5.88151, "y":6.75822, "heading":-1.34518, "vx":0.94243, "vy":-2.74727, "omega":-2.54535, "ax":-3.54767, "ay":-8.7, "alpha":11.0345, "fx":[-38.76354,-97.48855,-58.79599,-6.10122], "fy":[-132.08244,-97.87347,-125.23559,-138.09022]}, + {"t":4.64054, "x":5.92577, "y":6.6034, "heading":-1.46274, "vx":0.75774, "vy":-3.20021, "omega":-1.97088, "ax":-3.82742, "ay":-8.55488, "alpha":11.79007, "fx":[-35.74754,-103.47644,-66.47494,-11.31237], "fy":[-133.44369,-91.96457,-121.59445,-138.05068]}, + {"t":4.6926, "x":5.96003, "y":6.4252, "heading":-1.54937, "vx":0.55847, "vy":-3.64559, "omega":-1.35707, "ax":-1.50838, "ay":-3.2129, "alpha":50.69363, "fx":[124.59695,-138.36596,-91.94245,20.18785], "fy":[53.87033,4.66995,-103.70597,-137.00284]}, + {"t":4.73167, "x":5.9807, "y":6.28032, "heading":-1.5637, "vx":0.49954, "vy":-3.77112, "omega":0.62347, "ax":-7.51967, "ay":-5.85176, "alpha":8.23882, "fx":[-112.57036,-127.30346,-106.34715,-80.13688], "fy":[-78.7127,-52.95282,-88.01665,-112.10681]}, + {"t":4.77074, "x":5.99448, "y":6.12852, "heading":-1.53306, "vx":0.20576, "vy":-3.99974, "omega":0.94535, "ax":-8.20337, "ay":-4.69419, "alpha":-8.86837, "fx":[-114.15338,-92.60909,-125.79077,-132.56975], "fy":[-76.66989,-100.98643,-52.74092,-35.75859]}, + {"t":4.80981, "x":5.99626, "y":5.96867, "heading":-1.50289, "vx":-0.11474, "vy":-4.18313, "omega":0.59888, "ax":-8.71585, "ay":-3.62461, "alpha":-7.55724, "fx":[-120.78748,-108.63862,-130.96198,-133.79236], "fy":[-63.75224,-81.46789,-33.83826,-26.45363]}, + {"t":4.84888, "x":5.98512, "y":5.80248, "heading":-1.48526, "vx":-0.45525, "vy":-4.32474, "omega":0.30363, "ax":-9.36843, "ay":0.83023, "alpha":-1.24785, "fx":[-133.30908,-132.91859,-132.22727,-132.72628], "fy":[7.29737,8.42733,16.44924,14.89948]}, + {"t":4.88795, "x":5.96019, "y":5.63415, "heading":-1.47435, "vx":-0.82127, "vy":-4.29231, "omega":0.25487, "ax":-2.06919, "ay":8.79946, "alpha":11.80802, "fx":[0.46317,-53.06426,-74.48634,9.76646], "fy":[134.53584,123.82845,109.37894,131.17794]}, + {"t":4.92702, "x":5.92652, "y":5.47317, "heading":-1.45538, "vx":-0.90211, "vy":-3.94852, "omega":0.7162, "ax":2.99836, "ay":8.63644, "alpha":12.76523, "fx":[58.51039,-0.07183,15.36809,96.19738], "fy":[123.75249,136.54529,133.3596,96.02041]}, + {"t":4.96609, "x":5.89356, "y":5.3255, "heading":-1.41766, "vx":-0.78497, "vy":-3.61111, "omega":1.21492, "ax":4.9524, "ay":7.8246, "alpha":11.8977, "fx":[76.22235,22.97978,67.12543,114.46904], "fy":[114.62374,135.36003,117.99471,75.66866]}, + {"t":5.00515, "x":5.86668, "y":5.19039, "heading":-1.36111, "vx":-0.59148, "vy":-3.30541, "omega":1.67975, "ax":5.85931, "ay":7.30135, "alpha":10.96784, "fx":[83.04735,38.82084,91.91232,118.43651], "fy":[110.20132,132.08972,101.33633,70.35213]}, + {"t":5.04422, "x":5.84804, "y":5.06682, "heading":-1.28712, "vx":-0.36257, "vy":-3.02015, "omega":2.10825, "ax":6.42982, "ay":6.9226, "alpha":9.98592, "fx":[87.17525,52.99653,104.99547,119.39738], "fy":[107.20611,127.31464,88.69852,69.28544]}, + {"t":5.08329, "x":5.83878, "y":4.95411, "heading":-1.19713, "vx":-0.11136, "vy":-2.7497, "omega":2.49839, "ax":6.89084, "ay":6.58073, "alpha":8.81101, "fx":[90.92038,67.25083,112.93803,119.59507], "fy":[104.20675,120.57714,79.03257,69.30449]}, + {"t":5.12236, "x":5.83969, "y":4.8517, "heading":-1.09279, "vx":0.15786, "vy":-2.49259, "omega":2.84263, "ax":7.31527, "ay":6.22898, "alpha":7.28459, "fx":[95.30822,82.0816,117.84967,119.52926], "fy":[100.32388,111.174,72.01645,69.66281]}, + {"t":5.16143, "x":5.85144, "y":4.75908, "heading":-0.97618, "vx":0.44366, "vy":-2.24924, "omega":3.12723, "ax":7.70956, "ay":5.86133, "alpha":5.2903, "fx":[101.01549,96.69225,120.24475,119.17222], "fy":[94.66555,98.89755,68.32559,70.44337]}, + {"t":5.2005, "x":5.87466, "y":4.67567, "heading":-0.84996, "vx":0.74486, "vy":-2.02024, "omega":3.33391, "ax":8.04323, "ay":5.50349, "alpha":2.84027, "fx":[108.47605,109.05094,120.11077,118.40559], "fy":[86.09613,85.26732,68.83668,71.84263]}, + {"t":5.23957, "x":5.9099, "y":4.60095, "heading":-0.71754, "vx":1.0591, "vy":-1.80522, "omega":3.44488, "ax":8.08539, "ay":5.48349, "alpha":0.47234, "fx":[113.54456,113.97305,115.68236,115.23373], "fy":[79.29458,78.66103,76.1295,76.82341]}, + {"t":5.28263, "x":5.963, "y":4.5283, "heading":-0.56877, "vx":1.40725, "vy":-1.56911, "omega":3.46522, "ax":7.89054, "ay":5.73034, "alpha":-1.6815, "fx":[116.09358,113.69006,107.66996,109.93271], "fy":[75.21449,78.87196,86.88648,83.93189]}, + {"t":5.32569, "x":6.03091, "y":4.46604, "heading":-0.42112, "vx":1.74702, "vy":-1.32236, "omega":3.39281, "ax":7.70577, "ay":5.89264, "alpha":-3.86165, "fx":[119.59842,112.49128,98.86902,105.95102], "fy":[68.99352,80.25898,96.46331,88.39135]}, + {"t":5.36875, "x":6.11328, "y":4.41457, "heading":-0.2786, "vx":2.07883, "vy":-1.06863, "omega":3.22653, "ax":7.56217, "ay":5.93214, "alpha":-5.93883, "fx":[123.33085,110.72589,90.11756,104.59353], "fy":[61.09377,82.1277,104.12851,88.99655]}, + {"t":5.41181, "x":6.2098, "y":4.37405, "heading":-0.14518, "vx":2.40445, "vy":-0.81319, "omega":2.97081, "ax":7.4857, "ay":5.80142, "alpha":-7.80954, "fx":[126.42961,108.77794,82.40145,106.82325], "fy":[52.17872,83.57526,109.19082,83.99003]}, + {"t":5.45487, "x":6.32028, "y":4.34441, "heading":-0.02449, "vx":2.72678, "vy":-0.56338, "omega":2.63453, "ax":7.41421, "ay":5.48516, "alpha":-9.27705, "fx":[127.00385,106.21533,76.69949,110.46012], "fy":[44.55264,83.88913,110.21295,72.3487]}, + {"t":5.49793, "x":6.44456, "y":4.32524, "heading":0.08035, "vx":3.04603, "vy":-0.32719, "omega":2.23506, "ax":6.1444, "ay":5.83396, "alpha":-8.34219, "fx":[109.44444,89.44091,61.32276,88.17369], "fy":[58.01902,89.16601,106.68646,76.90821]}, + {"t":5.54099, "x":6.58142, "y":4.31656, "heading":0.16886, "vx":3.31061, "vy":-0.07599, "omega":1.87585, "ax":-6.78521, "ay":4.36605, "alpha":9.06181, "fx":[-102.20755,-114.51073,-92.8926,-75.10396], "fy":[67.37643,31.60516,57.14775,91.42168]}, + {"t":5.58405, "x":6.71768, "y":4.31734, "heading":0.25803, "vx":3.01844, "vy":0.11201, "omega":2.26605, "ax":-8.59798, "ay":2.13988, "alpha":10.41973, "fx":[-125.42975,-130.86969,-125.0097,-106.18815], "fy":[41.68308,-7.1252,12.46526,74.30612]}, + {"t":5.6271, "x":6.83969, "y":4.32414, "heading":0.36526, "vx":2.64822, "vy":0.20416, "omega":2.71472, "ax":-8.77056, "ay":2.4026, "alpha":10.27503, "fx":[-128.25863,-133.54124,-127.88819,-107.59415], "fy":[41.06258,-7.55222,24.11249,78.60213]}, + {"t":5.67016, "x":6.94559, "y":4.33516, "heading":0.49169, "vx":2.27056, "vy":0.30761, "omega":3.15716, "ax":-8.36154, "ay":3.95823, "alpha":9.45183, "fx":[-124.84528,-133.97713,-115.76064,-99.50845], "fy":[53.347,14.63643,65.04308,91.40149]}, + {"t":5.71322, "x":7.0356, "y":4.35208, "heading":0.63639, "vx":1.91051, "vy":0.47805, "omega":3.56415, "ax":-7.08474, "ay":6.29338, "alpha":6.51779, "fx":[-110.51472,-117.45633,-87.49385,-86.23287], "fy":[80.02097,67.90078,103.31952,105.58739]}, + {"t":5.75628, "x":7.1113, "y":4.37849, "heading":0.79591, "vx":1.60545, "vy":0.74904, "omega":3.84481, "ax":-5.25103, "ay":8.11499, "alpha":1.34547, "fx":[-78.7564,-77.31193,-69.86449,-71.79556], "fy":[112.25362,113.11232,117.89069,116.85572]}, + {"t":5.79934, "x":7.17556, "y":4.41827, "heading":0.96271, "vx":1.37934, "vy":1.09847, "omega":3.90274, "ax":-3.97921, "ay":8.83128, "alpha":-3.53766, "fx":[-41.24307,-53.34154,-70.37533,-60.65762], "fy":[131.45613,127.18218,118.51804,123.56906]}, + {"t":5.8424, "x":7.23127, "y":4.47376, "heading":1.12748, "vx":1.208, "vy":1.47874, "omega":3.75041, "ax":-3.95471, "ay":8.18205, "alpha":-14.91377, "fx":[13.89745,-54.29113,-110.81111,-73.02371], "fy":[137.5556,127.36137,82.76375,116.23393]}, + {"t":5.88546, "x":7.27962, "y":4.54502, "heading":1.27515, "vx":1.03771, "vy":1.83106, "omega":3.10823, "ax":-1.68839, "ay":1.40495, "alpha":-56.08998, "fx":[80.29341,-75.55415,-137.74253,37.2729], "fy":[113.05524,116.37006,-16.38105,-133.38514]}, + {"t":5.92852, "x":7.32274, "y":4.62517, "heading":1.35699, "vx":0.96501, "vy":1.89156, "omega":0.69301, "ax":-1.86173, "ay":1.05633, "alpha":-56.30544, "fx":[73.737,-87.7189,-134.65727,43.08102], "fy":[117.44396,107.50834,-33.37031,-131.68881]}, + {"t":5.97234, "x":7.36323, "y":4.70906, "heading":1.3333, "vx":0.88343, "vy":1.93784, "omega":-1.77419, "ax":-5.19586, "ay":1.4814, "alpha":-43.90172, "fx":[8.62113,-105.32966,-137.06907,-60.82253], "fy":[137.84645,90.00347,-20.07374,-123.78231]}, + {"t":6.01616, "x":7.39696, "y":4.7954, "heading":1.21341, "vx":0.65576, "vy":2.00276, "omega":-3.69788, "ax":-9.6448, "ay":0.43011, "alpha":5.15471, "fx":[-137.99608,-136.81891,-136.43194,-135.60385], "fy":[-3.26595,-16.63703,18.7879,25.50174]}, + {"t":6.05998, "x":7.41643, "y":4.88357, "heading":1.05633, "vx":0.23314, "vy":2.0216, "omega":-3.47201, "ax":-9.63413, "ay":0.84984, "alpha":3.56586, "fx":[-137.55838,-137.48867,-136.17048,-135.02874], "fy":[6.8725,-4.03168,18.57437,26.76966]}, + {"t":6.1038, "x":7.4174, "y":4.97297, "heading":0.90761, "vx":-0.18901, "vy":2.05884, "omega":-3.31576, "ax":-9.53898, "ay":1.44786, "alpha":2.73394, "fx":[-136.09183,-136.77674,-134.71657,-133.26591], "fy":[17.47712,8.14346,24.23372,32.23802]}, + {"t":6.14761, "x":7.39996, "y":5.06457, "heading":0.76494, "vx":-0.60699, "vy":2.12228, "omega":-3.19597, "ax":-9.2092, "ay":2.57474, "alpha":2.98817, "fx":[-131.94617,-133.82393,-129.49767,-126.88511], "fy":[33.63528,23.2983,40.14152,48.91001]}, + {"t":6.19143, "x":7.36452, "y":5.16004, "heading":0.62777, "vx":-1.01052, "vy":2.2351, "omega":-3.06503, "ax":-7.19623, "ay":5.77405, "alpha":6.46786, "fx":[-111.17071,-117.56013,-90.73748,-88.5513], "fy":[73.81619,59.63351,94.54436,99.38896]}, + {"t":6.23525, "x":7.31333, "y":5.26352, "heading":0.49968, "vx":-1.32585, "vy":2.48811, "omega":-2.78162, "ax":3.85896, "ay":8.10226, "alpha":7.94593, "fx":[21.61817,60.96141,83.78505,52.43472], "fy":[127.87833,110.85958,99.89786,120.75471]}, + {"t":6.27907, "x":7.25894, "y":5.38032, "heading":0.38542, "vx":-1.15676, "vy":2.84314, "omega":-2.43345, "ax":5.73735, "ay":5.45252, "alpha":4.85408, "fx":[67.49524,84.75374,93.57329,79.48015], "fy":[89.12985,68.75288,66.45749,84.81244]}, + {"t":6.32289, "x":7.21376, "y":5.51014, "heading":0.28345, "vx":-0.90536, "vy":3.08206, "omega":-2.22075, "ax":6.81411, "ay":-0.59064, "alpha":-2.97014, "fx":[99.53195,98.80521,93.73716,94.27904], "fy":[-12.57796,0.67889,-3.63616,-17.95341]}, + {"t":6.3667, "x":7.18063, "y":5.64462, "heading":0.18329, "vx":-0.60678, "vy":3.05618, "omega":-2.35089, "ax":-1.16954, "ay":-1.16116, "alpha":1.86548, "fx":[-20.26467,-18.94827,-12.9071,-14.1918], "fy":[-13.89624,-19.97957,-19.06246,-12.89851]}, + {"t":6.41052, "x":7.15292, "y":5.77742, "heading":0.08207, "vx":-0.65802, "vy":3.0053, "omega":-2.26915, "ax":-6.87238, "ay":2.38423, "alpha":9.08148, "fx":[-102.96562,-109.67767,-94.37142,-82.64243], "fy":[46.34098,8.4328,16.75818,63.65146]}, + {"t":6.45434, "x":7.11749, "y":5.9114, "heading":-0.00864, "vx":-0.95916, "vy":3.10977, "omega":-1.87122, "ax":-7.6071, "ay":3.28328, "alpha":9.59891, "fx":[-109.51618,-122.00996,-111.26744,-88.52171], "fy":[60.24639,18.222,27.0058,80.6848]}, + {"t":6.49816, "x":7.06816, "y":6.05082, "heading":-0.08142, "vx":-1.29249, "vy":3.25364, "omega":-1.45061, "ax":-8.32685, "ay":2.76073, "alpha":8.88789, "fx":[-117.21409,-128.33208,-123.16357,-103.41474], "fy":[55.50537,13.91842,15.15058,71.95662]}, + {"t":6.54198, "x":7.00353, "y":6.19604, "heading":-0.13645, "vx":-1.65735, "vy":3.37461, "omega":-1.06116, "ax":-9.06822, "ay":1.07211, "alpha":7.42306, "fx":[-128.00669,-132.65925,-129.80101,-123.69272], "fy":[34.87032,-3.25749,-11.6136,40.78818]}, + {"t":6.5858, "x":6.9222, "y":6.34493, "heading":-0.17582, "vx":-2.05471, "vy":3.42159, "omega":-0.7359, "ax":-9.34613, "ay":-0.88553, "alpha":5.96569, "fx":[-134.65645,-132.99405,-128.70276,-133.56354], "fy":[8.28253,-23.93744,-37.61234,3.05865]}, + {"t":6.62961, "x":6.8232, "y":6.49401, "heading":-0.20234, "vx":-2.46424, "vy":3.38278, "omega":-0.47449, "ax":-9.24704, "ay":-2.3149, "alpha":5.27194, "fx":[-135.87305,-130.66557,-124.13399,-133.62582], "fy":[-12.0003,-39.85716,-55.80144,-23.59351]}, + {"t":6.67343, "x":6.70634, "y":6.64002, "heading":-0.21807, "vx":-2.86943, "vy":3.28135, "omega":-0.24348, "ax":-9.05649, "ay":-3.16894, "alpha":5.48435, "fx":[-135.45305,-128.01483,-118.70668,-131.31991], "fy":[-22.43185,-50.09289,-68.71304,-38.43791]}, + {"t":6.71725, "x":6.57191, "y":6.78076, "heading":-0.22348, "vx":-3.26627, "vy":3.14249, "omega":-0.00317, "ax":-6.21545, "ay":-7.42233, "alpha":0.37187, "fx":[-89.35934,-88.5191,-86.84996,-87.68191], "fy":[-104.14325,-104.89253,-106.2616,-105.54139]}, + {"t":6.77116, "x":6.38681, "y":6.93937, "heading":-0.22311, "vx":-3.60132, "vy":2.74238, "omega":0.01688, "ax":-5.35152, "ay":-7.90609, "alpha":0.00079, "fx":[-75.8592,-75.85772,-75.85373,-75.85521], "fy":[-112.065,-112.06615,-112.06879,-112.06764]}, + {"t":6.82506, "x":6.1849, "y":7.07572, "heading":-0.2222, "vx":-3.8898, "vy":2.31619, "omega":0.01692, "ax":-4.15861, "ay":-7.96639, "alpha":0.05511, "fx":[-59.12527,-59.06156,-58.7697,-58.83286], "fy":[-112.81713,-112.88224,-113.02609,-112.96146]}, + {"t":6.87897, "x":5.96917, "y":7.189, "heading":-0.2212, "vx":-4.11398, "vy":1.88675, "omega":0.01989, "ax":-1.90359, "ay":-4.59766, "alpha":0.07981, "fx":[-27.13046,-27.15179,-26.83578,-26.81387], "fy":[-65.02293,-65.22382,-65.3184,-65.11768]}, + {"t":6.93288, "x":5.74463, "y":7.28403, "heading":-0.22002, "vx":-4.2166, "vy":1.6389, "omega":0.02419, "ax":-0.48759, "ay":-1.34709, "alpha":0.04125, "fx":[-6.96536,-6.99389,-6.85766,-6.82909], "fy":[-19.01354,-19.14459,-19.17582,-19.04477]}, + {"t":6.98678, "x":5.51662, "y":7.37042, "heading":-0.21865, "vx":-4.24288, "vy":1.56629, "omega":0.02642, "ax":-1.1845, "ay":-4.82568, "alpha":0.7529, "fx":[-18.14678,-18.49423,-15.46108,-15.05784], "fy":[-67.1351,-68.96131,-69.664,-67.85079]}, + {"t":7.04069, "x":5.28618, "y":7.44784, "heading":-0.21613, "vx":-4.30673, "vy":1.30615, "omega":0.067, "ax":1.7071, "ay":-6.71589, "alpha":6.18905, "fx":[14.77538,5.71374,31.81593,44.48601], "fy":[-90.60904,-102.17695,-100.52766,-87.47111]}, + {"t":7.0946, "x":5.0565, "y":7.50849, "heading":-0.20353, "vx":-4.21471, "vy":0.94412, "omega":0.40063, "ax":2.28333, "ay":-4.02321, "alpha":5.48583, "fx":[26.31119,19.25412,38.03128,45.86592], "fy":[-48.66006,-65.49425,-65.07724,-48.88075]}, + {"t":7.1485, "x":4.83262, "y":7.55354, "heading":-0.17396, "vx":-4.09162, "vy":0.72724, "omega":0.69636, "ax":-0.21608, "ay":-3.13295, "alpha":0.48351, "fx":[-3.79714,-4.07464,-2.33501,-2.04483], "fy":[-43.56393,-44.98171,-45.2523,-43.83717]}, + {"t":7.20241, "x":4.61174, "y":7.58819, "heading":-0.13572, "vx":-4.10327, "vy":0.55835, "omega":0.72242, "ax":-3.45903, "ay":-2.85851, "alpha":-5.58493, "fx":[-39.52277,-40.36433,-59.00326,-57.23344], "fy":[-53.0897,-34.2809,-28.18546,-46.51847]}, + {"t":7.25632, "x":4.38552, "y":7.61414, "heading":-0.10489, "vx":-4.28974, "vy":0.40426, "omega":0.42136, "ax":-4.53325, "ay":-6.42101, "alpha":-7.71829, "fx":[-43.04441,-52.75404,-87.38345,-73.84926], "fy":[-107.55704,-92.43992,-71.43904,-92.62895]}, + {"t":7.31022, "x":4.14769, "y":7.6266, "heading":-0.09339, "vx":-4.53411, "vy":0.05813, "omega":0.00529, "ax":6.00218, "ay":-5.19478, "alpha":6.4311, "fx":[84.97119,67.32245,87.00685,101.01757], "fy":[-66.86777,-91.8438,-80.04346,-55.78394]}, + {"t":7.36413, "x":3.91199, "y":7.62219, "heading":-0.08376, "vx":-4.21055, "vy":-0.22191, "omega":0.35197, "ax":9.73144, "ay":0.37701, "alpha":-0.3756, "fx":[137.99935,137.90946,137.87876,137.9756], "fy":[3.97456,6.41152,6.7348,4.25517]}, + {"t":7.41804, "x":3.69915, "y":7.61077, "heading":-0.06534, "vx":-3.68596, "vy":-0.20158, "omega":0.33172, "ax":9.75579, "ay":0.48558, "alpha":-0.72232, "fx":[138.4184,138.19718,138.1404,138.38819], "fy":[4.27773,8.94127,9.56766,4.74514]}, + {"t":7.47194, "x":3.51463, "y":7.60061, "heading":-0.0485, "vx":-3.16006, "vy":-0.17541, "omega":0.29278, "ax":9.76371, "ay":0.52057, "alpha":-0.82982, "fx":[138.55672,138.28311,138.2233,138.53008], "fy":[4.42314,9.77834,10.44038,4.87372]}, + {"t":7.52585, "x":3.35847, "y":7.59191, "heading":-0.03393, "vx":-2.63373, "vy":-0.14735, "omega":0.24805, "ax":9.76773, "ay":0.53651, "alpha":-0.87945, "fx":[138.62539,138.32491,138.26764,138.6028], "fy":[4.50882,10.18627,10.82059,4.90412]}, + {"t":7.57976, "x":3.23068, "y":7.58475, "heading":-0.02183, "vx":-2.10719, "vy":-0.11842, "omega":0.20064, "ax":9.7702, "ay":0.54473, "alpha":-0.90767, "fx":[138.66662,138.3502,138.29681,138.64758], "fy":[4.55838,10.42042,11.01244,4.89457]}, + {"t":7.63366, "x":3.13129, "y":7.57916, "heading":-0.01233, "vx":-1.58051, "vy":-0.08906, "omega":0.15171, "ax":9.77192, "ay":0.54906, "alpha":-0.92602, "fx":[138.69439,138.36773,138.31807,138.67824], "fy":[4.58247,10.56494,11.11679,4.86711]}, + {"t":7.68757, "x":3.06029, "y":7.57515, "heading":-0.0055, "vx":-1.05373, "vy":-0.05946, "omega":0.10179, "ax":9.7732, "ay":0.55116, "alpha":-0.93914, "fx":[138.71463,138.38115,138.33447,138.70069], "fy":[4.58679,10.65526,11.17621,4.83226]}, + {"t":7.74148, "x":3.01768, "y":7.57275, "heading":-0.00138, "vx":-0.52689, "vy":-0.02975, "omega":0.05117, "ax":9.7742, "ay":0.55188, "alpha":-0.94921, "fx":[138.73032,138.3923,138.34753,138.71789], "fy":[4.57475,10.70869,11.21156,4.79581]}, + {"t":7.79538, "x":3.00348, "y":7.57195, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0,75] }, "events":[ {"name":"hoodDown", "from":{"target":0, "targetTimestamp":0.0, "offset":{"exp":"0 s", "val":0.0}}, "event":{"type":"named", "data":{"name":"hoodDown"}}}, {"name":"hoodUp", "from":{"target":4, "targetTimestamp":3.70343, "offset":{"exp":"-0.2 s", "val":-0.2}}, "event":{"type":"named", "data":{"name":"hoodUp"}}}, - {"name":"hoodUp", "from":{"target":9, "targetTimestamp":7.75993, "offset":{"exp":"-0.2 s", "val":-0.2}}, "event":{"type":"named", "data":{"name":"hoodUp"}}}, + {"name":"hoodUp", "from":{"target":9, "targetTimestamp":7.79538, "offset":{"exp":"-0.2 s", "val":-0.2}}, "event":{"type":"named", "data":{"name":"hoodUp"}}}, {"name":"hoodDown", "from":{"target":4, "targetTimestamp":3.70343, "offset":{"exp":"0.2 s", "val":0.2}}, "event":{"type":"named", "data":{"name":"hoodDown"}}}] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a014318..0f1afad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -361,11 +361,15 @@ public class RobotContainer { addAuto(leftDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(true)); addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(false)); + ChoreoPathCommand choreo = new ChoreoPathCommand(intake, spindexer, turret, hood); + addAuto("testChoreo", ChoreoPathCommand.basicTrajectoryAuto("test.traj", true, autoFactory)); - addChoreoAuto("choreoLiberalLeft", ChoreoPathCommand.leftConservative(autoFactory, intake, spindexer, turret, hood)); + addChoreoAuto("choreoLiberalLeft", choreo.leftLiberal(autoFactory)); + addChoreoAuto("choreoLiberalRight", choreo.rightLiberal(autoFactory)); // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); + SmartDashboard.putData("Choreo auto chooser", choreoAutoChooser); } public static BooleanSupplier getAllianceColorBooleanSupplier() { diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java index 5b0469d..dab46ce 100644 --- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java +++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java @@ -21,7 +21,15 @@ import frc.robot.subsystems.turret.Turret; public class ChoreoPathCommand { - public ChoreoPathCommand(Intake intake, Spindexer spindexer, Turret turret) { + private Intake intake; + private Spindexer spindexer; + private Turret turret; + private Hood hood; + + public ChoreoPathCommand(Intake intake, Spindexer spindexer, Turret turret, Hood hood) { + this.intake = intake; + this.spindexer = spindexer; + this.turret = turret; } @@ -33,8 +41,7 @@ public class ChoreoPathCommand { command); } - public static AutoRoutine leftConservative(AutoFactory factory, Intake intake, Spindexer spindexer, Turret turret, - Hood hood) { + public AutoRoutine leftLiberal(AutoFactory factory) { AutoRoutine routine = factory.newRoutine("leftLiberal"); AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0); @@ -62,4 +69,33 @@ public class ChoreoPathCommand { return routine; } + + public AutoRoutine rightLiberal(AutoFactory factory) { + AutoRoutine routine = factory.newRoutine("leftLiberal"); + + AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0).mirrorY(); + AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1).mirrorY(); + + routine.active().onTrue( + Commands.sequence( + liberalSwipe.resetOdometry(), + new InstantCommand(() -> { + intake.extend(); + intake.spinStart(); + }), + liberalSwipe.cmd())); + + liberalSwipe.done() + .onTrue(Commands.sequence( + new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), + shallowSwipe.cmd())); + + shallowSwipe.done() + .onTrue(Commands.sequence( + new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), + shallowSwipe.cmd())); + + return routine; + + } } -- 2.39.5