diff --git a/src/main/deploy/choreo/DepotToOutpost.traj b/src/main/deploy/choreo/DepotToOutpost.traj index 1a92d88..27036fd 100644 --- a/src/main/deploy/choreo/DepotToOutpost.traj +++ b/src/main/deploy/choreo/DepotToOutpost.traj @@ -16,7 +16,7 @@ "waypoints":[ {"x":{"exp":"Depot.x", "val":1.15}, "y":{"exp":"Depot.y", "val":5.9625654220581055}, "heading":{"exp":"Depot.heading", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.899867296218872 m", "val":1.899867296218872}, "y":{"exp":"3.7941935062408447 m", "val":3.7941935062408447}, "heading":{"exp":"Depot.heading", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":4.71238898038469}, "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}, diff --git a/src/main/deploy/choreo/HubToOutpost.traj b/src/main/deploy/choreo/HubToOutpost.traj index 108ef2d..6b4e582 100644 --- a/src/main/deploy/choreo/HubToOutpost.traj +++ b/src/main/deploy/choreo/HubToOutpost.traj @@ -3,9 +3,9 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.61569, "y":4.03479, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.163153171539306, "y":2.451129674911499, "heading":2.356194490192345, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":0.6543505787849426, "y":0.6869499087333679, "heading":3.141592653589793, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.61569, "y":4.03479, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.163153171539306, "y":2.451129674911499, "heading":-0.7370179062160626, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.6543505787849426, "y":0.6869499087333679, "heading":4.71238898038469, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,9 +14,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"HubStart.x", "val":3.61569}, "y":{"exp":"HubStart.y", "val":4.03479}, "heading":{"exp":"HubStart.heading", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.1631531715393066 m", "val":2.163153171539306}, "y":{"exp":"2.451129674911499 m", "val":2.451129674911499}, "heading":{"exp":"135 deg", "val":2.356194490192345}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"0.6543505787849426 m", "val":0.6543505787849426}, "y":{"exp":"0.6869499087333679 m", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":3.141592653589793}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"HubStart.x", "val":3.61569}, "y":{"exp":"HubStart.y", "val":4.03479}, "heading":{"exp":"HubStart.heading", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.1631531715393066 m", "val":2.163153171539306}, "y":{"exp":"2.451129674911499 m", "val":2.451129674911499}, "heading":{"exp":"-0.7370179062160626 rad", "val":-0.7370179062160626}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.6543505787849426 m", "val":0.6543505787849426}, "y":{"exp":"0.6869499087333679 m", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":4.71238898038469}, "intervals":31, "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}, @@ -51,71 +51,66 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.82172,1.67965], + "waypoints":[0.0,0.80878,1.6667], "samples":[ - {"t":0.0, "x":3.61569, "y":4.03479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.70328, "ay":-6.27591, "alpha":9.52175, "fx":[-144.95659,-105.3381,-39.99133,-97.75864], "fy":[-49.88349,-111.46792,-148.00032,-117.6539]}, - {"t":0.0249, "x":3.61392, "y":4.03284, "heading":0.0, "vx":-0.14202, "vy":-0.15627, "omega":0.2371, "ax":-5.70236, "ay":-6.27589, "alpha":9.51816, "fx":[-144.92539,-105.32332,-40.01262,-97.72103], "fy":[-49.92439,-111.46569,-147.97841,-117.6362]}, - {"t":0.0498, "x":3.60862, "y":4.02701, "heading":0.0059, "vx":-0.28401, "vy":-0.31255, "omega":0.4741, "ax":-5.70566, "ay":-6.27301, "alpha":9.50534, "fx":[-144.82475,-105.07811,-39.84742,-98.4565], "fy":[-50.16128,-111.67842,-148.00387,-116.96492]}, - {"t":0.0747, "x":3.59978, "y":4.01728, "heading":0.01771, "vx":-0.42608, "vy":-0.46875, "omega":0.71079, "ax":-5.71292, "ay":-6.26704, "alpha":9.48661, "fx":[-144.65249,-104.60127,-39.49903,-99.94776], "fy":[-50.59601,-112.10397,-148.07449,-115.62744]}, - {"t":0.0996, "x":3.5874, "y":4.00366, "heading":0.03541, "vx":-0.56834, "vy":-0.6248, "omega":0.94702, "ax":-5.72364, "ay":-6.25754, "alpha":9.46867, "fx":[-144.40486,-103.89057,-38.97333,-102.16124], "fy":[-51.23186,-112.73833,-148.18641,-113.59897]}, - {"t":0.1245, "x":3.57147, "y":3.98617, "heading":0.05899, "vx":-0.71086, "vy":-0.78062, "omega":1.18279, "ax":-5.73702, "ay":-6.24395, "alpha":9.46146, "fx":[-144.0764,-102.9427,-38.27883,-105.04229], "fy":[-52.0733,-113.57565,-148.33421,-110.84795]}, - {"t":0.1494, "x":3.55199, "y":3.96479, "heading":0.08844, "vx":-0.85371, "vy":-0.9361, "omega":1.41839, "ax":-5.75185, "ay":-6.2257, "alpha":9.47782, "fx":[-143.65986,-101.75297,-37.4266,-108.50957], "fy":[-53.12573,-114.60845,-148.51082,-107.34449]}, - {"t":0.1743, "x":3.52895, "y":3.93955, "heading":0.12376, "vx":-0.99694, "vy":-1.09112, "omega":1.65439, "ax":-5.76643, "ay":-6.20237, "alpha":9.53264, "fx":[-143.14583,-100.31491,-36.4304,-112.45021], "fy":[-54.39536,-115.82776,-148.70742,-103.07187]}, - {"t":0.1992, "x":3.50234, "y":3.91046, "heading":0.16496, "vx":-1.14053, "vy":-1.24556, "omega":1.89176, "ax":-5.77856, "ay":-6.17392, "alpha":9.64132, "fx":[-142.52227,-98.6196,-35.3068,-116.71777], "fy":[-55.88929,-117.22331,-148.9132,-98.04059]}, - {"t":0.22411, "x":3.47215, "y":3.87753, "heading":0.21206, "vx":-1.28442, "vy":-1.3993, "omega":2.13183, "ax":-5.7855, "ay":-6.14086, "alpha":9.81746, "fx":[-141.7736,-96.65473,-34.07536,-121.13531], "fy":[-57.61551,-118.78359,-149.11461,-92.30306]}, - {"t":0.24901, "x":3.43837, "y":3.84079, "heading":0.26515, "vx":-1.42848, "vy":-1.55221, "omega":2.37629, "ax":-5.78412, "ay":-6.10443, "alpha":10.07005, "fx":[-140.87919,-94.40286,-32.75828,-125.50427], "fy":[-59.58305,-120.49564,-149.29386,-85.9656]}, - {"t":0.27391, "x":3.40101, "y":3.80024, "heading":0.32432, "vx":-1.57251, "vy":-1.70421, "omega":2.62704, "ax":-5.77089, "ay":-6.06659, "alpha":10.40059, "fx":[-139.8106,-91.83841,-31.37818,-129.61766], "fy":[-61.8014,-122.34415,-149.42572,-79.19274]}, - {"t":0.29881, "x":3.36006, "y":3.75593, "heading":0.38973, "vx":-1.7162, "vy":-1.85527, "omega":2.88603, "ax":-5.7418, "ay":-6.02973, "alpha":10.80125, "fx":[-138.5258,-88.92033,-29.9481,-133.2711], "fy":[-64.27856,-124.30926,-149.46993,-72.19795]}, - {"t":0.32371, "x":3.31555, "y":3.70786, "heading":0.4616, "vx":-1.85918, "vy":-2.00542, "omega":3.15498, "ax":-5.69104, "ay":-5.99561, "alpha":11.25634, "fx":[-136.95389,-85.57106,-28.43296,-136.25364], "fy":[-67.01419,-126.36062,-149.35001,-65.20985]}, - {"t":0.34861, "x":3.26749, "y":3.65606, "heading":0.54016, "vx":-2.00089, "vy":-2.15471, "omega":3.43527, "ax":-5.60512, "ay":-5.96216, "alpha":11.75445, "fx":[-134.94009,-81.59433,-26.57864,-138.25268], "fy":[-69.97387,-128.4374,-148.87571,-58.37124]}, - {"t":0.37351, "x":3.21593, "y":3.60056, "heading":0.6257, "vx":-2.14046, "vy":-2.30317, "omega":3.72797, "ax":-5.42404, "ay":-5.90446, "alpha":12.36855, "fx":[-131.91429,-76.15965,-22.78612,-138.18574], "fy":[-72.93235,-130.33173,-147.26189,-51.20656]}, - {"t":0.39841, "x":3.16095, "y":3.54138, "heading":0.71853, "vx":-2.27552, "vy":-2.4502, "omega":4.03595, "ax":-3.67284, "ay":-4.99975, "alpha":15.87075, "fx":[-115.45013,-50.56033,21.06905,-104.95461], "fy":[-69.84717,-126.91257,-121.69107,-21.72644]}, - {"t":0.42331, "x":3.10315, "y":3.47882, "heading":0.81902, "vx":-2.36698, "vy":-2.57469, "omega":4.43114, "ax":3.82822, "ay":-3.40407, "alpha":-5.14467, "fx":[82.89227,71.96954,44.88586,60.72011], "fy":[-52.30068,-36.50802,-66.22331,-76.57686]}, - {"t":0.44821, "x":3.04539, "y":3.41365, "heading":0.92936, "vx":-2.27165, "vy":-2.65946, "omega":4.30304, "ax":1.49586, "ay":-1.25819, "alpha":-7.78963, "fx":[53.92097,23.3606,-4.48711,28.98248], "fy":[-16.79018,8.98312,-28.12777,-49.67079]}, - {"t":0.47311, "x":2.98929, "y":3.34704, "heading":1.03651, "vx":-2.2344, "vy":-2.69079, "omega":4.10907, "ax":-0.15353, "ay":0.12867, "alpha":-7.03528, "fx":[23.29614,-9.27189,-28.44441,3.97385], "fy":[8.9344,27.96181,-4.36488,-23.77661]}, - {"t":0.49801, "x":2.93361, "y":3.28008, "heading":1.13883, "vx":-2.23823, "vy":-2.68758, "omega":3.93389, "ax":-0.81563, "ay":0.68513, "alpha":-5.58517, "fx":[6.12666,-20.98463,-33.74447,-6.89226], "fy":[19.62342,31.16313,4.3629,-8.53412]}, - {"t":0.52291, "x":2.87762, "y":3.21337, "heading":1.23678, "vx":-2.25854, "vy":-2.67052, "omega":3.79481, "ax":-1.03246, "ay":0.88208, "alpha":-4.12279, "fx":[-3.38166,-24.05183,-31.7395,-11.07456], "fy":[22.40465,28.75631,8.07477,0.77973]}, - {"t":0.54781, "x":2.82106, "y":3.14714, "heading":1.33128, "vx":-2.28425, "vy":-2.64856, "omega":3.69215, "ax":-1.07929, "ay":0.94057, "alpha":-2.82008, "fx":[-9.12599,-23.64883,-27.63379,-13.02532], "fy":[21.91454,24.93298,10.3065,6.84138]}, - {"t":0.57271, "x":2.76385, "y":3.08149, "heading":1.42321, "vx":-2.31112, "vy":-2.62514, "omega":3.62193, "ax":-1.06867, "ay":0.9505, "alpha":-1.66994, "fx":[-13.021,-21.79976,-23.36436,-14.52566], "fy":[20.11621,21.1461,12.29133,11.11701]}, - {"t":0.59761, "x":2.70597, "y":3.01641, "heading":1.5134, "vx":-2.33773, "vy":-2.60147, "omega":3.58035, "ax":-1.04458, "ay":0.94805, "alpha":-0.62207, "fx":[-15.97415,-19.2898,-19.5679,-16.24028], "fy":[17.7482,17.8505,14.51253,14.39276]}, - {"t":0.62251, "x":2.64743, "y":2.95193, "heading":1.60255, "vx":-2.36374, "vy":-2.57786, "omega":3.56486, "ax":-1.02138, "ay":0.94562, "alpha":0.37906, "fx":[-18.38259,-16.34794,-16.36703,-18.39608], "fy":[15.01152,15.1218,17.16069,17.04508]}, - {"t":0.64742, "x":2.58826, "y":2.88803, "heading":1.69132, "vx":-2.38918, "vy":-2.55432, "omega":3.5743, "ax":-0.99828, "ay":0.94252, "alpha":1.38602, "fx":[-20.33953,-12.89794,-13.66496,-21.0194], "fy":[11.82444,12.86329,20.26644,19.1735]}, - {"t":0.67232, "x":2.52846, "y":2.82472, "heading":1.78032, "vx":-2.41403, "vy":-2.53085, "omega":3.60881, "ax":-0.9606, "ay":0.92444, "alpha":2.44653, "fx":[-21.63571,-8.59111,-11.19105,-23.9405], "fy":[7.84708,10.80417,23.65631,20.59002]}, - {"t":0.69722, "x":2.46805, "y":2.76199, "heading":1.87019, "vx":-2.43795, "vy":-2.50783, "omega":3.66973, "ax":-0.86853, "ay":0.85088, "alpha":3.59529, "fx":[-21.51998,-2.66466,-8.33936,-26.5699], "fy":[2.31536,8.2876,26.68254,20.60728]}, - {"t":0.72212, "x":2.40707, "y":2.6998, "heading":1.96156, "vx":-2.45958, "vy":-2.48664, "omega":3.75925, "ax":-0.62306, "ay":0.61903, "alpha":4.81859, "fx":[-17.99976,6.43623,-3.61632,-27.21213], "fy":[-6.42154,3.6582,27.47577,17.40559]}, - {"t":0.74702, "x":2.34563, "y":2.63808, "heading":2.05517, "vx":-2.47509, "vy":-2.47123, "omega":3.87924, "ax":0.02624, "ay":-0.02772, "alpha":5.93425, "fx":[-6.21983,21.9472,7.13975,-21.08195], "fy":[-21.98334,-7.15345,21.04614,6.20455]}, - {"t":0.77192, "x":2.28401, "y":2.57653, "heading":2.15177, "vx":-2.47444, "vy":-2.47192, "omega":4.02701, "ax":1.60467, "ay":-1.58888, "alpha":6.25489, "fx":[21.85502,49.42901,34.4247,3.47127], "fy":[-50.13703,-30.78195,-3.0868,-24.1001]}, - {"t":0.79682, "x":2.22289, "y":2.51449, "heading":2.25204, "vx":-2.43448, "vy":-2.51148, "omega":4.18276, "ax":2.8391, "ay":-2.64853, "alpha":-17.19342, "fx":[77.0228,-25.8906,39.49586,102.54126], "fy":[28.6473,-59.81871,-102.93394,-46.09762]}, - {"t":0.82172, "x":2.16315, "y":2.45113, "heading":2.35619, "vx":-2.36379, "vy":-2.57743, "omega":3.75463, "ax":2.05644, "ay":-1.91237, "alpha":-16.66428, "fx":[47.91267,-34.15863,34.33911,91.82492], "fy":[36.93456,-45.37597,-90.12605,-31.54825]}, - {"t":0.8513, "x":2.09412, "y":2.37404, "heading":2.46727, "vx":-2.30295, "vy":-2.63401, "omega":3.26164, "ax":0.77321, "ay":-0.67026, "alpha":-15.13823, "fx":[9.08081,-45.36914,20.15044,68.74602], "fy":[47.74561,-20.58085,-66.59444,-6.17373]}, - {"t":0.88089, "x":2.02633, "y":2.29583, "heading":2.56376, "vx":-2.28008, "vy":-2.65384, "omega":2.81379, "ax":0.24769, "ay":-0.21225, "alpha":-12.369, "fx":[-5.17261,-41.95737,14.04899,49.93358], "fy":[42.80699,-13.82068,-49.0917,5.66399]}, - {"t":0.91047, "x":1.95899, "y":2.21722, "heading":2.64701, "vx":-2.27275, "vy":-2.66011, "omega":2.44787, "ax":0.07269, "ay":-0.06208, "alpha":-10.02013, "fx":[-9.6381,-35.23289,12.16665,37.65016], "fy":[35.48106,-12.06285,-37.40118,9.75937]}, - {"t":0.94005, "x":1.89178, "y":2.1385, "heading":2.71942, "vx":-2.2706, "vy":-2.66195, "omega":2.15143, "ax":0.02009, "ay":-0.01715, "alpha":-8.21247, "fx":[-10.73212,-28.80294,11.42079,29.48143], "fy":[28.86876,-11.38704,-29.41547,10.76693]}, - {"t":0.96964, "x":1.82462, "y":2.05974, "heading":2.78307, "vx":-2.27, "vy":-2.66246, "omega":1.90848, "ax":0.00526, "ay":-0.00449, "alpha":-6.86214, "fx":[-10.69599,-23.62305,10.87498,23.8019], "fy":[23.63944,-10.86532,-23.78551,10.70571]}, - {"t":0.99922, "x":1.75746, "y":1.98097, "heading":2.83953, "vx":-2.26985, "vy":-2.66259, "omega":1.70547, "ax":0.00127, "ay":-0.00109, "alpha":-5.887, "fx":[-10.36449,-19.76659,10.40756,19.80991], "fy":[19.77033,-10.40514,-19.80617,10.36691]}, - {"t":1.02881, "x":1.69031, "y":1.9022, "heading":2.88998, "vx":-2.26981, "vy":-2.66262, "omega":1.53131, "ax":0.00025, "ay":-0.00022, "alpha":-5.224, "fx":[-10.08597,-17.06827,10.09445,17.07683], "fy":[17.06893,-10.09402,-17.07617,10.0864]}, - {"t":1.05839, "x":1.62316, "y":1.82343, "heading":2.93529, "vx":-2.2698, "vy":-2.66263, "omega":1.37676, "ax":0.00001, "ay":-0.00002, "alpha":-4.82786, "fx":[-10.0298,-15.33921,10.03025,15.33967], "fy":[15.33919,-10.03029,-15.33969,10.02977]}, - {"t":1.08797, "x":1.55602, "y":1.74466, "heading":2.97602, "vx":-2.2698, "vy":-2.66263, "omega":1.23394, "ax":-0.00003, "ay":0.00002, "alpha":-4.66806, "fx":[-10.2944,-14.425,10.29344,14.42403], "fy":[14.42487,-10.29355,-14.42417,10.29429]}, - {"t":1.11756, "x":1.48887, "y":1.66589, "heading":3.01252, "vx":-2.2698, "vy":-2.66263, "omega":1.09584, "ax":-0.00003, "ay":0.00002, "alpha":-4.72607, "fx":[-10.94832,-14.21412,10.94743,14.21322], "fy":[14.214,-10.94753,-14.21334,10.94822]}, - {"t":1.14714, "x":1.42172, "y":1.58712, "heading":3.04494, "vx":-2.2698, "vy":-2.66263, "omega":0.95602, "ax":-0.00002, "ay":0.00001, "alpha":-4.99289, "fx":[-12.0469,-14.63364,12.04632,14.63306], "fy":[14.63355,-12.04639,-14.63315,12.04682]}, - {"t":1.17673, "x":1.35457, "y":1.50835, "heading":3.07322, "vx":-2.2698, "vy":-2.66263, "omega":0.80831, "ax":-0.00001, "ay":0.00001, "alpha":-5.46688, "fx":[-13.63821,-15.64328,13.6379,15.64297], "fy":[15.64321,-13.63796,-15.64303,13.63816]}, - {"t":1.20631, "x":1.28742, "y":1.42958, "heading":3.09714, "vx":-2.26981, "vy":-2.66263, "omega":0.64658, "ax":0.0, "ay":0.0, "alpha":-6.15162, "fx":[-15.76277,-17.2305,15.76272,17.23045], "fy":[17.23047,-15.76275,-17.23048,15.76274]}, - {"t":1.23589, "x":1.22027, "y":1.35081, "heading":3.11626, "vx":-2.26981, "vy":-2.66263, "omega":0.46459, "ax":0.00018, "ay":0.00019, "alpha":-7.05365, "fx":[-18.44556,-19.4048,18.45181,19.41064], "fy":[19.41099,-18.44559,-19.40445,18.45178]}, - {"t":1.26548, "x":1.15312, "y":1.27204, "heading":3.13001, "vx":-2.2698, "vy":-2.66262, "omega":0.25592, "ax":0.93295, "ay":1.09446, "alpha":-7.78914, "fx":[-4.69145,-5.57247,37.76315,35.97795], "fy":[40.66427,-2.00703,-2.38614,38.1948]}, - {"t":1.29506, "x":1.08638, "y":1.19374, "heading":3.13758, "vx":-2.2422, "vy":-2.63024, "omega":0.02549, "ax":5.74944, "ay":6.74446, "alpha":-0.185, "fx":[96.75056,97.69351,98.83977,97.90131], "fy":[115.61167,114.77963,113.82511,114.66877]}, - {"t":1.32464, "x":1.02256, "y":1.11888, "heading":3.13833, "vx":-2.07211, "vy":-2.43072, "omega":0.02002, "ax":5.80298, "ay":6.80727, "alpha":-0.10594, "fx":[98.10262,98.65269,99.31083,98.76232], "fy":[116.30457,115.828,115.27303,115.75353]}, - {"t":1.35423, "x":0.9638, "y":1.04995, "heading":3.13893, "vx":-1.90043, "vy":-2.22933, "omega":0.01688, "ax":5.82075, "ay":6.82811, "alpha":-0.07987, "fx":[98.55209,98.9695,99.46598,99.04947], "fy":[116.53359,116.17412,115.75374,116.11547]}, - {"t":1.38381, "x":0.91012, "y":0.98699, "heading":3.13943, "vx":-1.72823, "vy":-2.02733, "omega":0.01452, "ax":5.82961, "ay":6.8385, "alpha":-0.0669, "fx":[98.77635,99.12722,99.54317,99.19294], "fy":[116.64766,116.34638,115.99348,116.29635]}, - {"t":1.4134, "x":0.86155, "y":0.93, "heading":3.13986, "vx":-1.55577, "vy":-1.82502, "omega":0.01254, "ax":5.83491, "ay":6.84473, "alpha":-0.05915, "fx":[98.9107,99.22161,99.58936,99.27895], "fy":[116.71595,116.44946,116.13707,116.40481]}, - {"t":1.44298, "x":0.81807, "y":0.87901, "heading":3.14023, "vx":-1.38315, "vy":-1.62253, "omega":0.01079, "ax":5.83844, "ay":6.84887, "alpha":-0.054, "fx":[99.00016,99.28443,99.6201,99.33625], "fy":[116.76141,116.51804,116.23267,116.47708]}, - {"t":1.47256, "x":0.77971, "y":0.83401, "heading":3.14055, "vx":-1.21043, "vy":-1.41991, "omega":0.00919, "ax":5.84096, "ay":6.85183, "alpha":-0.05032, "fx":[99.064,99.32924,99.64203,99.37716], "fy":[116.79385,116.56696,116.30089,116.52868]}, - {"t":1.50215, "x":0.74646, "y":0.795, "heading":3.14082, "vx":-1.03763, "vy":-1.21721, "omega":0.0077, "ax":5.84285, "ay":6.85404, "alpha":-0.04757, "fx":[99.11185,99.36281,99.65847,99.40783], "fy":[116.81816,116.60362,116.35202,116.56737]}, - {"t":1.53173, "x":0.71832, "y":0.76199, "heading":3.14105, "vx":-0.86478, "vy":-1.01444, "omega":0.0063, "ax":5.84432, "ay":6.85576, "alpha":-0.04543, "fx":[99.14904,99.38891,99.67124,99.43168], "fy":[116.83706,116.6321,116.39175,116.59745]}, - {"t":1.56132, "x":0.69529, "y":0.73498, "heading":3.14123, "vx":-0.69188, "vy":-0.81162, "omega":0.00495, "ax":5.84549, "ay":6.85714, "alpha":-0.04372, "fx":[99.17878,99.40976,99.68146,99.45075], "fy":[116.85217,116.65488,116.42353,116.6215]}, - {"t":1.5909, "x":0.67738, "y":0.71397, "heading":3.14138, "vx":-0.51895, "vy":-0.60876, "omega":0.00366, "ax":5.84645, "ay":6.85827, "alpha":-0.04233, "fx":[99.2031,99.42681,99.68981,99.46636], "fy":[116.86453,116.67351,116.44951,116.64117]}, - {"t":1.62048, "x":0.66459, "y":0.69896, "heading":3.14149, "vx":-0.34599, "vy":-0.40587, "omega":0.00241, "ax":5.84725, "ay":6.85921, "alpha":-0.04116, "fx":[99.22336,99.44101,99.69677,99.47937], "fy":[116.87482,116.68903,116.47116,116.65755]}, - {"t":1.65007, "x":0.65691, "y":0.68995, "heading":3.14156, "vx":-0.173, "vy":-0.20295, "omega":0.00119, "ax":5.84793, "ay":6.86, "alpha":-0.04018, "fx":[99.2405,99.45301,99.70265,99.49038], "fy":[116.88353,116.70216,116.48947,116.67141]}, - {"t":1.67965, "x":0.65435, "y":0.68695, "heading":3.14159, "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":3.61569, "y":4.03479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.09776, "ay":-6.63823, "alpha":-0.115, "fx":[-103.07902,-103.69381,-104.36204,-103.74995], "fy":[-113.50262,-112.93946,-112.32392,-112.89128]}, + {"t":0.02889, "x":3.61315, "y":4.03202, "heading":0.0, "vx":-0.17613, "vy":-0.19175, "omega":-0.00332, "ax":-6.09703, "ay":-6.63743, "alpha":-0.1175, "fx":[-103.05274,-103.68076,-104.36346,-103.73826], "fy":[-113.50178,-112.92641,-112.29761,-112.87735]}, + {"t":0.05777, "x":3.60552, "y":4.02371, "heading":-0.0001, "vx":-0.35225, "vy":-0.38347, "omega":-0.00672, "ax":-6.09617, "ay":-6.63649, "alpha":-0.12045, "fx":[-103.02169,-103.66528,-104.36514,-103.72451], "fy":[-113.50078,-112.91106,-112.26652,-112.86084]}, + {"t":0.08666, "x":3.5928, "y":4.00987, "heading":-0.00029, "vx":-0.52834, "vy":-0.57516, "omega":-0.0102, "ax":-6.09514, "ay":-6.63536, "alpha":-0.124, "fx":[-102.98444,-103.64664,-104.36714,-103.70809], "fy":[-113.49957,-112.89268,-112.22923,-112.84098]}, + {"t":0.11554, "x":3.57499, "y":3.99049, "heading":-0.00058, "vx":-0.70439, "vy":-0.76683, "omega":-0.01378, "ax":-6.09388, "ay":-6.63398, "alpha":-0.12833, "fx":[-102.93893,-103.6238,-104.36959,-103.68808], "fy":[-113.49809,-112.87028,-112.18367,-112.81669]}, + {"t":0.14443, "x":3.5521, "y":3.96557, "heading":-0.00098, "vx":-0.88042, "vy":-0.95845, "omega":-0.01748, "ax":-6.0923, "ay":-6.63226, "alpha":-0.13374, "fx":[-102.88207,-103.59521,-104.37264,-103.66313], "fy":[-113.49623,-112.84231,-112.12673,-112.78632]}, + {"t":0.17331, "x":3.52413, "y":3.93512, "heading":-0.00149, "vx":-1.05639, "vy":-1.15002, "omega":-0.02135, "ax":-6.09027, "ay":-6.63005, "alpha":-0.1407, "fx":[-102.80902,-103.55844,-104.37655,-103.63113], "fy":[-113.49383,-112.80638,-112.05358,-112.74729]}, + {"t":0.2022, "x":3.49108, "y":3.89913, "heading":-0.0021, "vx":-1.23231, "vy":-1.34153, "omega":-0.02541, "ax":-6.08757, "ay":-6.6271, "alpha":-0.14997, "fx":[-102.7117,-103.5094,-104.38174,-103.58856], "fy":[-113.49062,-112.75851,-111.95611,-112.69534]}, + {"t":0.23108, "x":3.45294, "y":3.85762, "heading":-0.00284, "vx":-1.40815, "vy":-1.53295, "omega":-0.02974, "ax":-6.0838, "ay":-6.62298, "alpha":-0.16295, "fx":[-102.57563,-103.44078,-104.38895,-103.5291], "fy":[-113.48608,-112.69149,-111.81981,-112.62281]}, + {"t":0.25997, "x":3.40973, "y":3.81058, "heading":-0.0037, "vx":-1.58388, "vy":-1.72426, "omega":-0.03445, "ax":-6.07814, "ay":-6.61681, "alpha":-0.18239, "fx":[-102.37195,-103.33796,-104.39964,-103.4402], "fy":[-113.47918,-112.591,-111.61575,-112.51446]}, + {"t":0.28885, "x":3.36144, "y":3.75801, "heading":-0.00469, "vx":-1.75945, "vy":-1.91539, "omega":-0.03972, "ax":-6.06875, "ay":-6.60656, "alpha":-0.21474, "fx":[-102.03371,-103.16693,-104.41707,-103.29286], "fy":[-113.46741,-112.4236,-111.27676,-112.33512]}, + {"t":0.31774, "x":3.30809, "y":3.69993, "heading":-0.00584, "vx":-1.93474, "vy":-2.10622, "omega":-0.04592, "ax":-6.05008, "ay":-6.58619, "alpha":-0.27914, "fx":[-101.36232,-102.82606,-104.45035,-103.00151], "fy":[-113.44269,-112.08949,-110.60344,-111.98104]}, + {"t":0.34662, "x":3.24968, "y":3.63634, "heading":-0.00717, "vx":-2.1095, "vy":-2.29646, "omega":-0.05398, "ax":-5.99508, "ay":-6.52617, "alpha":-0.46939, "fx":[-99.39262,-101.81347,-104.53643,-102.15566], "fy":[-113.35861,-111.09527,-108.62454,-110.95486]}, + {"t":0.37551, "x":3.18625, "y":3.56729, "heading":-0.00873, "vx":-2.28267, "vy":-2.48497, "omega":-0.06754, "ax":-2.91361, "ay":-3.17565, "alpha":-11.36017, "fx":[-15.12584,-21.57838,-88.70555,-72.8288], "fy":[-91.59257,-29.09144,-19.38834,-75.99536]}, + {"t":0.40439, "x":3.1191, "y":3.49418, "heading":-0.01068, "vx":-2.36683, "vy":-2.5767, "omega":-0.39568, "ax":-0.00179, "ay":-0.00004, "alpha":-14.80101, "fx":[39.27355,40.12224,-39.33498,-40.18294], "fy":[-40.15736,39.30768,40.14783,-39.30086]}, + {"t":0.43328, "x":3.05073, "y":3.41976, "heading":-0.02211, "vx":-2.36688, "vy":-2.5767, "omega":-0.82321, "ax":-0.00026, "ay":0.00025, "alpha":-12.33936, "fx":[32.37851,33.84203,-32.3867,-33.85172], "fy":[-33.84308,32.38726,33.85066,-32.37795]}, + {"t":0.46216, "x":2.98236, "y":3.34533, "heading":-0.04588, "vx":-2.36689, "vy":-2.57669, "omega":-1.17963, "ax":0.00002, "ay":-0.00001, "alpha":-10.01275, "fx":[25.61665,28.08228,-25.61609,-28.0817], "fy":[-28.08208,25.61624,28.08189,-25.61651]}, + {"t":0.49105, "x":2.914, "y":3.2709, "heading":-0.07996, "vx":-2.36689, "vy":-2.57669, "omega":-1.46885, "ax":0.00018, "ay":-0.00016, "alpha":-7.92273, "fx":[19.50355,22.90103,-19.49757,-22.89471], "fy":[-22.9004,19.49776,22.89534,-19.50335]}, + {"t":0.51993, "x":2.84563, "y":3.19647, "heading":-0.12239, "vx":-2.36688, "vy":-2.5767, "omega":-1.6977, "ax":0.00031, "ay":-0.00028, "alpha":-6.13641, "fx":[14.34324,18.36522,-14.33273,-18.3544], "fy":[-18.36441,14.3331,18.35521,-14.34287]}, + {"t":0.54882, "x":2.77726, "y":3.12204, "heading":-0.17142, "vx":-2.36687, "vy":-2.5767, "omega":-1.87495, "ax":0.00042, "ay":-0.00038, "alpha":-4.6735, "fx":[10.22842,14.50856,-10.21416,-14.4941], "fy":[-14.50767,10.21472,14.49499,-10.22787]}, + {"t":0.5777, "x":2.70889, "y":3.04762, "heading":-0.22558, "vx":-2.36686, "vy":-2.57671, "omega":-2.00994, "ax":0.00052, "ay":-0.00048, "alpha":-3.51022, "fx":[7.08516,11.30035,-7.06737,-11.28246], "fy":[-11.2994,7.06809,11.28341,-7.08444]}, + {"t":0.60659, "x":2.64053, "y":2.97319, "heading":-0.28364, "vx":-2.36684, "vy":-2.57673, "omega":-2.11133, "ax":0.00078, "ay":-0.00071, "alpha":-2.59402, "fx":[4.74953,8.64685,-4.7231,-8.62038], "fy":[-8.64561,4.72418,8.62163,-4.74845]}, + {"t":0.63547, "x":2.57216, "y":2.89876, "heading":-0.34462, "vx":-2.36682, "vy":-2.57675, "omega":-2.18626, "ax":0.00184, "ay":-0.00169, "alpha":-1.85991, "fx":[3.04359,6.41702,-2.98107,-6.35449], "fy":[-6.41433,2.98358,6.35718,-3.04108]}, + {"t":0.66436, "x":2.5038, "y":2.82433, "heading":-0.40777, "vx":-2.36677, "vy":-2.5768, "omega":-2.23999, "ax":0.00617, "ay":-0.00566, "alpha":-1.24296, "fx":[1.84472,4.49099,-1.63483,-4.28115], "fy":[-4.48228,1.6433,4.28986,-1.83626]}, + {"t":0.69324, "x":2.43544, "y":2.74989, "heading":-0.47248, "vx":-2.36659, "vy":-2.57696, "omega":-2.27589, "ax":0.02291, "ay":-0.02104, "alpha":-0.68535, "fx":[1.19068,2.86511,-0.41108,-2.08564], "fy":[-2.83306,0.44294,2.11766,-1.15894]}, + {"t":0.72213, "x":2.36709, "y":2.67545, "heading":-0.53822, "vx":-2.36593, "vy":-2.57757, "omega":-2.29569, "ax":0.08614, "ay":-0.07899, "alpha":-0.13882, "fx":[1.594,1.97611,1.3363,0.95416], "fy":[-1.85448,-1.21455,-0.83258,-1.4726]}, + {"t":0.75101, "x":2.29878, "y":2.60096, "heading":-0.60453, "vx":-2.36344, "vy":-2.57985, "omega":-2.2997, "ax":0.32263, "ay":-0.29449, "alpha":0.4342, "fx":[5.19565,3.86578,5.78098,7.1093], "fy":[-3.38737,-5.31032,-6.62951,-4.70962]}, + {"t":0.7799, "x":2.23065, "y":2.52632, "heading":-0.67095, "vx":-2.35412, "vy":-2.58836, "omega":-2.28715, "ax":1.20586, "ay":-1.02661, "alpha":1.00994, "fx":[20.20182,16.69745,20.84155,24.30433], "fy":[-13.63417,-18.03188,-21.26275,-16.92035]}, + {"t":0.80878, "x":2.16315, "y":2.45113, "heading":-0.73702, "vx":-2.31929, "vy":-2.61801, "omega":-2.25798, "ax":1.1602, "ay":-1.06873, "alpha":0.97194, "fx":[19.67964,16.04251,19.81118,23.40566], "fy":[-14.47772,-18.48175,-21.85605,-17.89954]}, + {"t":0.83836, "x":2.09505, "y":2.37321, "heading":-0.80382, "vx":-2.28497, "vy":-2.64963, "omega":-2.22923, "ax":0.31825, "ay":-0.27348, "alpha":1.0226, "fx":[5.49662,1.52979,5.33708,9.28963], "fy":[-0.76618,-4.59141,-8.53166,-4.71785]}, + {"t":0.86795, "x":2.02759, "y":2.29471, "heading":-0.86977, "vx":-2.27555, "vy":-2.65772, "omega":-2.19898, "ax":0.08245, "ay":-0.07053, "alpha":1.08005, "fx":[1.74967,-2.68404,1.05751,5.4867], "fy":[2.88662,-0.85545,-5.28468,-1.54531]}, + {"t":0.89753, "x":1.96031, "y":2.21605, "heading":-0.93482, "vx":-2.27311, "vy":-2.6598, "omega":-2.16702, "ax":0.02129, "ay":-0.01819, "alpha":1.19071, "fx":[1.03539,-4.1081,-0.31048,4.83149], "fy":[4.1606,0.36331,-4.77904,-0.9824]}, + {"t":0.92711, "x":1.89307, "y":2.13736, "heading":-0.99893, "vx":-2.27248, "vy":-2.66034, "omega":-2.1318, "ax":0.00556, "ay":-0.00475, "alpha":1.36387, "fx":[1.19184,-4.96553,-1.0025,5.15433], "fy":[4.97919,1.01635,-5.14067,-1.17798]}, + {"t":0.9567, "x":1.82584, "y":2.05865, "heading":-1.06199, "vx":-2.27232, "vy":-2.66048, "omega":-2.09145, "ax":0.00157, "ay":-0.00134, "alpha":1.61397, "fx":[1.69988,-5.86746,-1.64644,5.92068], "fy":[5.87127,1.65034,-5.91687,-1.69598]}, + {"t":0.98628, "x":1.75862, "y":1.97995, "heading":-1.12387, "vx":-2.27227, "vy":-2.66052, "omega":-2.0437, "ax":0.00059, "ay":-0.0005, "alpha":1.96376, "fx":[2.48533,-7.02192,-2.4653,7.04182], "fy":[7.0233,2.46674,-7.04044,-2.48389]}, + {"t":1.01587, "x":1.6914, "y":1.90124, "heading":-1.18433, "vx":-2.27225, "vy":-2.66054, "omega":-1.98561, "ax":0.00035, "ay":-0.00031, "alpha":2.44412, "fx":[3.61005,-8.54386,-3.59797,8.55582], "fy":[8.54465,3.59882,-8.55504,-3.6092]}, + {"t":1.04545, "x":1.62418, "y":1.82253, "heading":-1.24307, "vx":-2.27224, "vy":-2.66055, "omega":-1.9133, "ax":0.00028, "ay":-0.00024, "alpha":3.09271, "fx":[5.19249,-10.52755,-5.18285,10.53704], "fy":[10.52813,5.18351,-10.53646,-5.19182]}, + {"t":1.07503, "x":1.55696, "y":1.74382, "heading":-1.29967, "vx":-2.27224, "vy":-2.66055, "omega":-1.82181, "ax":0.00022, "ay":-0.0002, "alpha":3.95083, "fx":[7.38148,-13.05441,-7.37374,13.06197], "fy":[13.05482,7.37427,-13.06157,-7.38095]}, + {"t":1.10462, "x":1.48974, "y":1.66511, "heading":-1.35356, "vx":-2.27223, "vy":-2.66056, "omega":-1.70493, "ax":0.00016, "ay":-0.00014, "alpha":5.0579, "fx":[10.33449,-16.18151,-10.32897,16.18682], "fy":[16.18173,10.32934,-16.1866,-10.33412]}, + {"t":1.1342, "x":1.42252, "y":1.5864, "heading":-1.404, "vx":-2.27222, "vy":-2.66056, "omega":-1.5553, "ax":0.00009, "ay":-0.00009, "alpha":6.44323, "fx":[14.18586,-19.9256,-14.18259,19.92868], "fy":[19.92565,14.18279,-19.92863,-14.18566]}, + {"t":1.16378, "x":1.3553, "y":1.5077, "heading":-1.45001, "vx":-2.27222, "vy":-2.66057, "omega":-1.36469, "ax":0.00005, "ay":-0.00004, "alpha":8.11625, "fx":[19.00368,-24.25224,-19.00206,24.25371], "fy":[24.25218,19.00213,-24.25376,-19.00361]}, + {"t":1.19337, "x":1.28808, "y":1.42899, "heading":-1.49039, "vx":-2.27222, "vy":-2.66057, "omega":-1.12458, "ax":0.00003, "ay":-0.00003, "alpha":10.05816, "fx":[24.74402,-29.08031,-24.74285,29.08131], "fy":[29.08023,24.74291,-29.08139,-24.74396]}, + {"t":1.22295, "x":1.22086, "y":1.35028, "heading":-1.52366, "vx":-2.27222, "vy":-2.66057, "omega":-0.82702, "ax":0.00027, "ay":0.00017, "alpha":12.21947, "fx":[31.2233,-34.3054,-31.21441,34.31515], "fy":[34.31265,31.22213,-34.3079,-31.21558]}, + {"t":1.25253, "x":1.15364, "y":1.27157, "heading":-1.54812, "vx":-2.27221, "vy":-2.66056, "omega":-0.46553, "ax":0.93537, "ay":1.09552, "alpha":13.87905, "fx":[49.8014,-20.95145,-21.98531,56.77692], "fy":[54.10787,57.38886,-20.51589,-16.44309]}, + {"t":1.28212, "x":1.08683, "y":1.19334, "heading":-1.56189, "vx":-2.24454, "vy":-2.62815, "omega":-0.05494, "ax":5.7553, "ay":6.73892, "alpha":0.37476, "fx":[98.08236,95.77363,97.72005,100.00803], "fy":[114.55113,116.42855,114.72613,112.80267]}, + {"t":1.3117, "x":1.02294, "y":1.11854, "heading":-1.56352, "vx":-2.07428, "vy":-2.42879, "omega":-0.04385, "ax":5.80906, "ay":6.80187, "alpha":0.2204, "fx":[98.91183,97.55143,98.71263,100.0656], "fy":[115.63679,116.76883,115.76736,114.61877]}, + {"t":1.34128, "x":0.96412, "y":1.04966, "heading":-1.56482, "vx":-1.90243, "vy":-2.22757, "omega":-0.03733, "ax":5.82687, "ay":6.82274, "alpha":0.16945, "fx":[99.1901,98.14287,99.03904,100.08184], "fy":[116.00099,116.87913,116.10975,115.22169]}, + {"t":1.37087, "x":0.91039, "y":0.98675, "heading":-1.56592, "vx":-1.73005, "vy":-2.02573, "omega":-0.03232, "ax":5.83576, "ay":6.83314, "alpha":0.1441, "fx":[99.32951,98.43802,99.20118,100.08948], "fy":[116.18329,116.93371,116.27999,115.52234]}, + {"t":1.40045, "x":0.86176, "y":0.92981, "heading":-1.56688, "vx":-1.5574, "vy":-1.82358, "omega":-0.02805, "ax":5.84108, "ay":6.83937, "alpha":0.12894, "fx":[99.41327,98.61485,99.29806,100.09393], "fy":[116.29267,116.96628,116.38182,115.70239]}, + {"t":1.43003, "x":0.81824, "y":0.87886, "heading":-1.56771, "vx":-1.3846, "vy":-1.62125, "omega":-0.02424, "ax":5.84462, "ay":6.84352, "alpha":0.11886, "fx":[99.46915,98.7326,99.36246,100.09685], "fy":[116.36557,116.98792,116.44959,115.82227]}, + {"t":1.45962, "x":0.77984, "y":0.83389, "heading":-1.56842, "vx":-1.2117, "vy":-1.41879, "omega":-0.02072, "ax":5.84714, "ay":6.84648, "alpha":0.11168, "fx":[99.50909,98.81664,99.40836,100.09891], "fy":[116.41762,117.00335,116.49794,115.9078]}, + {"t":1.4892, "x":0.74655, "y":0.79491, "heading":-1.56904, "vx":-1.03872, "vy":-1.21625, "omega":-0.01742, "ax":5.84904, "ay":6.8487, "alpha":0.1063, "fx":[99.53905,98.87961,99.44273,100.10045], "fy":[116.45665,117.01491,116.53415,115.9719]}, + {"t":1.51879, "x":0.71838, "y":0.76193, "heading":-1.56955, "vx":-0.86569, "vy":-1.01364, "omega":-0.01428, "ax":5.85051, "ay":6.85042, "alpha":0.10211, "fx":[99.56235,98.92856,99.46944,100.10164], "fy":[116.48701,117.02389,116.56229,116.02171]}, + {"t":1.54837, "x":0.69533, "y":0.73494, "heading":-1.56997, "vx":-0.69261, "vy":-0.81098, "omega":-0.01125, "ax":5.85169, "ay":6.8518, "alpha":0.09877, "fx":[99.58097,98.9677,99.4908,100.1026], "fy":[116.5113,117.03106,116.58477,116.06154]}, + {"t":1.57795, "x":0.6774, "y":0.71394, "heading":-1.57031, "vx":-0.51949, "vy":-0.60828, "omega":-0.00833, "ax":5.85265, "ay":6.85293, "alpha":0.09604, "fx":[99.59619,98.99971,99.50829,100.10338], "fy":[116.53119,117.03694,116.60314,116.09411]}, + {"t":1.60754, "x":0.6646, "y":0.69895, "heading":-1.57055, "vx":-0.34635, "vy":-0.40555, "omega":-0.00549, "ax":5.85345, "ay":6.85387, "alpha":0.09377, "fx":[99.60885,99.02637,99.52288,100.10403], "fy":[116.54779,117.04183,116.61841,116.12124]}, + {"t":1.63712, "x":0.65691, "y":0.68995, "heading":-1.57072, "vx":-0.17319, "vy":-0.20279, "omega":-0.00272, "ax":5.85413, "ay":6.85466, "alpha":0.09184, "fx":[99.61953,99.04892,99.53524,100.10458], "fy":[116.56185,117.04597,116.63131,116.14419]}, + {"t":1.6667, "x":0.65435, "y":0.68695, "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/OutpostToDepot.traj b/src/main/deploy/choreo/OutpostToDepot.traj index 86df11b..7cd9c70 100644 --- a/src/main/deploy/choreo/OutpostToDepot.traj +++ b/src/main/deploy/choreo/OutpostToDepot.traj @@ -14,7 +14,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":4.71238898038469}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.794987678527832 m", "val":1.794987678527832}, "y":{"exp":"3.710289478302002 m", "val":3.710289478302002}, "heading":{"exp":"3.14 rad", "val":3.14}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"Depot.x", "val":1.15}, "y":{"exp":"Depot.y", "val":5.9625654220581055}, "heading":{"exp":"Depot.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/OutpostToTower.traj b/src/main/deploy/choreo/OutpostToTower.traj index e885523..f38c280 100644 --- a/src/main/deploy/choreo/OutpostToTower.traj +++ b/src/main/deploy/choreo/OutpostToTower.traj @@ -3,7 +3,7 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":0.6543505787849426, "y":0.6869499087333679, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.6543505787849426, "y":0.6869499087333679, "heading":4.71238898038469, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.3544923067092896, "y":2.6405155658721924, "heading":-1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":4.71238898038469}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"TowerRight.x", "val":1.3544923067092896}, "y":{"exp":"TowerRight.y", "val":2.6405155658721924}, "heading":{"exp":"TowerRight.heading", "val":-1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -49,39 +49,29 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.97514], + "waypoints":[0.0,0.95975], "samples":[ - {"t":0.0, "x":0.65435, "y":0.68695, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.88453, "ay":8.17537, "alpha":7.98711, "fx":[102.94778,68.87491,8.74309,15.69409], "fy":[113.61648,137.06167,153.12685,152.43755]}, - {"t":0.0325, "x":0.65587, "y":0.69127, "heading":3.14159, "vx":0.09376, "vy":0.26574, "omega":0.25962, "ax":2.88947, "ay":8.17969, "alpha":7.90846, "fx":[102.51595,68.77305,9.08899,16.21811], "fy":[113.9831,137.09942,153.09341,152.36064]}, - {"t":0.06501, "x":0.66045, "y":0.70423, "heading":-3.13315, "vx":0.18768, "vy":0.53162, "omega":0.51668, "ax":2.89552, "ay":8.18631, "alpha":7.79156, "fx":[102.00192,68.34106,9.25344,17.4112], "fy":[114.41761,137.2997,153.06755,152.20246]}, - {"t":0.09751, "x":0.66808, "y":0.72583, "heading":-3.11636, "vx":0.2818, "vy":0.79771, "omega":0.76995, "ax":2.90302, "ay":8.19518, "alpha":7.63442, "fx":[101.37629,67.58307,9.27607,19.28271], "fy":[114.94327,137.656,153.04671,151.94483]}, - {"t":0.13002, "x":0.67877, "y":0.75609, "heading":-3.09133, "vx":0.37616, "vy":1.0641, "omega":1.0181, "ax":2.91248, "ay":8.20618, "alpha":7.43458, "fx":[100.59294,66.50483,9.2164,21.84763], "fy":[115.59601,138.15805,153.02627,151.55872]}, - {"t":0.16252, "x":0.69254, "y":0.79501, "heading":-3.05824, "vx":0.47083, "vy":1.33084, "omega":1.25976, "ax":2.92447, "ay":8.21913, "alpha":7.18921, "fx":[99.58639,65.11523,9.16168,25.11458], "fy":[116.42479,138.79127,152.99935,151.00472]}, - {"t":0.19503, "x":0.70939, "y":0.84262, "heading":-3.01729, "vx":0.56589, "vy":1.598, "omega":1.49345, "ax":2.93954, "ay":8.23384, "alpha":6.89466, "fx":[98.26608,63.42877,9.24052,29.06783], "fy":[117.49329,139.53585,152.95582,150.236]}, - {"t":0.22753, "x":0.72933, "y":0.89891, "heading":-2.96875, "vx":0.66144, "vy":1.86564, "omega":1.71756, "ax":2.95806, "ay":8.25023, "alpha":6.54434, "fx":[96.50319,61.46968,9.64826,33.64211], "fy":[118.88502,140.36529,152.87941,149.20623]}, - {"t":0.26004, "x":0.7524, "y":0.96391, "heading":-2.91292, "vx":0.75759, "vy":2.13381, "omega":1.93028, "ax":2.98015, "ay":8.26859, "alpha":6.12257, "fx":[94.10046,59.27991,10.69752,38.68842], "fy":[120.71628,141.24324,152.73939,147.88651]}, - {"t":0.29254, "x":0.7786, "y":1.03764, "heading":-2.85017, "vx":0.85446, "vy":2.40258, "omega":2.12929, "ax":3.00581, "ay":8.29005, "alpha":5.58914, "fx":[90.71917,56.93686,12.92855,43.92758], "fy":[123.16724,142.11594,152.46584,146.29617]}, - {"t":0.32505, "x":0.80796, "y":1.12011, "heading":-2.78096, "vx":0.95217, "vy":2.67204, "omega":2.31096, "ax":3.03561, "ay":8.31688, "alpha":4.84068, "fx":[85.68142,54.6007,17.37941,48.8781], "fy":[126.5588,142.89072,151.86746,144.55428]}, - {"t":0.35755, "x":0.84051, "y":1.21136, "heading":-2.70584, "vx":1.05084, "vy":2.94238, "omega":2.46831, "ax":3.07265, "ay":8.35091, "alpha":3.59994, "fx":[77.32308,52.67436,26.35518,52.70672], "fy":[131.55779,143.35865,150.30443,142.96535]}, - {"t":0.39006, "x":0.87629, "y":1.31141, "heading":-2.62561, "vx":1.15071, "vy":3.21383, "omega":2.58533, "ax":3.12403, "ay":8.37197, "alpha":0.99931, "fx":[60.09406,52.60597,46.06128,53.79413], "fy":[139.72298,142.77488,144.94263,142.17871]}, - {"t":0.42256, "x":0.91535, "y":1.4203, "heading":-2.54158, "vx":1.25226, "vy":3.48596, "omega":2.61781, "ax":3.13977, "ay":7.99418, "alpha":-6.8489, "fx":[2.77301,67.45837,95.34731,48.04786], "fy":[150.54081,132.92507,116.94617,143.50313]}, - {"t":0.45507, "x":0.95771, "y":1.53783, "heading":-2.45649, "vx":1.35432, "vy":3.74581, "omega":2.39519, "ax":3.16651, "ay":6.19032, "alpha":-16.50498, "fx":[-51.99138,111.31429,117.64948,38.47337], "fy":[134.20423,57.65234,86.85762,142.46789]}, - {"t":0.48757, "x":1.00341, "y":1.66286, "heading":-2.37863, "vx":1.45724, "vy":3.94702, "omega":1.85869, "ax":-2.98708, "ay":-5.93993, "alpha":18.35633, "fx":[61.7155,-118.48977,-115.28731,-31.17564], "fy":[-129.20582,-41.1312,-89.79762,-144.0112]}, - {"t":0.52008, "x":1.04919, "y":1.78802, "heading":-2.31821, "vx":1.36015, "vy":3.75395, "omega":2.45536, "ax":-3.14694, "ay":-8.14635, "alpha":4.82267, "fx":[-19.89412,-70.09654,-80.70506,-43.41862], "fy":[-149.19627,-132.58045,-127.6323,-144.85939]}, - {"t":0.55258, "x":1.09174, "y":1.90574, "heading":-2.2384, "vx":1.25786, "vy":3.48915, "omega":2.61212, "ax":-3.11006, "ay":-8.3697, "alpha":-1.63099, "fx":[-62.71155,-48.10371,-42.25216,-58.53731], "fy":[-138.65758,-144.42108,-146.08942,-140.29664]}, - {"t":0.58509, "x":1.13099, "y":2.01473, "heading":-2.1535, "vx":1.15677, "vy":3.21709, "omega":2.55911, "ax":-3.06522, "ay":-8.34475, "alpha":-3.85727, "fx":[-72.64345,-40.26455,-26.80057,-68.84564], "fy":[-134.30528,-147.32834,-150.13854,-135.99489]}, - {"t":0.61759, "x":1.16697, "y":2.11489, "heading":-2.07031, "vx":1.05713, "vy":2.94585, "omega":2.43373, "ax":-3.03591, "ay":-8.30962, "alpha":-5.05381, "fx":[-75.70844,-34.92285,-19.33273,-76.59611], "fy":[-132.87813,-148.91422,-151.50924,-132.07518]}, - {"t":0.6501, "x":1.19973, "y":2.20626, "heading":-1.99121, "vx":0.95845, "vy":2.67575, "omega":2.26946, "ax":-3.01299, "ay":-8.28057, "alpha":-5.81773, "fx":[-76.33576,-30.59981,-15.53287,-82.5318], "fy":[-132.67755,-149.98953,-152.08369,-128.65001]}, - {"t":0.6826, "x":1.22929, "y":2.28886, "heading":-1.91744, "vx":0.86051, "vy":2.40659, "omega":2.08035, "ax":-2.99293, "ay":-8.2589, "alpha":-6.33744, "fx":[-75.88636,-26.9023,-13.77202,-87.07471], "fy":[-133.03704,-150.7774,-152.34585,-125.76579]}, - {"t":0.71511, "x":1.25568, "y":2.36272, "heading":-1.84982, "vx":0.76323, "vy":2.13813, "omega":1.87435, "ax":-2.97501, "ay":-8.24359, "alpha":-6.69651, "fx":[-74.93124,-23.68524,-13.26302,-90.53674], "fy":[-133.6477,-151.37194,-152.45792,-123.40718]}, - {"t":0.74761, "x":1.27892, "y":2.42786, "heading":-1.78889, "vx":0.66653, "vy":1.87018, "omega":1.65668, "ax":-2.95933, "ay":-8.2332, "alpha":-6.94396, "fx":[-73.76073,-20.88049,-13.54162,-93.16675], "fy":[-134.34844,-151.8252,-152.48452,-121.51917]}, - {"t":0.78012, "x":1.29902, "y":2.4843, "heading":-1.73504, "vx":0.57033, "vy":1.60256, "omega":1.43097, "ax":-2.94603, "ay":-8.22626, "alpha":-7.11409, "fx":[-72.54232,-18.44794,-14.2899,-95.1645], "fy":[-135.04894,-152.17135,-152.45697,-120.02834]}, - {"t":0.81262, "x":1.316, "y":2.53205, "heading":-1.68853, "vx":0.47457, "vy":1.33517, "omega":1.19973, "ax":-2.93504, "ay":-8.22155, "alpha":-7.23273, "fx":[-71.38116,-16.3591,-15.26823,-96.68856], "fy":[-135.69645,-152.43503,-152.39572,-118.85785]}, - {"t":0.84513, "x":1.32988, "y":2.57111, "heading":-1.64953, "vx":0.37917, "vy":1.06793, "omega":0.96463, "ax":-2.9261, "ay":-8.21809, "alpha":-7.31949, "fx":[-70.34725,-14.59132,-16.28693,-97.86313], "fy":[-136.25948,-152.63483,-152.31873,-117.93622]}, - {"t":0.87763, "x":1.34065, "y":2.60148, "heading":-1.61817, "vx":0.28406, "vy":0.8008, "omega":0.72671, "ax":-2.91881, "ay":-8.21515, "alpha":-7.389, "fx":[-69.48912,-13.12596,-17.19323,-98.78415], "fy":[-136.71883,-152.78497,-152.24379,-117.20153]}, - {"t":0.91013, "x":1.34835, "y":2.62317, "heading":-1.59455, "vx":0.18918, "vy":0.53377, "omega":0.48654, "ax":-2.91273, "ay":-8.21222, "alpha":-7.45197, "fx":[-68.84151,-11.94809,-17.86454,-99.52457], "fy":[-137.06247,-152.89627,-152.18826,-116.60293]}, - {"t":0.94264, "x":1.35296, "y":2.63618, "heading":-1.57874, "vx":0.09451, "vy":0.26683, "omega":0.24431, "ax":-2.90745, "ay":-8.20894, "alpha":-7.51612, "fx":[-68.42996,-11.04679,-18.20425,-100.13848], "fy":[-137.28226,-152.97682,-152.16748,-116.10066]}, - {"t":0.97514, "x":1.35449, "y":2.64052, "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":0.65435, "y":0.68695, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.04278, "ay":8.49011, "alpha":0.0, "fx":[51.75688,51.75688,51.75688,51.75688], "fy":[144.41428,144.41428,144.41428,144.41428]}, + {"t":0.04799, "x":0.65785, "y":0.69673, "heading":-1.5708, "vx":0.14602, "vy":0.40742, "omega":0.0, "ax":3.04247, "ay":8.48923, "alpha":0.0, "fx":[51.75153,51.75153,51.75153,51.75153], "fy":[144.39936,144.39936,144.39936,144.39936]}, + {"t":0.09598, "x":0.66836, "y":0.72605, "heading":-1.5708, "vx":0.29202, "vy":0.8148, "omega":0.0, "ax":3.04207, "ay":8.48812, "alpha":0.0, "fx":[51.74477,51.74477,51.74477,51.74477], "fy":[144.38048,144.38048,144.38048,144.38048]}, + {"t":0.14396, "x":0.68588, "y":0.77492, "heading":-1.5708, "vx":0.438, "vy":1.22212, "omega":0.0, "ax":3.04155, "ay":8.48667, "alpha":0.0, "fx":[51.73591,51.73591,51.73591,51.73591], "fy":[144.35578,144.35578,144.35578,144.35578]}, + {"t":0.19195, "x":0.7104, "y":0.84334, "heading":-1.5708, "vx":0.58395, "vy":1.62938, "omega":0.0, "ax":3.04084, "ay":8.48469, "alpha":0.0, "fx":[51.72384,51.72384,51.72384,51.72384], "fy":[144.32209,144.32209,144.32209,144.32209]}, + {"t":0.23994, "x":0.74192, "y":0.9313, "heading":-1.5708, "vx":0.72988, "vy":2.03654, "omega":0.0, "ax":3.03982, "ay":8.48183, "alpha":0.0, "fx":[51.7064,51.7064,51.7064,51.7064], "fy":[144.27344,144.27344,144.27344,144.27344]}, + {"t":0.28793, "x":0.78045, "y":1.0388, "heading":-1.5708, "vx":0.87575, "vy":2.44356, "omega":0.0, "ax":3.03821, "ay":8.47733, "alpha":0.0, "fx":[51.679,51.679,51.679,51.679], "fy":[144.19699,144.19699,144.19699,144.19699]}, + {"t":0.33591, "x":0.82597, "y":1.16582, "heading":-1.5708, "vx":1.02155, "vy":2.85036, "omega":0.0, "ax":3.03531, "ay":8.46924, "alpha":0.0, "fx":[51.6297,51.6297,51.6297,51.6297], "fy":[144.05941,144.05941,144.05941,144.05941]}, + {"t":0.3839, "x":0.87849, "y":1.31235, "heading":-1.5708, "vx":1.1672, "vy":3.25678, "omega":0.0, "ax":3.02855, "ay":8.45039, "alpha":0.0, "fx":[51.51475,51.51475,51.51475,51.51475], "fy":[143.73869,143.73869,143.73869,143.73869]}, + {"t":0.43189, "x":0.93799, "y":1.47837, "heading":-1.5708, "vx":1.31254, "vy":3.6623, "omega":0.0, "ax":2.99494, "ay":8.35662, "alpha":0.0, "fx":[50.9431,50.9431,50.9431,50.9431], "fy":[142.14364,142.14364,142.14364,142.14364]}, + {"t":0.47988, "x":1.00442, "y":1.66373, "heading":-1.5708, "vx":1.45626, "vy":4.06331, "omega":0.0, "ax":-2.99494, "ay":-8.35662, "alpha":0.0, "fx":[-50.9431,-50.9431,-50.9431,-50.9431], "fy":[-142.14364,-142.14364,-142.14364,-142.14364]}, + {"t":0.52786, "x":1.07086, "y":1.8491, "heading":-1.5708, "vx":1.31254, "vy":3.6623, "omega":0.0, "ax":-3.02855, "ay":-8.45039, "alpha":0.0, "fx":[-51.51475,-51.51475,-51.51475,-51.51475], "fy":[-143.73869,-143.73869,-143.73869,-143.73869]}, + {"t":0.57585, "x":1.13035, "y":2.01511, "heading":-1.5708, "vx":1.1672, "vy":3.25678, "omega":0.0, "ax":-3.03531, "ay":-8.46924, "alpha":0.0, "fx":[-51.6297,-51.6297,-51.6297,-51.6297], "fy":[-144.05941,-144.05941,-144.05941,-144.05941]}, + {"t":0.62384, "x":1.18287, "y":2.16165, "heading":-1.5708, "vx":1.02155, "vy":2.85036, "omega":0.0, "ax":-3.03821, "ay":-8.47733, "alpha":0.0, "fx":[-51.679,-51.679,-51.679,-51.679], "fy":[-144.19699,-144.19699,-144.19699,-144.19699]}, + {"t":0.67183, "x":1.22839, "y":2.28867, "heading":-1.5708, "vx":0.87575, "vy":2.44356, "omega":0.0, "ax":-3.03982, "ay":-8.48183, "alpha":0.0, "fx":[-51.7064,-51.7064,-51.7064,-51.7064], "fy":[-144.27344,-144.27344,-144.27344,-144.27344]}, + {"t":0.71981, "x":1.26692, "y":2.39616, "heading":-1.5708, "vx":0.72988, "vy":2.03654, "omega":0.0, "ax":-3.04084, "ay":-8.48469, "alpha":0.0, "fx":[-51.72384,-51.72384,-51.72384,-51.72384], "fy":[-144.32209,-144.32209,-144.32209,-144.32209]}, + {"t":0.7678, "x":1.29844, "y":2.48412, "heading":-1.5708, "vx":0.58395, "vy":1.62938, "omega":0.0, "ax":-3.04155, "ay":-8.48667, "alpha":0.0, "fx":[-51.73591,-51.73591,-51.73591,-51.73591], "fy":[-144.35578,-144.35578,-144.35578,-144.35578]}, + {"t":0.81579, "x":1.32296, "y":2.55254, "heading":-1.5708, "vx":0.438, "vy":1.22212, "omega":0.0, "ax":-3.04207, "ay":-8.48812, "alpha":0.0, "fx":[-51.74477,-51.74477,-51.74477,-51.74477], "fy":[-144.38048,-144.38048,-144.38048,-144.38048]}, + {"t":0.86378, "x":1.34048, "y":2.60141, "heading":-1.5708, "vx":0.29202, "vy":0.8148, "omega":0.0, "ax":-3.04247, "ay":-8.48923, "alpha":0.0, "fx":[-51.75153,-51.75153,-51.75153,-51.75153], "fy":[-144.39936,-144.39936,-144.39936,-144.39936]}, + {"t":0.91176, "x":1.35099, "y":2.63074, "heading":-1.5708, "vx":0.14602, "vy":0.40742, "omega":0.0, "ax":-3.04278, "ay":-8.49011, "alpha":0.0, "fx":[-51.75688,-51.75688,-51.75688,-51.75688], "fy":[-144.41428,-144.41428,-144.41428,-144.41428]}, + {"t":0.95975, "x":1.35449, "y":2.64052, "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/Rebuilt.chor b/src/main/deploy/choreo/Rebuilt.chor index 860a4c2..d29b455 100644 --- a/src/main/deploy/choreo/Rebuilt.chor +++ b/src/main/deploy/choreo/Rebuilt.chor @@ -85,8 +85,8 @@ "val":0.6869499087333679 }, "heading":{ - "exp":"180 deg", - "val":3.141592653589793 + "exp":"270 deg", + "val":4.71238898038469 } }, "RightBump":{ diff --git a/src/main/deploy/choreo/RightBumpToOutpost.traj b/src/main/deploy/choreo/RightBumpToOutpost.traj index 2480b7c..4ed91f8 100644 --- a/src/main/deploy/choreo/RightBumpToOutpost.traj +++ b/src/main/deploy/choreo/RightBumpToOutpost.traj @@ -14,7 +14,7 @@ "params":{ "waypoints":[ {"x":{"exp":"RightBump.x", "val":3.616}, "y":{"exp":"RightBump.y", "val":2.511}, "heading":{"exp":"RightBump.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":4.71238898038469}, "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}, diff --git a/src/main/deploy/choreo/RightTrenchToOutpost.traj b/src/main/deploy/choreo/RightTrenchToOutpost.traj index 6bcf59a..7870768 100644 --- a/src/main/deploy/choreo/RightTrenchToOutpost.traj +++ b/src/main/deploy/choreo/RightTrenchToOutpost.traj @@ -18,7 +18,7 @@ "waypoints":[ {"x":{"exp":"RightTrench.x", "val":3.616}, "y":{"exp":"RightTrench.y", "val":0.62}, "heading":{"exp":"RightTrench.heading", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"2.168159246444702 m", "val":2.168159246444702}, "y":{"exp":"0.8994683027267456 m", "val":0.8994683027267456}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"Outpost.x", "val":0.6543505787849426}, "y":{"exp":"Outpost.y", "val":0.6869499087333679}, "heading":{"exp":"Outpost.heading", "val":4.71238898038469}, "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}, diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index cc25176..013bedb 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -3,6 +3,7 @@ import java.util.Arrays; import java.util.Collections; import java.util.List; +import java.util.ArrayList; import java.util.stream.IntStream; import choreo.auto.AutoFactory; @@ -45,6 +46,7 @@ private enum AutoMode ShootWithDelay("Shoot + Delay"), DriveThenShoot("Drive Then Shoot"), ShootWithDelayThenDrive("Shoot + Delay + Drive"), + HubOutpostDelayTowerShoot("Hub -> Outpost -> 4s -> Tower -> Shoot"), DrivePickupThenShoot("Drive + Pickup + Shoot"), DriveWithDelayPickupThenShoot("Drive + Delay + Pickup + Shoot"); // @formatter:on @@ -146,6 +148,8 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. @NotLogged private static final int DEFAULT_DELAY_SECONDS = 0; @NotLogged + private static final int HUB_OUTPOST_TOWER_DELAY = 4; + @NotLogged private final Drive _driveSubsystem; @NotLogged private final Shooter _shooterSubsystem; @@ -213,19 +217,20 @@ public void periodic() public Command buildAuto() { - var mode = _modeChooser.getSelected(); - var startPosition = _startChooser.getSelected(); - var delaySeconds = _delayChooser.getSelected(); - var driveOption = _driveChooser.getSelected(); - var startPose = getStartPose(mode, startPosition, driveOption); - var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); - var shoot = _shooterSubsystem.shoot(); - var delay = Commands.waitSeconds(delaySeconds); - var extend = _intakeSubsystem.getExtendCmd(); - var pickup = _intakeSubsystem.runRollersForward(); - var jiggle = _intakeSubsystem.jiggle(); - var drive = driveOption.staysPut() ? Commands.none() - : _autoFactory.trajectoryCmd(driveOption.trajectory().name()).andThen(_driveSubsystem.runOnce(() -> _driveSubsystem.setControl(_autoRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0)))); + var mode = _modeChooser.getSelected(); + var startPosition = _startChooser.getSelected(); + var delaySeconds = _delayChooser.getSelected(); + var driveOption = _driveChooser.getSelected(); + var startPose = getStartPose(mode, startPosition, driveOption); + var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); + var shoot = _shooterSubsystem.shoot(); + var delay = Commands.waitSeconds(delaySeconds); + var extend = _intakeSubsystem.getExtendCmd(); + var pickup = _intakeSubsystem.runRollersForward(); + var jiggle = _intakeSubsystem.jiggle(); + var drive = buildDriveCommand(driveOption.trajectory()); + var hubToOutpost = buildDriveCommand(ChoreoTraj.HubToOutpost); + var outpostToTower = buildDriveCommand(ChoreoTraj.OutpostToTower); return switch (mode) { @@ -235,6 +240,7 @@ public Command buildAuto() case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot); case DriveThenShoot -> Commands.sequence(resetPose, drive, shoot); case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive); + case HubOutpostDelayTowerShoot -> Commands.sequence(resetPose, hubToOutpost, Commands.waitSeconds(HUB_OUTPOST_TOWER_DELAY), outpostToTower, shoot); case DrivePickupThenShoot -> Commands.sequence(resetPose, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle)); case DriveWithDelayPickupThenShoot -> Commands.sequence(resetPose, delay, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle)); }; @@ -291,13 +297,8 @@ private void updateDashboard() var startPosition = _startChooser.getSelected(); var driveOption = _driveChooser.getSelected(); var startPose = getStartPose(mode, startPosition, driveOption); - var endPose = driveOption.staysPut() ? startPose : flip(driveOption.endPoseBlue()); - var trajectory = Collections.emptyList(); - - if (!driveOption.staysPut() && mode.usesDrivePath()) - { - trajectory = flip(Arrays.asList(driveOption.trajectory.asAutoTraj(_trajectoryDrawRoutine).getRawTrajectory().getPoses())); - } + var endPose = getEndPose(mode, startPose, driveOption); + var trajectory = getTrajectoryPreview(mode, driveOption); if (startPose != null) { @@ -311,6 +312,11 @@ private void updateDashboard() private String buildSummary(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption, int delaySeconds) { + if (mode == AutoMode.HubOutpostDelayTowerShoot) + { + return "Hub -> Outpost | 4s Delay | Tower -> Shoot"; + } + var summary = new StringBuilder(); summary.append(mode.displayName).append(" | ").append(startPosition.displayName); @@ -345,6 +351,11 @@ private void followTrajectory(SwerveSample sample) private Pose2d getStartPose(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption) { + if (mode == AutoMode.HubOutpostDelayTowerShoot) + { + return flip(ChoreoTraj.HubToOutpost.initialPoseBlue()); + } + var pose = startPosition.blueStartPose; if (driveOption != null && (!driveOption.staysPut() || mode.usesDrivePath())) @@ -355,6 +366,44 @@ private Pose2d getStartPose(AutoMode mode, StartPosition startPosition, AutoDriv return flip(pose); } + private Pose2d getEndPose(AutoMode mode, Pose2d startPose, AutoDriveOption driveOption) + { + if (mode == AutoMode.HubOutpostDelayTowerShoot) + { + return flip(ChoreoTraj.OutpostToTower.endPoseBlue()); + } + + return driveOption.staysPut() ? startPose : flip(driveOption.endPoseBlue()); + } + + private List getTrajectoryPreview(AutoMode mode, AutoDriveOption driveOption) + { + if (mode == AutoMode.HubOutpostDelayTowerShoot) + { + var trajectory = new ArrayList(); + trajectory.addAll(Arrays.asList(ChoreoTraj.HubToOutpost.asAutoTraj(_trajectoryDrawRoutine).getRawTrajectory().getPoses())); + trajectory.addAll(Arrays.asList(ChoreoTraj.OutpostToTower.asAutoTraj(_trajectoryDrawRoutine).getRawTrajectory().getPoses())); + return flip(trajectory); + } + + if (!driveOption.staysPut() && mode.usesDrivePath()) + { + return flip(Arrays.asList(driveOption.trajectory.asAutoTraj(_trajectoryDrawRoutine).getRawTrajectory().getPoses())); + } + + return Collections.emptyList(); + } + + private Command buildDriveCommand(ChoreoTraj trajectory) + { + if (trajectory == null) + { + return Commands.none(); + } + + return _autoFactory.trajectoryCmd(trajectory.name()).andThen(_driveSubsystem.runOnce(() -> _driveSubsystem.setControl(_autoRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0)))); + } + private List flip(List bluePoses) { if (Utilities.isRedAlliance()) diff --git a/src/main/java/frc/robot/generated/ChoreoTraj.java b/src/main/java/frc/robot/generated/ChoreoTraj.java index 775a2f9..e6c401a 100644 --- a/src/main/java/frc/robot/generated/ChoreoTraj.java +++ b/src/main/java/frc/robot/generated/ChoreoTraj.java @@ -20,34 +20,40 @@ public record ChoreoTraj(String name, OptionalInt segment, double totalTimeSecs, Pose2d initialPoseBlue, Pose2d endPoseBlue) { - public static final ChoreoTraj DepotToOutpost = new ChoreoTraj("DepotToOutpost", OptionalInt.empty(), 1.70772, new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj DepotToTower = new ChoreoTraj("DepotToTower", OptionalInt.empty(), 1.1836, new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); - public static final ChoreoTraj HubToDepot = new ChoreoTraj("HubToDepot", OptionalInt.empty(), 1.25742, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj HubToOutpost = new ChoreoTraj("HubToOutpost", OptionalInt.empty(), 1.67965, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj HubToTower = new ChoreoTraj("HubToTower", OptionalInt.empty(), 1.10566, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(1.851, 4.056, Rotation2d.fromRadians(-1.555))); + public static final ChoreoTraj LeftBumpToDepot = new ChoreoTraj("LeftBumpToDepot", OptionalInt.empty(), 1.11437, new Pose2d(3.61569, 5.55879, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.96257, Rotation2d.fromRadians(3.14159))); + public static final ChoreoTraj OutpostToTower = new ChoreoTraj( + "OutpostToTower", OptionalInt.empty(), 0.95975, new Pose2d(0.65435, 0.68695, Rotation2d.fromRadians(-1.5708)), new Pose2d(1.35449, 2.64052, Rotation2d.fromRadians(-1.5708)) + ); + public static final ChoreoTraj RightTrenchToOutpost = new ChoreoTraj( + "RightTrenchToOutpost", OptionalInt.empty(), 1.61166, new Pose2d(3.616, 0.62, Rotation2d.fromRadians(0)), new Pose2d(0.65435, 0.68695, Rotation2d.fromRadians(3.14159)) + ); + public static final ChoreoTraj HubToOutpost = new ChoreoTraj("HubToOutpost", OptionalInt.empty(), 1.6667, new Pose2d(3.61569, 4.03479, Rotation2d.fromRadians(0)), new Pose2d(0.65435, 0.68695, Rotation2d.fromRadians(-1.5708))); + public static final ChoreoTraj HubToTower = new ChoreoTraj("HubToTower", OptionalInt.empty(), 1.10566, new Pose2d(3.61569, 4.03479, Rotation2d.fromRadians(0)), new Pose2d(1.85137, 4.05598, Rotation2d.fromRadians(-1.55492))); + public static final ChoreoTraj DepotToTower = new ChoreoTraj("DepotToTower", OptionalInt.empty(), 1.1836, new Pose2d(1.15, 5.96257, Rotation2d.fromRadians(3.14159)), new Pose2d(0.79933, 4.89541, Rotation2d.fromRadians(1.5708))); + public static final ChoreoTraj RightBumpToTower = new ChoreoTraj("RightBumpToTower", OptionalInt.empty(), 0.76635, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(1.35449, 2.64052, Rotation2d.fromRadians(-1.5708))); + public static final ChoreoTraj DepotToOutpost = new ChoreoTraj("DepotToOutpost", OptionalInt.empty(), 1.70772, new Pose2d(1.15, 5.96257, Rotation2d.fromRadians(3.14159)), new Pose2d(0.65435, 0.68695, Rotation2d.fromRadians(3.14159))); public static final ChoreoTraj LeftBumpPickupFromMidScore = new ChoreoTraj( - "LeftBumpPickupFromMidScore", OptionalInt.empty(), 6.27732, new Pose2d(3.652, 5.514, Rotation2d.fromRadians(0)), new Pose2d(1.974, 5.587, Rotation2d.fromRadians(-2.437)) + "LeftBumpPickupFromMidScore", OptionalInt.empty(), 6.27732, new Pose2d(3.65206, 5.51369, Rotation2d.fromRadians(0)), new Pose2d(1.97387, 5.58719, Rotation2d.fromRadians(-2.4371)) ); - public static final ChoreoTraj LeftBumpToDepot = new ChoreoTraj("LeftBumpToDepot", OptionalInt.empty(), 1.11437, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj LeftBumpToTower = new ChoreoTraj("LeftBumpToTower", OptionalInt.empty(), 1.71082, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); - public static final ChoreoTraj LeftTrenchToDepot = new ChoreoTraj("LeftTrenchToDepot", OptionalInt.empty(), 1.66476, new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj OutpostToDepot = new ChoreoTraj("OutpostToDepot", OptionalInt.empty(), 1.6899, new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj OutpostToTower = new ChoreoTraj("OutpostToTower", OptionalInt.empty(), 0.97514, new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571))); public static final ChoreoTraj RightBumpPickupFromMidScore = new ChoreoTraj( - "RightBumpPickupFromMidScore", OptionalInt.empty(), 6.62509, new Pose2d(3.625, 2.535, Rotation2d.fromRadians(0)), new Pose2d(2.456, 2.535, Rotation2d.fromRadians(-1.622)) + "RightBumpPickupFromMidScore", OptionalInt.empty(), 6.62509, new Pose2d(3.62538, 2.53495, Rotation2d.fromRadians(0)), new Pose2d(2.45598, 2.53495, Rotation2d.fromRadians(-1.62203)) + ); + public static final ChoreoTraj LeftTrenchToDepot = new ChoreoTraj("LeftTrenchToDepot", OptionalInt.empty(), 1.66476, new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.14159)), new Pose2d(1.15, 5.96257, Rotation2d.fromRadians(3.14159))); + public static final ChoreoTraj HubToDepot = new ChoreoTraj("HubToDepot", OptionalInt.empty(), 1.25742, new Pose2d(3.61569, 4.03479, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.96257, Rotation2d.fromRadians(3.14159))); + public static final ChoreoTraj LeftBumpToTower = new ChoreoTraj( + "LeftBumpToTower", OptionalInt.empty(), 1.71082, new Pose2d(3.61569, 5.55879, Rotation2d.fromRadians(3.14159)), new Pose2d(0.79933, 4.89541, Rotation2d.fromRadians(1.5708)) ); - public static final ChoreoTraj RightBumpToOutpost = new ChoreoTraj("RightBumpToOutpost", OptionalInt.empty(), 1.32723, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj RightBumpToTower = new ChoreoTraj("RightBumpToTower", OptionalInt.empty(), 0.76635, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571))); - public static final ChoreoTraj RightTrenchToOutpost = new ChoreoTraj("RightTrenchToOutpost", OptionalInt.empty(), 1.61166, new Pose2d(3.616, 0.62, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj RightBumpToOutpost = new ChoreoTraj("RightBumpToOutpost", OptionalInt.empty(), 1.32723, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(0.65435, 0.68695, Rotation2d.fromRadians(3.14159))); + public static final ChoreoTraj OutpostToDepot = new ChoreoTraj("OutpostToDepot", OptionalInt.empty(), 1.6899, new Pose2d(0.65435, 0.68695, Rotation2d.fromRadians(3.14159)), new Pose2d(1.15, 5.96257, Rotation2d.fromRadians(3.14159))); /** * A map between trajectory names and their corresponding data. This allows for * trajectory data to be looked up with strings during runtime. */ public static final Map ALL_TRAJECTORIES = Map.ofEntries( - Map.entry("DepotToOutpost", DepotToOutpost), Map.entry("DepotToTower", DepotToTower), Map.entry("HubToDepot", HubToDepot), Map.entry("HubToOutpost", HubToOutpost), Map.entry("HubToTower", HubToTower), - Map.entry("LeftBumpPickupFromMidScore", LeftBumpPickupFromMidScore), Map.entry("LeftBumpToDepot", LeftBumpToDepot), Map.entry("LeftBumpToTower", LeftBumpToTower), Map.entry("LeftTrenchToDepot", LeftTrenchToDepot), - Map.entry("OutpostToDepot", OutpostToDepot), Map.entry("OutpostToTower", OutpostToTower), Map.entry("RightBumpPickupFromMidScore", RightBumpPickupFromMidScore), Map.entry("RightBumpToOutpost", RightBumpToOutpost), - Map.entry("RightBumpToTower", RightBumpToTower), Map.entry("RightTrenchToOutpost", RightTrenchToOutpost) + Map.entry("LeftBumpToDepot", LeftBumpToDepot), Map.entry("OutpostToTower", OutpostToTower), Map.entry("RightTrenchToOutpost", RightTrenchToOutpost), Map.entry("HubToOutpost", HubToOutpost), Map.entry("HubToTower", HubToTower), + Map.entry("DepotToTower", DepotToTower), Map.entry("RightBumpToTower", RightBumpToTower), Map.entry("DepotToOutpost", DepotToOutpost), Map.entry("LeftBumpPickupFromMidScore", LeftBumpPickupFromMidScore), + Map.entry("RightBumpPickupFromMidScore", RightBumpPickupFromMidScore), Map.entry("LeftTrenchToDepot", LeftTrenchToDepot), Map.entry("HubToDepot", HubToDepot), Map.entry("LeftBumpToTower", LeftBumpToTower), + Map.entry("RightBumpToOutpost", RightBumpToOutpost), Map.entry("OutpostToDepot", OutpostToDepot) ); /** * Looks up the ChoreoTraj segment of the given overall ChoreoTraj. WARNING: diff --git a/src/main/java/frc/robot/generated/ChoreoVars.java b/src/main/java/frc/robot/generated/ChoreoVars.java index 404e809..de6be0f 100644 --- a/src/main/java/frc/robot/generated/ChoreoVars.java +++ b/src/main/java/frc/robot/generated/ChoreoVars.java @@ -1,3 +1,4 @@ +// spotless:off package frc.robot.generated; import edu.wpi.first.math.geometry.Pose2d; @@ -13,23 +14,16 @@ public final class ChoreoVars { public static final class Poses { - public static final Pose2d Depot = new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)); - public static final Pose2d HubStart = new Pose2d(3.616, 4.035, Rotation2d.kZero); - public static final Pose2d LeftBump = new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)); - public static final Pose2d LeftMiddle = new Pose2d(6.619, 5.588, Rotation2d.kZero); - public static final Pose2d LeftTrench = new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)); - public static final Pose2d Outpost = new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)); - public static final Pose2d RightBump = new Pose2d(3.616, 2.511, Rotation2d.kZero); - public static final Pose2d RightTrench = new Pose2d(3.616, 0.62, Rotation2d.kZero); - public static final Pose2d TowerLeft = new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)); - public static final Pose2d TowerRight = new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)); - - private Poses() - { - } - } - - private ChoreoVars() - { + public static final Pose2d Depot = new Pose2d(1.15, 5.9625654, Rotation2d.fromRadians(3.1415927)); + public static final Pose2d HubStart = new Pose2d(3.61569, 4.03479, Rotation2d.fromRadians(0)); + public static final Pose2d LeftBump = new Pose2d(3.61569, 5.55879, Rotation2d.fromRadians(3.1415927)); + public static final Pose2d LeftMiddle = new Pose2d(6.6194592, 5.5876384, Rotation2d.fromRadians(0)); + public static final Pose2d LeftTrench = new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.1415927)); + public static final Pose2d Outpost = new Pose2d(0.6543506, 0.6869499, Rotation2d.fromRadians(4.712389)); + public static final Pose2d RightBump = new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)); + public static final Pose2d RightTrench = new Pose2d(3.616, 0.62, Rotation2d.fromRadians(0)); + public static final Pose2d TowerLeft = new Pose2d(0.7993333, 4.8954082, Rotation2d.fromRadians(1.5707963)); + public static final Pose2d TowerRight = new Pose2d(1.3544923, 2.6405156, Rotation2d.fromRadians(-1.5707963)); } } +// spotless:on