diff --git a/src/main/deploy/choreo/AtoB better.traj b/src/main/deploy/choreo/AtoB better.traj new file mode 100644 index 00000000..8e287702 --- /dev/null +++ b/src/main/deploy/choreo/AtoB better.traj @@ -0,0 +1,155 @@ +{ + "name":"AtoB better", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":2.7247390747070312, "y":4.084929943084717, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.307336926460266, "y":5.2087788581848145, "heading":-0.08749516032270202, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":0.852199912071228, "y":5.028620719909668, "heading":-0.7188302916734751, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":0.7194516062736511, "y":4.516591548919678, "heading":-0.7140904273857694, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.601279616355896, "y":3.3692667484283447, "heading":-0.5813800977925542, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":false}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"2.7247390747070312 m", "val":2.7247390747070312}, "y":{"exp":"4.084929943084717 m", "val":4.084929943084717}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3073369264602661 m", "val":1.307336926460266}, "y":{"exp":"5.2087788581848145 m", "val":5.2087788581848145}, "heading":{"exp":"-87.49516032270202 mrad", "val":-0.08749516032270202}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"0.852199912071228 m", "val":0.852199912071228}, "y":{"exp":"5.028620719909668 m", "val":5.028620719909668}, "heading":{"exp":"-0.7188302916734751 rad", "val":-0.7188302916734751}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"0.7194516062736511 m", "val":0.7194516062736511}, "y":{"exp":"4.516591548919678 m", "val":4.516591548919678}, "heading":{"exp":"-0.7140904273857694 rad", "val":-0.7140904273857694}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.601279616355896 m", "val":1.601279616355896}, "y":{"exp":"3.3692667484283447 m", "val":3.3692667484283447}, "heading":{"exp":"-0.5813800977925542 rad", "val":-0.5813800977925542}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":false}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.91306,1.14726,1.4959,2.05812,3.0279], + "samples":[ + {"t":0.0, "x":2.72474, "y":4.08493, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.08498, "ay":5.47462, "alpha":-3.39028, "fx":[-43.05186,-29.48869,-56.11438,-73.07916], "fy":[95.09497,100.19714,88.1108,74.59577]}, + {"t":0.03044, "x":2.72331, "y":4.08747, "heading":0.0, "vx":-0.09389, "vy":0.16662, "omega":-0.10318, "ax":-3.12893, "ay":5.45011, "alpha":-3.37382, "fx":[-44.09088,-30.27696,-56.65449,-73.58615], "fy":[94.60444,99.9528,87.75574,74.08278]}, + {"t":0.06087, "x":2.719, "y":4.09506, "heading":-0.00314, "vx":-0.18912, "vy":0.3325, "omega":-0.20587, "ax":-3.17872, "ay":5.42186, "alpha":-3.35386, "fx":[-45.35875,-31.14023,-57.21059,-74.15454], "fy":[93.98794,99.67688,87.38426,73.49935]}, + {"t":0.09131, "x":2.71178, "y":4.10769, "heading":-0.00941, "vx":-0.28587, "vy":0.49751, "omega":-0.30794, "ax":-3.23552, "ay":5.38894, "alpha":-3.33013, "fx":[-46.87937,-32.10561,-57.79926,-74.79412], "fy":[93.22126,99.35809,86.98455,72.83197]}, + {"t":0.12174, "x":2.70158, "y":4.12533, "heading":-0.01878, "vx":-0.38434, "vy":0.66153, "omega":-0.4093, "ax":-3.30085, "ay":5.35011, "alpha":-3.30233, "fx":[-48.68271,-33.20892,-58.44205,-75.51707], "fy":[92.27188,98.98062,86.54068,72.06325]}, + {"t":0.15218, "x":2.68835, "y":4.14794, "heading":-0.03124, "vx":-0.4848, "vy":0.82436, "omega":-0.5098, "ax":-3.37671, "ay":5.30365, "alpha":-3.27011, "fx":[-50.80641,-34.49822,-59.16748,-76.33916], "fy":[91.09572,98.52181,86.03067,71.1699]}, + {"t":0.18261, "x":2.67203, "y":4.17549, "heading":-0.04675, "vx":-0.58757, "vy":0.98578, "omega":-0.60933, "ax":-3.46573, "ay":5.24715, "alpha":-3.23295, "fx":[-53.29783,-36.03895,-60.01415,-77.28143], "fy":[89.63213,97.94837,85.42349,70.11971]}, + {"t":0.21305, "x":2.65254, "y":4.20792, "heading":-0.0653, "vx":-0.69305, "vy":1.14547, "omega":-0.70772, "ax":-3.57147, "ay":5.17716, "alpha":-3.19012, "fx":[-56.21681,-37.92218,-61.03558,-78.37266], "fy":[87.79609,97.20978,84.67396,68.86681]}, + {"t":0.24348, "x":2.6298, "y":4.24518, "heading":-0.08684, "vx":-0.80175, "vy":1.30304, "omega":-0.80482, "ax":-3.69887, "ay":5.0885, "alpha":-3.1403, "fx":[-59.63905,-40.27773,-62.30791,-79.65304], "fy":[85.46555,96.22638,83.71389,67.3436]}, + {"t":0.27392, "x":2.60368, "y":4.28719, "heading":-0.11133, "vx":-0.91433, "vy":1.45791, "omega":-0.90039, "ax":-3.85485, "ay":4.97321, "alpha":-3.08102, "fx":[-63.65977,-43.29574,-63.94282,-81.17977], "fy":[82.46056,94.86654,82.43584,65.44696]}, + {"t":0.30435, "x":2.57407, "y":4.33387, "heading":-0.13873, "vx":-1.03165, "vy":1.60927, "omega":-0.99416, "ax":-4.04945, "ay":4.81833, "alpha":-3.00726, "fx":[-68.39561,-47.26271,-66.10945,-83.03548], "fy":[78.50757,92.9005,80.66115,63.01305]}, + {"t":0.33479, "x":2.54079, "y":4.38508, "heading":-0.16899, "vx":-1.1549, "vy":1.75592, "omega":-1.08569, "ax":-4.29733, "ay":4.60175, "alpha":-2.90813, "fx":[-73.97847,-52.62039,-69.07298,-85.34059], "fy":[73.17761,89.90049,78.07274,59.76843]}, + {"t":0.36522, "x":2.50365, "y":4.44065, "heading":-0.20204, "vx":-1.28569, "vy":1.89597, "omega":-1.1742, "ax":-4.61981, "ay":4.28305, "alpha":-2.75898, "fx":[-80.52112,-60.05,-73.26111,-88.26841], "fy":[65.77938,85.00993,74.0605,55.22885]}, + {"t":0.39566, "x":2.46238, "y":4.50034, "heading":-0.23777, "vx":-1.42629, "vy":2.02633, "omega":-1.25817, "ax":-5.04512, "ay":3.78297, "alpha":-2.5023, "fx":[-87.99542,-70.50996,-79.35969,-92.04724], "fy":[55.18699,76.38789,67.3296,48.47297]}, + {"t":0.42609, "x":2.41664, "y":4.56377, "heading":-0.27606, "vx":-1.57984, "vy":2.14146, "omega":-1.33433, "ax":-5.59222, "ay":2.93694, "alpha":-2.00981, "fx":[-95.8671,-84.69035,-88.27966,-96.85128], "fy":[39.6401,59.985,54.82504,37.6036]}, + {"t":0.45653, "x":2.36597, "y":4.6303, "heading":-0.31668, "vx":-1.75004, "vy":2.23085, "omega":-1.3955, "ax":-6.16747, "ay":1.42362, "alpha":-1.0632, "fx":[-102.20519,-99.54168,-99.53131,-102.02739], "fy":[16.89639,28.64586,29.04864,18.50279]}, + {"t":0.48696, "x":2.30985, "y":4.69886, "heading":-0.35915, "vx":-1.93775, "vy":2.27418, "omega":-1.42786, "ax":-6.23608, "ay":-1.06453, "alpha":0.48334, "fx":[-102.52868,-101.85417,-101.30166,-102.10733], "fy":[-13.90043,-18.34784,-20.95977,-16.40444]}, + {"t":0.5174, "x":2.24798, "y":4.76758, "heading":-0.4026, "vx":-2.12755, "vy":2.24178, "omega":-1.41315, "ax":-5.02987, "ay":-3.77252, "alpha":2.14469, "fx":[-91.68032,-84.999,-72.68086,-79.55522], "fy":[-47.8819,-59.25932,-73.72042,-65.8321]}, + {"t":0.54783, "x":2.1809, "y":4.83406, "heading":-0.44561, "vx":-2.28063, "vy":2.12696, "omega":-1.34787, "ax":-3.22226, "ay":-5.34929, "alpha":3.27636, "fx":[-71.04242,-64.34853,-38.94056,-36.3799], "fy":[-75.28496,-81.45537,-96.20603,-96.85652]}, + {"t":0.57827, "x":2.11, "y":4.89632, "heading":-0.48664, "vx":-2.3787, "vy":1.96416, "omega":-1.24815, "ax":-1.81625, "ay":-5.95849, "alpha":3.72354, "fx":[-48.45143,-48.60118,-17.95745,-3.75884], "fy":[-91.63494,-91.8991,-102.42692,-103.67907]}, + {"t":0.6087, "x":2.03676, "y":4.95334, "heading":-0.52462, "vx":-2.43398, "vy":1.78281, "omega":-1.13483, "ax":-0.88061, "ay":-6.16966, "alpha":3.79044, "fx":[-29.25663,-37.6061,-5.72864,15.00591], "fy":[-99.59084,-97.03696,-103.9608,-102.86]}, + {"t":0.63914, "x":1.96227, "y":5.00474, "heading":-0.55916, "vx":-2.46078, "vy":1.59503, "omega":-1.01946, "ax":-0.25068, "ay":-6.23414, "alpha":3.76423, "fx":[-14.42288,-29.82036,1.82763,26.02319], "fy":[-102.91386,-99.78779,-104.18805,-100.77537]}, + {"t":0.66957, "x":1.88726, "y":5.0504, "heading":-0.59019, "vx":-2.46841, "vy":1.4053, "omega":-0.9049, "ax":0.19093, "ay":-6.24236, "alpha":3.73424, "fx":[-3.19158,-24.10642,6.8136,32.97002], "fy":[-103.96601,-101.38016,-104.04083,-98.81566]}, + {"t":0.70001, "x":1.81222, "y":5.09028, "heading":-0.61773, "vx":-2.4626, "vy":1.21531, "omega":-0.79125, "ax":0.5132, "ay":-6.22871, "alpha":3.71635, "fx":[5.37701,-19.75686,10.29349,37.6456], "fy":[-103.95095,-102.36235,-103.79811,-97.19893]}, + {"t":0.73045, "x":1.73751, "y":5.12438, "heading":-0.64181, "vx":-2.44698, "vy":1.02574, "omega":-0.67814, "ax":0.75671, "ay":-6.20713, "alpha":3.70911, "fx":[12.01584,-16.33688,12.84067,40.96362], "fy":[-103.45379,-102.99919,-103.5472,-95.899]}, + {"t":0.76088, "x":1.66339, "y":5.15272, "heading":-0.66245, "vx":-2.42395, "vy":0.83682, "omega":-0.56525, "ax":0.9462, "ay":-6.1835, "alpha":3.70853, "fx":[17.23949,-13.57292,14.78547,43.42217], "fy":[-102.76083,-103.42838,-103.31361,-94.85075]}, + {"t":0.79132, "x":1.59005, "y":5.17533, "heading":-0.67966, "vx":-2.39515, "vy":0.64862, "omega":-0.45238, "ax":1.09731, "ay":-6.16031, "alpha":3.71126, "fx":[21.40171,-11.28734,16.32969,45.31169], "fy":[-102.01487,-103.72627,-103.10189,-93.99419]}, + {"t":0.82175, "x":1.51766, "y":5.19222, "heading":-0.69342, "vx":-2.36176, "vy":0.46113, "omega":-0.33943, "ax":1.22034, "ay":-6.13859, "alpha":3.71495, "fx":[24.74744,-9.36114,17.60286,46.81157], "fy":[-101.28822,-103.93758,-102.90951,-93.28176]}, + {"t":0.85219, "x":1.44635, "y":5.20341, "heading":-0.70376, "vx":-2.32461, "vy":0.2743, "omega":-0.22636, "ax":1.32227, "ay":-6.1187, "alpha":3.71803, "fx":[27.44905,-7.71209,18.69155,48.03797], "fy":[-100.61744,-104.08977,-102.73187,-92.67718]}, + {"t":0.88262, "x":1.37621, "y":5.20892, "heading":-0.71065, "vx":-2.28437, "vy":0.08808, "omega":-0.11321, "ax":1.40801, "ay":-6.10068, "alpha":3.71954, "fx":[29.63011,-6.28173,19.65568,49.06905], "fy":[-100.02047,-104.20044,-102.5641,-92.15301]}, + {"t":0.91306, "x":1.30734, "y":5.20878, "heading":-0.71409, "vx":-2.24152, "vy":-0.09759, "omega":0.0, "ax":1.55462, "ay":-6.18682, "alpha":0.0, "fx":[25.41508,25.41508,25.41508,25.41508], "fy":[-101.14267,-101.14267,-101.14267,-101.14267]}, + {"t":0.93257, "x":1.26389, "y":5.2057, "heading":-0.71409, "vx":-2.21118, "vy":-0.21834, "omega":0.0, "ax":1.75968, "ay":-6.13043, "alpha":0.0, "fx":[28.76736,28.76736,28.76736,28.76736], "fy":[-100.22093,-100.22093,-100.22093,-100.22093]}, + {"t":0.95209, "x":1.22107, "y":5.20027, "heading":-0.71409, "vx":-2.17683, "vy":-0.33799, "omega":0.0, "ax":1.99083, "ay":-6.05794, "alpha":0.0, "fx":[32.54633,32.54633,32.54633,32.54633], "fy":[-99.03583,-99.03583,-99.03583,-99.03583]}, + {"t":0.97161, "x":1.17896, "y":5.19252, "heading":-0.71409, "vx":-2.13798, "vy":-0.45622, "omega":0.0, "ax":2.25213, "ay":-5.96418, "alpha":0.0, "fx":[36.81805,36.81805,36.81805,36.81805], "fy":[-97.50299,-97.50299,-97.50299,-97.50299]}, + {"t":0.99112, "x":1.13766, "y":5.18248, "heading":-0.71409, "vx":-2.09403, "vy":-0.57262, "omega":0.0, "ax":2.54794, "ay":-5.84216, "alpha":0.0, "fx":[41.65395,41.65395,41.65395,41.65395], "fy":[-95.50817,-95.50817,-95.50817,-95.50817]}, + {"t":1.01064, "x":1.09728, "y":5.17019, "heading":-0.71409, "vx":-2.0443, "vy":-0.68664, "omega":0.0, "ax":2.8826, "ay":-5.68246, "alpha":0.0, "fx":[47.12509,47.12509,47.12509,47.12509], "fy":[-92.89738,-92.89738,-92.89738,-92.89738]}, + {"t":1.03016, "x":1.05793, "y":5.15571, "heading":-0.71409, "vx":-1.98804, "vy":-0.79754, "omega":0.0, "ax":3.2597, "ay":-5.47254, "alpha":0.0, "fx":[53.28983,53.28983,53.28983,53.28983], "fy":[-89.46553,-89.46553,-89.46553,-89.46553]}, + {"t":1.04967, "x":1.01975, "y":5.1391, "heading":-0.71409, "vx":-1.92442, "vy":-0.90434, "omega":0.0, "ax":3.68058, "ay":-5.19614, "alpha":0.0, "fx":[60.1705,60.1705,60.1705,60.1705], "fy":[-84.947,-84.947,-84.947,-84.947]}, + {"t":1.06919, "x":0.9829, "y":5.12046, "heading":-0.71409, "vx":-1.85259, "vy":-1.00575, "omega":0.0, "ax":4.142, "ay":-4.83332, "alpha":0.0, "fx":[67.71388,67.71388,67.71388,67.71388], "fy":[-79.01563,-79.01563,-79.01563,-79.01563]}, + {"t":1.08871, "x":0.94753, "y":5.09991, "heading":-0.71409, "vx":-1.77175, "vy":-1.10008, "omega":0.0, "ax":4.63256, "ay":-4.36198, "alpha":0.0, "fx":[75.73357,75.73357,75.73357,75.73357], "fy":[-71.31005,-71.31005,-71.31005,-71.31005]}, + {"t":1.10822, "x":0.91383, "y":5.07761, "heading":-0.71409, "vx":-1.68134, "vy":-1.18522, "omega":0.0, "ax":5.12876, "ay":-3.76237, "alpha":0.0, "fx":[83.8454,83.8454,83.8454,83.8454], "fy":[-61.50757,-61.50757,-61.50757,-61.50757]}, + {"t":1.12774, "x":0.882, "y":5.05376, "heading":-0.71409, "vx":-1.58124, "vy":-1.25864, "omega":0.0, "ax":5.59303, "ay":-3.02563, "alpha":0.0, "fx":[91.43541,91.43541,91.43541,91.43541], "fy":[-49.46335,-49.46335,-49.46335,-49.46335]}, + {"t":1.14726, "x":0.8522, "y":5.02862, "heading":-0.71409, "vx":-1.47209, "vy":-1.31769, "omega":0.0, "ax":5.96372, "ay":-2.24642, "alpha":0.0, "fx":[97.49542,97.49542,97.49542,97.49542], "fy":[-36.72461,-36.72461,-36.72461,-36.72461]}, + {"t":1.17631, "x":0.81195, "y":4.98939, "heading":-0.71409, "vx":-1.29882, "vy":-1.38296, "omega":0.0, "ax":6.16271, "ay":-1.63518, "alpha":0.0, "fx":[100.74863,100.74863,100.74863,100.74863], "fy":[-26.73212,-26.73212,-26.73212,-26.73212]}, + {"t":1.20536, "x":0.77681, "y":4.94852, "heading":-0.71409, "vx":-1.11977, "vy":-1.43047, "omega":0.0, "ax":6.27246, "ay":-1.15835, "alpha":0.0, "fx":[102.54283,102.54283,102.54283,102.54283], "fy":[-18.93686,-18.93686,-18.93686,-18.93686]}, + {"t":1.23442, "x":0.74693, "y":4.90647, "heading":-0.71409, "vx":-0.93753, "vy":-1.46412, "omega":0.0, "ax":6.33245, "ay":-0.78236, "alpha":0.0, "fx":[103.52349,103.52349,103.52349,103.52349], "fy":[-12.79011,-12.79011,-12.79011,-12.79011]}, + {"t":1.26347, "x":0.72236, "y":4.8636, "heading":-0.71409, "vx":-0.75354, "vy":-1.48686, "omega":0.0, "ax":6.36411, "ay":-0.48132, "alpha":0.0, "fx":[104.04107,104.04107,104.04107,104.04107], "fy":[-7.8686,-7.8686,-7.8686,-7.8686]}, + {"t":1.29253, "x":0.70315, "y":4.8202, "heading":-0.71409, "vx":-0.56864, "vy":-1.50084, "omega":0.0, "ax":6.3793, "ay":-0.23639, "alpha":0.0, "fx":[104.28939,104.28939,104.28939,104.28939], "fy":[-3.86455,-3.86455,-3.86455,-3.86455]}, + {"t":1.32158, "x":0.68932, "y":4.77649, "heading":-0.71409, "vx":-0.3833, "vy":-1.50771, "omega":0.0, "ax":6.38475, "ay":-0.03407, "alpha":0.0, "fx":[104.37851,104.37851,104.37851,104.37851], "fy":[-0.55704,-0.55704,-0.55704,-0.55704]}, + {"t":1.35063, "x":0.68088, "y":4.73267, "heading":-0.71409, "vx":-0.19779, "vy":-1.5087, "omega":0.0, "ax":6.38439, "ay":0.13539, "alpha":0.0, "fx":[104.37261,104.37261,104.37261,104.37261], "fy":[2.2133,2.2133,2.2133,2.2133]}, + {"t":1.37969, "x":0.67783, "y":4.6889, "heading":-0.71409, "vx":-0.0123, "vy":-1.50476, "omega":0.0, "ax":6.38056, "ay":0.2791, "alpha":0.0, "fx":[104.31008,104.31008,104.31008,104.31008], "fy":[4.5628,4.5628,4.5628,4.5628]}, + {"t":1.40874, "x":0.68017, "y":4.64529, "heading":-0.71409, "vx":0.17308, "vy":-1.49665, "omega":0.0, "ax":6.37471, "ay":0.40235, "alpha":0.0, "fx":[104.21432,104.21432,104.21432,104.21432], "fy":[6.57767,6.57767,6.57767,6.57767]}, + {"t":1.4378, "x":0.68789, "y":4.60198, "heading":-0.71409, "vx":0.35829, "vy":-1.48497, "omega":0.0, "ax":6.36771, "ay":0.5091, "alpha":0.0, "fx":[104.09985,104.09985,104.09985,104.09985], "fy":[8.32278,8.32278,8.32278,8.32278]}, + {"t":1.46685, "x":0.70098, "y":4.55905, "heading":-0.71409, "vx":0.54329, "vy":-1.47017, "omega":0.0, "ax":6.36012, "ay":0.60237, "alpha":0.0, "fx":[103.9758,103.9758,103.9758,103.9758], "fy":[9.84765,9.84765,9.84765,9.84765]}, + {"t":1.4959, "x":0.71945, "y":4.51659, "heading":-0.71409, "vx":0.72808, "vy":-1.45267, "omega":0.0, "ax":6.37916, "ay":0.26873, "alpha":-0.363, "fx":[104.38718,104.30632,104.14907,104.30618], "fy":[1.57207,4.50455,7.21722,4.27903]}, + {"t":1.52035, "x":0.73915, "y":4.48116, "heading":-0.71409, "vx":0.88402, "vy":-1.4461, "omega":-0.00887, "ax":6.31793, "ay":-0.89127, "alpha":-0.43673, "fx":[102.80554,103.41902,103.72415,103.19591], "fy":[-17.81706,-13.86076,-11.27301,-15.33149]}, + {"t":1.54479, "x":0.76265, "y":4.44555, "heading":-0.71431, "vx":1.03845, "vy":-1.46789, "omega":-0.01955, "ax":5.62794, "ay":-2.99191, "alpha":-0.53655, "fx":[90.39374,93.03736,93.65564,90.93794], "fy":[-51.92792,-47.03612,-45.7525,-50.93142]}, + {"t":1.56924, "x":0.78972, "y":4.40877, "heading":-0.71479, "vx":1.17602, "vy":-1.54102, "omega":-0.03266, "ax":4.38624, "ay":-4.61835, "alpha":-0.55225, "fx":[69.81872,74.02297,73.72566,69.25909], "fy":[-77.32554,-73.30799,-73.57442,-77.7969]}, + {"t":1.59368, "x":0.81977, "y":4.36972, "heading":-0.71558, "vx":1.28324, "vy":-1.65392, "omega":-0.04616, "ax":4.08604, "ay":-4.88235, "alpha":-0.51041, "fx":[65.09325,69.17869,68.62423,64.29989], "fy":[-81.279,-77.82657,-78.28414,-81.87847]}, + {"t":1.61812, "x":0.85236, "y":4.32784, "heading":-0.71671, "vx":1.38312, "vy":-1.77326, "omega":-0.05864, "ax":4.00048, "ay":-4.9483, "alpha":-0.45447, "fx":[63.88963,67.57905,67.00668,63.12575], "fy":[-82.14842,-79.13588,-79.58954,-82.70722]}, + {"t":1.64257, "x":0.88737, "y":4.28301, "heading":-0.71815, "vx":1.48091, "vy":-1.89422, "omega":-0.06975, "ax":3.94105, "ay":-4.98992, "alpha":-0.38044, "fx":[63.16598,66.2882,65.75896,62.5018], "fy":[-82.60046,-80.11109,-80.51591,-83.07555]}, + {"t":1.66701, "x":0.92474, "y":4.23522, "heading":-0.71985, "vx":1.57725, "vy":-2.01619, "omega":-0.07905, "ax":3.8901, "ay":-5.02156, "alpha":-0.27814, "fx":[62.67142,64.97858,64.55635,62.17684], "fy":[-82.8295,-81.02729,-81.33822,-83.17651]}, + {"t":1.69146, "x":0.96446, "y":4.18443, "heading":-0.72178, "vx":1.67234, "vy":-2.13894, "omega":-0.08585, "ax":3.83927, "ay":-5.04816, "alpha":-0.12763, "fx":[62.33889,63.41066,63.19823,62.1112], "fy":[-82.86142,-82.04123,-82.19058,-83.01808]}, + {"t":1.7159, "x":1.00649, "y":4.13064, "heading":-0.72388, "vx":1.76618, "vy":-2.26234, "omega":-0.08897, "ax":3.77929, "ay":-5.07237, "alpha":0.11575, "fx":[62.17328,61.18514,61.40123,62.37685], "fy":[-82.62438,-83.3624,-83.22002,-82.48773]}, + {"t":1.74035, "x":1.05079, "y":4.07382, "heading":-0.72606, "vx":1.85857, "vy":-2.38633, "omega":-0.08614, "ax":3.68956, "ay":-5.0956, "alpha":0.57691, "fx":[62.28409,57.22785,58.50468,63.25261], "fy":[-81.8267,-85.46797,-84.71376,-81.20475]}, + {"t":1.76479, "x":1.09732, "y":4.01397, "heading":-0.72816, "vx":1.94875, "vy":-2.51089, "omega":-0.07204, "ax":3.48452, "ay":-5.10906, "alpha":1.79173, "fx":[63.25668,46.66893,52.09883,65.83658], "fy":[-78.98824,-89.98805,-87.52842,-77.58897]}, + {"t":1.78923, "x":1.146, "y":3.95107, "heading":-0.72992, "vx":2.03393, "vy":-2.63578, "omega":-0.02824, "ax":1.85008, "ay":-3.48864, "alpha":13.94557, "fx":[67.01358,-45.47403,20.56147,78.88034], "fy":[-0.49972,-79.89413,-94.57029,-53.16664]}, + {"t":1.81368, "x":1.19627, "y":3.88559, "heading":-0.73061, "vx":2.07915, "vy":-2.72105, "omega":0.31265, "ax":-3.91396, "ay":4.32265, "alpha":5.69518, "fx":[-52.39935,-83.30131,-86.06466,-34.17803], "fy":[86.2236,56.70222,47.14182,92.60102]}, + {"t":1.83812, "x":1.24592, "y":3.82037, "heading":-0.72297, "vx":1.98348, "vy":-2.61539, "omega":0.45186, "ax":-3.89329, "ay":4.819, "alpha":2.87964, "fx":[-55.94924,-75.74773,-74.84431,-48.05003], "fy":[86.19393,69.33928,69.37899,90.2138]}, + {"t":1.86257, "x":1.29325, "y":3.75788, "heading":-0.71192, "vx":1.88831, "vy":-2.49759, "omega":0.52225, "ax":-3.8494, "ay":4.95319, "alpha":2.15683, "fx":[-56.89443,-72.70029,-71.03937,-51.08718], "fy":[86.3067,73.42713,74.61102,89.55625]}, + {"t":1.88701, "x":1.33825, "y":3.69831, "heading":-0.69916, "vx":1.79422, "vy":-2.37652, "omega":0.57498, "ax":-3.80793, "ay":5.02723, "alpha":1.8235, "fx":[-57.12969,-70.89514,-68.88988,-52.09484], "fy":[86.51573,75.60439,77.17986,89.44274]}, + {"t":1.91145, "x":1.38097, "y":3.64172, "heading":-0.6851, "vx":1.70114, "vy":-2.25363, "omega":0.61955, "ax":-3.76194, "ay":5.08489, "alpha":1.63079, "fx":[-56.98403,-69.51345,-67.24068,-52.26405], "fy":[86.82937,77.12982,78.94183,89.61188]}, + {"t":1.9359, "x":1.42143, "y":3.58815, "heading":-0.66996, "vx":1.60918, "vy":-2.12933, "omega":0.65941, "ax":-3.70216, "ay":5.14316, "alpha":1.50446, "fx":[-56.46929,-68.17518,-65.62131,-51.82741], "fy":[87.30929,78.48035,80.49553,90.03812]}, + {"t":1.96034, "x":1.45966, "y":3.53764, "heading":-0.65384, "vx":1.51868, "vy":-2.00361, "omega":0.69619, "ax":-3.60257, "ay":5.2234, "alpha":1.4139, "fx":[-55.25329,-66.38883,-63.4537,-50.48468], "fy":[88.18625,80.11224,82.35322,90.91861]}, + {"t":1.98479, "x":1.49571, "y":3.49022, "heading":-0.63682, "vx":1.43062, "vy":-1.87593, "omega":0.73075, "ax":-1.07699, "ay":6.26647, "alpha":1.16858, "fx":[-17.37195,-26.50191,-17.94137,-8.61149], "fy":[102.72201,100.71975,102.54719,103.79062]}, + {"t":2.00923, "x":1.53036, "y":3.44624, "heading":-0.61896, "vx":1.40429, "vy":-1.72275, "omega":0.75932, "ax":1.63503, "ay":6.16222, "alpha":0.76674, "fx":[24.50678,21.27723,29.2374,31.89735], "fy":[101.40756,102.11401,100.11694,99.3239]}, + {"t":2.03368, "x":1.56517, "y":3.40597, "heading":-0.6004, "vx":1.44426, "vy":-1.57212, "omega":0.77806, "ax":2.68574, "ay":5.7905, "alpha":0.56201, "fx":[41.68075,40.54263,46.30913,47.09457], "fy":[95.72255,96.19858,93.5571,93.17648]}, + {"t":2.05812, "x":1.60128, "y":3.36927, "heading":-0.58138, "vx":1.50991, "vy":-1.43058, "omega":0.7918, "ax":2.95783, "ay":5.6619, "alpha":0.44767, "fx":[46.43897,45.87444,50.38178,50.72448], "fy":[93.57384,93.84566,91.50423,91.3212]}, + {"t":2.09156, "x":1.65343, "y":3.32459, "heading":-0.5549, "vx":1.60882, "vy":-1.24124, "omega":0.80677, "ax":2.91081, "ay":5.68583, "alpha":0.42631, "fx":[45.70839,45.25198,49.56465,49.81968], "fy":[93.92365,94.13804,91.93995,91.80859]}, + {"t":2.125, "x":1.70885, "y":3.28626, "heading":-0.52792, "vx":1.70616, "vy":-1.0511, "omega":0.82102, "ax":2.85629, "ay":5.71297, "alpha":0.40165, "fx":[44.87607,44.52081,48.60312,48.77951], "fy":[94.31355,94.47532,92.44103,92.35472]}, + {"t":2.15844, "x":1.7675, "y":3.25431, "heading":-0.50047, "vx":1.80168, "vy":-0.86006, "omega":0.83445, "ax":2.79235, "ay":5.74396, "alpha":0.37286, "fx":[43.91589,43.65273,47.46046,47.56938], "fy":[94.75212,94.86718,93.02036,92.97134]}, + {"t":2.19188, "x":1.82932, "y":3.22876, "heading":-0.47256, "vx":1.89505, "vy":-0.66798, "omega":0.84692, "ax":2.71636, "ay":5.77963, "alpha":0.33882, "fx":[42.79192,42.60942,46.08656,46.14156], "fy":[95.2505,95.3259,93.69405,93.67351]}, + {"t":2.22532, "x":1.89421, "y":3.20965, "heading":-0.44424, "vx":1.98589, "vy":-0.4747, "omega":0.85825, "ax":2.62463, "ay":5.82107, "alpha":0.29795, "fx":[41.45341,41.33755,44.41142,44.42844], "fy":[95.8233,95.86719,94.48234,94.48059]}, + {"t":2.25876, "x":1.96208, "y":3.19704, "heading":-0.41554, "vx":2.07366, "vy":-0.28004, "omega":0.86822, "ax":2.5118, "ay":5.86963, "alpha":0.24802, "fx":[39.82649,39.76083,42.33432,42.33118], "fy":[96.48985,96.51128,95.4104,95.4175]}, + {"t":2.2922, "x":2.03283, "y":3.19095, "heading":-0.38651, "vx":2.15766, "vy":-0.08376, "omega":0.87651, "ax":2.36987, "ay":5.92706, "alpha":0.18568, "fx":[37.79958,37.7664,39.70553,39.69985], "fy":[97.27579,97.28396,96.50873,96.51582]}, + {"t":2.32564, "x":2.10631, "y":3.19147, "heading":-0.3572, "vx":2.23691, "vy":0.11444, "omega":0.88272, "ax":2.18631, "ay":5.99545, "alpha":0.10582, "fx":[35.19658,35.18071,36.29379,36.29689], "fy":[98.21464,98.2173,97.81144,97.81333]}, + {"t":2.35908, "x":2.18234, "y":3.19864, "heading":-0.32768, "vx":2.31002, "vy":0.31494, "omega":0.88626, "ax":1.94059, "ay":6.07699, "alpha":0.00016, "fx":[31.72407,31.72403,31.72575,31.72578], "fy":[99.34756,99.34756,99.34701,99.34701]}, + {"t":2.39253, "x":2.26067, "y":3.21257, "heading":-0.29804, "vx":2.37491, "vy":0.51815, "omega":0.88626, "ax":1.59698, "ay":6.1726, "alpha":-0.1454, "fx":[26.85917,26.9198,25.36856,25.28302], "fy":[100.71313,100.70252,101.10468,101.12056]}, + {"t":2.42597, "x":2.34098, "y":3.23335, "heading":-0.2684, "vx":2.42832, "vy":0.72457, "omega":0.8814, "ax":1.08913, "ay":6.27642, "alpha":-0.35639, "fx":[19.58994,19.90148,16.0956,15.6337], "fy":[102.29186,102.24817,102.91739,102.97239]}, + {"t":2.45941, "x":2.42279, "y":3.26109, "heading":-0.23893, "vx":2.46474, "vy":0.93445, "omega":0.86948, "ax":0.28585, "ay":6.35381, "alpha":-0.68141, "fx":[7.75997,8.98175,1.84534,0.10561], "fy":[103.7331,103.67381,104.05241,104.03113]}, + {"t":2.49285, "x":2.50537, "y":3.29589, "heading":-0.20985, "vx":2.4743, "vy":1.14693, "omega":0.8467, "ax":-1.07032, "ay":6.24984, "alpha":-1.20707, "fx":[-13.5767,-9.25193,-20.85287,-26.30909], "fy":[102.92047,103.4902,101.81705,100.464]}, + {"t":2.52629, "x":2.58752, "y":3.33774, "heading":-0.18154, "vx":2.4385, "vy":1.35593, "omega":0.80633, "ax":-3.28961, "ay":5.37908, "alpha":-1.98455, "fx":[-53.28267,-40.51458,-54.63856,-66.67937], "fy":[88.72593,95.43171,88.2279,79.36547]}, + {"t":2.55973, "x":2.66722, "y":3.38609, "heading":-0.15458, "vx":2.3285, "vy":1.53581, "omega":0.73997, "ax":-5.60943, "ay":2.806, "alpha":-2.70164, "fx":[-96.95902,-82.1259,-88.83912,-98.88984], "fy":[35.8508,62.98824,53.54108,31.11074]}, + {"t":2.59317, "x":2.74195, "y":3.43902, "heading":-0.12983, "vx":2.14092, "vy":1.62964, "omega":0.64962, "ax":-6.2855, "ay":-0.16631, "alpha":-2.61256, "fx":[-101.67639,-103.05969,-103.21473,-103.07325], "fy":[-20.23346,10.76227,11.40891,-12.81298]}, + {"t":2.62661, "x":2.81003, "y":3.49342, "heading":-0.10811, "vx":1.93072, "vy":1.62408, "omega":0.56226, "ax":-5.99676, "ay":-1.98727, "alpha":-2.18077, "fx":[-92.25182,-100.55681,-102.35164,-96.98223], "fy":[-47.8746,-25.96669,-18.4386,-37.67276]}, + {"t":2.66005, "x":2.87124, "y":3.54662, "heading":-0.0893, "vx":1.73019, "vy":1.55762, "omega":0.48933, "ax":-5.59885, "ay":-2.96853, "alpha":-1.86449, "fx":[-84.30942,-93.5403,-97.58836,-90.68435], "fy":[-61.05521,-45.52463,-36.29796,-51.24161]}, + {"t":2.69349, "x":2.92597, "y":3.59705, "heading":-0.07294, "vx":1.54296, "vy":1.45835, "omega":0.42698, "ax":-5.27495, "ay":-3.53412, "alpha":-1.65238, "fx":[-78.72503,-87.57426,-92.90712,-85.73497], "fy":[-68.25745,-56.3646,-47.19209,-59.29011]}, + {"t":2.72693, "x":2.97462, "y":3.64384, "heading":-0.05866, "vx":1.36656, "vy":1.34017, "omega":0.37173, "ax":-5.02845, "ay":-3.89002, "alpha":-1.50453, "fx":[-74.75593,-83.05875,-89.03693,-81.97072], "fy":[-72.67894,-62.9576,-54.25802,-64.48329]}, + {"t":2.76037, "x":3.01751, "y":3.68648, "heading":-0.04623, "vx":1.19841, "vy":1.21009, "omega":0.32141, "ax":-4.83996, "ay":-4.13084, "alpha":-1.3968, "fx":[-71.8342,-79.64926,-85.93895,-79.07392], "fy":[-75.6335,-67.30444,-59.12129,-68.06612]}, + {"t":2.79381, "x":3.05488, "y":3.72464, "heading":-0.03548, "vx":1.03656, "vy":1.07195, "omega":0.2747, "ax":-4.69294, "ay":-4.30319, "alpha":-1.31527, "fx":[-69.60886,-77.02213,-83.4531,-76.79857], "fy":[-77.73377,-70.35603,-62.63772,-70.66837]}, + {"t":2.82725, "x":3.08691, "y":3.75808, "heading":-0.0263, "vx":0.87963, "vy":0.92805, "omega":0.23072, "ax":-4.57579, "ay":-4.43203, "alpha":-1.25161, "fx":[-67.86382,-74.94936,-81.4341,-74.97448], "fy":[-79.29753,-72.60493,-65.28335,-72.63494]}, + {"t":2.86069, "x":3.11377, "y":3.78664, "heading":-0.01858, "vx":0.72661, "vy":0.77984, "omega":0.18887, "ax":-4.48057, "ay":-4.53168, "alpha":-1.20062, "fx":[-66.46188,-73.27706,-79.77067,-73.48531], "fy":[-80.50408,-74.32688,-67.33862,-74.16805]}, + {"t":2.89413, "x":3.13556, "y":3.81018, "heading":-0.01227, "vx":0.57678, "vy":0.6283, "omega":0.14872, "ax":-4.40181, "ay":-4.61092, "alpha":-1.15889, "fx":[-65.3127,-71.90062,-78.38089,-72.25041], "fy":[-81.46156,-75.68657,-68.9775,-75.39315]}, + {"t":2.92757, "x":3.15239, "y":3.82861, "heading":-0.00729, "vx":0.42958, "vy":0.47411, "omega":0.10996, "ax":-4.33567, "ay":-4.67533, "alpha":-1.12412, "fx":[-64.35472,-70.74748,-77.20468,-71.21276], "fy":[-82.23881,-76.78778,-70.31283,-76.39179]}, + {"t":2.96101, "x":3.16433, "y":3.84185, "heading":-0.00362, "vx":0.28459, "vy":0.31776, "omega":0.07237, "ax":-4.27939, "ay":-4.72869, "alpha":-1.0947, "fx":[-63.54476,-69.76617,-76.19761,-70.33112], "fy":[-82.88157,-77.69884,-71.42065,-77.21912]}, + {"t":2.99445, "x":3.17146, "y":3.84983, "heading":-0.0012, "vx":0.14149, "vy":0.15963, "omega":0.03576, "ax":-4.23096, "ay":-4.77357, "alpha":-1.06948, "fx":[-62.85165,-68.91933,-75.32634,-69.57504], "fy":[-83.42137,-78.46644,-72.3539,-77.91365]}, + {"t":3.0279, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/AtoB.traj b/src/main/deploy/choreo/AtoB.traj index bcf37a9a..16e33a66 100644 --- a/src/main/deploy/choreo/AtoB.traj +++ b/src/main/deploy/choreo/AtoB.traj @@ -4,31 +4,29 @@ "snapshot":{ "waypoints":[ {"x":2.7247390747070312, "y":4.084929943084717, "heading":3.141592653589793, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.0569186210632324, "y":4.02510404586792, "heading":3.141592653589793, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.667668342590332, "y":4.032181262969971, "heading":3.141592653589793, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.4166265726089478, "y":4.017168045043945, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.0569186210632324, "y":4.02510404586792, "heading":3.141592653589793, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2978549003601074, "y":4.0045623779296875, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"2.7247390747070312 m", "val":2.7247390747070312}, "y":{"exp":"4.084929943084717 m", "val":4.084929943084717}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.0569186210632324 m", "val":2.0569186210632324}, "y":{"exp":"4.02510404586792 m", "val":4.02510404586792}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.667668342590332 m", "val":1.667668342590332}, "y":{"exp":"4.032181262969971 m", "val":4.032181262969971}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.4166265726089478 m", "val":1.4166265726089478}, "y":{"exp":"4.017168045043945 m", "val":4.017168045043945}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.0569186210632324 m", "val":2.0569186210632324}, "y":{"exp":"4.02510404586792 m", "val":4.02510404586792}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2978549003601074 m", "val":1.2978549003601074}, "y":{"exp":"4.0045623779296875 m", "val":4.0045623779296875}, "heading":{"exp":"pi rad", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, - {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,80 +34,76 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.52985,0.76716,1.03679,2.16108], + "waypoints":[0.0,0.45838,0.91847,2.10529], "samples":[ - {"t":0.0, "x":2.72474, "y":4.08493, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.34728, "ay":-0.7307, "alpha":-0.35411, "fx":[-104.00489,-103.58165,-103.51146,-103.96601], "fy":[-9.85684,-13.60535,-14.10103,-10.21906]}, - {"t":0.04076, "x":2.71947, "y":4.08432, "heading":3.14159, "vx":-0.2587, "vy":-0.02978, "omega":-0.01443, "ax":-6.34645, "ay":-0.72982, "alpha":-0.35302, "fx":[-103.99041,-103.56899,-103.49866,-103.95116], "fy":[-9.84921,-13.58599,-14.07937,-10.20996]}, - {"t":0.08152, "x":2.70365, "y":4.0825, "heading":3.141, "vx":-0.51737, "vy":-0.05953, "omega":-0.02882, "ax":-6.34533, "ay":-0.72864, "alpha":-0.35157, "fx":[-103.9712,-103.55226,-103.48146,-103.93123], "fy":[-9.83805,-13.55898,-14.05145,-10.19906]}, - {"t":0.12227, "x":2.67729, "y":4.07947, "heading":3.13983, "vx":-0.77599, "vy":-0.08922, "omega":-0.04315, "ax":-6.34376, "ay":-0.72699, "alpha":-0.34954, "fx":[-103.94435,-103.52892,-103.4573,-103.90327], "fy":[-9.82185,-13.52047,-14.01293,-10.1845]}, - {"t":0.16303, "x":2.6404, "y":4.07523, "heading":3.13807, "vx":-1.03454, "vy":-0.11886, "omega":-0.0574, "ax":-6.34142, "ay":-0.72452, "alpha":-0.34649, "fx":[-103.90406,-103.49387,-103.42105,-103.86135], "fy":[-9.79759,-13.46278,-13.95515,-10.16259]}, - {"t":0.20379, "x":2.59297, "y":4.06979, "heading":3.13573, "vx":-1.293, "vy":-0.14839, "omega":-0.07152, "ax":-6.3375, "ay":-0.7204, "alpha":-0.34142, "fx":[-103.83678,-103.43519,-103.36073,-103.79164], "fy":[-9.75819,-13.36796,-13.85792,-10.12475]}, - {"t":0.24455, "x":2.535, "y":4.06314, "heading":3.13282, "vx":-1.5513, "vy":-0.17775, "omega":-0.08543, "ax":-6.32966, "ay":-0.71217, "alpha":-0.33128, "fx":[-103.70183,-103.317,-103.24032,-103.65271], "fy":[-9.6824,-13.18221,-13.66084,-10.04524]}, - {"t":0.2853, "x":2.46652, "y":4.0553, "heading":3.12933, "vx":-1.80929, "vy":-0.20677, "omega":-0.09894, "ax":-6.30609, "ay":-0.68754, "alpha":-0.30087, "fx":[-103.29529,-102.95848,-102.87893,-103.23787], "fy":[-9.46402,-12.63646,-13.06348,-9.79618]}, - {"t":0.32606, "x":2.38754, "y":4.0463, "heading":3.1253, "vx":-2.06631, "vy":-0.2348, "omega":-0.1112, "ax":0.87667, "ay":2.21037, "alpha":5.25367, "fx":[32.60995,27.83893,-1.11294,-2.00864], "fy":[21.61968,47.65043,50.89191,24.37962]}, - {"t":0.36682, "x":2.30405, "y":4.03857, "heading":3.12077, "vx":-2.03058, "vy":-0.14471, "omega":0.10293, "ax":6.29569, "ay":0.77394, "alpha":0.42214, "fx":[103.24202,102.71919,102.57819,103.15097], "fy":[10.1348,14.54097,15.26161,10.6721]}, - {"t":0.40758, "x":2.22652, "y":4.03332, "heading":3.12497, "vx":-1.77398, "vy":-0.11316, "omega":0.12013, "ax":6.32434, "ay":0.7556, "alpha":0.39225, "fx":[103.67301,103.1942,103.08773,103.60852], "fy":[10.01309,14.13428,14.77254,10.49083]}, - {"t":0.44833, "x":2.15947, "y":4.02933, "heading":3.12986, "vx":-1.51621, "vy":-0.08237, "omega":0.13612, "ax":6.33392, "ay":0.74944, "alpha":0.38227, "fx":[103.81705,103.3523,103.25843,103.76216], "fy":[9.97801,14.00469,14.6025,10.42232]}, - {"t":0.48909, "x":2.10293, "y":4.0266, "heading":3.13541, "vx":-1.25806, "vy":-0.05182, "omega":0.1517, "ax":6.33871, "ay":0.74634, "alpha":0.37728, "fx":[103.88874,103.43062,103.34451,103.83972], "fy":[9.96568,13.94622,14.51207,10.38122]}, - {"t":0.52985, "x":2.05692, "y":4.0251, "heading":3.14159, "vx":-0.99971, "vy":-0.0214, "omega":0.16708, "ax":-6.35188, "ay":0.51615, "alpha":0.31489, "fx":[-103.96597,-103.68136,-103.72514,-103.99218], "fy":[6.84312,10.3028,9.9794,6.62719]}, - {"t":0.55358, "x":2.03141, "y":4.02474, "heading":-3.13763, "vx":-1.15044, "vy":-0.00915, "omega":0.17455, "ax":-6.34718, "ay":0.51931, "alpha":0.2891, "fx":[-103.87764,-103.61556,-103.65861,-103.90546], "fy":[7.03573,10.20465,9.89842,6.8198]}, - {"t":0.57731, "x":2.00232, "y":4.02467, "heading":-3.13349, "vx":-1.30107, "vy":0.00317, "omega":0.18141, "ax":-6.34016, "ay":0.52398, "alpha":0.25065, "fx":[-103.7462,-103.5181,-103.55864,-103.77495], "fy":[7.31646,10.05468,9.78181,7.11119]}, - {"t":0.60104, "x":1.96966, "y":4.02489, "heading":-3.12918, "vx":-1.45153, "vy":0.01561, "omega":0.18736, "ax":-6.32852, "ay":0.53163, "alpha":0.18719, "fx":[-103.5297,-103.35839,-103.39213,-103.55663], "fy":[7.76869,9.80267,9.59478,7.59828]}, - {"t":0.62477, "x":1.93343, "y":4.02541, "heading":-3.12473, "vx":-1.60171, "vy":0.02822, "omega":0.1918, "ax":-6.30551, "ay":0.54646, "alpha":0.0626, "fx":[-103.10553,-103.04771,-103.06101,-103.11803], "fy":[8.63068,9.30393,9.23436,8.56525]}, - {"t":0.6485, "x":1.89364, "y":4.02624, "heading":-3.12018, "vx":-1.75134, "vy":0.04119, "omega":0.19329, "ax":-6.23866, "ay":0.58766, "alpha":-0.29303, "fx":[-101.90031,-102.17634,-102.0894,-101.79488], "fy":[10.96534,7.90277,8.2046,11.35568]}, - {"t":0.67223, "x":1.85033, "y":4.02738, "heading":-3.1156, "vx":-1.89939, "vy":0.05513, "omega":0.18633, "ax":-4.13305, "ay":1.06126, "alpha":-9.18885, "fx":[-79.30561,-86.19914,-60.40049,-44.36458], "fy":[40.34184,-12.92778,-23.17735,65.16136]}, - {"t":0.69596, "x":1.80409, "y":4.02899, "heading":-3.11117, "vx":-1.99747, "vy":0.08032, "omega":-0.03173, "ax":-0.01857, "ay":-0.64882, "alpha":-20.65279, "fx":[-65.63821,-49.5858,52.37641,61.63304], "fy":[43.7265,-68.47226,-66.06494,48.38295]}, - {"t":0.7197, "x":1.75668, "y":4.03071, "heading":-3.11193, "vx":-1.99791, "vy":0.06492, "omega":-0.52184, "ax":4.81335, "ay":-1.64598, "alpha":-8.69864, "fx":[83.07657,50.71938,84.85965,96.1006], "fy":[11.68497,-78.11685,-47.96952,6.76662]}, - {"t":0.74343, "x":1.71062, "y":4.03179, "heading":-3.12431, "vx":-1.88369, "vy":0.02586, "omega":-0.72826, "ax":6.20197, "ay":-0.78157, "alpha":-1.7941, "fx":[102.36583,99.59874,100.844,102.75291], "fy":[-3.84271,-24.2306,-20.12601,-2.90954]}, - {"t":0.76716, "x":1.66767, "y":4.03218, "heading":3.14159, "vx":-1.73651, "vy":0.00731, "omega":-0.77084, "ax":6.18502, "ay":-0.59122, "alpha":4.07753, "fx":[100.90283,103.58629,102.80047,97.16366], "fy":[-25.2475,9.25057,13.75074,-36.41529]}, - {"t":0.80086, "x":1.61265, "y":4.03209, "heading":3.11561, "vx":-1.52805, "vy":-0.01261, "omega":-0.63341, "ax":6.04051, "ay":-0.48379, "alpha":6.12686, "fx":[99.91309,102.99619,99.64399,92.44979], "fy":[-29.72907,16.0285,29.73834,-47.67367]}, - {"t":0.83456, "x":1.56458, "y":4.03139, "heading":3.09426, "vx":-1.32447, "vy":-0.02892, "omega":-0.42691, "ax":5.94226, "ay":-0.43778, "alpha":7.17927, "fx":[99.30956,102.69554,96.95314,89.6206], "fy":[-32.00187,18.42935,38.00565,-53.0604]}, - {"t":0.86827, "x":1.52332, "y":4.03017, "heading":3.07988, "vx":-1.12419, "vy":-0.04367, "omega":-0.18495, "ax":5.87567, "ay":-0.41488, "alpha":7.81249, "fx":[98.91183,102.5345,95.00539,87.77245], "fy":[-33.37734,19.59949,42.86371,-56.21563]}, - {"t":0.90197, "x":1.48877, "y":4.02846, "heading":3.07364, "vx":-0.92616, "vy":-0.05765, "omega":0.07836, "ax":5.82852, "ay":-0.40019, "alpha":8.23301, "fx":[98.67369,102.41284,93.61649,86.43759], "fy":[-34.17843,20.40258,45.95387,-58.34726]}, - {"t":0.93567, "x":1.46086, "y":4.02629, "heading":3.07628, "vx":-0.72972, "vy":-0.07114, "omega":0.35584, "ax":5.79375, "ay":-0.38677, "alpha":8.53255, "fx":[98.58113,102.27972,92.62749,85.3789], "fy":[-34.51582,21.17605,48.00602,-59.95818]}, - {"t":0.96938, "x":1.43956, "y":4.02367, "heading":3.08828, "vx":-0.53445, "vy":-0.08418, "omega":0.64342, "ax":5.76754, "ay":-0.37097, "alpha":8.75515, "fx":[98.62881,102.10595,91.94045,84.47772], "fy":[-34.43221,22.07916,49.37143,-61.2772]}, - {"t":1.00308, "x":1.42482, "y":4.02063, "heading":3.10996, "vx":-0.34007, "vy":-0.09668, "omega":0.9385, "ax":5.74805, "ay":-0.35073, "alpha":8.92008, "fx":[98.81273,101.87138,91.50869,83.68574], "fy":[-33.94228,23.19483,50.21162,-62.3995]}, - {"t":1.03679, "x":1.41663, "y":4.01717, "heading":3.14159, "vx":-0.14634, "vy":-0.1085, "omega":1.23913, "ax":5.8589, "ay":-0.34425, "alpha":8.03187, "fx":[99.62575,101.99184,94.57672,86.9329], "fy":[-31.41485,22.56709,44.08301,-57.74632]}, - {"t":1.06489, "x":1.41483, "y":4.01398, "heading":-3.10676, "vx":0.01834, "vy":-0.11818, "omega":1.46489, "ax":5.86626, "ay":-0.3263, "alpha":7.96531, "fx":[99.98899,101.74453,95.01089,86.86407], "fy":[-30.21838,23.62349,43.09471,-57.83727]}, - {"t":1.093, "x":1.41766, "y":4.01053, "heading":-3.06559, "vx":0.18322, "vy":-0.12735, "omega":1.68877, "ax":5.87653, "ay":-0.30664, "alpha":7.86733, "fx":[100.4009,101.44489,95.58567,86.84885], "fy":[-28.79649,24.84184,41.74993,-57.8473]}, - {"t":1.12111, "x":1.42513, "y":4.00683, "heading":-3.01812, "vx":0.3484, "vy":-0.13597, "omega":1.9099, "ax":5.89044, "ay":-0.28635, "alpha":7.72889, "fx":[100.85243,101.09153,96.32622,86.91985], "fy":[-27.14457,26.20217,39.94447,-57.72711]}, - {"t":1.14921, "x":1.43725, "y":4.0029, "heading":-2.96444, "vx":0.51396, "vy":-0.14402, "omega":2.12714, "ax":5.90858, "ay":-0.26693, "alpha":7.54202, "fx":[101.33227,100.68489,97.25067,87.10842], "fy":[-25.2598,27.67858,37.55352,-57.42757]}, - {"t":1.17732, "x":1.45403, "y":3.99874, "heading":-2.90465, "vx":0.68004, "vy":-0.15152, "omega":2.33913, "ax":5.93123, "ay":-0.25032, "alpha":7.30187, "fx":[101.82683,100.22836,98.35959,87.44216], "fy":[-23.14218,29.23802,34.43689,-56.90172]}, - {"t":1.20543, "x":1.47549, "y":3.99439, "heading":-2.83891, "vx":0.84675, "vy":-0.15856, "omega":2.54436, "ax":5.95809, "ay":-0.2387, "alpha":7.00943, "fx":[102.32027,99.72965,99.62127,87.94254], "fy":[-20.79563,30.83882,30.45376,-56.10616]}, - {"t":1.23354, "x":1.50164, "y":3.98983, "heading":-2.76739, "vx":1.01422, "vy":-0.16526, "omega":2.74138, "ax":5.98809, "ay":-0.2341, "alpha":6.67456, "fx":[102.79484,99.20229,100.95518,88.62321], "fy":[-18.22906,32.42897,25.49313,-55.00162]}, - {"t":1.26164, "x":1.53251, "y":3.9851, "heading":-2.69034, "vx":1.18253, "vy":-0.17184, "omega":2.92898, "ax":6.01921, "ay":-0.23766, "alpha":6.31805, "fx":[103.23149,98.6675,102.22222,89.48917], "fy":[-15.45724,33.9436,19.5245,-53.5523]}, - {"t":1.28975, "x":1.56813, "y":3.98017, "heading":-2.60801, "vx":1.35171, "vy":-0.17852, "omega":3.10657, "ax":6.04873, "ay":-0.24865, "alpha":5.97013, "fx":[103.61068,98.15642,103.23659,90.53724], "fy":[-12.50157,35.30104,12.66396,-51.72339]}, - {"t":1.31786, "x":1.60851, "y":3.97506, "heading":-2.52069, "vx":1.52172, "vy":-0.18551, "omega":3.27437, "ax":6.07408, "ay":-0.26361, "alpha":5.66269, "fx":[103.91331,97.71293,103.81419,91.75781], "fy":[-9.39091,36.3958,5.23284,-49.47597]}, - {"t":1.34597, "x":1.65368, "y":3.96974, "heading":-2.42866, "vx":1.69245, "vy":-0.19292, "omega":3.43354, "ax":6.09414, "ay":-0.27626, "alpha":5.41449, "fx":[104.12168,97.39751,103.85329,93.13756], "fy":[-6.1636,37.08566,-2.22972,-46.75773]}, - {"t":1.37407, "x":1.70366, "y":3.96421, "heading":-2.33215, "vx":1.86374, "vy":-0.20069, "omega":3.58572, "ax":6.11058, "ay":-0.27854, "alpha":5.21386, "fx":[104.22044,97.29214,103.40989,94.66273], "fy":[-2.87253,37.16797,-9.02328,-43.48667]}, - {"t":1.40218, "x":1.75846, "y":3.95846, "heading":-2.23137, "vx":2.03549, "vy":-0.20852, "omega":3.73227, "ax":6.12827, "ay":-0.2624, "alpha":5.00559, "fx":[104.19786,97.50509,102.71696,96.32227], "fy":[0.40209,36.33585,-14.37735,-39.51946]}, - {"t":1.43029, "x":1.81809, "y":3.95249, "heading":-2.12646, "vx":2.20774, "vy":-0.21589, "omega":3.87297, "ax":6.15447, "ay":-0.22091, "alpha":4.68346, "fx":[104.048,98.16909,102.12839,98.10987], "fy":[3.52098,34.09571,-17.48234,-34.58039]}, - {"t":1.4584, "x":1.88258, "y":3.94634, "heading":-2.0176, "vx":2.38073, "vy":-0.2221, "omega":4.0046, "ax":6.1966, "ay":-0.14679, "alpha":4.07686, "fx":[103.77493,99.40838,102.00834,100.01881], "fy":[6.19838,29.60726,-17.33777,-28.06662]}, - {"t":1.4865, "x":1.95194, "y":3.94004, "heading":-1.90504, "vx":2.5549, "vy":-0.22623, "omega":4.11919, "ax":6.25585, "ay":-0.02334, "alpha":2.89554, "fx":[103.39668,101.18541,102.52882,101.97375], "fy":[7.70137,21.36916,-12.22719,-18.36947]}, - {"t":1.51461, "x":2.02622, "y":3.93367, "heading":-1.78926, "vx":2.73073, "vy":-0.22688, "omega":4.20058, "ax":6.29161, "ay":0.2164, "alpha":0.45867, "fx":[102.86031,102.7033,102.88386,102.97582], "fy":[5.27532,6.65954,1.69367,0.52262]}, - {"t":1.54272, "x":2.10546, "y":3.92738, "heading":-1.6712, "vx":2.90757, "vy":-0.2208, "omega":4.21347, "ax":5.64499, "ay":0.77739, "alpha":-7.62111, "fx":[95.00698,100.33849,96.80213,76.99165], "fy":[-28.33721,-18.82413,33.09984,64.89713]}, - {"t":1.57083, "x":2.18942, "y":3.92148, "heading":-1.55277, "vx":3.06624, "vy":-0.19895, "omega":3.99926, "ax":3.89493, "ay":0.56269, "alpha":-16.15304, "fx":[45.05433,96.26947,87.4786,25.89659], "fy":[-77.98018,-25.97592,48.65474,92.09669]}, - {"t":1.59893, "x":2.27714, "y":3.91611, "heading":-1.44036, "vx":3.17572, "vy":-0.18313, "omega":3.54524, "ax":-3.39363, "ay":0.3697, "alpha":3.71261, "fx":[-48.3058,-63.33629,-62.66874,-47.60653], "fy":[22.39792,15.4112,-7.8498,-5.78365]}, - {"t":1.62704, "x":2.36506, "y":3.91111, "heading":-1.34071, "vx":3.08033, "vy":-0.17274, "omega":3.6496, "ax":-5.39169, "ay":-0.52206, "alpha":8.58395, "fx":[-84.36102,-99.47637,-92.03051,-76.70743], "fy":[47.8863,14.06075,-39.72991,-56.35619]}, - {"t":1.65515, "x":2.44951, "y":3.90604, "heading":-1.23813, "vx":2.92878, "vy":-0.18742, "omega":3.89087, "ax":-6.21244, "ay":-0.16218, "alpha":1.58289, "fx":[-101.46,-102.19207,-101.30967,-101.28432], "fy":[8.67315,2.31117,-12.8074,-8.78226]}, - {"t":1.68325, "x":2.52938, "y":3.90071, "heading":-1.12877, "vx":2.75417, "vy":-0.19198, "omega":3.93536, "ax":-6.21026, "ay":0.1184, "alpha":-3.29932, "fx":[-101.33102,-102.38588,-99.30565,-103.08131], "fy":[-20.02028,-8.30178,27.61885,8.44572]}, - {"t":1.71136, "x":2.60434, "y":3.89536, "heading":-1.01816, "vx":2.57961, "vy":-0.18865, "omega":3.84262, "ax":-6.10619, "ay":0.20452, "alpha":-4.97787, "fx":[-99.00205,-102.6868,-94.14825,-103.46123], "fy":[-30.93881,-8.12783,43.22421,9.21663]}, - {"t":1.73947, "x":2.67443, "y":3.89014, "heading":-0.91015, "vx":2.40799, "vy":-0.1829, "omega":3.70271, "ax":-6.04464, "ay":0.26536, "alpha":-5.67323, "fx":[-96.83872,-103.19335,-91.4106,-103.83103], "fy":[-37.71738,-1.30311,49.34475,7.02835]}, - {"t":1.76758, "x":2.73972, "y":3.88511, "heading":-0.80608, "vx":2.23809, "vy":-0.17544, "omega":3.54325, "ax":-5.99863, "ay":0.34338, "alpha":-6.11545, "fx":[-94.85951,-102.90237,-90.39706,-104.10592], "fy":[-42.72785,9.75164,51.51875,3.91186]}, - {"t":1.79568, "x":2.80026, "y":3.88031, "heading":-0.70649, "vx":2.06948, "vy":-0.16579, "omega":3.37136, "ax":-5.94409, "ay":0.43133, "alpha":-6.61245, "fx":[-93.05715,-101.01309,-90.37927,-104.24859], "fy":[-46.68335,22.63986,51.76941,0.47991]}, - {"t":1.82379, "x":2.85608, "y":3.87582, "heading":-0.61173, "vx":1.90241, "vy":-0.15367, "omega":3.1855, "ax":-5.8752, "ay":0.50643, "alpha":-7.23419, "fx":[-91.44577,-97.57804,-90.91508,-104.25459], "fy":[-49.86766,35.01743,50.97587,-3.00914]}, - {"t":1.8519, "x":2.90723, "y":3.8717, "heading":-0.52219, "vx":1.73727, "vy":-0.13943, "omega":2.98217, "ax":-5.80046, "ay":0.55208, "alpha":-7.91264, "fx":[-90.04809,-93.38204,-91.74024,-104.13549], "fy":[-52.41806,45.34864,49.58895,-6.41793]}, - {"t":1.88001, "x":2.95377, "y":3.868, "heading":-0.43837, "vx":1.57424, "vy":-0.12391, "omega":2.75976, "ax":-5.73122, "ay":0.56559, "alpha":-8.55095, "fx":[-88.88467,-89.29115,-92.69109,-103.91153], "fy":[-54.41622,53.1862,47.87583,-9.66022]}, - {"t":1.90811, "x":2.99575, "y":3.86474, "heading":-0.3608, "vx":1.41315, "vy":-0.10802, "omega":2.51942, "ax":-5.67436, "ay":0.55367, "alpha":-9.08698, "fx":[-87.96646,-85.82159,-93.66414,-103.6076], "fy":[-55.92447,58.79479,46.0132,-12.67756]}, - {"t":1.93622, "x":3.03323, "y":3.86192, "heading":-0.28998, "vx":1.25365, "vy":-0.09245, "omega":2.26401, "ax":-5.63157, "ay":0.52562, "alpha":-9.50083, "fx":[-87.29034,-83.12615,-94.59474,-103.25032], "fy":[-57.00203,62.67847,44.12597,-15.43072]}, - {"t":1.96433, "x":3.06625, "y":3.85953, "heading":-0.22635, "vx":1.09537, "vy":-0.07768, "omega":1.99696, "ax":-5.60148, "ay":0.48963, "alpha":-9.7997, "fx":[-86.83768,-81.14664,-95.44424,-102.86576], "fy":[-57.71208,65.3202,42.30531,-17.8953]}, - {"t":1.99244, "x":3.09482, "y":3.85754, "heading":-0.17022, "vx":0.93792, "vy":-0.06392, "omega":1.72152, "ax":-5.58153, "ay":0.45169, "alpha":-10.00313, "fx":[-86.57571,-79.74472,-96.19157,-102.47777], "fy":[-58.12402,67.10093,40.61844,-20.05822]}, - {"t":2.02054, "x":3.11898, "y":3.85592, "heading":-0.12183, "vx":0.78104, "vy":-0.05122, "omega":1.44036, "ax":-5.56896, "ay":0.41575, "alpha":-10.13417, "fx":[-86.4608,-78.77272,-96.82701,-102.10712], "fy":[-58.3128,68.29902,39.11484,-21.91443]}, - {"t":2.04865, "x":3.13873, "y":3.85465, "heading":-0.08135, "vx":0.62451, "vy":-0.03954, "omega":1.15551, "ax":-5.56127, "ay":0.38417, "alpha":-10.21498, "fx":[-86.44314,-78.10233,-97.34785,-101.77113], "fy":[-58.35629,69.11174,37.83068,-23.46413]}, - {"t":2.07676, "x":3.15409, "y":3.85369, "heading":-0.04887, "vx":0.4682, "vy":-0.02874, "omega":0.8684, "ax":-5.55635, "ay":0.35827, "alpha":-10.2649, "fx":[-86.47188,-77.63215,-97.7552,-101.48375], "fy":[-58.33108,69.67705,36.79216,-24.71027]}, - {"t":2.10487, "x":3.16505, "y":3.85302, "heading":-0.02446, "vx":0.31202, "vy":-0.01867, "omega":0.57988, "ax":-5.55255, "ay":0.33864, "alpha":-10.29967, "fx":[-86.50013,-77.2867,-98.05176,-101.25588], "fy":[-58.30713,70.0902,36.01797,-25.65661]}, - {"t":2.13297, "x":3.17163, "y":3.85263, "heading":-0.00816, "vx":0.15596, "vy":-0.00915, "omega":0.29038, "ax":-5.54863, "ay":0.32555, "alpha":-10.33113, "fx":[-86.48925,-77.01281,-98.24027,-101.09568], "fy":[-58.34205,70.41533,35.52128,-26.30626]}, - {"t":2.16108, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":2.72474, "y":4.08493, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.35666, "ay":-0.64557, "alpha":0.0, "fx":[-103.91934,-103.91934,-103.91934,-103.91934], "fy":[-10.55377,-10.55377,-10.55377,-10.55377]}, + {"t":0.03526, "x":2.72079, "y":4.08453, "heading":3.14159, "vx":-0.22414, "vy":-0.02276, "omega":0.0, "ax":-6.35687, "ay":-0.63724, "alpha":0.0, "fx":[-103.92273,-103.92273,-103.92273,-103.92273], "fy":[-10.41766,-10.41766,-10.41766,-10.41766]}, + {"t":0.07052, "x":2.70893, "y":4.08333, "heading":3.14159, "vx":-0.44828, "vy":-0.04523, "omega":0.0, "ax":-6.3571, "ay":-0.62739, "alpha":0.0, "fx":[-103.92652,-103.92652,-103.92652,-103.92652], "fy":[-10.25669,-10.25669,-10.25669,-10.25669]}, + {"t":0.10578, "x":2.68917, "y":4.08135, "heading":3.14159, "vx":-0.67243, "vy":-0.06735, "omega":0.0, "ax":-6.35736, "ay":-0.61557, "alpha":0.0, "fx":[-103.93074,-103.93074,-103.93074,-103.93074], "fy":[-10.06337,-10.06337,-10.06337,-10.06337]}, + {"t":0.14104, "x":2.66151, "y":4.07859, "heading":3.14159, "vx":-0.89659, "vy":-0.08906, "omega":0.0, "ax":-6.35765, "ay":-0.6011, "alpha":0.0, "fx":[-103.93543,-103.93543,-103.93543,-103.93543], "fy":[-9.82684,-9.82684,-9.82684,-9.82684]}, + {"t":0.1763, "x":2.62595, "y":4.07507, "heading":3.14159, "vx":-1.12076, "vy":-0.11025, "omega":0.0, "ax":-6.35796, "ay":-0.58299, "alpha":0.0, "fx":[-103.94054,-103.94054,-103.94054,-103.94054], "fy":[-9.53083,-9.53083,-9.53083,-9.53083]}, + {"t":0.21156, "x":2.58248, "y":4.07082, "heading":3.14159, "vx":-1.34495, "vy":-0.13081, "omega":0.0, "ax":-6.35829, "ay":-0.55968, "alpha":0.0, "fx":[-103.9459,-103.9459,-103.9459,-103.9459], "fy":[-9.14969,-9.14969,-9.14969,-9.14969]}, + {"t":0.24682, "x":2.5311, "y":4.06586, "heading":3.14159, "vx":-1.56914, "vy":-0.15054, "omega":0.0, "ax":-6.35859, "ay":-0.52854, "alpha":0.0, "fx":[-103.9509,-103.9509,-103.9509,-103.9509], "fy":[-8.64056,-8.64056,-8.64056,-8.64056]}, + {"t":0.28208, "x":2.47182, "y":4.06023, "heading":3.14159, "vx":-1.79334, "vy":-0.16918, "omega":0.0, "ax":-6.35877, "ay":-0.48483, "alpha":0.0, "fx":[-103.95374,-103.95374,-103.95374,-103.95374], "fy":[-7.92605,-7.92605,-7.92605,-7.92605]}, + {"t":0.31734, "x":2.40463, "y":4.05396, "heading":3.14159, "vx":-2.01755, "vy":-0.18628, "omega":0.0, "ax":-6.35847, "ay":-0.41906, "alpha":0.0, "fx":[-103.94887,-103.94887,-103.94887,-103.94887], "fy":[-6.8508,-6.8508,-6.8508,-6.8508]}, + {"t":0.3526, "x":2.32954, "y":4.04713, "heading":3.14159, "vx":-2.24175, "vy":-0.20105, "omega":0.0, "ax":-6.35646, "ay":-0.30892, "alpha":0.0, "fx":[-103.91603,-103.91603,-103.91603,-103.91603], "fy":[-5.05031,-5.05031,-5.05031,-5.05031]}, + {"t":0.38786, "x":2.24655, "y":4.03985, "heading":3.14159, "vx":-2.46588, "vy":-0.21194, "omega":0.0, "ax":-6.34667, "ay":-0.0871, "alpha":0.0, "fx":[-103.75593,-103.75593,-103.75593,-103.75593], "fy":[-1.42394,-1.42394,-1.42394,-1.42394]}, + {"t":0.42312, "x":2.15565, "y":4.03232, "heading":3.14159, "vx":-2.68967, "vy":-0.21502, "omega":0.0, "ax":-6.26988, "ay":0.58335, "alpha":0.0, "fx":[-102.50058,-102.50058,-102.50058,-102.50058], "fy":[9.53666,9.53666,9.53666,9.53666]}, + {"t":0.45838, "x":2.05692, "y":4.0251, "heading":3.14159, "vx":-2.91074, "vy":-0.19445, "omega":0.0, "ax":0.03195, "ay":5.56699, "alpha":0.0, "fx":[0.52234,0.52234,0.52234,0.52234], "fy":[91.00975,91.00975,91.00975,91.00975]}, + {"t":0.49124, "x":1.96128, "y":4.02172, "heading":3.14159, "vx":-2.90969, "vy":-0.0115, "omega":0.0, "ax":6.27511, "ay":0.37619, "alpha":0.0, "fx":[102.58603,102.58603,102.58603,102.58603], "fy":[6.15005,6.15005,6.15005,6.15005]}, + {"t":0.52411, "x":1.86904, "y":4.02155, "heading":3.14159, "vx":-2.70347, "vy":0.00087, "omega":0.0, "ax":6.34125, "ay":-0.01693, "alpha":0.0, "fx":[103.66734,103.66734,103.66734,103.66734], "fy":[-0.27684,-0.27684,-0.27684,-0.27684]}, + {"t":0.55697, "x":1.78362, "y":4.02156, "heading":3.14159, "vx":-2.49508, "vy":0.00031, "omega":0.0, "ax":6.358, "ay":-0.14972, "alpha":0.0, "fx":[103.94119,103.94119,103.94119,103.94119], "fy":[-2.44766,-2.44766,-2.44766,-2.44766]}, + {"t":0.58983, "x":1.70506, "y":4.02149, "heading":3.14159, "vx":-2.28613, "vy":-0.00461, "omega":0.0, "ax":6.36536, "ay":-0.21634, "alpha":0.0, "fx":[104.06144,104.06144,104.06144,104.06144], "fy":[-3.53668,-3.53668,-3.53668,-3.53668]}, + {"t":0.6227, "x":1.63337, "y":4.02123, "heading":3.14159, "vx":-2.07694, "vy":-0.01172, "omega":0.0, "ax":6.36944, "ay":-0.25636, "alpha":0.0, "fx":[104.12822,104.12822,104.12822,104.12822], "fy":[-4.19096,-4.19096,-4.19096,-4.19096]}, + {"t":0.65556, "x":1.56855, "y":4.0207, "heading":3.14159, "vx":-1.86762, "vy":-0.02014, "omega":0.0, "ax":6.37203, "ay":-0.28306, "alpha":0.0, "fx":[104.17049,104.17049,104.17049,104.17049], "fy":[-4.62743,-4.62743,-4.62743,-4.62743]}, + {"t":0.68842, "x":1.51062, "y":4.01989, "heading":3.14159, "vx":-1.65821, "vy":-0.02945, "omega":0.0, "ax":6.37381, "ay":-0.30213, "alpha":0.0, "fx":[104.19958,104.19958,104.19958,104.19958], "fy":[-4.93928,-4.93928,-4.93928,-4.93928]}, + {"t":0.72129, "x":1.45956, "y":4.01876, "heading":3.14159, "vx":-1.44875, "vy":-0.03938, "omega":0.0, "ax":6.3751, "ay":-0.31644, "alpha":0.0, "fx":[104.22079,104.22079,104.22079,104.22079], "fy":[-5.17321,-5.17321,-5.17321,-5.17321]}, + {"t":0.75415, "x":1.41539, "y":4.01729, "heading":3.14159, "vx":-1.23924, "vy":-0.04977, "omega":0.0, "ax":6.37609, "ay":-0.32757, "alpha":0.0, "fx":[104.23694,104.23694,104.23694,104.23694], "fy":[-5.35516,-5.35516,-5.35516,-5.35516]}, + {"t":0.78702, "x":1.37811, "y":4.01548, "heading":3.14159, "vx":-1.0297, "vy":-0.06054, "omega":0.0, "ax":6.37687, "ay":-0.33648, "alpha":0.0, "fx":[104.24963,104.24963,104.24963,104.24963], "fy":[-5.50073,-5.50073,-5.50073,-5.50073]}, + {"t":0.81988, "x":1.34772, "y":4.01331, "heading":3.14159, "vx":-0.82013, "vy":-0.0716, "omega":0.0, "ax":6.37749, "ay":-0.34376, "alpha":0.0, "fx":[104.25986,104.25986,104.25986,104.25986], "fy":[-5.61982,-5.61982,-5.61982,-5.61982]}, + {"t":0.85274, "x":1.32421, "y":4.01077, "heading":3.14159, "vx":-0.61055, "vy":-0.08289, "omega":0.0, "ax":6.37801, "ay":-0.34983, "alpha":0.0, "fx":[104.26829,104.26829,104.26829,104.26829], "fy":[-5.71906,-5.71906,-5.71906,-5.71906]}, + {"t":0.88561, "x":1.30759, "y":4.00786, "heading":3.14159, "vx":-0.40094, "vy":-0.09439, "omega":0.0, "ax":6.37844, "ay":-0.35497, "alpha":0.0, "fx":[104.27534,104.27534,104.27534,104.27534], "fy":[-5.80303,-5.80303,-5.80303,-5.80303]}, + {"t":0.91847, "x":1.29785, "y":4.00456, "heading":3.14159, "vx":-0.19133, "vy":-0.10606, "omega":0.0, "ax":5.53486, "ay":-0.25983, "alpha":10.45437, "fx":[98.56364,100.76743,85.26448,77.34183], "fy":[-34.6479,27.58124,60.16118,-70.08566]}, + {"t":0.94814, "x":1.29461, "y":4.0013, "heading":3.14159, "vx":-0.0271, "vy":-0.11377, "omega":0.31019, "ax":5.53904, "ay":-0.26103, "alpha":10.42252, "fx":[98.57077,100.77826,85.39593,77.46626], "fy":[-34.61017,27.51917,59.95247,-69.93099]}, + {"t":0.97781, "x":1.29625, "y":3.99781, "heading":-3.13239, "vx":0.13724, "vy":-0.12151, "omega":0.61943, "ax":5.54572, "ay":-0.25446, "alpha":10.37309, "fx":[98.69782,100.69998,85.64465,77.60563], "fy":[-34.22694,27.7784,59.56823,-69.75969]}, + {"t":1.00748, "x":1.30276, "y":3.99409, "heading":-3.11401, "vx":0.30179, "vy":-0.12906, "omega":0.9272, "ax":5.55561, "ay":-0.24065, "alpha":10.29948, "fx":[98.94084,100.5327,86.04001,77.78109], "fy":[-33.49623,28.34822,58.95942,-69.54783]}, + {"t":1.03715, "x":1.31416, "y":3.99016, "heading":-3.0865, "vx":0.46662, "vy":-0.1362, "omega":1.23279, "ax":5.56992, "ay":-0.22052, "alpha":10.19007, "fx":[99.29233,100.27544,86.63379,78.02879], "fy":[-32.41444,29.21135,58.03694,-69.25386]}, + {"t":1.06682, "x":1.33046, "y":3.98602, "heading":-3.04992, "vx":0.63189, "vy":-0.14274, "omega":1.53514, "ax":5.59032, "ay":-0.19568, "alpha":10.02858, "fx":[99.74074,99.92727,87.50062,78.39544], "fy":[-30.97751,30.34222,56.6618,-68.82239]}, + {"t":1.09649, "x":1.35167, "y":3.9817, "heading":-3.00437, "vx":0.79775, "vy":-0.14855, "omega":1.83269, "ax":5.6188, "ay":-0.16877, "alpha":9.79497, "fx":[100.27,99.48918,88.73488,78.93253], "fy":[-29.18258,31.70448,54.63069,-68.18873]}, + {"t":1.12616, "x":1.37781, "y":3.97722, "heading":-2.95, "vx":0.96447, "vy":-0.15356, "omega":2.12331, "ax":5.65744, "ay":-0.14386, "alpha":9.46763, "fx":[100.85907,98.96671,90.43791,79.68963], "fy":[-27.03041,33.24746,51.65909,-67.28355]}, + {"t":1.15583, "x":1.40892, "y":3.9726, "heading":-2.887, "vx":1.13233, "vy":-0.15783, "omega":2.40422, "ax":5.70781, "ay":-0.12682, "alpha":9.02809, "fx":[101.48175,98.37373,92.68399,80.70785], "fy":[-24.52846,34.90141,47.37064,-66.03667]}, + {"t":1.1855, "x":1.44503, "y":3.96786, "heading":-2.81566, "vx":1.30168, "vy":-0.16159, "omega":2.67209, "ax":5.76991, "ay":-0.12515, "alpha":8.47034, "fx":[102.10698,97.73767,95.44925,82.01453], "fy":[-21.69472,36.57061,41.31954,-64.3793]}, + {"t":1.21517, "x":1.48619, "y":3.96301, "heading":-2.73638, "vx":1.47288, "vy":-0.1653, "omega":2.92341, "ax":5.84056, "ay":-0.14648, "alpha":7.81533, "fx":[102.69992,97.10664,98.5009,83.6204], "fy":[-18.56218,38.12269,33.10441,-62.24343]}, + {"t":1.24484, "x":1.53246, "y":3.95804, "heading":-2.64964, "vx":1.64617, "vy":-0.16965, "omega":3.15529, "ax":5.91221, "ay":-0.19455, "alpha":7.12395, "fx":[103.22395,96.55981,101.30853,85.52098], "fy":[-15.18434,39.36942,22.6486,-59.55582]}, + {"t":1.27452, "x":1.5839, "y":3.95292, "heading":-2.55602, "vx":1.82159, "vy":-0.17542, "omega":3.36666, "ax":5.97507, "ay":-0.2634, "alpha":6.48539, "fx":[103.64339,96.22299,103.15459,87.70332], "fy":[-11.6436,40.02764,10.61269,-56.22086]}, + {"t":1.30419, "x":1.64058, "y":3.9476, "heading":-2.45613, "vx":1.99887, "vy":-0.18324, "omega":3.55909, "ax":6.02456, "ay":-0.33516, "alpha":5.95469, "fx":[103.92656,96.29286,103.58196,90.15867], "fy":[-8.06941,39.63356,-1.39992,-52.0813]}, + {"t":1.33386, "x":1.70254, "y":3.94202, "heading":-2.35053, "vx":2.17762, "vy":-0.19318, "omega":3.73577, "ax":6.06935, "ay":-0.38868, "alpha":5.45895, "fx":[104.04858,97.06698,102.87299,92.90062], "fy":[-4.68841,37.34341,-11.24467,-46.82728]}, + {"t":1.36353, "x":1.76982, "y":3.93611, "heading":-2.23969, "vx":2.3577, "vy":-0.20471, "omega":3.89774, "ax":6.13039, "ay":-0.41588, "alpha":4.72817, "fx":[103.99139,98.92519,101.97575,95.98847], "fy":[-1.98435,31.44225,-16.90518,-39.74804]}, + {"t":1.3932, "x":1.84248, "y":3.92986, "heading":-2.12404, "vx":2.53959, "vy":-0.21705, "omega":4.03802, "ax":6.22377, "ay":-0.43607, "alpha":3.21049, "fx":[103.7121,101.86896,101.88151,99.52473], "fy":[-1.33304,18.12725,-16.48305,-28.82677]}, + {"t":1.42287, "x":1.92057, "y":3.92322, "heading":-2.00423, "vx":2.72426, "vy":-0.22999, "omega":4.13328, "ax":6.27522, "ay":-0.49067, "alpha":-0.31652, "fx":[102.48064,102.42355,102.71001,102.73728], "fy":[-9.04463,-10.22737,-7.04386,-5.77013]}, + {"t":1.45254, "x":2.00416, "y":3.91618, "heading":-1.8816, "vx":2.91044, "vy":-0.24455, "omega":4.12389, "ax":5.74348, "ay":-0.45566, "alpha":-7.18806, "fx":[88.40834,94.53892,101.80008,90.83255], "fy":[-45.81763,-38.79245,10.92707,43.88636]}, + {"t":1.48221, "x":2.09304, "y":3.90873, "heading":-1.75924, "vx":3.08086, "vy":-0.25807, "omega":3.91062, "ax":5.49808, "ay":0.79308, "alpha":-7.2306, "fx":[91.46888,97.65758,95.69116,74.7147], "fy":[-20.80471,-20.1839,30.00567,62.84451]}, + {"t":1.51188, "x":2.18687, "y":3.90142, "heading":-1.64321, "vx":3.24399, "vy":-0.23454, "omega":3.69608, "ax":-0.57467, "ay":4.66236, "alpha":-2.24527, "fx":[-20.89985,-0.72755,0.64529,-16.59663], "fy":[72.52684,74.01695,79.63933,78.69974]}, + {"t":1.54155, "x":2.28287, "y":3.89651, "heading":-1.53354, "vx":3.22694, "vy":-0.0962, "omega":3.62946, "ax":-5.51541, "ay":-0.03196, "alpha":7.62199, "fx":[-83.69796,-97.05373,-96.28227,-83.63198], "fy":[46.89703,23.81487,-26.55893,-46.24301]}, + {"t":1.57122, "x":2.37619, "y":3.89365, "heading":-1.42586, "vx":3.06329, "vy":-0.09715, "omega":3.85561, "ax":-5.65838, "ay":-0.58898, "alpha":7.79747, "fx":[-91.41781,-101.20491,-95.23783,-82.15442], "fy":[40.95074,14.43955,-37.05541,-56.8499]}, + {"t":1.60089, "x":2.46458, "y":3.8905, "heading":-1.31146, "vx":2.8954, "vy":-0.11463, "omega":4.08697, "ax":-6.27301, "ay":-0.33611, "alpha":0.80135, "fx":[-102.73462,-102.85545,-102.31523,-102.3016], "fy":[-0.1291,-2.30064,-10.51464,-9.0349]}, + {"t":1.63056, "x":2.54773, "y":3.88695, "heading":-1.19019, "vx":2.70928, "vy":-0.1246, "omega":4.11074, "ax":-6.22586, "ay":-0.08489, "alpha":-3.38016, "fx":[-101.13889,-102.10264,-100.4424,-103.43994], "fy":[-22.43124,-14.66443,24.47136,7.07306]}, + {"t":1.66023, "x":2.62538, "y":3.88322, "heading":-1.06823, "vx":2.52456, "vy":-0.12712, "omega":4.01045, "ax":-6.10597, "ay":0.00207, "alpha":-5.15664, "fx":[-98.51345,-101.95846,-95.15345,-103.65832], "fy":[-33.00755,-16.61744,41.34721,8.4134]}, + {"t":1.6899, "x":2.69759, "y":3.87945, "heading":-0.94923, "vx":2.34339, "vy":-0.12706, "omega":3.85745, "ax":-6.03406, "ay":0.05703, "alpha":-5.89746, "fx":[-95.95104,-102.81079,-91.85797,-103.96194], "fy":[-40.22986,-11.03754,48.70988,6.28695]}, + {"t":1.71957, "x":2.76447, "y":3.8757, "heading":-0.83478, "vx":2.16436, "vy":-0.12536, "omega":3.68247, "ax":-5.98895, "ay":0.14044, "alpha":-6.29036, "fx":[-93.49159,-103.48586,-90.45605,-104.19848], "fy":[-45.85,0.52248,51.55291,2.95849]}, + {"t":1.74924, "x":2.82605, "y":3.87205, "heading":-0.72552, "vx":1.98666, "vy":-0.1212, "omega":3.49583, "ax":-5.93474, "ay":0.2553, "alpha":-6.75327, "fx":[-91.16266,-102.36619,-90.26318,-104.2946], "fy":[-50.43504,15.87089,52.07775,-0.81881]}, + {"t":1.77891, "x":2.88238, "y":3.86856, "heading":-0.6218, "vx":1.81057, "vy":-0.11362, "omega":3.29546, "ax":-5.85341, "ay":0.36802, "alpha":-7.4468, "fx":[-89.00788,-98.75583,-90.77386,-104.23112], "fy":[-54.2186,31.68267,51.31496,-4.71333]}, + {"t":1.80858, "x":2.93353, "y":3.86535, "heading":-0.52402, "vx":1.6369, "vy":-0.1027, "omega":3.07451, "ax":-5.75485, "ay":0.44331, "alpha":-8.28597, "fx":[-87.07618,-93.56962,-91.6612,-104.01639], "fy":[-57.31694,45.03882,49.81135,-8.5442]}, + {"t":1.83825, "x":2.97956, "y":3.8625, "heading":-0.4328, "vx":1.46615, "vy":-0.08955, "omega":2.82866, "ax":-5.65994, "ay":0.47031, "alpha":-9.10039, "fx":[-85.41119,-88.31751,-92.71421,-103.67413], "fy":[-59.80255,54.85151,47.89952,-12.19409]}, + {"t":1.86793, "x":3.02057, "y":3.86005, "heading":-0.34887, "vx":1.29822, "vy":-0.0756, "omega":2.55865, "ax":-5.58208, "ay":0.45854, "alpha":-9.77879, "fx":[-84.04212,-83.94981,-93.79677,-103.23703], "fy":[-61.73541,61.49604,45.8049,-15.58042]}, + {"t":1.8976, "x":3.05663, "y":3.85801, "heading":-0.27295, "vx":1.13259, "vy":-0.06199, "omega":2.26851, "ax":-5.52439, "ay":0.42328, "alpha":-10.29019, "fx":[-82.97695,-80.71051,-94.82394,-102.7417], "fy":[-63.17778,65.81395,43.68944,-18.64651]}, + {"t":1.92727, "x":3.08781, "y":3.85636, "heading":-0.20565, "vx":0.96868, "vy":-0.04943, "omega":1.96319, "ax":-5.48438, "ay":0.37743, "alpha":-10.6496, "fx":[-82.1998,-78.46611,-95.74636,-102.22448], "fy":[-64.20018,68.56676,41.67161,-21.35684]}, + {"t":1.95694, "x":3.11413, "y":3.85506, "heading":-0.1474, "vx":0.80596, "vy":-0.03823, "omega":1.64721, "ax":-5.45786, "ay":0.32989, "alpha":-10.88863, "fx":[-81.67263,-76.97281,-96.53864,-101.71861], "fy":[-64.88226,70.30878,39.838,-23.69231]}, + {"t":1.98661, "x":3.13564, "y":3.85407, "heading":-0.09852, "vx":0.64402, "vy":-0.02844, "omega":1.32414, "ax":-5.44071, "ay":0.28613, "alpha":-11.04097, "fx":[-81.3404,-75.99774,-97.19053,-101.25262], "fy":[-65.31046,71.41502,38.25133,-25.6452]}, + {"t":2.01628, "x":3.15236, "y":3.85335, "heading":-0.05924, "vx":0.48259, "vy":-0.01995, "omega":0.99655, "ax":-5.42945, "ay":0.2493, "alpha":-11.13668, "fx":[-81.13875,-75.35546,-97.70052,-100.84983], "fy":[-65.57334,72.13377,36.95641,-27.21477]}, + {"t":2.04595, "x":3.16429, "y":3.85287, "heading":-0.02967, "vx":0.3215, "vy":-0.01256, "omega":0.66612, "ax":-5.42132, "ay":0.22105, "alpha":-11.20028, "fx":[-81.00279,-74.91036,-98.07119,-100.52851], "fy":[-65.75469,72.6282,35.98484,-28.40347]}, + {"t":2.07562, "x":3.17144, "y":3.85259, "heading":-0.0099, "vx":0.16064, "vy":-0.006, "omega":0.3338, "ax":-5.41429, "ay":0.20221, "alpha":-11.25026, "fx":[-80.87597,-74.56908,-98.30618,-100.30231], "fy":[-65.92544,73.00386,35.35843,-29.21416]}, + {"t":2.10529, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/BtoB.traj b/src/main/deploy/choreo/BtoB.traj index cc888d85..de609a5d 100644 --- a/src/main/deploy/choreo/BtoB.traj +++ b/src/main/deploy/choreo/BtoB.traj @@ -3,28 +3,32 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.178781509399414, "y":3.3376660346984863, "heading":-1.377584993861348, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.299587607383728, "y":2.5006520748138428, "heading":-1.2021004738085692, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.9659841656684875, "y":2.8667194843292236, "heading":-1.377584993861348, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.2125167846679688, "y":2.193495988845825, "heading":-1.2021004738085692, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.178781509399414 m", "val":1.178781509399414}, "y":{"exp":"3.3376660346984863 m", "val":3.3376660346984863}, "heading":{"exp":"-1.377584993861348 rad", "val":-1.377584993861348}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.299587607383728 m", "val":1.299587607383728}, "y":{"exp":"2.5006520748138428 m", "val":2.5006520748138428}, "heading":{"exp":"-1.2021004738085692 rad", "val":-1.2021004738085692}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.9659841656684875 m", "val":0.9659841656684875}, "y":{"exp":"2.8667194843292236 m", "val":2.8667194843292236}, "heading":{"exp":"-1.377584993861348 rad", "val":-1.377584993861348}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.2125167846679688 m", "val":1.2125167846679688}, "y":{"exp":"2.193495988845825 m", "val":2.193495988845825}, "heading":{"exp":"-1.2021004738085692 rad", "val":-1.2021004738085692}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,111 +36,96 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.10213,1.62735,2.95102], + "waypoints":[0.0,1.22863,1.63136,2.94312], "samples":[ - {"t":0.0, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.30779, "ay":0.91263, "alpha":-1.31429, "fx":[-104.08674,-101.79162,-102.41701,-104.18618], "fy":[8.44915,23.33635,20.50695,7.38645]}, - {"t":0.03149, "x":3.17069, "y":3.85296, "heading":0.0, "vx":-0.19863, "vy":0.02874, "omega":-0.04139, "ax":-6.31086, "ay":0.88074, "alpha":-1.35419, "fx":[-104.13719,-101.84541,-102.47352,-104.22594], "fy":[7.67273,23.05777,20.18257,6.68049]}, - {"t":0.06298, "x":3.16131, "y":3.8543, "heading":-0.0013, "vx":-0.39735, "vy":0.05647, "omega":-0.08403, "ax":-6.31411, "ay":0.84453, "alpha":-1.39955, "fx":[-104.18838,-101.90744,-102.53413,-104.26466], "fy":[6.77623,22.73267,19.82616,5.89109]}, - {"t":0.09447, "x":3.14567, "y":3.85649, "heading":-0.00395, "vx":-0.59618, "vy":0.08307, "omega":-0.1281, "ax":-6.31753, "ay":0.80309, "alpha":-1.45156, "fx":[-104.23836,-101.97915,-102.59974,-104.30108], "fy":[5.73399,22.35089,19.43013,5.00064]}, - {"t":0.12596, "x":3.12376, "y":3.85951, "heading":-0.00798, "vx":-0.79512, "vy":0.10835, "omega":-0.17381, "ax":-6.32109, "ay":0.75517, "alpha":-1.51178, "fx":[-104.28383,-102.06239,-102.67141,-104.33323], "fy":[4.51228,21.89888,18.98457,3.9865]}, - {"t":0.15745, "x":3.09559, "y":3.86329, "heading":-0.01346, "vx":-0.99416, "vy":0.13213, "omega":-0.22141, "ax":-6.3247, "ay":0.69916, "alpha":-1.58226, "fx":[-104.31933,-102.1595,-102.75049,-104.35797], "fy":[3.06595,21.35822,18.47629,2.81896]}, - {"t":0.18894, "x":3.06115, "y":3.8678, "heading":-0.02043, "vx":-1.19333, "vy":0.15415, "omega":-0.27124, "ax":-6.32823, "ay":0.63283, "alpha":-1.66582, "fx":[-104.33567,-102.27348,-102.83865,-104.37026], "fy":[1.33327,20.70312,17.88744,1.45834]}, - {"t":0.22043, "x":3.02043, "y":3.87297, "heading":-0.02897, "vx":-1.3926, "vy":0.17408, "omega":-0.32369, "ax":-6.3314, "ay":0.55309, "alpha":-1.76639, "fx":[-104.31701,-102.40819,-102.93792,-104.36188], "fy":[-0.77235,19.89656,17.19314,-0.14955]}, - {"t":0.25192, "x":2.97344, "y":3.87873, "heading":-0.03916, "vx":-1.59197, "vy":0.19149, "omega":-0.37932, "ax":-6.33367, "ay":0.45548, "alpha":-1.8896, "fx":[-104.23543,-102.5685,-103.05082,-104.31904], "fy":[-3.37572,18.88335,16.35782,-2.08058]}, - {"t":0.2834, "x":2.92017, "y":3.88498, "heading":-0.05111, "vx":-1.79141, "vy":0.20584, "omega":-0.43882, "ax":-6.33404, "ay":0.33337, "alpha":-2.04381, "fx":[-104.03968,-102.7604,-103.18026,-104.21791], "fy":[-6.66274,17.57764,15.32869,-4.44401]}, - {"t":0.31489, "x":2.86062, "y":3.89163, "heading":-0.06493, "vx":-1.99087, "vy":0.21633, "omega":-0.50318, "ax":-6.3305, "ay":0.17647, "alpha":-2.24187, "fx":[-103.63123,-102.99029,-103.32925,-104.01557], "fy":[-10.92026,15.83838,14.02414,-7.40253]}, - {"t":0.34638, "x":2.79479, "y":3.89853, "heading":-0.08077, "vx":-2.19021, "vy":0.22189, "omega":-0.57377, "ax":-6.31879, "ay":-0.03193, "alpha":-2.50421, "fx":[-102.80902,-103.26148,-103.49958,-103.63079], "fy":[-16.60833,13.41716,12.31095,-11.20766]}, - {"t":0.37787, "x":2.72269, "y":3.9055, "heading":-0.09884, "vx":-2.38919, "vy":0.22089, "omega":-0.65263, "ax":-6.28939, "ay":-0.32061, "alpha":-2.86444, "fx":[-101.13244,-103.55896,-103.68716,-102.89991], "fy":[-24.49131,9.83436,9.9565,-16.26515]}, - {"t":0.40936, "x":2.64434, "y":3.9123, "heading":-0.11939, "vx":-2.58724, "vy":0.21079, "omega":-0.74283, "ax":-6.21886, "ay":-0.74256, "alpha":-3.37709, "fx":[-97.55425,-103.77959,-103.86525,-101.46668], "fy":[-35.85392,4.03921,6.51637,-23.25951]}, - {"t":0.44085, "x":2.55978, "y":3.91857, "heading":-0.14278, "vx":-2.78307, "vy":0.18741, "omega":-0.84917, "ax":-6.04262, "ay":-1.40283, "alpha":-4.11391, "fx":[-89.40855,-103.33727,-103.9148,-98.48042], "fy":[-52.65486,-6.72413,1.03045,-33.38588]}, - {"t":0.47234, "x":2.46915, "y":3.92377, "heading":-0.16952, "vx":-2.97334, "vy":0.14323, "omega":-0.97872, "ax":-5.55178, "ay":-2.52948, "alpha":-5.06122, "fx":[-70.17272,-97.90626,-103.2632,-91.70212], "fy":[-76.13118,-31.63585,-8.96747,-48.67423]}, - {"t":0.50383, "x":2.37277, "y":3.92703, "heading":-0.20034, "vx":-3.14817, "vy":0.06358, "omega":-1.13809, "ax":-3.80488, "ay":-4.47666, "alpha":-6.84919, "fx":[-30.80241,-44.82054,-98.15321,-75.03383], "fy":[-98.67879,-91.19504,-31.45282,-71.41274]}, - {"t":0.53532, "x":2.27175, "y":3.92681, "heading":-0.23618, "vx":-3.26798, "vy":-0.07739, "omega":-1.35377, "ax":-0.23156, "ay":-5.6989, "alpha":-8.40403, "fx":[18.15155,55.16845,-51.66103,-36.80147], "fy":[-101.852,-86.47431,-87.74703,-96.59152]}, - {"t":0.56681, "x":2.16873, "y":3.92155, "heading":-0.27881, "vx":-3.27527, "vy":-0.25684, "omega":-1.61841, "ax":3.14052, "ay":-5.23571, "alpha":-4.84015, "fx":[52.07439,81.18042,54.83517,17.27581], "fy":[-89.65312,-64.08424,-86.68113,-101.95735]}, - {"t":0.5983, "x":2.06715, "y":3.91086, "heading":-0.32977, "vx":-3.17638, "vy":-0.42171, "omega":-1.77082, "ax":4.55278, "ay":-4.26874, "alpha":-3.42102, "fx":[70.21324,88.97405,83.18674,55.34284], "fy":[-76.54952,-53.44948,-61.49854,-87.64546]}, - {"t":0.62979, "x":1.96938, "y":3.89547, "heading":-0.38553, "vx":-3.03301, "vy":-0.55613, "omega":-1.87854, "ax":5.16651, "ay":-3.61733, "alpha":-2.40513, "fx":[80.03142,92.4196,91.02372,74.37568], "fy":[-66.43437,-47.66467,-49.93615,-72.51056]}, - {"t":0.66128, "x":1.87644, "y":3.87616, "heading":-0.44468, "vx":-2.87032, "vy":-0.67004, "omega":-1.95428, "ax":5.48, "ay":-3.1864, "alpha":-1.74616, "fx":[85.80171,94.32059,94.30314,83.92491], "fy":[-58.96583,-44.06039,-43.88081,-61.45943]}, - {"t":0.69277, "x":1.78877, "y":3.85348, "heading":-0.50622, "vx":-2.69776, "vy":-0.77038, "omega":-2.00927, "ax":5.66154, "ay":-2.88929, "alpha":-1.30788, "fx":[89.4803,95.52339,96.02598,89.19221], "fy":[-53.35687,-41.58482,-40.26764,-53.72824]}, - {"t":0.72426, "x":1.70663, "y":3.82779, "heading":-0.56949, "vx":-2.51948, "vy":-0.86136, "omega":-2.05045, "ax":5.77711, "ay":-2.67475, "alpha":-1.00039, "fx":[91.98667,96.35904,97.05944,92.37358], "fy":[-49.02278,-39.75701,-37.91817,-48.21016]}, - {"t":0.75575, "x":1.63015, "y":3.79934, "heading":-0.63406, "vx":-2.33757, "vy":-0.94559, "omega":-2.08195, "ax":5.85602, "ay":-2.5135, "alpha":-0.77403, "fx":[93.78603,96.98084,97.73433,94.43792], "fy":[-45.58023,-38.32947,-36.29855,-44.15578]}, - {"t":0.78724, "x":1.55945, "y":3.76832, "heading":-0.69962, "vx":-2.15316, "vy":-1.02473, "omega":-2.10633, "ax":5.91284, "ay":-2.38828, "alpha":-0.60084, "fx":[95.13182,97.46865,98.20174,95.85267], "fy":[-42.78022,-37.16309,-35.13408,-41.09784]}, - {"t":0.81872, "x":1.49458, "y":3.73487, "heading":-0.76595, "vx":-1.96697, "vy":-1.09994, "omega":-2.12525, "ax":5.95547, "ay":-2.28839, "alpha":-0.46424, "fx":[96.17135,97.86774,98.53984,96.86336], "fy":[-40.45781,-36.17424,-34.26931,-38.74196]}, - {"t":0.85021, "x":1.43559, "y":3.6991, "heading":-0.83287, "vx":-1.77944, "vy":-1.172, "omega":-2.13986, "ax":5.9885, "ay":-2.20695, "alpha":-0.35388, "fx":[96.99507,98.20548,98.79297,97.60865], "fy":[-38.50087,-35.31008,-33.60966,-36.89683]}, - {"t":0.8817, "x":1.38253, "y":3.6611, "heading":-0.90025, "vx":-1.59086, "vy":-1.24149, "omega":-2.15101, "ax":6.01477, "ay":-2.13932, "alpha":-0.26297, "fx":[97.66113,98.49923,98.98812,98.17168], "fy":[-36.83111,-34.53591,-33.0942,-35.43378]}, - {"t":0.91319, "x":1.33541, "y":3.62094, "heading":-0.96799, "vx":-1.40146, "vy":-1.30886, "omega":-2.15929, "ax":6.03612, "ay":-2.08229, "alpha":-0.1869, "fx":[98.20844,98.76039,99.14267,98.6049], "fy":[-35.39243,-33.82828,-32.68185,-34.26338]}, - {"t":0.94468, "x":1.29427, "y":3.5787, "heading":-1.03598, "vx":-1.21139, "vy":-1.37443, "omega":-2.16517, "ax":6.05379, "ay":-2.03357, "alpha":-0.12242, "fx":[98.66393,98.99668,99.2683,98.94282], "fy":[-34.14369,-33.17106,-32.34386,-33.32161]}, - {"t":0.97617, "x":1.25913, "y":3.53441, "heading":-1.10416, "vx":-1.02076, "vy":-1.43847, "omega":-2.16903, "ax":6.06863, "ay":-1.99148, "alpha":-0.06714, "fx":[99.04678,99.21344,99.37317,99.20899], "fy":[-33.05404,-32.55297,-32.05951,-32.56138]}, - {"t":1.00766, "x":1.23, "y":3.48812, "heading":-1.17246, "vx":-0.82966, "vy":-1.50118, "omega":-2.17114, "ax":6.08127, "ay":-1.95476, "alpha":-0.0193, "fx":[99.37104,99.41439,99.46314,99.42002], "fy":[-32.09987,-31.96615,-31.8135,-31.94722]}, - {"t":1.03915, "x":1.20689, "y":3.43988, "heading":-1.24083, "vx":-0.63816, "vy":-1.56273, "omega":-2.17175, "ax":6.09214, "ay":-1.92246, "alpha":0.02245, "fx":[99.64722,99.60216,99.54252,99.58791], "fy":[-31.26273,-31.4051,-31.59427,-31.45182]}, - {"t":1.07064, "x":1.18981, "y":3.38972, "heading":-1.30922, "vx":-0.44632, "vy":-1.62327, "omega":-2.17104, "ax":6.1016, "ay":-1.89381, "alpha":0.05916, "fx":[99.88335,99.77863,99.61457,99.72159], "fy":[-30.52796,-30.86607,-31.393,-31.05374]}, - {"t":1.10213, "x":1.17878, "y":3.33767, "heading":-1.37758, "vx":-0.25419, "vy":-1.6829, "omega":-2.16918, "ax":1.91547, "ay":-0.54898, "alpha":13.57729, "fx":[63.68219,-13.25648,6.93328,67.89787], "fy":[31.3742,29.17201,-62.516,-33.92917]}, - {"t":1.11907, "x":1.17475, "y":3.30907, "heading":-1.41434, "vx":-0.22174, "vy":-1.69221, "omega":-1.93915, "ax":1.93968, "ay":-0.47168, "alpha":13.25873, "fx":[63.62646,-9.69559,6.13388,66.77536], "fy":[30.51356,31.9512,-59.72259,-33.58671]}, - {"t":1.13602, "x":1.17127, "y":3.28034, "heading":-1.44719, "vx":-0.18887, "vy":-1.7002, "omega":-1.71451, "ax":1.95783, "ay":-0.3891, "alpha":12.93195, "fx":[63.31641,-6.41831,5.4743,65.65507], "fy":[29.915,34.32946,-56.69552,-32.99302]}, - {"t":1.15296, "x":1.16835, "y":3.25147, "heading":-1.47624, "vx":-0.1557, "vy":-1.70679, "omega":-1.4954, "ax":1.97301, "ay":-0.30238, "alpha":12.59978, "fx":[62.89593,-3.72393,5.18445,64.66352], "fy":[29.50821,36.22782,-53.43577,-32.0735]}, - {"t":1.1699, "x":1.166, "y":3.22251, "heading":-1.50158, "vx":-0.12227, "vy":-1.71191, "omega":-1.28193, "ax":1.98471, "ay":-0.21174, "alpha":12.24499, "fx":[62.27827,-1.16285,4.998,63.6718], "fy":[29.3041,37.7457,-49.97418,-30.92204]}, - {"t":1.18684, "x":1.16421, "y":3.19348, "heading":-1.5233, "vx":-0.08865, "vy":-1.7155, "omega":-1.07447, "ax":1.99245, "ay":-0.11754, "alpha":11.88407, "fx":[61.55879,0.79515,5.16446,62.77262], "fy":[29.25738,38.90917,-46.33747,-29.51516]}, - {"t":1.20379, "x":1.16299, "y":3.1644, "heading":-1.5415, "vx":-0.05489, "vy":-1.71749, "omega":-0.87312, "ax":1.99575, "ay":-0.02021, "alpha":11.50234, "fx":[60.67162,2.67379,5.31952,61.84221], "fy":[29.35214,39.79679,-42.56695,-27.90344]}, - {"t":1.22073, "x":1.16235, "y":3.13529, "heading":-1.55629, "vx":-0.02107, "vy":-1.71783, "omega":-0.67824, "ax":1.99422, "ay":0.07967, "alpha":11.11587, "fx":[59.67892,4.0424,5.72199,60.96335], "fy":[29.57441,40.43024,-38.7037,-26.09095]}, - {"t":1.23767, "x":1.16228, "y":3.1062, "heading":-1.56778, "vx":0.01271, "vy":-1.71648, "omega":-0.4899, "ax":1.98751, "ay":0.18144, "alpha":10.71252, "fx":[58.53586,5.26522,6.15076,60.01594], "fy":[29.89311,40.86757,-34.78888,-24.10719]}, - {"t":1.25461, "x":1.16278, "y":3.07714, "heading":-1.57608, "vx":0.04639, "vy":-1.71341, "omega":-0.3084, "ax":1.97539, "ay":0.28432, "alpha":10.30957, "fx":[57.26718,6.10838,6.71716,59.08258], "fy":[30.334,41.14395,-30.94033,-21.94549]}, - {"t":1.27156, "x":1.16385, "y":3.04816, "heading":-1.58131, "vx":0.07985, "vy":-1.70859, "omega":-0.13373, "ax":1.95775, "ay":0.38749, "alpha":9.89122, "fx":[55.89072,6.84558,7.23191,58.05363], "fy":[30.79712,41.25345,-27.05706,-19.65458]}, - {"t":1.2885, "x":1.16548, "y":3.01926, "heading":-1.58358, "vx":0.11302, "vy":-1.70203, "omega":0.03385, "ax":1.93461, "ay":0.49009, "alpha":9.46858, "fx":[54.36168,7.35801,7.81736,56.97177], "fy":[31.35762,41.22535,-23.27902,-17.25556]}, - {"t":1.30544, "x":1.16768, "y":2.9905, "heading":-1.583, "vx":0.1458, "vy":-1.69372, "omega":0.19428, "ax":1.90613, "ay":0.59127, "alpha":9.04103, "fx":[52.78226,7.66809,8.38396,55.81215], "fy":[31.87956,41.08692,-19.56909,-14.73255]}, - {"t":1.32239, "x":1.17042, "y":2.96189, "heading":-1.57971, "vx":0.1781, "vy":-1.68371, "omega":0.34746, "ax":1.8726, "ay":0.69021, "alpha":8.60287, "fx":[51.02132,7.91455,8.98297,54.53503], "fy":[32.47538,40.82015,-16.01393,-12.14745]}, - {"t":1.33933, "x":1.17371, "y":2.93346, "heading":-1.57382, "vx":0.20982, "vy":-1.67201, "omega":0.49321, "ax":1.83443, "ay":0.78614, "alpha":8.17118, "fx":[49.25932,7.98938,9.50335,53.20548], "fy":[33.0018,40.49413,-12.62693,-9.46129]}, - {"t":1.35627, "x":1.17753, "y":2.90524, "heading":-1.56547, "vx":0.2409, "vy":-1.65869, "omega":0.63165, "ax":1.79211, "ay":0.87842, "alpha":7.71836, "fx":[47.30074,8.10138,10.09515,51.69338], "fy":[33.55434,40.03028,-9.36463,-6.7777]}, - {"t":1.37321, "x":1.18186, "y":2.87727, "heading":-1.55476, "vx":0.27127, "vy":-1.64381, "omega":0.76242, "ax":1.74624, "ay":0.96652, "alpha":7.2817, "fx":[45.4052,8.04993,10.5602,50.17515], "fy":[33.9982,39.53288,-6.28885,-4.03888]}, - {"t":1.39016, "x":1.18671, "y":2.84955, "heading":-1.54185, "vx":0.30085, "vy":-1.62743, "omega":0.8858, "ax":1.69741, "ay":1.05002, "alpha":6.81747, "fx":[43.30472,8.09314,11.17413,48.42559], "fy":[34.43596,38.91702,-3.35721,-1.33228]}, - {"t":1.4071, "x":1.19205, "y":2.82213, "heading":-1.52684, "vx":0.32961, "vy":-1.60964, "omega":1.0013, "ax":1.64626, "ay":1.12865, "alpha":6.37673, "fx":[41.31342,8.06368,11.55906,46.71655], "fy":[34.74966,38.28521,-0.61197,1.38202]}, - {"t":1.42404, "x":1.19787, "y":2.79502, "heading":-1.50987, "vx":0.3575, "vy":-1.59052, "omega":1.10934, "ax":1.59341, "ay":1.20223, "alpha":5.90023, "fx":[39.11913,8.18297,12.15956,44.73489], "fy":[35.01802,37.54452,2.03443,4.01975]}, - {"t":1.44098, "x":1.20416, "y":2.76825, "heading":-1.49108, "vx":0.3845, "vy":-1.57015, "omega":1.20931, "ax":1.53943, "ay":1.27074, "alpha":5.45565, "fx":[37.09747,8.20721,12.5055,42.8567], "fy":[35.14038,36.80917,4.52162,6.62524]}, - {"t":1.45793, "x":1.21089, "y":2.74183, "heading":-1.47059, "vx":0.41058, "vy":-1.54862, "omega":1.30174, "ax":1.48487, "ay":1.3342, "alpha":4.96785, "fx":[34.86548,8.4862,13.08611,40.66138], "fy":[35.20211,35.98809,6.92018,9.13626]}, - {"t":1.47487, "x":1.21806, "y":2.71578, "heading":-1.44854, "vx":0.43574, "vy":-1.52602, "omega":1.38591, "ax":1.4302, "ay":1.39277, "alpha":4.51961, "fx":[32.87112,8.61583,13.39091,38.64631], "fy":[35.0948,35.18526,9.2372,11.55901]}, - {"t":1.49181, "x":1.22565, "y":2.69012, "heading":-1.42505, "vx":0.45997, "vy":-1.50242, "omega":1.46249, "ax":1.37584, "ay":1.44661, "alpha":4.02301, "fx":[30.67259,9.09525,13.93258,36.26905], "fy":[34.92013,34.33394,11.44916,13.8939]}, - {"t":1.50876, "x":1.23364, "y":2.66488, "heading":-1.40028, "vx":0.48328, "vy":-1.47791, "omega":1.53065, "ax":1.32213, "ay":1.49597, "alpha":3.57351, "fx":[28.7375,9.38392,14.19282,34.14321], "fy":[34.59105,33.52077,13.60858,16.10451]}, - {"t":1.5257, "x":1.24202, "y":2.64005, "heading":-1.37434, "vx":0.50568, "vy":-1.45257, "omega":1.59119, "ax":1.26937, "ay":1.5411, "alpha":3.06651, "fx":[26.60545,10.11294,14.68701,31.60199], "fy":[34.17702,32.6799,15.69607,18.22329]}, - {"t":1.54264, "x":1.25077, "y":2.61566, "heading":-1.34738, "vx":0.52719, "vy":-1.42645, "omega":1.64315, "ax":1.21778, "ay":1.58229, "alpha":2.61565, "fx":[24.80849,10.56047,14.86634,29.39848], "fy":[33.58646,31.89952,17.79613,20.1878]}, - {"t":1.55958, "x":1.25988, "y":2.59172, "heading":-1.31954, "vx":0.54782, "vy":-1.39965, "omega":1.68746, "ax":1.16755, "ay":1.61982, "alpha":2.10388, "fx":[22.83451,11.51022,15.24286,26.76104], "fy":[32.91502,31.11256,19.84744,22.04905]}, - {"t":1.57653, "x":1.26933, "y":2.56824, "heading":-1.29095, "vx":0.5676, "vy":-1.3722, "omega":1.72311, "ax":1.11879, "ay":1.65398, "alpha":1.6488, "fx":[21.17161,12.16428,15.36168,24.46246], "fy":[32.0985,30.41832,21.86822,23.77256]}, - {"t":1.59347, "x":1.2791, "y":2.54523, "heading":-1.26176, "vx":0.58656, "vy":-1.34418, "omega":1.75104, "ax":1.0716, "ay":1.68503, "alpha":1.12866, "fx":[19.35726,13.37538,15.65497,21.68665], "fy":[31.20721,29.74429,23.83784,25.39896]}, - {"t":1.61041, "x":1.28919, "y":2.5227, "heading":-1.23209, "vx":0.60472, "vy":-1.31563, "omega":1.77017, "ax":1.02603, "ay":1.71325, "alpha":0.6774, "fx":[17.90246,14.17593,15.64089,19.37535], "fy":[30.16337,29.1787,25.83293,26.8587]}, - {"t":1.62735, "x":1.29959, "y":2.50065, "heading":-1.2021, "vx":0.6221, "vy":-1.2866, "omega":1.78164, "ax":1.00316, "ay":1.7286, "alpha":-8.41315, "fx":[3.48183,49.28221,28.40516,-15.57049], "fy":[-1.128,15.85829,54.08636,44.22083]}, - {"t":1.66412, "x":1.32314, "y":2.45451, "heading":-1.13659, "vx":0.65898, "vy":-1.22305, "omega":1.47231, "ax":3.19748, "ay":5.53358, "alpha":-0.02586, "fx":[52.30115,52.44564,52.24454,52.09963], "fy":[90.44693,90.36348,90.48008,90.56334]}, - {"t":1.70089, "x":1.34953, "y":2.41329, "heading":-1.08246, "vx":0.77655, "vy":-1.01958, "omega":1.47135, "ax":3.18575, "ay":5.53994, "alpha":-0.04218, "fx":[52.14232,52.36061,52.02028,51.80073], "fy":[90.53205,90.40643,90.60296,90.72821]}, - {"t":1.73766, "x":1.38024, "y":2.37954, "heading":-1.02836, "vx":0.89368, "vy":-0.81589, "omega":1.4698, "ax":3.17258, "ay":5.54704, "alpha":-0.06043, "fx":[51.97479,52.26169,51.7578,51.468], "fy":[90.62098,90.45654,90.74619,90.91016]}, - {"t":1.77443, "x":1.41524, "y":2.35329, "heading":-0.97432, "vx":1.01033, "vy":-0.61193, "omega":1.46758, "ax":3.15767, "ay":5.55501, "alpha":-0.081, "fx":[51.79655,52.14529,51.45003,51.09561], "fy":[90.71467,90.51571,90.9132,91.11183]}, - {"t":1.8112, "x":1.45452, "y":2.33455, "heading":-0.92036, "vx":1.12644, "vy":-0.40769, "omega":1.4646, "ax":3.14067, "ay":5.56403, "alpha":-0.10436, "fx":[51.60495,52.00686,51.08804,50.67601], "fy":[90.81434,90.58627,91.10822,91.33659]}, - {"t":1.84796, "x":1.49806, "y":2.32332, "heading":-0.8665, "vx":1.24191, "vy":-0.20311, "omega":1.46077, "ax":3.12111, "ay":5.57432, "alpha":-0.13114, "fx":[51.39643,51.84057,50.66031,50.19925], "fy":[90.92164,90.67119,91.3366,91.58882]}, - {"t":1.88473, "x":1.54584, "y":2.31962, "heading":-0.81279, "vx":1.35667, "vy":0.00185, "omega":1.45594, "ax":3.09835, "ay":5.58616, "alpha":-0.16219, "fx":[51.16617,51.63882,50.15172,49.65202], "fy":[91.03879,90.77431,91.60513,91.87423]}, - {"t":1.9215, "x":1.59781, "y":2.32346, "heading":-0.75926, "vx":1.47059, "vy":0.20725, "omega":1.44998, "ax":3.07157, "ay":5.59993, "alpha":-0.1986, "fx":[50.90752,51.39151,49.54197,49.01608], "fy":[91.16883,90.90064,91.92266,92.20051]}, - {"t":1.95827, "x":1.65396, "y":2.33487, "heading":-0.70595, "vx":1.58353, "vy":0.41315, "omega":1.44268, "ax":3.03957, "ay":5.61613, "alpha":-0.24195, "fx":[50.61113,51.08487,48.80314,48.26581], "fy":[91.31599,91.05688,92.3009,92.57811]}, - {"t":1.99504, "x":1.71424, "y":2.35386, "heading":-0.6529, "vx":1.69529, "vy":0.61964, "omega":1.43378, "ax":3.0007, "ay":5.63546, "alpha":-0.29441, "fx":[50.26347,50.69959,47.89562,47.3641], "fy":[91.48633,91.25224,92.7557,93.02161]}, - {"t":2.03181, "x":1.7786, "y":2.38045, "heading":-0.60019, "vx":1.80562, "vy":0.82685, "omega":1.42296, "ax":2.95246, "ay":5.65891, "alpha":-0.35921, "fx":[49.84431,50.20767,46.7614,46.25523], "fy":[91.68879,91.49972,93.309,93.55189]}, - {"t":2.06857, "x":1.84699, "y":2.41467, "heading":-0.54787, "vx":1.91418, "vy":1.03492, "omega":1.40975, "ax":2.89105, "ay":5.68793, "alpha":-0.44128, "fx":[49.32205,49.56686,45.31187,44.85172], "fy":[91.93702,91.81832,93.99193,94.19978]}, - {"t":2.10534, "x":1.91932, "y":2.45657, "heading":-0.49603, "vx":2.02048, "vy":1.24405, "omega":1.39353, "ax":2.81024, "ay":5.7247, "alpha":-0.54857, "fx":[48.64469,48.71012,43.40486,43.00836], "fy":[92.25291,92.23701,94.84979,95.01227]}, - {"t":2.14211, "x":1.99551, "y":2.50618, "heading":-0.44479, "vx":2.12381, "vy":1.45454, "omega":1.37336, "ax":2.69922, "ay":5.77268, "alpha":-0.69473, "fx":[47.7203,47.5242,40.79762,40.46612], "fy":[92.67379,92.80212,95.94983,96.06349]}, - {"t":2.17888, "x":2.07542, "y":2.56357, "heading":-0.3943, "vx":2.22305, "vy":1.66679, "omega":1.34781, "ax":2.53744, "ay":5.83746, "alpha":-0.9054, "fx":[46.3701,45.80142,37.04129,36.71669], "fy":[93.2686,93.59246,97.39175,97.47286]}, - {"t":2.21565, "x":2.15888, "y":2.6288, "heading":-0.34474, "vx":2.31635, "vy":1.88143, "omega":1.31452, "ax":2.2808, "ay":5.92827, "alpha":-1.23493, "fx":[44.19558,43.11593,31.21388,30.62172], "fy":[94.17897,94.75225,99.30967,99.42262]}, - {"t":2.25242, "x":2.24559, "y":2.70198, "heading":-0.29641, "vx":2.40021, "vy":2.0994, "omega":1.26911, "ax":1.81628, "ay":6.05731, "alpha":-1.82133, "fx":[40.09763,38.44043,21.13511,19.09802], "fy":[95.74167,96.5666,101.76536,102.02815]}, - {"t":2.28919, "x":2.33507, "y":2.78327, "heading":-0.24975, "vx":2.46699, "vy":2.32212, "omega":1.20215, "ax":0.7677, "ay":6.19394, "alpha":-3.11884, "fx":[29.62503,28.56878,0.6662,-8.65835], "fy":[98.88177,99.58924,103.63186,102.93379]}, - {"t":2.32595, "x":2.42629, "y":2.87284, "heading":-0.20554, "vx":2.49522, "vy":2.54986, "omega":1.08747, "ax":-2.45909, "ay":5.40186, "alpha":-5.91716, "fx":[-30.84697,-2.15136,-47.33873,-80.46848], "fy":[95.6113,102.66685,91.55722,63.40516]}, - {"t":2.36272, "x":2.51638, "y":2.97024, "heading":-0.16556, "vx":2.4048, "vy":2.74848, "omega":0.86991, "ax":-5.85942, "ay":-0.7352, "alpha":-6.59554, "fx":[-83.87998,-99.24313,-101.1979,-98.84039], "fy":[-58.5756,20.88873,19.09642,-29.48631]}, - {"t":2.39949, "x":2.60084, "y":3.0708, "heading":-0.13357, "vx":2.18936, "vy":2.72145, "omega":0.6274, "ax":-5.13606, "ay":-3.54484, "alpha":-3.19838, "fx":[-68.57415,-85.95926,-96.67288,-84.65292], "fy":[-77.59211,-57.09699,-37.1593,-59.9572]}, - {"t":2.43626, "x":2.67786, "y":3.16847, "heading":-0.11051, "vx":2.00052, "vy":2.59111, "omega":0.5098, "ax":-4.60133, "ay":-4.32297, "alpha":-2.16642, "fx":[-63.4324,-74.6474,-86.34368,-76.46829], "fy":[-82.3175,-72.08723,-57.79833,-70.48617]}, - {"t":2.47303, "x":2.74831, "y":3.26082, "heading":-0.09176, "vx":1.83133, "vy":2.43216, "omega":0.43015, "ax":-4.30725, "ay":-4.65506, "alpha":-1.65792, "fx":[-60.92755,-69.31525,-79.688,-71.73052], "fy":[-84.40393,-77.55388,-66.95139,-75.49652]}, - {"t":2.5098, "x":2.81273, "y":3.3471, "heading":-0.07595, "vx":1.67296, "vy":2.261, "omega":0.36919, "ax":-4.12643, "ay":-4.83518, "alpha":-1.35991, "fx":[-59.45405,-66.29069,-75.38446,-68.70764], "fy":[-85.57171,-80.32464,-71.91525,-78.37254]}, - {"t":2.54656, "x":2.87146, "y":3.42696, "heading":-0.06237, "vx":1.52124, "vy":2.08322, "omega":0.31919, "ax":-4.00499, "ay":-4.94731, "alpha":-1.16541, "fx":[-58.48639,-64.3519,-72.43025,-66.62725], "fy":[-86.31582,-81.99009,-74.98599,-80.22428]}, - {"t":2.58333, "x":2.92468, "y":3.50021, "heading":-0.05064, "vx":1.37398, "vy":1.90131, "omega":0.27634, "ax":-3.91809, "ay":-5.02354, "alpha":-1.02884, "fx":[-57.80353,-63.00445,-70.29172,-65.11378], "fy":[-86.83045,-83.1,-77.06011,-81.51092]}, - {"t":2.6201, "x":2.97255, "y":3.56673, "heading":-0.04047, "vx":1.22992, "vy":1.71661, "omega":0.23851, "ax":-3.85294, "ay":-5.07864, "alpha":-0.92782, "fx":[-57.29661,-62.01317,-68.67701,-63.96612], "fy":[-87.20702,-83.89262,-78.55041,-82.45436]}, - {"t":2.65687, "x":3.01517, "y":3.62641, "heading":-0.03171, "vx":1.08826, "vy":1.52987, "omega":0.20439, "ax":-3.80232, "ay":-5.12027, "alpha":-0.8501, "fx":[-56.90592,-61.25251,-67.41657,-63.06768], "fy":[-87.49416,-84.48746,-79.67108,-83.17425]}, - {"t":2.69364, "x":3.05261, "y":3.6792, "heading":-0.02419, "vx":0.94845, "vy":1.34161, "omega":0.17313, "ax":-3.76188, "ay":-5.15282, "alpha":-0.78848, "fx":[-56.59596,-60.64949,-66.40612,-62.3465], "fy":[-87.7201,-84.95089,-80.54366,-83.7406]}, - {"t":2.73041, "x":3.08494, "y":3.72505, "heading":-0.01782, "vx":0.81013, "vy":1.15215, "omega":0.14414, "ax":-3.72883, "ay":-5.17895, "alpha":-0.73843, "fx":[-56.34436,-60.15884,-65.57831,-61.75585], "fy":[-87.90232,-85.32272,-81.24195,-84.19698]}, - {"t":2.76717, "x":3.11221, "y":3.76391, "heading":-0.01252, "vx":0.67303, "vy":0.96173, "omega":0.11699, "ax":-3.70134, "ay":-5.20039, "alpha":-0.69697, "fx":[-56.1363,-59.75101,-64.88785,-61.26412], "fy":[-88.05223,-85.6282,-81.81328,-84.57193]}, - {"t":2.80394, "x":3.13445, "y":3.79575, "heading":-0.00822, "vx":0.53694, "vy":0.77052, "omega":0.09137, "ax":-3.6781, "ay":-5.21828, "alpha":-0.66206, "fx":[-55.96159,-59.40588,-64.30318,-60.84915], "fy":[-88.17759,-85.88417,-82.28932,-84.88488]}, - {"t":2.84071, "x":3.15171, "y":3.82056, "heading":-0.00486, "vx":0.4017, "vy":0.57865, "omega":0.06702, "ax":-3.65821, "ay":-5.23345, "alpha":-0.63226, "fx":[-55.813,-59.10933,-63.80169,-60.49499], "fy":[-88.28385,-86.10223,-82.69208,-85.14952]}, - {"t":2.87748, "x":3.16401, "y":3.8383, "heading":-0.0024, "vx":0.26719, "vy":0.38622, "omega":0.04378, "ax":-3.64099, "ay":-5.24646, "alpha":-0.60652, "fx":[-55.68526,-58.8511,-63.36674,-60.18984], "fy":[-88.37496,-86.29068,-83.0373,-85.37575]}, - {"t":2.91425, "x":3.17137, "y":3.84895, "heading":-0.00079, "vx":0.13332, "vy":0.19332, "omega":0.02147, "ax":-3.62594, "ay":-5.25775, "alpha":-0.58406, "fx":[-55.57442,-58.6236,-62.98584,-59.9248], "fy":[-88.45384,-86.45558,-83.33654,-85.57095]}, - {"t":2.95102, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.28147, "ay":-0.36295, "alpha":-3.36884, "fx":[-100.73663,-103.50075,-103.99191,-102.53097], "fy":[-27.5229,13.86092,9.95784,-20.0299]}, + {"t":0.03413, "x":3.17016, "y":3.85229, "heading":0.0, "vx":-0.21438, "vy":-0.01239, "omega":-0.11497, "ax":-6.27958, "ay":-0.38675, "alpha":-3.36839, "fx":[-100.60794,-103.55196,-104.01661,-102.46009], "fy":[-27.95568,13.39812,9.6221,-20.35516]}, + {"t":0.06826, "x":3.15919, "y":3.85164, "heading":-0.00392, "vx":-0.42869, "vy":-0.02559, "omega":-0.22993, "ax":-6.27737, "ay":-0.4136, "alpha":-3.36711, "fx":[-100.44599,-103.61624,-104.03769,-102.39216], "fy":[-28.49504,12.8015,9.30346,-20.65624]}, + {"t":0.10239, "x":3.1409, "y":3.85053, "heading":-0.01177, "vx":-0.64293, "vy":-0.0397, "omega":-0.34485, "ax":-6.27475, "ay":-0.44417, "alpha":-3.36471, "fx":[-100.24639,-103.69296,-104.05606,-102.32553], "fy":[-29.14875,12.05371,8.99095,-20.94139]}, + {"t":0.13651, "x":3.11531, "y":3.84892, "heading":-0.02354, "vx":-0.85708, "vy":-0.05486, "omega":-0.45968, "ax":-6.27161, "ay":-0.47936, "alpha":-3.36074, "fx":[-100.00371,-103.78096,-104.07272,-102.25792], "fy":[-29.92587,11.13104,8.66979,-21.22116]}, + {"t":0.17064, "x":3.0824, "y":3.84676, "heading":-0.03923, "vx":-1.07112, "vy":-0.07122, "omega":-0.57438, "ax":-6.26776, "ay":-0.52033, "alpha":-3.35458, "fx":[-99.71082,-103.87811,-104.08881,-102.18611], "fy":[-30.83778,10.0015,8.31994,-21.50954]}, + {"t":0.20477, "x":3.0422, "y":3.84403, "heading":-0.05883, "vx":-1.28503, "vy":-0.08898, "omega":-0.68887, "ax":-6.26296, "ay":-0.56872, "alpha":-3.34539, "fx":[-99.35808,-103.98051,-104.10562,-102.10547], "fy":[-31.89997,8.62182,7.91385,-21.82571]}, + {"t":0.2389, "x":2.99469, "y":3.84066, "heading":-0.08234, "vx":-1.49878, "vy":-0.10839, "omega":-0.80304, "ax":-6.2568, "ay":-0.62677, "alpha":-3.33216, "fx":[-98.93186,-104.08114,-104.12452,-102.00924], "fy":[-33.13469,6.93281,7.41286,-22.19682]}, + {"t":0.27303, "x":2.9399, "y":3.8366, "heading":-0.10975, "vx":-1.71232, "vy":-0.12978, "omega":-0.91676, "ax":-6.24864, "ay":-0.69771, "alpha":-3.31361, "fx":[-98.41195,-104.16743,-104.14672,-101.88706], "fy":[-34.57533,4.85171,6.76127,-22.66256]}, + {"t":0.30716, "x":2.87782, "y":3.83176, "heading":-0.14104, "vx":-1.92557, "vy":-0.15359, "omega":-1.02985, "ax":-6.2374, "ay":-0.78635, "alpha":-3.28812, "fx":[-97.76699,-104.21646,-104.17252,-101.72253], "fy":[-36.27379,2.25925,5.87588,-23.28295]}, + {"t":0.34129, "x":2.80847, "y":3.82606, "heading":-0.17618, "vx":-2.13845, "vy":-0.18043, "omega":-1.14207, "ax":-6.22119, "ay":-0.90015, "alpha":-3.25369, "fx":[-96.94565,-104.18527,-104.1993,-101.48813], "fy":[-38.31352,-1.02374,4.62671,-24.15247]}, + {"t":0.37542, "x":2.73186, "y":3.81938, "heading":-0.21516, "vx":-2.35077, "vy":-0.21115, "omega":-1.25312, "ax":-6.19641, "ay":-1.05124, "alpha":-3.20761, "fx":[-95.8586,-103.98918,-104.21585,-101.13444], "fy":[-40.8338,-5.28063,2.79867,-25.42755]}, + {"t":0.40954, "x":2.64803, "y":3.81156, "heading":-0.25793, "vx":-2.56225, "vy":-0.24703, "omega":-1.36259, "ax":-6.1557, "ay":-1.26075, "alpha":-3.14593, "fx":[-94.33817,-103.44856,-104.18502,-100.56407], "fy":[-44.07821,-10.98912,0.00818,-27.38432]}, + {"t":0.44367, "x":2.55699, "y":3.8024, "heading":-0.30443, "vx":-2.77233, "vy":-0.29006, "omega":-1.46995, "ax":-6.0821, "ay":-1.56852, "alpha":-3.06193, "fx":[-92.03694,-102.141,-103.98496,-99.56034], "fy":[-48.49949,-19.01685,-4.4993,-30.55389]}, + {"t":0.4778, "x":2.45884, "y":3.79159, "heading":-0.3546, "vx":-2.97991, "vy":-0.34359, "omega":-1.57445, "ax":-5.92998, "ay":-2.05789, "alpha":-2.94112, "fx":[-88.13121,-98.92494,-103.16872,-97.55067], "fy":[-55.00664,-31.06575,-12.41516,-36.08291]}, + {"t":0.51193, "x":2.35368, "y":3.77866, "heading":-0.40833, "vx":-3.18229, "vy":-0.41382, "omega":-1.67483, "ax":-5.54603, "ay":-2.92356, "alpha":-2.73554, "fx":[-80.25967,-90.14068,-99.68133,-92.58632], "fy":[-65.57334,-50.54326,-28.20847,-46.85355]}, + {"t":0.54606, "x":2.24185, "y":3.76283, "heading":-0.46549, "vx":-3.37157, "vy":-0.5136, "omega":-1.76819, "ax":-4.25389, "ay":-4.59538, "alpha":-2.13966, "fx":[-59.75928,-62.89174,-80.49507,-75.02557], "fy":[-84.1017,-81.32275,-64.15911,-70.91926]}, + {"t":0.58019, "x":2.1243, "y":3.74263, "heading":-0.52584, "vx":-3.51675, "vy":-0.67043, "omega":-1.84122, "ax":0.20655, "ay":-6.26704, "alpha":0.00075, "fx":[3.37544,3.37104,3.37794,3.38233], "fy":[-102.45402,-102.45431,-102.45417,-102.45388]}, + {"t":0.61432, "x":2.0044, "y":3.7161, "heading":-0.58868, "vx":-3.5097, "vy":-0.88432, "omega":-1.84119, "ax":4.38611, "ay":-4.47855, "alpha":2.09501, "fx":[78.70805,61.19962,66.53516,80.37551], "fy":[-66.28286,-82.83271,-78.98072,-64.76696]}, + {"t":0.64845, "x":1.88717, "y":3.68331, "heading":-0.65152, "vx":-3.36001, "vy":-1.03717, "omega":-1.76969, "ax":5.56221, "ay":-2.89755, "alpha":2.78739, "fx":[99.00647,84.31162,84.03672,96.37132], "fy":[-30.24856,-59.95437,-60.81295,-38.46177]}, + {"t":0.68257, "x":1.77574, "y":3.64623, "heading":-0.71191, "vx":-3.17017, "vy":-1.13605, "omega":-1.67456, "ax":5.92477, "ay":-2.07884, "alpha":2.9592, "fx":[103.07537,93.59819,90.32362,100.43761], "fy":[-12.69656,-44.73395,-51.47567,-27.03388]}, + {"t":0.7167, "x":1.67099, "y":3.60624, "heading":-0.76906, "vx":-2.96797, "vy":-1.207, "omega":-1.57357, "ax":6.07544, "ay":-1.6101, "alpha":3.00311, "fx":[103.98098,98.04669,93.32001,101.93984], "fy":[-3.41184,-34.42747,-46.11803,-21.33068]}, + {"t":0.75083, "x":1.57324, "y":3.56411, "heading":-0.82277, "vx":-2.76062, "vy":-1.26195, "omega":-1.47107, "ax":6.15166, "ay":-1.31111, "alpha":3.01556, "fx":[104.12864,100.48297,95.02581,102.63444], "fy":[2.08405,-26.94615,-42.69863,-18.17561]}, + {"t":0.78496, "x":1.48261, "y":3.52028, "heading":-0.87297, "vx":-2.55067, "vy":-1.3067, "omega":-1.36816, "ax":6.1954, "ay":-1.10484, "alpha":3.02161, "fx":[104.07426,101.93663,96.11868,103.00232], "fy":[5.60224,-21.19596,-40.32951,-16.32487]}, + {"t":0.81909, "x":1.39916, "y":3.47504, "heading":-0.91967, "vx":-2.33923, "vy":-1.34441, "omega":-1.26503, "ax":6.22271, "ay":-0.95428, "alpha":3.02825, "fx":[103.97317,102.8494,96.88092,103.21435], "fy":[7.97623,-16.59279,-38.5786,-15.20732]}, + {"t":0.85322, "x":1.32295, "y":3.4286, "heading":-0.96284, "vx":-2.12686, "vy":-1.37697, "omega":-1.16168, "ax":6.24081, "ay":-0.83968, "alpha":3.03712, "fx":[103.87255,103.43786,97.44786,103.34319], "fy":[9.63768,-12.80427,-37.21545,-14.52695]}, + {"t":0.88735, "x":1.254, "y":3.38112, "heading":-1.00249, "vx":-1.91387, "vy":-1.40563, "omega":-1.05803, "ax":6.25335, "ay":-0.74964, "alpha":3.04809, "fx":[103.78585,103.82,97.8911,103.42419], "fy":[10.83204,-9.62852,-36.10862,-14.1159]}, + {"t":0.92148, "x":1.19232, "y":3.33271, "heading":-1.0386, "vx":-1.70045, "vy":-1.43122, "omega":-0.954, "ax":6.26233, "ay":-0.6771, "alpha":3.06047, "fx":[103.71515,104.06571,98.25132,103.47641], "fy":[11.7095,-6.93533,-35.17938,-13.87222]}, + {"t":0.9556, "x":1.13794, "y":3.28347, "heading":-1.07116, "vx":-1.48673, "vy":-1.45433, "omega":-0.84955, "ax":6.26895, "ay":-0.61748, "alpha":3.07349, "fx":[103.65871,104.21914,98.55281,103.51105], "fy":[12.36754,-4.63615,-34.37904,-13.73061]}, + {"t":0.98973, "x":1.09085, "y":3.23348, "heading":-1.10015, "vx":-1.27277, "vy":-1.4754, "omega":-0.74466, "ax":6.27396, "ay":-0.56765, "alpha":3.08645, "fx":[103.6137,104.30963,98.81067,103.53518], "fy":[12.87242,-2.66785,-33.67683,-13.64745]}, + {"t":1.02386, "x":1.05106, "y":3.18279, "heading":-1.12556, "vx":-1.05865, "vy":-1.49477, "omega":-0.63932, "ax":6.27784, "ay":-0.52542, "alpha":3.09877, "fx":[103.57722,104.3574,99.03453,103.55352], "fy":[13.27093,-0.98369,-33.05306,-13.59259]}, + {"t":1.05799, "x":1.01859, "y":3.13147, "heading":-1.14738, "vx":-0.8444, "vy":-1.5127, "omega":-0.53356, "ax":6.28091, "ay":-0.48921, "alpha":3.11004, "fx":[103.54661,104.37677,99.23074,103.56936], "fy":[13.59711,0.45209,-32.49502,-13.54463]}, + {"t":1.09212, "x":0.99343, "y":3.07956, "heading":-1.16559, "vx":-0.63004, "vy":-1.5294, "omega":-0.42742, "ax":6.2834, "ay":-0.45783, "alpha":3.11997, "fx":[103.5196,104.37804,99.40357,103.58504], "fy":[13.87631,1.66741,-31.99441,-13.48796]}, + {"t":1.12625, "x":0.97559, "y":3.0271, "heading":-1.18018, "vx":-0.41559, "vy":-1.54503, "omega":-0.32094, "ax":6.28546, "ay":-0.43039, "alpha":3.12838, "fx":[103.49428,104.36871,99.55603,103.60231], "fy":[14.12769,2.68448,-31.54572,-13.41099]}, + {"t":1.16038, "x":0.96506, "y":2.97412, "heading":-1.19113, "vx":-0.20108, "vy":-1.55971, "omega":-0.21417, "ax":6.28722, "ay":-0.40621, "alpha":3.13517, "fx":[103.46906,104.35427,99.69033,103.62242], "fy":[14.36594,3.52098,-31.14503,-13.30492]}, + {"t":1.19451, "x":0.96186, "y":2.92065, "heading":-1.19844, "vx":0.0135, "vy":-1.57358, "omega":-0.10717, "ax":6.28874, "ay":-0.38474, "alpha":3.14029, "fx":[103.44262,104.33873,99.8082,103.64629], "fy":[14.60233,4.19104,-30.78931,-13.16294]}, + {"t":1.22863, "x":0.96598, "y":2.86672, "heading":-1.2021, "vx":0.22812, "vy":-1.58671, "omega":0.0, "ax":6.34427, "ay":-0.73227, "alpha":0.0, "fx":[103.71675,103.71675,103.71675,103.71675], "fy":[-11.97117,-11.97117,-11.97117,-11.97117]}, + {"t":1.2574, "x":0.97517, "y":2.82077, "heading":-1.2021, "vx":0.41062, "vy":-1.60777, "omega":0.0, "ax":5.95844, "ay":-2.27438, "alpha":0.0, "fx":[97.40911,97.40911,97.40911,97.40911], "fy":[-37.1818,-37.1818,-37.1818,-37.1818]}, + {"t":1.28617, "x":0.98945, "y":2.77358, "heading":-1.2021, "vx":0.58202, "vy":-1.6732, "omega":0.0, "ax":3.97032, "ay":-4.96867, "alpha":0.0, "fx":[64.90721,64.90721,64.90721,64.90721], "fy":[-81.22825,-81.22825,-81.22825,-81.22825]}, + {"t":1.31493, "x":1.00783, "y":2.7234, "heading":-1.2021, "vx":0.69623, "vy":-1.81613, "omega":0.0, "ax":2.32991, "ay":-5.89524, "alpha":0.0, "fx":[38.08962,38.08962,38.08962,38.08962], "fy":[-96.37602,-96.37602,-96.37602,-96.37602]}, + {"t":1.3437, "x":1.02883, "y":2.66871, "heading":-1.2021, "vx":0.76326, "vy":-1.98571, "omega":0.0, "ax":2.22431, "ay":-5.87525, "alpha":0.0, "fx":[36.36329,36.36329,36.36329,36.36329], "fy":[-96.04921,-96.04921,-96.04921,-96.04921]}, + {"t":1.37246, "x":1.0517, "y":2.60916, "heading":-1.2021, "vx":0.82724, "vy":-2.15472, "omega":0.0, "ax":-0.90508, "ay":-2.2502, "alpha":0.0, "fx":[-14.79638,-14.79638,-14.79638,-14.79638], "fy":[-36.78655,-36.78655,-36.78655,-36.78655]}, + {"t":1.40123, "x":1.07512, "y":2.54625, "heading":-1.2021, "vx":0.8012, "vy":-2.21945, "omega":0.0, "ax":-2.29302, "ay":5.84582, "alpha":0.0, "fx":[-37.48654,-37.48654,-37.48654,-37.48654], "fy":[95.5681,95.5681,95.5681,95.5681]}, + {"t":1.43, "x":1.09722, "y":2.48482, "heading":-1.2021, "vx":0.73524, "vy":-2.05129, "omega":0.0, "ax":-2.25083, "ay":5.92512, "alpha":0.0, "fx":[-36.79685,-36.79685,-36.79685,-36.79685], "fy":[96.86446,96.86446,96.86446,96.86446]}, + {"t":1.45876, "x":1.11744, "y":2.42827, "heading":-1.2021, "vx":0.6705, "vy":-1.88084, "omega":0.0, "ax":-2.21, "ay":5.96134, "alpha":0.0, "fx":[-36.12927,-36.12927,-36.12927,-36.12927], "fy":[97.4566,97.4566,97.4566,97.4566]}, + {"t":1.48753, "x":1.13581, "y":2.37663, "heading":-1.2021, "vx":0.60692, "vy":-1.70936, "omega":0.0, "ax":-2.15973, "ay":5.99013, "alpha":0.0, "fx":[-35.30753,-35.30753,-35.30753,-35.30753], "fy":[97.92723,97.92723,97.92723,97.92723]}, + {"t":1.51629, "x":1.15238, "y":2.32994, "heading":-1.2021, "vx":0.5448, "vy":-1.53705, "omega":0.0, "ax":-2.08291, "ay":6.02348, "alpha":0.0, "fx":[-34.05164,-34.05164,-34.05164,-34.05164], "fy":[98.47247,98.47247,98.47247,98.47247]}, + {"t":1.54506, "x":1.16719, "y":2.28821, "heading":-1.2021, "vx":0.48488, "vy":-1.36377, "omega":0.0, "ax":-0.19977, "ay":6.37521, "alpha":0.0, "fx":[-3.26583,-3.26583,-3.26583,-3.26583], "fy":[104.22259,104.22259,104.22259,104.22259]}, + {"t":1.57383, "x":1.18105, "y":2.25162, "heading":-1.2021, "vx":0.47913, "vy":-1.18038, "omega":0.0, "ax":2.08598, "ay":6.03348, "alpha":0.0, "fx":[34.1018,34.1018,34.1018,34.1018], "fy":[98.63597,98.63597,98.63597,98.63597]}, + {"t":1.60259, "x":1.1957, "y":2.22016, "heading":-1.2021, "vx":0.53914, "vy":-1.00683, "omega":0.0, "ax":3.15961, "ay":5.55121, "alpha":0.0, "fx":[51.65356,51.65356,51.65356,51.65356], "fy":[90.75178,90.75178,90.75178,90.75178]}, + {"t":1.63136, "x":1.21252, "y":2.1935, "heading":-1.2021, "vx":0.63003, "vy":-0.84714, "omega":0.0, "ax":3.43799, "ay":5.30256, "alpha":2.7899, "fx":[54.55485,36.86884,60.26308,73.13148], "fy":[89.11015,97.74005,85.29271,74.6043]}, + {"t":1.66681, "x":1.23701, "y":2.16679, "heading":-1.2021, "vx":0.75191, "vy":-0.65915, "omega":0.09891, "ax":3.42942, "ay":5.30791, "alpha":2.78546, "fx":[54.43789,36.74628,60.07954,72.99428], "fy":[89.17546,97.77935,85.41231,74.73006]}, + {"t":1.70226, "x":1.26583, "y":2.14676, "heading":-1.19859, "vx":0.8735, "vy":-0.47096, "omega":0.19766, "ax":3.41985, "ay":5.31385, "alpha":2.7807, "fx":[54.25225,36.60581,59.95307,72.82123], "fy":[89.28159,97.82426,85.49022,74.88932]}, + {"t":1.73772, "x":1.29894, "y":2.1334, "heading":-1.19159, "vx":0.99474, "vy":-0.28257, "omega":0.29625, "ax":3.40909, "ay":5.32049, "alpha":2.77554, "fx":[53.99536,36.44509,59.87888,72.60894], "fy":[89.42938,97.87541,85.52997,75.08473]}, + {"t":1.77317, "x":1.33635, "y":2.12673, "heading":-1.18108, "vx":1.11561, "vy":-0.09394, "omega":0.39465, "ax":3.39686, "ay":5.32798, "alpha":2.76989, "fx":[53.66394,36.26166,59.85059,72.35277], "fy":[89.61977,97.93337,85.53593,75.3199]}, + {"t":1.80862, "x":1.37804, "y":2.12675, "heading":-1.16709, "vx":1.23603, "vy":0.09495, "omega":0.49285, "ax":3.38285, "ay":5.33649, "alpha":2.76365, "fx":[53.25375,36.05263,59.85978,72.04652], "fy":[89.85396,97.99878,85.51372,75.59965]}, + {"t":1.84408, "x":1.42399, "y":2.13347, "heading":-1.14962, "vx":1.35597, "vy":0.28414, "omega":0.59083, "ax":3.36661, "ay":5.34629, "alpha":2.75668, "fx":[52.75917,35.81425,59.89538,71.68198], "fy":[90.13351,98.07247,85.4706,75.93022]}, + {"t":1.87953, "x":1.47418, "y":2.1469, "heading":-1.12867, "vx":1.47532, "vy":0.47369, "omega":0.68856, "ax":3.34756, "ay":5.35769, "alpha":2.74878, "fx":[52.17268,35.54123,59.94261,71.24828], "fy":[90.46052,98.15563,85.4163,76.31978]}, + {"t":1.91498, "x":1.52858, "y":2.16706, "heading":-1.10426, "vx":1.59401, "vy":0.66363, "omega":0.78602, "ax":3.32488, "ay":5.37113, "alpha":2.73961, "fx":[51.48393,35.22563,59.98154,70.73078], "fy":[90.83802,98.25011,85.36397,76.77905]}, + {"t":1.95044, "x":1.58719, "y":2.19397, "heading":-1.07639, "vx":1.71188, "vy":0.85406, "omega":0.88314, "ax":3.29744, "ay":5.38723, "alpha":2.72864, "fx":[50.67828,34.85515,59.98463,70.10933], "fy":[91.27037,98.35889,85.33184,77.32238]}, + {"t":1.98589, "x":1.64995, "y":2.22763, "heading":-1.04508, "vx":1.82879, "vy":1.04505, "omega":0.97988, "ax":3.26357, "ay":5.40684, "alpha":2.715, "fx":[49.73425,34.41014,59.91271,69.35522], "fy":[91.76421,98.48681,85.3458,77.96965]}, + {"t":2.02134, "x":1.71684, "y":2.26808, "heading":-1.01034, "vx":1.94449, "vy":1.23674, "omega":1.07614, "ax":3.22073, "ay":5.43129, "alpha":2.6973, "fx":[48.61908,33.85834,59.70785,68.42575], "fy":[92.32981,98.64181,85.44363,78.74945]}, + {"t":2.0568, "x":1.7878, "y":2.31534, "heading":-0.97219, "vx":2.05868, "vy":1.4293, "omega":1.17177, "ax":3.16487, "ay":5.46254, "alpha":2.67324, "fx":[47.28002,33.14497,59.27983,67.2537], "fy":[92.98378,98.83715,85.68262,79.70513]}, + {"t":2.09225, "x":1.86278, "y":2.36944, "heading":-0.93065, "vx":2.17088, "vy":1.62296, "omega":1.26654, "ax":3.08909, "ay":5.50385, "alpha":2.63889, "fx":[45.6266,32.17267,58.47836,65.72536], "fy":[93.75407,99.09544,86.15382,80.90667]}, + {"t":2.1277, "x":1.94168, "y":2.43044, "heading":-0.88575, "vx":2.2804, "vy":1.81809, "omega":1.3601, "ax":2.98056, "ay":5.56081, "alpha":2.58713, "fx":[43.48995,30.75668,57.02978,63.62977], "fy":[94.69065,99.45635,87.01117,82.47629]}, + {"t":2.16316, "x":2.0244, "y":2.49839, "heading":-0.83753, "vx":2.38607, "vy":2.01524, "omega":1.45182, "ax":2.81259, "ay":5.64387, "alpha":2.50358, "fx":[40.51639,28.51146,54.37171,60.52273], "fy":[95.88933,99.99166,88.53566,84.64961]}, + {"t":2.19861, "x":2.11076, "y":2.57339, "heading":-0.78605, "vx":2.48578, "vy":2.21533, "omega":1.54058, "ax":2.51936, "ay":5.77447, "alpha":2.3542, "fx":[35.82773,24.50649,49.13224,55.28054], "fy":[97.54991,100.83037,91.28519,87.94067]}, + {"t":2.23406, "x":2.20047, "y":2.65556, "heading":-0.73144, "vx":2.5751, "vy":2.42005, "omega":1.62404, "ax":1.88837, "ay":5.99736, "alpha":2.03386, "fx":[26.54367,15.89424,36.9684,44.07839], "fy":[100.09044,102.11732,96.35873,93.61511]}, + {"t":2.26951, "x":2.29296, "y":2.74512, "heading":-0.67386, "vx":2.64205, "vy":2.63268, "omega":1.69615, "ax":-0.17308, "ay":6.24145, "alpha":1.00936, "fx":[-3.36033,-10.56421,-2.21072,4.81687], "fy":[102.33259,101.66671,102.01018,102.13357]}, + {"t":2.30497, "x":2.38652, "y":2.84238, "heading":-0.61372, "vx":2.63591, "vy":2.85395, "omega":1.73193, "ax":-6.00064, "ay":1.38712, "alpha":-0.72273, "fx":[-99.11695,-97.45771,-97.03907,-98.78214], "fy":[17.72102,24.82487,27.42119,20.74001]}, + {"t":2.34042, "x":2.4762, "y":2.94444, "heading":-0.55232, "vx":2.42317, "vy":2.90313, "omega":1.70631, "ax":-5.27161, "ay":-3.34854, "alpha":-2.40575, "fx":[-77.08044,-82.59443,-95.33171,-89.71663], "fy":[-68.41685,-60.90298,-38.66561,-50.98349]}, + {"t":2.37587, "x":2.55879, "y":3.04526, "heading":-0.49183, "vx":2.23628, "vy":2.78442, "omega":1.62102, "ax":-4.63044, "ay":-4.2339, "alpha":-2.68916, "fx":[-64.04996,-68.69181,-88.60677,-81.4467], "fy":[-81.58908,-77.33258,-53.6178,-64.32534]}, + {"t":2.41133, "x":2.63517, "y":3.14131, "heading":-0.43436, "vx":2.07211, "vy":2.63431, "omega":1.52568, "ax":-4.33278, "ay":-4.55551, "alpha":-2.78703, "fx":[-58.03426,-62.76436,-85.29736,-77.23451], "fy":[-86.28729,-82.64571,-59.27309,-69.68964]}, + {"t":2.44678, "x":2.70591, "y":3.23184, "heading":-0.38027, "vx":1.9185, "vy":2.4728, "omega":1.42687, "ax":-4.16548, "ay":-4.71844, "alpha":-2.83171, "fx":[-54.47062,-59.8882,-83.46713,-74.5647], "fy":[-88.74181,-84.978,-62.10438,-72.72595]}, + {"t":2.48223, "x":2.7713, "y":3.31655, "heading":-0.32968, "vx":1.77083, "vy":2.30552, "omega":1.32648, "ax":-4.05904, "ay":-4.81635, "alpha":-2.85449, "fx":[-52.04268,-58.41562,-82.33969,-72.63258], "fy":[-90.28505,-86.13519,-63.76377,-74.76852]}, + {"t":2.51769, "x":2.83153, "y":3.39526, "heading":-0.28265, "vx":1.62692, "vy":2.13477, "omega":1.22528, "ax":-3.98562, "ay":-4.88154, "alpha":-2.86651, "fx":[-50.24636,-57.68062,-81.5834,-71.11857], "fy":[-91.3625,-86.72291,-64.8446,-76.28526]}, + {"t":2.55314, "x":2.88671, "y":3.46787, "heading":-0.23921, "vx":1.48562, "vy":1.9617, "omega":1.12365, "ax":-3.93202, "ay":-4.92799, "alpha":-2.87286, "fx":[-48.84765,-57.36399,-81.03929,-69.87325], "fy":[-92.16484,-87.00069,-65.60656,-77.48102]}, + {"t":2.58859, "x":2.93691, "y":3.53432, "heading":-0.19938, "vx":1.34621, "vy":1.78699, "omega":1.0218, "ax":-3.89124, "ay":-4.96273, "alpha":-2.8762, "fx":[-47.72235,-57.29262,-80.62425,-68.8184], "fy":[-92.78779,-87.09942,-66.17838,-78.45935]}, + {"t":2.62405, "x":2.98219, "y":3.59456, "heading":-0.16315, "vx":1.20826, "vy":1.61104, "omega":0.91983, "ax":-3.85921, "ay":-4.98966, "alpha":-2.87802, "fx":[-46.79757,-57.36376,-80.29212,-67.90955], "fy":[-93.28513,-87.09335,-66.62941,-79.27817]}, + {"t":2.6595, "x":3.0226, "y":3.64854, "heading":-0.13054, "vx":1.07144, "vy":1.43415, "omega":0.81779, "ax":-3.8334, "ay":-5.01113, "alpha":-2.87918, "fx":[-46.02707,-57.51218,-80.01615,-67.1198], "fy":[-93.68978,-87.02841,-66.99916,-79.97246]}, + {"t":2.69495, "x":3.05818, "y":3.69624, "heading":-0.10155, "vx":0.93553, "vy":1.25649, "omega":0.71572, "ax":-3.81216, "ay":-5.02863, "alpha":-2.88015, "fx":[-45.37963,-57.6944,-79.7804,-66.43206], "fy":[-94.02324,-86.93504,-67.31109,-80.56464]}, + {"t":2.7304, "x":3.08895, "y":3.73762, "heading":-0.07617, "vx":0.80038, "vy":1.0782, "omega":0.61361, "ax":-3.79438, "ay":-5.04316, "alpha":-2.88118, "fx":[-44.83309,-57.88033,-79.57516,-65.83502], "fy":[-94.30029,-86.83436,-67.57955,-81.06982]}, + {"t":2.76586, "x":3.11494, "y":3.77268, "heading":-0.05442, "vx":0.66586, "vy":0.89941, "omega":0.51146, "ax":-3.77926, "ay":-5.05541, "alpha":-2.88236, "fx":[-44.37109,-58.04855,-79.39443,-65.32093], "fy":[-94.53151,-86.74164,-67.81356,-81.49856]}, + {"t":2.80131, "x":3.13617, "y":3.80139, "heading":-0.03628, "vx":0.53187, "vy":0.72018, "omega":0.40927, "ax":-3.76624, "ay":-5.06589, "alpha":-2.88371, "fx":[-43.98124,-58.1835,-79.23443,-64.88431], "fy":[-94.72472,-86.66816,-68.01893,-81.85851]}, + {"t":2.83676, "x":3.15266, "y":3.82374, "heading":-0.02177, "vx":0.39834, "vy":0.54058, "omega":0.30704, "ax":-3.75489, "ay":-5.07495, "alpha":-2.88519, "fx":[-43.65397,-58.27364,-79.09268,-64.52116], "fy":[-94.88578,-86.6224,-68.19955,-82.15534]}, + {"t":2.87222, "x":3.16442, "y":3.83971, "heading":-0.01089, "vx":0.26522, "vy":0.36066, "omega":0.20475, "ax":-3.7449, "ay":-5.08288, "alpha":-2.88677, "fx":[-43.38189,-58.3103,-78.96744,-64.2285], "fy":[-95.01914,-86.61078,-68.35824,-82.39332]}, + {"t":2.90767, "x":3.17147, "y":3.8493, "heading":-0.00363, "vx":0.13245, "vy":0.18045, "omega":0.1024, "ax":-3.73602, "ay":-5.08988, "alpha":-2.88841, "fx":[-43.15928,-58.28687,-78.85734,-64.00399], "fy":[-95.12818,-86.63812,-68.49717,-82.57574]}, + {"t":2.94312, "x":3.17382, "y":3.8525, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index abce1bc2..1885b188 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -335,7 +335,11 @@ public Command CMtoGH() { // algae path routine // run first path .active() - .whileTrue(Commands.sequence(steps.get("CMtoG").resetOdometry(), steps.get("CMtoG").cmd())); + .whileTrue( + Commands.sequence( + steps.get("CMtoG").resetOdometry(), + Commands.waitSeconds(1.5), + steps.get("CMtoG").cmd())); routine .observe(steps.get("CMtoG").done()) // TODO change to time based @@ -360,6 +364,68 @@ public Command CMtoGH() { // algae path Commands.sequence( intakeAlgaeInAuto(() -> steps.get("NItoIJ").getFinalPose()), swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2))); + + // routine + // .observe( + // steps + // .get("IJtoNI") + // .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2)) // TODO + // tune + // .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd())); + // routine + // .observe(steps.get("NItoEF").done()) + // .onTrue(Commands.sequence(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose()))); + + // --------------------------- + // Commands.sequence( + // // AutoAim.translateToXCoord( + // // swerve, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? AutoAim.BLUE_NET_X + // // : AutoAim.RED_NET_X, + // // () -> 0, + // // () -> + // // DriverStation.getAlliance().get() == Alliance.Blue + // // ? Rotation2d.k180deg + // // : Rotation2d.kZero.plus(Rotation2d.fromDegrees(20.0))) + // AutoAim.translateToPose(swerve, + // () -> new Pose2d(DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // steps.get("GhtoNI").)) + // .until( + // () -> + // (MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? AutoAim.BLUE_NET_X + // : AutoAim.RED_NET_X, + // swerve.getPose().getX(), + // Units.inchesToMeters(5)) + // && MathUtil.isNear( + // DriverStation.getAlliance().get() == Alliance.Blue + // ? Rotation2d.k180deg + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees() + // : Rotation2d.kZero + // .plus(Rotation2d.fromDegrees(20.0)) + // .getDegrees(), + // swerve.getPose().getRotation().getDegrees(), + // 5.0))), + // scoreAlgaeInAuto()) + // ) + + // routine.observe(steps.get("NItoIJ").done()).; //TODO cancel into autoalign + + // for (int i = 0; i < stops.length - 2; i++) { + // String startPos = stops[i]; + // String endPos = stops[i + 1]; + // String nextPos = stops[i + 2]; + // runAlgaePath(routine, startPos, endPos, nextPos, steps); + // } + // routine + // .observe(steps.get("NItoEF").done()) + // .onTrue(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose())); return routine.cmd(); } @@ -370,6 +436,9 @@ public Command LOtoA() { // 2910 // lo a b4 b2 dealgae steps.put("LOtoA", routine.trajectory("LOtoA")); steps.put("AtoB", routine.trajectory("AtoB")); + steps.put("BtoB", routine.trajectory("BtoB")); + + if (Robot.isSimulation()) manipulator.setSecondBeambreak(true); // gah routine // run first path .active() @@ -381,12 +450,41 @@ public Command LOtoA() { // 2910 .onTrue( Commands.sequence( scoreCoralInAuto(() -> steps.get("LOtoA").getFinalPose().get()), - Commands.runOnce(() -> autoGroundCoralIntake = true), - swerve - .driveTeleop(() -> new ChassisSpeeds(-0.3, 0, 0)) + AutoAim.translateToPose(swerve, () -> steps.get("AtoB").getInitialPose().get()) .until( - () -> elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)), + () -> + elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + && AutoAim.isInTolerance( + swerve.getPose(), steps.get("AtoB").getInitialPose().get())), + Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2)), steps.get("AtoB").cmd())); + routine + .observe( + steps + .get("AtoB") + .active() + .and(() -> manipulator.getSecondBeambreak() || manipulator.getFirstBeambreak())) + .onTrue( + Commands.runOnce( + () -> { + autoGroundCoralIntake = false; + Robot.setCurrentTarget(ReefTarget.L4); + })); + + routine + .observe( + steps.get("AtoB").atTime(steps.get("AtoB").getRawTrajectory().getTotalTime() - 0.3)) + .onTrue( + Commands.sequence( + scoreCoralInAuto(() -> steps.get("AtoB").getFinalPose().get()), + AutoAim.translateToPose(swerve, () -> steps.get("BtoB").getInitialPose().get()) + .until( + () -> + elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + && AutoAim.isInTolerance( + swerve.getPose(), steps.get("BtoB").getInitialPose().get())), + Commands.runOnce(() -> autoGroundCoralIntake = true), + steps.get("BtoB").cmd())); // routine // .observe(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) @@ -443,7 +541,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { 0.0, swerve.getVelocityRobotRelative().omegaRadiansPerSecond, 3.0)) - .debounce(0.06)), + .debounce(0.06 * 2)), Commands.print("Scoring!"), Commands.runOnce( () -> { @@ -517,6 +615,13 @@ public void bindCoralElevatorExtension(AutoRoutine routine, double toleranceMete .whileFalse(Commands.run(() -> autoPreScore = false)); } + public void bindGroundCoralElevatorExtension( + AutoRoutine routine, double toleranceMeters, Trigger trigger) { + routine.observe(trigger).debounce(3).onTrue(Commands.run(() -> autoGroundCoralIntake = true)); + } + + // ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ ([[[){ + public void bindAlgaeElevatorExtension(AutoRoutine routine, double toleranceMeters) { routine .observe( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5467006d..cd62681c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -188,9 +188,9 @@ public static enum AlgaeScoreTarget { PROCESSOR } - private static ReefTarget currentTarget = ReefTarget.L4; - private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; - private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; + @AutoLogOutput private static ReefTarget currentTarget = ReefTarget.L4; + @AutoLogOutput private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; + @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; private boolean leftHandedTarget = false; @AutoLogOutput private boolean killVisionIK = true; @@ -327,8 +327,8 @@ public static enum AlgaeScoreTarget { new SimpleMotorFeedforward(0.0, 0.7), new ProfiledPIDController( 0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(15, 1))), - new BeambreakIOReal(1, true), - new BeambreakIOReal(0, true)); + new BeambreakIOReal(0, true), + new BeambreakIOReal(1, true)); private final ShoulderSubsystem shoulder = new ShoulderSubsystem( @@ -492,6 +492,7 @@ public static enum AlgaeScoreTarget { .debounce(0.5) .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)), driver.a(), + driver.b(), driver.start(), operator.rightBumper(), operator.leftBumper(), @@ -1007,7 +1008,7 @@ public Robot() { > Math.abs(swerve.getPose().getX() - AutoAim.RED_NET_X) ? Rotation2d.kZero : Rotation2d.k180deg) - .plus(Rotation2d.fromDegrees(30.0))), + .plus(Rotation2d.fromDegrees(20.0))), Commands.waitUntil( () -> { final var diff = diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index c9f6d3ce..cfebf6c7 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -50,7 +50,7 @@ public class ExtensionKinematics { Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(115.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java index b69d7435..b445fc03 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ b/src/main/java/frc/robot/subsystems/ExtensionPathing.java @@ -9,6 +9,7 @@ import com.google.common.graph.MutableGraph; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import frc.robot.subsystems.ExtensionKinematics.ExtensionState; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.shoulder.ShoulderSubsystem; @@ -56,6 +57,9 @@ public class ExtensionPathing { graph.addNode(l3Tucked); graph.putEdge(tucked, l3Tucked); graph.putEdge(l3Tucked, l2Tucked); + final var l3 = ExtensionKinematics.L3_EXTENSION; + graph.addNode(l3); + graph.putEdge(l3, l3Tucked); final var l4Tucked = new ExtensionState( ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), @@ -124,7 +128,7 @@ public class ExtensionPathing { new ExtensionState( ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); graph.addNode(algaeLow); graph.putEdge(betweenTucked, algaeLow); @@ -132,13 +136,22 @@ public class ExtensionPathing { new ExtensionState( ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS); + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); graph.addNode(algaeHigh); graph.putEdge(betweenTucked, algaeHigh); graph.putEdge(algaeLow, algaeHigh); graph.putEdge(l4Tucked, algaeHigh); graph.putEdge(l4Tucked, algaeLow); + + final var algaeHalfTucked = + new ExtensionState( + Units.inchesToMeters(27.6), + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, + WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); + graph.addNode(algaeHalfTucked); + graph.putEdge(algaeHalfTucked, algaeLow); + graph.putEdge(algaeHalfTucked, algaeHigh); } private ExtensionPathing() {} diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 473a824b..a0a9ede8 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; @@ -48,7 +49,8 @@ public static enum SuperState { PRE_L3, PRE_L4, SCORE_CORAL, - ANTI_JAM, + ANTI_CORAL_JAM, + ANTI_ALGAE_JAM, INTAKE_ALGAE_GROUND, INTAKE_ALGAE_HIGH, INTAKE_ALGAE_LOW, @@ -91,8 +93,11 @@ public static enum SuperState { @AutoLogOutput(key = "Superstructure/Climb Cancel Request") private final Trigger climbCancelReq; - @AutoLogOutput(key = "Superstructure/Anti Jam Request") - private final Trigger antiJamReq; + @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") + private final Trigger antiCoralJamReq; + + @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") + private final Trigger antiAlgaeJamReq; @AutoLogOutput(key = "Superstructure/Home Request") private final Trigger homeReq; @@ -145,7 +150,8 @@ public Superstructure( Trigger climbReq, Trigger climbConfReq, Trigger climbCancelReq, - Trigger antiJamReq, + Trigger antiCoralJamReq, + Trigger antiAlgaeJamReq, Trigger homeReq, Trigger revFunnelReq, Trigger forceFunnelReq, @@ -175,7 +181,8 @@ public Superstructure( this.climbConfReq = climbConfReq; this.climbCancelReq = climbCancelReq; - this.antiJamReq = antiJamReq; + this.antiCoralJamReq = antiCoralJamReq; + this.antiAlgaeJamReq = antiAlgaeJamReq; this.homeReq = homeReq; @@ -337,8 +344,13 @@ private void configureStateTransitionCommands() { .and(() -> manipulator.getSecondBeambreak() && manipulator.getFirstBeambreak()) .and(intakeCoralReq.negate()) .debounce(0.060) - .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.792))) - .onTrue(this.forceState(SuperState.READY_CORAL)); + .onTrue( + Commands.parallel( + manipulator.setVoltage(0.0), + Commands.waitSeconds(0.12) + .andThen( + Commands.runOnce(() -> manipulator.resetPosition(0.792)), + this.forceState(SuperState.READY_CORAL)))); stateTriggers .get(SuperState.INTAKE_CORAL_GROUND) @@ -441,7 +453,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L1_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())); + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())); stateTriggers .get(SuperState.PRE_L1) @@ -475,7 +487,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -494,7 +506,7 @@ private void configureStateTransitionCommands() { : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) + manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -622,19 +634,35 @@ private void configureStateTransitionCommands() { ? SuperState.INTAKE_ALGAE_HIGH : SuperState.INTAKE_ALGAE_LOW)); - antiJamReq - .onTrue(this.forceState(SuperState.ANTI_JAM)) + antiCoralJamReq + .onTrue(this.forceState(SuperState.ANTI_CORAL_JAM)) + .onFalse(this.forceState(SuperState.IDLE)); + antiAlgaeJamReq + .onTrue(this.forceState(SuperState.ANTI_ALGAE_JAM)) .onFalse(this.forceState(SuperState.IDLE)); - // ANTI_JAM logic stateTriggers - .get(SuperState.ANTI_JAM) + .get(SuperState.ANTI_CORAL_JAM) .whileTrue(elevator.hold()) .whileTrue(wrist.hold()) .whileTrue(shoulder.hold()) .whileTrue(manipulator.setVelocity(-10)) .whileTrue(funnel.setVoltage(-10.0)); + stateTriggers + .get(SuperState.ANTI_ALGAE_JAM) + .whileTrue( + Commands.parallel( + elevator.hold(), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND)) + .until(() -> wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) + .andThen( + Commands.parallel( + wrist.hold(), + shoulder.hold(), + elevator.setExtension(Units.inchesToMeters(40)).andThen(elevator.hold())))); + stateTriggers .get(SuperState.CHECK_ALGAE) .and(() -> stateTimer.hasElapsed(1.0)) @@ -685,9 +713,17 @@ private void configureStateTransitionCommands() { .get(SuperState.INTAKE_ALGAE_HIGH) .whileTrue( this.extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) + new ExtensionState( + 0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS)) + .until( + () -> + shoulder.isNearAngle(Rotation2d.fromDegrees(35.0)) + && wrist.isNearAngle(WristSubsystem.WRIST_CLEARANCE_POS)) + .andThen( + this.extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS))) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); stateTriggers diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index eb1ef49f..691a94de 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -14,6 +14,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; public class ClimberIOReal implements ClimberIO { private final TalonFX motor = new TalonFX(18, "*"); @@ -60,8 +61,15 @@ public ClimberIOReal() { @Override public void updateInputs(ClimberIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll( - angularVelocityRPS, temp, supplyCurrentAmps, statorCurrentAmps, position, appliedVoltage); + Logger.recordOutput( + "Climber refreshall statuscode", + BaseStatusSignal.refreshAll( + angularVelocityRPS, + temp, + supplyCurrentAmps, + statorCurrentAmps, + position, + appliedVoltage)); inputs.position = position.getValueAsDouble(); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index accda5f0..b8a6d8af 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -20,6 +20,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; /** Elevator IO using TalonFXs. */ public class ElevatorIOReal implements ElevatorIO { @@ -107,7 +108,10 @@ public ElevatorIOReal() { @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(position, velocity, voltage, statorCurrent, supplyCurrent, temp); + Logger.recordOutput( + "Elevator refreshall statuscode", + BaseStatusSignal.refreshAll( + position, velocity, voltage, statorCurrent, supplyCurrent, temp)); inputs.positionMeters = position.getValueAsDouble(); inputs.velocityMetersPerSec = velocity.getValueAsDouble(); inputs.appliedVolts = voltage.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 17e63b96..2b9e6d34 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -51,8 +51,8 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double L4_EXTENSION_METERS = ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); - public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14; - public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(11.5); + public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14 - Units.inchesToMeters(0.75); + public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(9.0); public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java index d9f289f3..0f917ba7 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java @@ -30,6 +30,7 @@ import edu.wpi.first.units.measure.Voltage; import java.util.Optional; import java.util.function.Consumer; +import org.littletonrobotics.junction.Logger; public class RollerIOReal implements RollerIO { private final TalonFX motor; @@ -84,7 +85,10 @@ public static TalonFXConfiguration getDefaultConfig() { @Override public void updateInputs(RollerIOInputsAutoLogged inputs) { - BaseStatusSignal.refreshAll(velocity, voltage, statorCurrent, supplyCurrent, temp, position); + Logger.recordOutput( + "Roller" + motor.getDeviceID() + "refreshall statuscode", + BaseStatusSignal.refreshAll( + velocity, voltage, statorCurrent, supplyCurrent, temp, position)); inputs.velocityRotationsPerSec = velocity.getValue().in(RotationsPerSecond); inputs.appliedVolts = voltage.getValue().in(Volts); inputs.statorCurrentAmps = statorCurrent.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index ade79204..54677024 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -20,6 +20,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; public class ShoulderIOReal implements ShoulderIO { private final TalonFX motor; @@ -80,7 +81,7 @@ public ShoulderIOReal() { final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - cancoderConfig.MagnetSensor.MagnetOffset = -0.385174; + cancoderConfig.MagnetSensor.MagnetOffset = -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.9; cancoder.getConfigurator().apply(cancoderConfig); @@ -102,14 +103,16 @@ public ShoulderIOReal() { @Override public void updateInputs(ShoulderIOInputs inputs) { - BaseStatusSignal.refreshAll( - angularVelocityRPS, - temp, - supplyCurrentAmps, - statorCurrentAmps, - motorPositionRotations, - cancoderPositionRotations, - appliedVoltage); + Logger.recordOutput( + "shoulder refreshall statuscode", + BaseStatusSignal.refreshAll( + angularVelocityRPS, + temp, + supplyCurrentAmps, + statorCurrentAmps, + motorPositionRotations, + cancoderPositionRotations, + appliedVoltage)); inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.cancoderPosition = cancoderPositionRotations.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index a157f09e..00977b67 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -34,7 +34,9 @@ public class ShoulderSubsystem extends SubsystemBase { public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = - Rotation2d.fromRadians(0.505).plus(Rotation2d.fromDegrees(-5.0)); + Rotation2d.fromRadians(0.505) + .plus(Rotation2d.fromDegrees(-5.0)) + .minus(Rotation2d.fromDegrees(2)); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java index 2d3cd0cb..dfbdfe64 100644 --- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java +++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java @@ -28,6 +28,7 @@ import frc.robot.subsystems.swerve.PhoenixOdometryThread.Registration; import frc.robot.subsystems.swerve.PhoenixOdometryThread.SignalType; import java.util.Optional; +import org.littletonrobotics.junction.Logger; /** * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and @@ -139,20 +140,22 @@ public ModuleIOReal(ModuleConstants moduleConstants, SwerveConstants swerveConst @Override public void updateInputs(final ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - driveSupplyCurrent, - driveTempC, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnStatorCurrent, - turnSupplyCurrent, - turnTempC); + Logger.recordOutput( + "module" + constants.prefix() + "refreshall statuscode", + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + driveSupplyCurrent, + driveTempC, + turnAbsolutePosition, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnStatorCurrent, + turnSupplyCurrent, + turnTempC)); inputs.prefix = constants.prefix(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 32500415..37a6afcc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -93,6 +94,8 @@ public class SwerveSubsystem extends SubsystemBase { private Alert usingSyncOdometryAlert = new Alert("Using Sync Odometry", AlertType.kInfo); private Alert missingModuleData = new Alert("Missing Module Data", AlertType.kError); private Alert missingGyroData = new Alert("Missing Gyro Data", AlertType.kWarning); + private Alert futureVisionData = + new Alert("Vision Data Coming from ✨The Future✨", AlertType.kError); private boolean useModuleForceFF = !Robot.isSimulation(); @@ -331,6 +334,7 @@ private void updateOdometry() { private void updateVision() { var i = 0; hasFrontTags = false; + var hasFutureData = false; for (var camera : cameras) { if ((camera.getName().equals("Front_Left_Camera") || camera.getName().equals("Front_Right_Camera")) @@ -411,6 +415,7 @@ private void updateVision() { // the sussifier }); lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; + hasFutureData |= camera.inputs.captureTimestampMicros > RobotController.getTime(); // if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Good Update"); cameraPoses[i] = visionPose; @@ -448,6 +453,7 @@ private void updateVision() { } if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/Camera Poses on Robot", arr); + futureVisionData.set(hasFutureData); } /** diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index 521dd93e..639a4131 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -20,6 +20,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.littletonrobotics.junction.Logger; public class WristIOReal implements WristIO { private final TalonFX motor; @@ -62,13 +63,15 @@ public WristIOReal(final int motorId, final TalonFXConfiguration config) { @Override public void updateInputs(ArmIOInputs inputs) { - BaseStatusSignal.refreshAll( - angularVelocityRPS, - temp, - supplyCurrentAmps, - statorCurrentAmps, - motorPositionRotations, - appliedVoltage); + Logger.recordOutput( + "wrist refreshall statuscode", + BaseStatusSignal.refreshAll( + angularVelocityRPS, + temp, + supplyCurrentAmps, + statorCurrentAmps, + motorPositionRotations, + appliedVoltage)); inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index 5d52fc45..aaec10d3 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -144,7 +144,13 @@ public Command currentZero(Supplier shoulderInputs) System.out.println("Wrist Zeroing"); }), this.run(() -> io.setMotorVoltage(-1.0)) - .until(() -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 15.0), + .raceWith( + Commands.waitSeconds(0.5) + .andThen( + Commands.waitUntil( + () -> + Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) + > 7.0))), this.runOnce( () -> { hasZeroed = true;