diff --git a/src/main/deploy/choreo/G_Right.traj b/src/main/deploy/choreo/G_Right.traj new file mode 100644 index 00000000..9183db27 --- /dev/null +++ b/src/main/deploy/choreo/G_Right.traj @@ -0,0 +1,177 @@ +{ + "name":"G_Right", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":5.772265434265137, "y":3.775489447021484, "heading":3.141592653589793, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.235852241516113, "y":3.2450315952301025, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.715206623077393, "y":1.373961329460144, "heading":0.0, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.729013442993164, "y":0.8370455503463745, "heading":0.9332802447333852, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.540279984474182, "y":0.7697955369949341, "heading":0.9332802447333852, "intervals":9, "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":0, "to":0, "data":{"type":"MaxVelocity", "props":{"max":1.75}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":4.0}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.6}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"G.x", "val":5.772265434265137}, "y":{"exp":"G.y", "val":3.775489447021484}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.235852241516113 m", "val":6.235852241516113}, "y":{"exp":"3.2450315952301025 m", "val":3.2450315952301025}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.715206623077393 m", "val":5.715206623077393}, "y":{"exp":"1.373961329460144 m", "val":1.373961329460144}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":55, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.729013442993164 m", "val":1.729013442993164}, "y":{"exp":"0.8370455503463745 m", "val":0.8370455503463745}, "heading":{"exp":"HPRight.heading", "val":0.9332802447333852}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"HPRight.x", "val":1.540279984474182}, "y":{"exp":"HPRight.y", "val":0.7697955369949341}, "heading":{"exp":"HPRight.heading", "val":0.9332802447333852}, "intervals":9, "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":0, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"reefSpeed", "val":1.75}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"maxAccel", "val":4.0}}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"movingSpeed", "val":2.6}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"coralSpeed", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.61816,1.44703,3.13994,3.46641], + "samples":[ + {"t":0.0, "x":5.77227, "y":3.77549, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.51295, "ay":-1.9054, "alpha":-1.97146, "fx":[54.4927,49.65644,57.48439,61.4492], "fy":[-25.61929,-39.23142,-34.34581,-21.80188]}, + {"t":0.02289, "x":5.77319, "y":3.77499, "heading":3.14159, "vx":0.08043, "vy":-0.04362, "omega":-0.04514, "ax":3.4739, "ay":-1.97916, "alpha":-1.8805, "fx":[53.86825,49.19456,56.81475,60.72501], "fy":[-27.18774,-39.91061,-35.21805,-23.36617]}, + {"t":0.04579, "x":5.77594, "y":3.77347, "heading":3.14056, "vx":0.15996, "vy":-0.08894, "omega":-0.08819, "ax":3.42773, "ay":-2.05791, "alpha":-1.80052, "fx":[53.13611,48.58259,56.04064,59.91171], "fy":[-28.7849,-40.72579,-36.19436,-24.97816]}, + {"t":0.06868, "x":5.7805, "y":3.7709, "heading":3.13854, "vx":0.23844, "vy":-0.13605, "omega":-0.12941, "ax":3.37504, "ay":-2.14306, "alpha":-1.72632, "fx":[52.30626,47.83874,55.16176,59.01804], "fy":[-30.46366,-41.66396,-37.28867,-26.67415]}, + {"t":0.09158, "x":5.78684, "y":3.76722, "heading":3.13558, "vx":0.31571, "vy":-0.18512, "omega":-0.16894, "ax":3.3146, "ay":-2.23521, "alpha":-1.64962, "fx":[51.34735,46.99229,54.16076,57.9867], "fy":[-32.23673,-42.68936,-38.50588,-28.51026]}, + {"t":0.11447, "x":5.79494, "y":3.7624, "heading":3.13171, "vx":0.3916, "vy":-0.23629, "omega":-0.2067, "ax":3.24496, "ay":-2.33497, "alpha":-1.57712, "fx":[50.22944,45.97832,53.02512,56.8318], "fy":[-34.13407,-43.84505,-39.8437,-30.45448]}, + {"t":0.13737, "x":5.80475, "y":3.75637, "heading":3.12698, "vx":0.46589, "vy":-0.28975, "omega":-0.24281, "ax":3.16433, "ay":-2.44293, "alpha":-1.50563, "fx":[48.92544,44.79405,51.72613,55.49862], "fy":[-36.15228,-45.13145,-41.31415,-32.53533]}, + {"t":0.16026, "x":5.81625, "y":3.7491, "heading":3.12142, "vx":0.53834, "vy":-0.34568, "omega":-0.27728, "ax":3.07054, "ay":-2.55961, "alpha":-1.43517, "fx":[47.40624,43.39093,50.23285,53.95829], "fy":[-38.29086,-46.5491,-42.92415,-34.77885]}, + {"t":0.18316, "x":5.82938, "y":3.74052, "heading":3.11507, "vx":0.60863, "vy":-0.40428, "omega":-0.31014, "ax":2.96098, "ay":-2.68538, "alpha":-1.36861, "fx":[45.64712,41.72977,48.48477,52.16938], "fy":[-40.5312,-48.12264,-44.70838,-37.16752]}, + {"t":0.20605, "x":5.84409, "y":3.73056, "heading":3.10797, "vx":0.67643, "vy":-0.46576, "omega":-0.34147, "ax":2.83253, "ay":-2.82032, "alpha":-1.29789, "fx":[43.5265,39.80581,46.4718,50.07006], "fy":[-42.95932,-49.79524,-46.61817,-39.72608]}, + {"t":0.22895, "x":5.86032, "y":3.71915, "heading":3.10015, "vx":0.74128, "vy":-0.53033, "omega":-0.37119, "ax":2.68153, "ay":-2.96404, "alpha":-1.23093, "fx":[41.08772,37.49472,44.11661,47.58588], "fy":[-45.44586,-51.63694,-48.68467,-42.458]}, + {"t":0.25184, "x":5.87799, "y":3.70623, "heading":3.09165, "vx":0.80267, "vy":-0.59819, "omega":-0.39937, "ax":2.50378, "ay":-3.11542, "alpha":-1.16692, "fx":[38.14868,34.79165,41.34886,44.70792], "fy":[-48.09307,-53.56932,-50.89264,-45.28346]}, + {"t":0.27474, "x":5.89703, "y":3.69172, "heading":3.08251, "vx":0.85999, "vy":-0.66952, "omega":-0.42609, "ax":2.2947, "ay":-3.27226, "alpha":-1.10175, "fx":[34.72503,31.61064,38.10123,41.28303], "fy":[-50.76544,-55.58369,-53.20948,-48.23921]}, + {"t":0.29763, "x":5.91732, "y":3.67554, "heading":3.07275, "vx":0.91253, "vy":-0.74444, "omega":-0.45131, "ax":2.04965, "ay":-3.43085, "alpha":-1.03671, "fx":[30.66759,27.90241,34.31826,37.2705], "fy":[-53.46996,-57.60015,-55.56509,-51.23376]}, + {"t":0.32053, "x":5.93875, "y":3.65759, "heading":3.06242, "vx":0.95945, "vy":-0.82299, "omega":-0.47505, "ax":1.76456, "ay":-3.58561, "alpha":-0.97642, "fx":[25.99728,23.55987,29.91276,32.58464], "fy":[-56.03675,-59.58353,-57.90311,-54.17367]}, + {"t":0.34342, "x":5.96118, "y":3.63781, "heading":3.05155, "vx":0.99985, "vy":-0.90508, "omega":-0.4974, "ax":1.43687, "ay":-3.72885, "alpha":-0.91881, "fx":[20.634,18.57674,24.83612,27.19851], "fy":[-58.38099,-61.38445,-60.10313,-56.92405]}, + {"t":0.36632, "x":5.98444, "y":3.61611, "heading":3.04016, "vx":1.03275, "vy":-0.99045, "omega":-0.51844, "ax":1.06685, "ay":-3.8509, "alpha":-0.86217, "fx":[14.62033,12.96175,19.09379,21.07242], "fy":[-60.32136,-62.87261,-62.01433,-59.33527]}, + {"t":0.38921, "x":6.00837, "y":3.59243, "heading":3.02829, "vx":1.05718, "vy":-1.07861, "omega":-0.53818, "ax":0.65893, "ay":-3.94112, "alpha":-0.80772, "fx":[8.00929,6.80126,12.72727,14.30603], "fy":[-61.71317,-63.88409,-63.47575,-61.19971]}, + {"t":0.4121, "x":6.03274, "y":3.5667, "heading":3.01597, "vx":1.07226, "vy":-1.16884, "omega":-0.55667, "ax":0.22248, "ay":-3.98954, "alpha":-0.75386, "fx":[1.00829,0.22405,5.88428,7.01165], "fy":[-62.369,-64.28761,-64.32131,-62.36983]}, + {"t":0.435, "x":6.05735, "y":3.53889, "heading":3.00322, "vx":1.07735, "vy":-1.26018, "omega":-0.57393, "ax":-0.22847, "ay":-3.98917, "alpha":-0.70149, "fx":[-6.17239,-6.55694,-1.24056,-0.53877], "fy":[-62.23058,-63.97544,-64.42661,-62.69166]}, + {"t":0.45789, "x":6.08196, "y":3.509, "heading":2.99008, "vx":1.07212, "vy":-1.35151, "omega":-0.58999, "ax":-0.67716, "ay":-3.93794, "alpha":-0.65126, "fx":[-13.2551,-13.2915,-8.38901,-8.06623], "fy":[-61.2664,-62.93257,-63.74258,-62.12909]}, + {"t":0.48079, "x":6.10633, "y":3.47702, "heading":2.97657, "vx":1.05662, "vy":-1.44167, "omega":-0.6049, "ax":-1.107, "ay":-3.83941, "alpha":-0.6009, "fx":[-19.95789,-19.75467,-15.29231,-15.29296], "fy":[-59.57547,-61.19696,-62.29957,-60.74183]}, + {"t":0.50368, "x":6.13023, "y":3.44301, "heading":2.96273, "vx":1.03128, "vy":-1.52958, "omega":-0.61866, "ax":-1.50448, "ay":-3.70189, "alpha":-0.55416, "fx":[-26.12401,-25.70302,-21.7301,-21.98207], "fy":[-57.27477,-58.92545,-60.22347,-58.65743]}, + {"t":0.52658, "x":6.15344, "y":3.40702, "heading":2.94856, "vx":0.99683, "vy":-1.61433, "omega":-0.63134, "ax":-1.86097, "ay":-3.53631, "alpha":-0.51077, "fx":[-31.58601,-31.07167,-27.55073,-27.96878], "fy":[-54.5742,-56.24555,-57.67746,-56.06904]}, + {"t":0.54947, "x":6.17578, "y":3.36913, "heading":2.93411, "vx":0.95422, "vy":-1.69529, "omega":-0.64304, "ax":-2.1729, "ay":-3.3539, "alpha":-0.46965, "fx":[-36.35248,-35.749,-32.6917,-33.19256], "fy":[-51.63066,-53.3469,-54.81668,-53.18801]}, + {"t":0.57237, "x":6.19706, "y":3.32944, "heading":2.91939, "vx":0.90448, "vy":-1.77208, "omega":-0.65379, "ax":-2.44085, "ay":-3.16447, "alpha":-0.42715, "fx":[-40.36596,-39.81671,-37.14565,-37.67308], "fy":[-48.65685,-50.31884,-51.80523,-50.17197]}, + {"t":0.59526, "x":6.21712, "y":3.28804, "heading":2.90442, "vx":0.84859, "vy":-1.84453, "omega":-0.66357, "ax":-2.66811, "ay":-2.97562, "alpha":-0.38986, "fx":[-43.79713,-43.21894,-40.95368,-41.4633], "fy":[-45.67318,-47.36174,-48.76558,-47.1598]}, + {"t":0.61816, "x":6.23585, "y":3.24503, "heading":2.88922, "vx":0.78751, "vy":-1.91265, "omega":-0.6725, "ax":-2.78877, "ay":-2.8577, "alpha":-0.34427, "fx":[-45.48742,-45.03874,-43.05476,-43.51404], "fy":[-43.95608,-45.43913,-46.78125,-45.29599]}, + {"t":0.63997, "x":6.25237, "y":3.20263, "heading":2.87456, "vx":0.72668, "vy":-1.97499, "omega":-0.68, "ax":-2.84239, "ay":-2.8091, "alpha":-0.31243, "fx":[-46.21822,-45.78862,-44.04311,-44.45033], "fy":[-43.27968,-44.68459,-45.89321,-44.52842]}, + {"t":0.66178, "x":6.26754, "y":3.15888, "heading":2.85972, "vx":0.66468, "vy":-2.03626, "omega":-0.68682, "ax":-2.89849, "ay":-2.75086, "alpha":-0.28288, "fx":[-46.96952,-46.6425,-45.05296,-45.39762], "fy":[-42.47907,-43.71268,-44.87187,-43.62377]}, + {"t":0.68359, "x":6.28135, "y":3.11381, "heading":2.84474, "vx":0.60146, "vy":-2.09626, "omega":-0.69299, "ax":-2.96014, "ay":-2.68403, "alpha":-0.2585, "fx":[-47.85969,-47.5434,-46.147,-46.42758], "fy":[-41.48227,-42.66385,-43.71286,-42.58495]}, + {"t":0.70541, "x":6.29376, "y":3.06745, "heading":2.82963, "vx":0.53689, "vy":-2.15481, "omega":-0.69863, "ax":-3.02803, "ay":-2.60677, "alpha":-0.23323, "fx":[-48.8172,-48.58111,-47.31581,-47.57485], "fy":[-40.36759,-41.40346,-42.41415,-41.35247]}, + {"t":0.72722, "x":6.30476, "y":3.01983, "heading":2.81439, "vx":0.47084, "vy":-2.21167, "omega":-0.70372, "ax":-3.1029, "ay":-2.5167, "alpha":-0.21452, "fx":[-49.94288,-49.68997,-48.59481,-48.81527], "fy":[-38.97327,-40.00581,-40.90864,-39.93012]}, + {"t":0.74903, "x":6.31429, "y":2.97099, "heading":2.79904, "vx":0.40316, "vy":-2.26656, "omega":-0.70839, "ax":-3.18546, "ay":-2.41076, "alpha":-0.19202, "fx":[-51.14188,-50.97634,-49.98797,-50.17963], "fy":[-37.4022,-38.28244,-39.16108,-38.24465]}, + {"t":0.77084, "x":6.32232, "y":2.92098, "heading":2.78359, "vx":0.33367, "vy":-2.31915, "omega":-0.71258, "ax":-3.27635, "ay":-2.28499, "alpha":-0.17838, "fx":[-52.53854,-52.3508,-51.50628,-51.66238], "fy":[-35.41784,-36.32668,-37.10468,-36.2544]}, + {"t":0.79266, "x":6.32882, "y":2.86985, "heading":2.76804, "vx":0.26221, "vy":-2.36899, "omega":-0.71647, "ax":-3.37594, "ay":-2.13425, "alpha":-0.16002, "fx":[-54.02317,-53.91481,-53.15478,-53.28933], "fy":[-33.12442,-33.89475,-34.66352,-33.84831]}, + {"t":0.81447, "x":6.33374, "y":2.81766, "heading":2.75241, "vx":0.18857, "vy":-2.41554, "omega":-0.71996, "ax":-3.48395, "ay":-1.95188, "alpha":-0.15147, "fx":[-55.7021,-55.57443,-54.93316,-55.03155], "fy":[-30.21825,-31.05067,-31.72513,-30.95626]}, + {"t":0.83628, "x":6.33702, "y":2.76451, "heading":2.73671, "vx":0.11258, "vy":-2.45812, "omega":-0.72327, "ax":-3.59889, "ay":-1.72945, "alpha":-0.13775, "fx":[-57.44588,-57.38708,-56.81401,-56.89291], "fy":[-26.76981,-27.49011,-28.16535,-27.39976]}, + {"t":0.85809, "x":6.33862, "y":2.71048, "heading":2.72093, "vx":0.03408, "vy":-2.49584, "omega":-0.72627, "ax":-3.71696, "ay":-1.45663, "alpha":-0.13388, "fx":[-59.28426,-59.22654,-58.7452,-58.78174], "fy":[-22.41232,-23.21491,-23.8086,-23.06452]}, + {"t":0.87991, "x":6.33848, "y":2.65569, "heading":2.70509, "vx":-0.047, "vy":-2.52762, "omega":-0.72919, "ax":-3.83047, "ay":-1.12206, "alpha":-0.12569, "fx":[-61.01909,-61.01926,-60.59989,-60.60789], "fy":[-17.15534,-17.90313,-18.48521,-17.71014]}, + {"t":0.90172, "x":6.33654, "y":2.60029, "heading":2.68919, "vx":-0.13055, "vy":-2.55209, "omega":-0.73194, "ax":-3.92579, "ay":-0.7164, "alpha":-0.12405, "fx":[-62.48228,-62.52436,-62.17133,-62.12096], "fy":[-10.7066,-11.51697,-12.0279,-11.24176]}, + {"t":0.92353, "x":6.33276, "y":2.54446, "heading":2.67322, "vx":-0.21618, "vy":-2.56772, "omega":-0.73464, "ax":-3.98244, "ay":-0.24206, "alpha":-0.12373, "fx":[-63.32168,-63.42862,-63.12694,-63.01943], "fy":[-3.17985,-4.0443,-4.49145,-3.65588]}, + {"t":0.94534, "x":6.3271, "y":2.48839, "heading":2.6572, "vx":-0.30305, "vy":-2.573, "omega":-0.73734, "ax":-3.98087, "ay":0.25489, "alpha":-0.1201, "fx":[-63.24534,-63.42617,-63.15773,-62.96783], "fy":[4.64037,3.84956,3.41376,4.28283]}, + {"t":0.96716, "x":6.31954, "y":2.43233, "heading":2.64111, "vx":-0.38988, "vy":-2.56744, "omega":-0.73996, "ax":-3.94157, "ay":0.60908, "alpha":-0.12879, "fx":[-62.57373,-62.83338,-62.5717,-62.32268], "fy":[10.33462,9.36215,9.04384,9.93777]}, + {"t":0.98897, "x":6.3101, "y":2.37647, "heading":2.62497, "vx":-0.47586, "vy":-2.55415, "omega":-0.74277, "ax":-3.90915, "ay":0.78775, "alpha":-0.12572, "fx":[-62.05199,-62.3283,-62.07925,-61.78268], "fy":[13.09853,12.26015,11.86941,12.79617]}, + {"t":1.01078, "x":6.29879, "y":2.32095, "heading":2.60877, "vx":-0.56113, "vy":-2.53697, "omega":-0.74551, "ax":-3.87857, "ay":0.92402, "alpha":-0.13437, "fx":[-61.53336,-61.86995,-61.61217,-61.28509], "fy":[15.33951,14.35619,14.02206,14.96002]}, + {"t":1.03259, "x":6.28563, "y":2.26583, "heading":2.59251, "vx":-0.64573, "vy":-2.51681, "omega":-0.74844, "ax":-3.84469, "ay":1.05398, "alpha":-0.13499, "fx":[-60.99169,-61.34415,-61.09461,-60.71858], "fy":[17.36231,16.45262,16.06773,17.04847]}, + {"t":1.05441, "x":6.27063, "y":2.21118, "heading":2.57618, "vx":-0.72959, "vy":-2.49382, "omega":-0.75139, "ax":-3.80668, "ay":1.18216, "alpha":-0.14198, "fx":[-60.35472,-60.76827,-60.50912,-60.10331], "fy":[19.45666,18.44307,18.09276,19.07809]}, + {"t":1.07622, "x":6.25381, "y":2.15707, "heading":2.5598, "vx":-0.81262, "vy":-2.46804, "omega":-0.75448, "ax":-3.7645, "ay":1.30891, "alpha":-0.14461, "fx":[-59.68089,-60.11558,-59.86206,-59.39803], "fy":[21.43964,20.47376,20.08509,21.12109]}, + {"t":1.09803, "x":6.23519, "y":2.10354, "heading":2.54334, "vx":-0.89473, "vy":-2.43949, "omega":-0.75764, "ax":-3.71819, "ay":1.43416, "alpha":-0.15143, "fx":[-58.90853,-59.41123,-59.14534,-58.65058], "fy":[23.48401,22.42461,22.06467,23.10025]}, + {"t":1.11984, "x":6.21479, "y":2.05067, "heading":2.52681, "vx":-0.97584, "vy":-2.4082, "omega":-0.76094, "ax":-3.66781, "ay":1.5578, "alpha":-0.1552, "fx":[-58.10552,-58.63168,-58.37123,-57.80836], "fy":[25.42029,24.40284,24.00489,25.09714]}, + {"t":1.14166, "x":6.19263, "y":1.99851, "heading":2.51021, "vx":-1.05584, "vy":-2.37422, "omega":-0.76433, "ax":-3.61344, "ay":1.67972, "alpha":-0.16125, "fx":[-57.20222,-57.80621,-57.5217,-56.93376], "fy":[27.40767,26.29785,25.94584,27.01573]}, + {"t":1.16347, "x":6.16874, "y":1.94713, "heading":2.49354, "vx":-1.13466, "vy":-2.33759, "omega":-0.76784, "ax":-3.55513, "ay":1.79979, "alpha":-0.16675, "fx":[-56.27296,-56.90148,-56.62932,-55.95714], "fy":[29.29103,28.22375,27.81611,28.96108]}, + {"t":1.18528, "x":6.14314, "y":1.89657, "heading":2.47679, "vx":-1.21221, "vy":-2.29833, "omega":-0.77148, "ax":-3.49293, "ay":1.9179, "alpha":-0.17486, "fx":[-55.23748,-55.95667,-55.66347,-54.95388], "fy":[31.22505,30.06257,29.68191,30.82285]}, + {"t":1.20709, "x":6.11587, "y":1.84689, "heading":2.45997, "vx":-1.2884, "vy":-2.25649, "omega":-0.77529, "ax":-3.42692, "ay":2.03394, "alpha":-0.18012, "fx":[-54.18825,-54.9345,-54.64634,-53.85014], "fy":[33.04144,31.92004,31.49999,32.69943]}, + {"t":1.22891, "x":6.08695, "y":1.79815, "heading":2.44305, "vx":-1.36314, "vy":-2.21213, "omega":-0.77922, "ax":-3.35713, "ay":2.14777, "alpha":-0.18892, "fx":[-53.02724,-53.87563,-53.56173,-52.72295], "fy":[34.90672,33.69266,33.29789,34.49256]}, + {"t":1.25072, "x":6.05642, "y":1.75041, "heading":2.42606, "vx":-1.43637, "vy":-2.16528, "omega":-0.78334, "ax":-3.28363, "ay":2.25929, "alpha":-0.19477, "fx":[-51.85924,-52.7369,-52.42827,-51.49553], "fy":[36.65315,35.48057,35.0446,36.29314]}, + {"t":1.27253, "x":6.02431, "y":1.70372, "heading":2.40897, "vx":-1.508, "vy":-2.116, "omega":-0.78759, "ax":-3.20647, "ay":2.36836, "alpha":-0.20443, "fx":[-50.57717,-51.56794,-51.22927,-50.24558], "fy":[38.44006,37.17988,36.76586,38.01208]}, + {"t":1.29434, "x":5.99065, "y":1.65813, "heading":2.39179, "vx":-1.57794, "vy":-2.06434, "omega":-0.79205, "ax":-3.1257, "ay":2.47486, "alpha":-0.21115, "fx":[-49.29525,-50.32132,-49.9803,-48.89455], "fy":[40.10713,38.88398,38.43724,39.73282]}, + {"t":1.31616, "x":5.95549, "y":1.61369, "heading":2.37452, "vx":-1.64612, "vy":-2.01036, "omega":-0.79666, "ax":-3.04141, "ay":2.57868, "alpha":-0.22183, "fx":[-47.89335,-49.04354,-48.67391,-47.52759], "fy":[41.81085,40.50684,40.07034,41.36544]}, + {"t":1.33797, "x":5.91886, "y":1.57045, "heading":2.35714, "vx":-1.71246, "vy":-1.95411, "omega":-0.8015, "ax":-2.95364, "ay":2.67968, "alpha":-0.22941, "fx":[-46.49569,-47.6871,-47.31852,-46.06371], "fy":[43.39735,42.12838,41.65214,42.98945]}, + {"t":1.35978, "x":5.88081, "y":1.52847, "heading":2.33966, "vx":-1.77688, "vy":-1.89566, "omega":-0.8065, "ax":-2.86244, "ay":2.77779, "alpha":-0.24126, "fx":[-44.98311,-46.31118,-45.90349,-44.57557], "fy":[45.00398,43.66026,43.19717,44.53638]}, + {"t":1.38159, "x":5.84137, "y":1.48778, "heading":2.32206, "vx":-1.83932, "vy":-1.83507, "omega":-0.81176, "ax":-2.76754, "ay":2.87321, "alpha":-0.24815, "fx":[-43.46374,-44.83928,-44.43245,-43.01175], "fy":[46.51054,45.20167,44.69616,46.04906]}, + {"t":1.40341, "x":5.80059, "y":1.44843, "heading":2.30436, "vx":-1.89969, "vy":-1.7724, "omega":-0.81717, "ax":-2.66672, "ay":2.96785, "alpha":-0.26191, "fx":[-41.81192,-43.32576,-42.86805,-41.33874], "fy":[48.04205,46.67752,46.18686,47.56078]}, + {"t":1.42522, "x":5.75852, "y":1.41048, "heading":2.28653, "vx":-1.95786, "vy":-1.70766, "omega":-0.82289, "ax":-2.54657, "ay":3.07233, "alpha":-0.27181, "fx":[-39.882,-41.46753,-41.00912,-39.356], "fy":[49.69622,48.36176,47.82563,49.21849]}, + {"t":1.44703, "x":5.71521, "y":1.37396, "heading":2.26858, "vx":-2.0134, "vy":-1.64065, "omega":-0.82882, "ax":-2.47916, "ay":3.1216, "alpha":-0.36064, "fx":[-38.59029,-40.73445,-40.16176,-37.94764], "fy":[50.78475,49.04438,48.30067,50.10083]}, + {"t":1.47838, "x":5.65087, "y":1.32406, "heading":2.2426, "vx":-2.09112, "vy":-1.54278, "omega":-0.84012, "ax":-2.29643, "ay":3.26531, "alpha":-0.32999, "fx":[-35.72707,-37.78395,-37.20225,-35.11704], "fy":[52.90774,51.39284,50.76321,52.29305]}, + {"t":1.50973, "x":5.58418, "y":1.2773, "heading":2.21626, "vx":-2.16312, "vy":-1.44042, "omega":-0.85047, "ax":-2.13328, "ay":3.37238, "alpha":-0.30168, "fx":[-33.18692,-35.11409,-34.56219,-32.60659], "fy":[54.47239,53.1736,52.59557,53.91442]}, + {"t":1.54108, "x":5.51532, "y":1.2338, "heading":2.1896, "vx":-2.23, "vy":-1.33469, "omega":-0.85993, "ax":-1.9671, "ay":3.46966, "alpha":-0.27824, "fx":[-30.5843,-32.41296,-31.88376,-30.03538], "fy":[55.90682,54.77907,54.25701,55.39041]}, + {"t":1.57243, "x":5.44444, "y":1.19366, "heading":2.16264, "vx":-2.29167, "vy":-1.22592, "omega":-0.86865, "ax":-1.79645, "ay":3.55798, "alpha":-0.25293, "fx":[-27.93266,-29.61093,-29.13101,-27.40543], "fy":[57.19811,56.25502,55.7611,56.72783]}, + {"t":1.60378, "x":5.37172, "y":1.15698, "heading":2.13541, "vx":-2.34798, "vy":-1.11437, "omega":-0.87658, "ax":-1.62164, "ay":3.63683, "alpha":-0.2342, "fx":[-25.18167,-26.79599,-26.30523,-24.6958], "fy":[58.3686,57.54459,57.11094,57.92494]}, + {"t":1.63513, "x":5.29731, "y":1.12383, "heading":2.10793, "vx":-2.39882, "vy":-1.00036, "omega":-0.88392, "ax":-1.44301, "ay":3.70534, "alpha":-0.21137, "fx":[-22.40073,-23.86027,-23.43015,-21.94431], "fy":[59.36772,58.69822,58.27716,58.95671]}, + {"t":1.66648, "x":5.2214, "y":1.09429, "heading":2.08022, "vx":-2.44406, "vy":-0.88419, "omega":-0.89055, "ax":-1.26093, "ay":3.76204, "alpha":-0.1967, "fx":[-19.53201,-20.92811,-20.5055,-19.10705], "fy":[60.20701,59.62744,59.2478,59.81812]}, + {"t":1.69783, "x":5.14415, "y":1.06842, "heading":2.0523, "vx":-2.48359, "vy":-0.76625, "omega":-0.89671, "ax":-1.07569, "ay":3.80397, "alpha":-0.17626, "fx":[-16.64818,-17.89513,-17.51539,-16.25077], "fy":[60.80171,60.34755,59.97751,60.43659]}, + {"t":1.72918, "x":5.06576, "y":1.04626, "heading":2.02419, "vx":-2.51732, "vy":-0.647, "omega":-0.90224, "ax":-0.88744, "ay":3.82415, "alpha":-0.16439, "fx":[-13.68341,-14.87628,-14.48266,-13.31258], "fy":[61.08264,60.68214,60.35301,60.72713]}, + {"t":1.76053, "x":4.98641, "y":1.02786, "heading":1.9959, "vx":-2.54514, "vy":-0.52711, "omega":-0.90739, "ax":-0.69578, "ay":3.80201, "alpha":-0.15182, "fx":[-10.68148,-11.76027,-11.41276,-10.32961], "fy":[60.68783,60.38526,60.03077,60.33464]}, + {"t":1.79188, "x":4.90628, "y":1.0132, "heading":1.96745, "vx":-2.56695, "vy":-0.40792, "omega":-0.91215, "ax":-0.49833, "ay":3.65815, "alpha":-0.15016, "fx":[-7.5629,-8.61126,-8.248,-7.22326], "fy":[58.4379,58.09191,57.75303,58.02027]}, + {"t":1.82323, "x":4.82556, "y":1.00221, "heading":1.93886, "vx":-2.58257, "vy":-0.29323, "omega":-0.91686, "ax":-0.28919, "ay":3.04651, "alpha":-0.17379, "fx":[-4.25341,-5.30262,-4.91172,-3.8966], "fy":[48.83407,48.5417,47.89649,48.18982]}, + {"t":1.85458, "x":4.74445, "y":0.99452, "heading":1.91011, "vx":-2.59164, "vy":-0.19773, "omega":-0.92231, "ax":-0.1136, "ay":1.72556, "alpha":-0.18694, "fx":[-1.48314,-2.46712,-2.12395,-1.13997], "fy":[27.93619,27.71292,26.81162,27.11761]}, + {"t":1.88593, "x":4.66315, "y":0.98917, "heading":1.8812, "vx":-2.5952, "vy":-0.14363, "omega":-0.92817, "ax":-0.03717, "ay":0.72965, "alpha":-0.17314, "fx":[-0.28801,-1.20354,-0.85761,-0.01116], "fy":[12.12876,11.85954,11.03871,11.308]}, + {"t":1.91728, "x":4.58177, "y":0.98502, "heading":1.8521, "vx":-2.59637, "vy":-0.12075, "omega":-0.9336, "ax":-0.0154, "ay":0.34685, "alpha":-0.155, "fx":[0.01141,-0.71989,-0.50025,0.23071], "fy":[5.86239,6.02463,4.96541,5.17341]}, + {"t":1.94863, "x":4.50037, "y":0.98141, "heading":1.82283, "vx":-2.59685, "vy":-0.10988, "omega":-0.93846, "ax":-0.00198, "ay":0.04703, "alpha":-0.11827, "fx":[0.18748,-0.42917,-0.22777,0.34389], "fy":[1.11808,0.96313,0.37527,0.53005]}, + {"t":1.97998, "x":4.41895, "y":0.97798, "heading":1.79341, "vx":-2.59691, "vy":-0.10841, "omega":-0.94216, "ax":-0.0018, "ay":0.0433, "alpha":-0.09849, "fx":[0.14042,-0.30724,-0.19734,0.25007], "fy":[0.87769,1.08502,0.34197,0.44471]}, + {"t":2.01133, "x":4.33754, "y":0.97461, "heading":1.76387, "vx":-2.59697, "vy":-0.10705, "omega":-0.94525, "ax":0.0011, "ay":-0.02658, "alpha":-0.06098, "fx":[0.1373,-0.18084,-0.0898,0.20327], "fy":[-0.23352,-0.29896,-0.61037,-0.54504]}, + {"t":2.04268, "x":4.25612, "y":0.97124, "heading":1.73424, "vx":-2.59693, "vy":-0.10788, "omega":-0.94716, "ax":-0.00021, "ay":0.00512, "alpha":-0.03998, "fx":[0.06116,-0.10234,-0.06791,0.09553], "fy":[0.12184,0.30466,-0.06655,-0.03508]}, + {"t":2.07404, "x":4.17471, "y":0.96786, "heading":1.70455, "vx":-2.59694, "vy":-0.10772, "omega":-0.94842, "ax":0.00037, "ay":-0.00877, "alpha":-0.0045, "fx":[0.0105,-0.01489,0.00779,0.01979], "fy":[-0.12238,-0.13096,-0.15611,-0.14758]}, + {"t":2.10539, "x":4.0933, "y":0.96448, "heading":1.67481, "vx":-2.59693, "vy":-0.108, "omega":-0.94856, "ax":-0.00022, "ay":0.00519, "alpha":0.01853, "fx":[-0.06353,0.06345,0.05663,-0.07028], "fy":[-0.02222,0.12973,0.11502,0.10734]}, + {"t":2.13674, "x":4.01188, "y":0.96109, "heading":1.64508, "vx":-2.59693, "vy":-0.10783, "omega":-0.94798, "ax":-0.00007, "ay":0.00173, "alpha":0.05314, "fx":[-0.13272,0.14141,0.13373,-0.14699], "fy":[-0.11707,-0.10184,0.17205,0.15681]}, + {"t":2.16809, "x":3.93047, "y":0.95771, "heading":1.61536, "vx":-2.59694, "vy":-0.10778, "omega":-0.94631, "ax":-0.00017, "ay":0.00422, "alpha":0.07907, "fx":[-0.21301,0.22139,0.2074,-0.22688], "fy":[-0.18125,-0.06882,0.26598,0.25205]}, + {"t":2.19944, "x":3.84905, "y":0.95434, "heading":1.58569, "vx":-2.59694, "vy":-0.10765, "omega":-0.94383, "ax":-0.0001, "ay":0.00241, "alpha":0.1147, "fx":[-0.29746,0.29675,0.29534,-0.30101], "fy":[-0.26174,-0.25734,0.33835,0.33393]}, + {"t":2.23079, "x":3.76764, "y":0.95096, "heading":1.5561, "vx":-2.59694, "vy":-0.10757, "omega":-0.94024, "ax":-0.00007, "ay":0.00166, "alpha":0.14466, "fx":[-0.39425,0.37668,0.392,-0.37879], "fy":[-0.36534,-0.31343,0.38429,0.39982]}, + {"t":2.26214, "x":3.68622, "y":0.94759, "heading":1.52662, "vx":-2.59695, "vy":-0.10752, "omega":-0.9357, "ax":-0.00002, "ay":0.00044, "alpha":0.18325, "fx":[-0.49695,0.45161,0.49571,-0.45155], "fy":[-0.44675,-0.49198,0.46079,0.50599]}, + {"t":2.29349, "x":3.60481, "y":0.94422, "heading":1.49729, "vx":-2.59695, "vy":-0.10751, "omega":-0.92996, "ax":0.00001, "ay":-0.00033, "alpha":0.21885, "fx":[-0.61821,0.53187,0.61858,-0.53139], "fy":[-0.543,-0.58483,0.51034,0.59669]}, + {"t":2.32484, "x":3.52339, "y":0.94085, "heading":1.46814, "vx":-2.59695, "vy":-0.10752, "omega":-0.9231, "ax":0.00005, "ay":-0.00114, "alpha":0.26233, "fx":[-0.74412,0.60819,0.74326,-0.60432], "fy":[-0.62742,-0.76846,0.59117,0.73217]}, + {"t":2.35619, "x":3.44198, "y":0.93748, "heading":1.4392, "vx":-2.59695, "vy":-0.10755, "omega":-0.91487, "ax":0.00005, "ay":-0.00131, "alpha":0.3054, "fx":[-0.89748,0.68985,0.89919,-0.68811], "fy":[-0.70891,-0.89064,0.65437,0.86195]}, + {"t":2.38754, "x":3.36056, "y":0.93411, "heading":1.41051, "vx":-2.59694, "vy":-0.10759, "omega":-0.9053, "ax":0.00007, "ay":-0.00158, "alpha":0.3561, "fx":[-1.05358,0.76981,1.05088,-0.76296], "fy":[-0.79453,-1.08874,0.74443,1.03863]}, + {"t":2.41889, "x":3.27915, "y":0.93073, "heading":1.38213, "vx":-2.59694, "vy":-0.10764, "omega":-0.89413, "ax":0.00005, "ay":-0.00122, "alpha":0.40905, "fx":[-1.24798,0.85303,1.24966,-0.8515], "fy":[-0.86393,-1.24785,0.82095,1.21344]}, + {"t":2.45024, "x":3.19773, "y":0.92736, "heading":1.3541, "vx":-2.59694, "vy":-0.10768, "omega":-0.88131, "ax":-0.00002, "ay":0.0005, "alpha":0.46975, "fx":[-1.44454,0.93965,1.43469,-0.93113], "fy":[-0.92874,-1.44922,0.94445,1.4651]}, + {"t":2.48159, "x":3.11632, "y":0.92398, "heading":1.32647, "vx":-2.59694, "vy":-0.10766, "omega":-0.86658, "ax":-0.00002, "ay":0.00041, "alpha":0.53565, "fx":[-1.69026,1.02283,1.69001,-1.02363], "fy":[-1.00198,-1.67184,1.02019,1.67947]}, + {"t":2.51294, "x":3.03491, "y":0.92061, "heading":1.29931, "vx":-2.59694, "vy":-0.10765, "omega":-0.84979, "ax":-0.00027, "ay":0.00644, "alpha":0.60969, "fx":[-1.94137,1.1206,1.91586,-1.11205], "fy":[-1.01039,-1.85131,1.21429,2.05606]}, + {"t":2.54429, "x":2.95349, "y":0.91724, "heading":1.27266, "vx":-2.59695, "vy":-0.10745, "omega":-0.83068, "ax":0.00005, "ay":-0.00132, "alpha":0.69234, "fx":[-2.24603,1.2029,2.24861,-1.20199], "fy":[-1.20136,-2.26775,1.17652,2.20879]}, + {"t":2.57564, "x":2.87208, "y":0.91387, "heading":1.24662, "vx":-2.59695, "vy":-0.10749, "omega":-0.80897, "ax":-0.00012, "ay":0.00296, "alpha":0.7836, "fx":[-2.56377,1.3231,2.53098,-1.29809], "fy":[-1.25148,-2.53327,1.34367,2.62882]}, + {"t":2.60699, "x":2.79066, "y":0.9105, "heading":1.22126, "vx":-2.59695, "vy":-0.1074, "omega":-0.78441, "ax":0.00189, "ay":-0.04533, "alpha":0.8864, "fx":[-2.91721,1.41644,2.97986,-1.3593], "fy":[-2.08285,-3.67605,0.66926,2.21097]}, + {"t":2.63834, "x":2.70925, "y":0.90711, "heading":1.19667, "vx":-2.59689, "vy":-0.10882, "omega":-0.75662, "ax":0.00436, "ay":-0.10254, "alpha":0.99928, "fx":[-3.27911,1.60796,3.37289,-1.42472], "fy":[-3.12259,-4.99097,-0.14038,1.74224]}, + {"t":2.66969, "x":2.62784, "y":0.90365, "heading":1.17295, "vx":-2.59676, "vy":-0.11203, "omega":-0.72529, "ax":0.01389, "ay":-0.30869, "alpha":1.11886, "fx":[-3.59082,1.78518,4.04119,-1.35318], "fy":[-6.47255,-8.67345,-3.33574,-1.12127]}, + {"t":2.70104, "x":2.54643, "y":0.89998, "heading":1.15021, "vx":-2.59632, "vy":-0.12171, "omega":-0.69021, "ax":0.03817, "ay":-0.70936, "alpha":1.22183, "fx":[-3.50321,2.34745,4.65824,-1.07866], "fy":[-12.96344,-15.46726,-9.59911,-7.01654]}, + {"t":2.73239, "x":2.46506, "y":0.89582, "heading":1.12857, "vx":-2.59512, "vy":-0.14395, "omega":-0.65191, "ax":3.20766, "ay":-0.78494, "alpha":0.79351, "fx":[48.84503,51.29093,52.96035,50.59949], "fy":[-14.16015,-15.79939,-10.95198,-8.93445]}, + {"t":2.76374, "x":2.38528, "y":0.89092, "heading":1.10814, "vx":-2.49456, "vy":-0.16856, "omega":-0.62703, "ax":3.97283, "ay":0.08539, "alpha":0.62437, "fx":[62.02649,63.45473,64.12883,62.67652], "fy":[0.19446,-1.88192,2.43714,4.673]}, + {"t":2.79509, "x":2.30902, "y":0.88568, "heading":1.08848, "vx":-2.37001, "vy":-0.16588, "omega":-0.60746, "ax":3.98488, "ay":0.12403, "alpha":0.69701, "fx":[62.1093,63.69855,64.43209,62.81136], "fy":[0.78476,-1.70007,3.08859,5.70308]}, + {"t":2.82644, "x":2.23668, "y":0.88054, "heading":1.06944, "vx":-2.24509, "vy":-0.16199, "omega":-0.58561, "ax":3.98883, "ay":0.13742, "alpha":0.77943, "fx":[62.02075,63.78995,64.65213,62.8394], "fy":[0.94596,-1.93022,3.31947,6.39108]}, + {"t":2.85779, "x":2.16826, "y":0.87553, "heading":1.05108, "vx":-2.12004, "vy":-0.15769, "omega":-0.56117, "ax":3.99079, "ay":0.14423, "alpha":0.88164, "fx":[61.87357,63.87002,64.86736,62.81575], "fy":[1.00464,-2.39285,3.47619,7.07093]}, + {"t":2.88914, "x":2.10376, "y":0.87066, "heading":1.03348, "vx":-1.99493, "vy":-0.15316, "omega":-0.53353, "ax":3.99196, "ay":0.14841, "alpha":0.98378, "fx":[61.70967,63.91877,65.08141,62.79105], "fy":[1.00418,-2.87208,3.56943,7.72303]}, + {"t":2.92049, "x":2.04318, "y":0.86593, "heading":1.01676, "vx":-1.86978, "vy":-0.14851, "omega":-0.50269, "ax":3.99273, "ay":0.1512, "alpha":1.09775, "fx":[61.51476,63.96882,65.3031,62.76355], "fy":[0.99793,-3.45656,3.65425,8.4063]}, + {"t":2.95184, "x":1.98652, "y":0.86135, "heading":1.001, "vx":-1.7446, "vy":-0.14377, "omega":-0.46827, "ax":3.99329, "ay":0.15319, "alpha":1.21348, "fx":[61.31739,64.00331,65.53147,62.73324], "fy":[0.98659,-4.05652,3.69978,9.09843]}, + {"t":2.98319, "x":1.93379, "y":0.85691, "heading":0.98632, "vx":-1.61941, "vy":-0.13897, "omega":-0.43023, "ax":3.9937, "ay":0.1547, "alpha":1.33791, "fx":[61.10205,64.0408,65.76801,62.70087], "fy":[0.97184,-4.72353,3.75527,9.82028]}, + {"t":3.01454, "x":1.88498, "y":0.85263, "heading":0.97283, "vx":-1.49421, "vy":-0.13412, "omega":-0.38829, "ax":3.99402, "ay":0.15587, "alpha":1.46307, "fx":[60.88433,64.06734,66.01097,62.66954], "fy":[0.96388,-5.38839,3.76778,10.55506]}, + {"t":3.04589, "x":1.8401, "y":0.8485, "heading":0.96066, "vx":-1.369, "vy":-0.12923, "omega":-0.34242, "ax":3.99428, "ay":0.15681, "alpha":1.59338, "fx":[60.65471,64.09808,66.26065,62.63507], "fy":[0.95195,-6.09353,3.80118,11.29855]}, + {"t":3.07724, "x":1.79915, "y":0.84453, "heading":0.94992, "vx":-1.24378, "vy":-0.12432, "omega":-0.29247, "ax":3.99449, "ay":0.15758, "alpha":1.7259, "fx":[60.42326,64.121,66.51547,62.60212], "fy":[0.94258,-6.80507,3.80359,12.06598]}, + {"t":3.10859, "x":1.76212, "y":0.84071, "heading":0.94075, "vx":-1.11855, "vy":-0.11938, "omega":-0.23836, "ax":3.99467, "ay":0.15823, "alpha":1.86178, "fx":[60.18463,64.15019,66.7759,62.56226], "fy":[0.91659,-7.53987,3.83652,12.83489]}, + {"t":3.13994, "x":1.72901, "y":0.83705, "heading":0.93328, "vx":-0.99331, "vy":-0.11441, "omega":-0.17999, "ax":0.72381, "ay":-3.83406, "alpha":2.05601, "fx":[0.35449,11.72276,22.43494,11.45168], "fy":[-62.83033,-64.76337,-58.94111,-56.93943]}, + {"t":3.17622, "x":1.69346, "y":0.83037, "heading":0.92675, "vx":-0.96706, "vy":-0.25349, "omega":-0.10541, "ax":1.05361, "ay":-3.20487, "alpha":2.20667, "fx":[6.28179,16.58434,26.87705,17.16427], "fy":[-53.46105,-56.43739,-48.72612,-44.89426]}, + {"t":3.21249, "x":1.65907, "y":0.81907, "heading":0.92293, "vx":-0.92884, "vy":-0.36975, "omega":-0.02537, "ax":3.47116, "ay":0.96198, "alpha":1.39657, "fx":[52.01713,56.55217,58.16449,53.69518], "fy":[15.10452,8.15665,15.57607,22.25162]}, + {"t":3.24877, "x":1.62766, "y":0.80629, "heading":0.92201, "vx":-0.80292, "vy":-0.33485, "omega":0.02529, "ax":3.68836, "ay":1.52924, "alpha":0.76206, "fx":[56.94057,59.78589,60.17136,57.32395], "fy":[24.68295,20.33919,23.91844,28.17057]}, + {"t":3.28504, "x":1.60096, "y":0.79515, "heading":0.92292, "vx":-0.66913, "vy":-0.27938, "omega":0.05294, "ax":3.68899, "ay":1.53703, "alpha":0.40903, "fx":[57.6718,59.23211,59.44865,57.90892], "fy":[24.65278,22.2991,24.187,26.46712]}, + {"t":3.32131, "x":1.57912, "y":0.78603, "heading":0.92485, "vx":-0.53531, "vy":-0.22363, "omega":0.06778, "ax":3.68919, "ay":1.53967, "alpha":0.05566, "fx":[58.43961,58.64816,58.70099,58.48539], "fy":[24.44998,24.17517,24.42938,24.7191]}, + {"t":3.35759, "x":1.56213, "y":0.77893, "heading":0.9273, "vx":-0.40149, "vy":-0.16778, "omega":0.06979, "ax":3.68928, "ay":1.541, "alpha":-0.29408, "fx":[59.18575,58.07336,57.95552,59.06572], "fy":[24.2753,25.9837,24.66055,22.93844]}, + {"t":3.39386, "x":1.54999, "y":0.77385, "heading":0.92984, "vx":-0.26766, "vy":-0.11188, "omega":0.05913, "ax":3.68934, "ay":1.5418, "alpha":-0.64201, "fx":[59.93268,57.50298,57.2003,59.64813], "fy":[24.10014,27.74753,24.9141,21.1469]}, + {"t":3.43014, "x":1.54271, "y":0.77081, "heading":0.93198, "vx":-0.13383, "vy":-0.05595, "omega":0.03584, "ax":3.68938, "ay":1.54233, "alpha":-0.98797, "fx":[60.67484,56.93625,56.44413,60.23133], "fy":[23.93899,29.47699,25.1805,19.34607]}, + {"t":3.46641, "x":1.54028, "y":0.7698, "heading":0.93328, "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/Constants.java b/src/main/java/frc/robot/Constants.java index 6820f472..29630d5e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -356,7 +356,7 @@ public static AngularVelocity metersToRotationVel(double metersPerSecond){ public class RobotK { public static final String kLogTab = "SuperStructure"; // TODO: get a real distance from the reef for this - public static final double kRobotCenterDistanceFromReef = Units.inchesToMeters(-17); + public static final double kRobotCenterDistanceFromReef = Units.inchesToMeters(-16.5); public static final double kRobotScoringOffset = Units.inchesToMeters(2.9); // positive left robot, measured 4/1/2025 public static final Transform2d kTransformReefPoseToRobotPosition = new Transform2d(kRobotCenterDistanceFromReef, kRobotScoringOffset, Rotation2d.fromDegrees(0)); } @@ -453,7 +453,7 @@ public static class SharedAutoAlignK { public static final Distance kFieldTranslationTolerance = Meters.of(0.025); // meters public static final Angle kFieldRotationTolerance = Degrees.of(0.5); // degrees - public static final double kIntermediatePoseDistance = -Units.inchesToMeters(6); // value in meters + public static final double kIntermediatePoseDistance = -Units.inchesToMeters(8); // value in meters public static final Transform2d kIntermediatePoseTransform = new Transform2d(kIntermediatePoseDistance, 0, Rotation2d.kZero); @@ -511,8 +511,8 @@ public static class LegacyAutoAlignK { public static class MovingAutoAlignK { public static final String kLogTab = "MovingAutoAlign"; - public static double kXKP = 8; - public static double kYKP = 8; + public static double kXKP = 8.5; + public static double kYKP = 8.5; public static double kThetaKP = 10; // SUPER COOL AUTO ALIGN :sunglasses: - this should eventually allow you to replace all code using above constants diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ed292e25..02503e20 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,8 @@ import java.util.List; import java.util.Optional; import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -46,6 +48,8 @@ import frc.robot.autons.TrajsAndLocs.HPStation; import frc.robot.autons.TrajsAndLocs.ReefLocs; import frc.robot.autons.TrajsAndLocs.StartingLocs; + +import static frc.robot.Constants.kTestingAutonOnCart; import static frc.robot.autons.TrajsAndLocs.ReefLocs.*; import frc.robot.autons.WaltAutonFactory; @@ -94,6 +98,7 @@ public class Robot extends TimedRobot { VisionK.kElevatorForwardsCamRoboToCam, visionSim, VisionK.kEleForwardCamSimProps); private final Vision lowerRightCam = new Vision(VisionK.kLowerRightCamName, VisionK.kLowerRightCamSimVisualName, VisionK.kLowerRightCamRoboToCam, visionSim, VisionK.kLowerRightCamSimProps); + private final int[] tagsToUse = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; // this should be updated with all of our cameras private final Vision[] cameras = {eleForwardsCam, lowerRightCam}; // lower right cam removed readded and ready to rumble @@ -444,6 +449,20 @@ public void robotPeriodic() { if (estimatedPoseOptional.isPresent()) { EstimatedRobotPose estimatedRobotPose = estimatedPoseOptional.get(); Pose2d estimatedRobotPose2d = estimatedRobotPose.estimatedPose.toPose2d(); + boolean invalidEstimation = false; + // if it isn't multitag + if (estimatedRobotPose.strategy != PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) { + // loop through used targets + for (PhotonTrackedTarget target : estimatedRobotPose.targetsUsed) { + // if any of them are invalid targets set invalidEstimation flag to true + if (!isUsedTagId(target.getFiducialId())) { invalidEstimation = true; } + } + } + + if (invalidEstimation == true) { + continue; + } + var ctreTime = Utils.fpgaToCurrentTime(estimatedRobotPose.timestampSeconds); drivetrain.addVisionMeasurement(estimatedRobotPose2d, ctreTime, camera.getEstimationStdDevs()); lastGotTagMsmtTimer.restart(); @@ -483,24 +502,42 @@ public void disabledPeriodic() { // --- PRESET AUTONS if (WaltAutonBuilder.nte_rightThreePiece.getBoolean(false)) { - waltAutonFactory = Optional.of(autonFactoryFactory( - StartingLocs.RIGHT, - List.of(REEF_E, REEF_D, REEF_C, REEF_B), - List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4), - List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT) + if(kTestingAutonOnCart) { + waltAutonFactory = Optional.of(autonFactoryFactory( + StartingLocs.RIGHT, + List.of(REEF_E, REEF_D, REEF_C, REEF_B), + List.of(EleHeight.L2, EleHeight.L2, EleHeight.L2, EleHeight.L2), + List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT) + )); + } else { + waltAutonFactory = Optional.of(autonFactoryFactory( + StartingLocs.RIGHT, + List.of(REEF_E, REEF_D, REEF_C, REEF_B), + List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4), + List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT) )); + } Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Right 3 piece auton generated")); WaltAutonBuilder.nte_rightThreePiece.setBoolean(false); } if (WaltAutonBuilder.nte_leftThreePiece.getBoolean(false)) { - waltAutonFactory = Optional.of(autonFactoryFactory( - StartingLocs.LEFT, - List.of(REEF_J, REEF_K, REEF_L, REEF_A), - List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4), - List.of(HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT) + if(kTestingAutonOnCart) { + waltAutonFactory = Optional.of(autonFactoryFactory( + StartingLocs.LEFT, + List.of(REEF_J, REEF_K, REEF_L, REEF_A), + List.of(EleHeight.L2, EleHeight.L2, EleHeight.L2, EleHeight.L2), + List.of(HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT) )); + } else { + waltAutonFactory = Optional.of(autonFactoryFactory( + StartingLocs.LEFT, + List.of(REEF_J, REEF_K, REEF_L, REEF_A), + List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4), + List.of(HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT) + )); + } // autonName = "Left 3 Piece: "; Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Left 3 piece auton generated")); @@ -521,6 +558,18 @@ public void disabledPeriodic() { WaltAutonBuilder.nte_midOnePiece.setBoolean(false); } + if(WaltAutonBuilder.nte_midTown.getBoolean(false)) { + waltAutonFactory = Optional.of(autonFactoryFactory( + StartingLocs.MID_G, + List.of(REEF_G, REEF_D, REEF_E), + List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4), + List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT) + )); + + Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Mid 3 piece auton generated")); + WaltAutonBuilder.nte_midTown.setBoolean(false); + } + // fail-case (no auton selected) - do nothing (its no longer that now) if (readyToMakeAuton && waltAutonFactory.isEmpty()) { @@ -576,6 +625,15 @@ public void disabledExit() { } + public boolean isUsedTagId(int tagId) { + for (int i = 0; i < tagsToUse.length; i++) { + if (tagsToUse[i] == tagId) { + return true; + } + } + return false; + } + private Command autonCmdBuilder(Command chooserCommand) { return Commands.parallel( Commands.print("running autonCmdBuilder"), diff --git a/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java b/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java index dc89f15c..aad67cd5 100644 --- a/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java +++ b/src/main/java/frc/robot/autoalign/LegacyAutoAlign.java @@ -3,6 +3,7 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; @@ -32,7 +33,8 @@ public class LegacyAutoAlign { private static final SwerveRequest.FieldCentric swreq_driveFieldCentricBlue = new SwerveRequest.FieldCentric() - .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); + .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) + .withDriveRequestType(DriveRequestType.Velocity); public static Command moveToPose(Swerve drivetrain, Supplier destinationPose) { final Pose2d[] cachedTarget = {Pose2d.kZero}; diff --git a/src/main/java/frc/robot/autoalign/MovingAutoAlign.java b/src/main/java/frc/robot/autoalign/MovingAutoAlign.java index 20588322..4e7a08fd 100644 --- a/src/main/java/frc/robot/autoalign/MovingAutoAlign.java +++ b/src/main/java/frc/robot/autoalign/MovingAutoAlign.java @@ -3,6 +3,7 @@ import java.util.function.Supplier; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.MathUtil; @@ -112,15 +113,16 @@ public static Command moveToPose( headingController.enableContinuousInput(-Math.PI, Math.PI); // ok, use passed constraints on X controller final ProfiledPIDController vxController = - new ProfiledPIDController(MovingAutoAlignK.kXKP, 0.01, 0.015, xyConstraints.get()); + new ProfiledPIDController(MovingAutoAlignK.kXKP, 0.01, 0.01, xyConstraints.get()); // use constraints from constants for y controller? // why define them with different constraints?? it's literally field relative // the difference in x and y dimensions almost definitely do not mean anything to robot movement final ProfiledPIDController vyController = - new ProfiledPIDController(MovingAutoAlignK.kYKP, 0.01, 0.015, xyConstraints.get()); + new ProfiledPIDController(MovingAutoAlignK.kYKP, 0.01, 0.01, xyConstraints.get()); // this is created at trigger binding, not created every time the command is scheduled - final SwerveRequest.ApplyFieldSpeeds swreq_driveFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds(); + final SwerveRequest.ApplyFieldSpeeds swreq_driveFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds() + .withDriveRequestType(DriveRequestType.Velocity); return Commands.runOnce( () -> { diff --git a/src/main/java/frc/robot/autons/TrajsAndLocs.java b/src/main/java/frc/robot/autons/TrajsAndLocs.java index 3e19edec..d14714d3 100644 --- a/src/main/java/frc/robot/autons/TrajsAndLocs.java +++ b/src/main/java/frc/robot/autons/TrajsAndLocs.java @@ -201,6 +201,7 @@ public static class Trajectories { ReefToHPTrajs.put(new Pair(ReefLocs.REEF_D, HPStation.HP_RIGHT), "D_Right"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_E, HPStation.HP_RIGHT), "E_Right"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_F, HPStation.HP_RIGHT), "F_Right"); + ReefToHPTrajs.put(new Pair(ReefLocs.REEF_G, HPStation.HP_RIGHT), "G_Right"); ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_A, HPStation.HP_RIGHT), "A_Right_short"); ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_B, HPStation.HP_RIGHT), "B_Right_short"); ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_C, HPStation.HP_RIGHT), "C_Right_short"); diff --git a/src/main/java/frc/robot/autons/WaltAutonBuilder.java b/src/main/java/frc/robot/autons/WaltAutonBuilder.java index 5ddd75bc..c7a1a0b6 100644 --- a/src/main/java/frc/robot/autons/WaltAutonBuilder.java +++ b/src/main/java/frc/robot/autons/WaltAutonBuilder.java @@ -17,7 +17,7 @@ public class WaltAutonBuilder { public static GenericEntry nte_customAutonReady; public static GenericEntry nte_autonRobotPush; // button to select if we are pushing another robot b4 starting path public static GenericEntry nte_clearAll; - + public static GenericEntry nte_autonReadyToGo; // to let the user know that an auton is loaded public static GenericEntry nte_autonName; @@ -26,6 +26,8 @@ public class WaltAutonBuilder { public static GenericEntry nte_rightThreePiece; public static GenericEntry nte_leftThreePiece; public static GenericEntry nte_midOnePiece; + public static GenericEntry nte_midTown; + // public static GenericEntry nte_midGOnly; // ---- Initial @@ -101,6 +103,11 @@ public class WaltAutonBuilder { .add("Mid 1 Piece", false) .withWidget(BuiltInWidgets.kToggleSwitch) .getEntry(); + + nte_midTown = Shuffleboard.getTab("AutonChooser") + .add("Mid 3 Piece", false) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index c6ee26c2..159b2248 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -29,7 +29,7 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.35).withKI(0).withKD(0) + .withKP(0.25).withKI(0).withKD(0) .withKS(0.2034625).withKV(0.1232525).withKA(0.0085018); // The closed-loop output type to use for the steer motors; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 946d8fb5..eb5318a9 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -326,6 +326,7 @@ public void simulationPeriodic() { } private static final double kInch = 0.169; + private static final double kEighthInch = kInch / 8; public enum EleHeight { HOME(0.3), diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 29e57978..503d106f 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -460,9 +460,8 @@ private void configureStateActions() { m_coral.slowScore(), Commands.waitSeconds(0.1845), m_ele.toHeightCoral(() -> L2), - m_finger.l1HelperCmd(), Commands.waitSeconds(0.18), - m_finger.inCmd() + m_coral.stopCmd() ) );