diff --git a/src/main/deploy/choreo/AccelLimit_StartCenterShoot.traj b/src/main/deploy/choreo/AccelLimit_StartCenterShoot.traj deleted file mode 100644 index 4151c76c..00000000 --- a/src/main/deploy/choreo/AccelLimit_StartCenterShoot.traj +++ /dev/null @@ -1,242 +0,0 @@ -{ - "name":"AccelLimit_StartCenterShoot", - "version":3, - "snapshot":{ - "waypoints":[ - {"x":4.425225734710693, "y":7.51403624535, "heading":3.141592653589793, "intervals":23, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.70665979385376, "y":7.51403624535, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.924712657928467, "y":6.82694191456, "heading":4.363323129985824, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.74509859085083, "y":4.726553554534912, "heading":4.1887902047863905, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.536667346954346, "y":7.103303797245026, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.002538681030273, "y":7.41730172157, "heading":3.141592653589793, "intervals":90, "split":true, "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.07450246810913086, "y":0.04413539171218872, "w":16.541, "h":8.0692}}, "enabled":true}, - {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"MaxAngularVelocity", "props":{"max":3.0}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":4.0}}, "enabled":true}], - "targetDt":0.05 - }, - "params":{ - "waypoints":[ - {"x":{"exp":"SideStart.x", "val":4.425225734710693}, "y":{"exp":"SideStart.y", "val":7.51403624535}, "heading":{"exp":"SideStart.heading", "val":3.141592653589793}, "intervals":23, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.70665979385376 m", "val":5.70665979385376}, "y":{"exp":"SideStart.y", "val":7.51403624535}, "heading":{"exp":"SideStart.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.924712657928467 m", "val":7.924712657928467}, "y":{"exp":"6.82694191456 m", "val":6.82694191456}, "heading":{"exp":"250 deg", "val":4.363323129985824}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.74509859085083 m", "val":7.74509859085083}, "y":{"exp":"8.07 m - 3.343446445465088 m", "val":4.726553554534912}, "heading":{"exp":"240 deg", "val":4.1887902047863905}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.536667346954346 m", "val":6.536667346954346}, "y":{"exp":"8.07 m - 0.9666962027549744 m", "val":7.103303797245026}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"SideShoot.x", "val":4.002538681030273}, "y":{"exp":"SideShoot.y", "val":7.41730172157}, "heading":{"exp":"SideShoot.heading", "val":3.141592653589793}, "intervals":90, "split":true, "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":"74.50246810913086 mm", "val":0.07450246810913086}, "y":{"exp":"44.13539171218872 mm", "val":0.04413539171218872}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}, - {"from":2, "to":3, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"3 rad / s", "val":3.0}}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"4 m / s ^ 2", "val":4.0}}}, "enabled":true}], - "targetDt":{ - "exp":"0.05 s", - "val":0.05 - } - }, - "trajectory":{ - "config":{ - "frontLeft":{ - "x":0.2525, - "y":0.2525 - }, - "backLeft":{ - "x":-0.2525, - "y":0.2525 - }, - "mass":56.7, - "inertia":1.5, - "gearing":6.03, - "radius":0.0508, - "vmax":314.1592653589793, - "tmax":3.0, - "cof":1.5, - "bumper":{ - "front":0.4305, - "side":0.4305, - "back":0.4305 - }, - "differentialTrackWidth":0.5588 - }, - "sampleType":"Swerve", - "waypoints":[0.0,0.81586,1.98275,3.14033,4.51419,5.83144], - "samples":[ - {"t":0.0, "x":4.42523, "y":7.51404, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.99711, "ay":-0.06724, "alpha":-0.00312, "fx":[57.23517,57.23026,56.08534,56.08554], "fy":[-0.3751,-3.76913,0.7062,-0.37476]}, - {"t":0.03547, "x":4.42774, "y":7.51399, "heading":3.14159, "vx":0.14179, "vy":-0.00239, "omega":-0.00011, "ax":3.99821, "ay":-0.06703, "alpha":0.07156, "fx":[57.07826,57.07713,56.27135,56.27171], "fy":[-0.65345,-2.59468,0.10088,-0.65312]}, - {"t":0.07094, "x":4.43529, "y":7.51387, "heading":3.14159, "vx":0.28361, "vy":-0.00476, "omega":0.00243, "ax":3.99813, "ay":-0.06698, "alpha":0.11442, "fx":[56.88179,56.88387,56.46395,56.46451], "fy":[-0.91031,-1.44205,-0.53555,-0.90983]}, - {"t":0.10642, "x":4.44786, "y":7.51366, "heading":-3.14151, "vx":0.42544, "vy":-0.00714, "omega":0.00649, "ax":3.99804, "ay":-0.06704, "alpha":0.10578, "fx":[56.70907,56.71101,56.63408,56.63494], "fy":[-1.06964,-0.63928,-1.0226,-1.06954]}, - {"t":0.14189, "x":4.46547, "y":7.51336, "heading":-3.14128, "vx":0.56726, "vy":-0.00952, "omega":0.01024, "ax":3.99794, "ay":-0.06721, "alpha":0.04592, "fx":[56.58035,56.58063,56.76049,56.7618], "fy":[-1.11078,-0.27934,-1.30917,-1.1115]}, - {"t":0.17736, "x":4.4881, "y":7.51298, "heading":-3.14092, "vx":0.70907, "vy":-0.0119, "omega":0.01187, "ax":3.99782, "ay":-0.06753, "alpha":-0.00627, "fx":[56.48794,56.48659,56.85005,56.85184], "fy":[-1.12871,-0.06986,-1.5,-1.13029]}, - {"t":0.21283, "x":4.51577, "y":7.51252, "heading":-3.1405, "vx":0.85088, "vy":-0.0143, "omega":0.01165, "ax":3.99768, "ay":-0.06804, "alpha":-0.04596, "fx":[56.4244,56.42213,56.90978,56.91199], "fy":[-1.13849,0.04445,-1.6232,-1.14066]}, - {"t":0.24831, "x":4.54847, "y":7.51197, "heading":-3.14008, "vx":0.99269, "vy":-0.01671, "omega":0.01001, "ax":3.99751, "ay":-0.06878, "alpha":-0.05964, "fx":[56.38222,56.3799,56.94694,56.9495], "fy":[-1.16805,0.15963,-1.72092,-1.17042]}, - {"t":0.28378, "x":4.5862, "y":7.51133, "heading":-3.13973, "vx":1.13449, "vy":-0.01915, "omega":0.0079, "ax":3.99731, "ay":-0.06886, "alpha":-0.10003, "fx":[56.36029,56.35847,56.96294,56.96594], "fy":[-1.12796,0.06541,-1.71124,-1.1304]}, - {"t":0.31925, "x":4.62896, "y":7.51061, "heading":-3.13945, "vx":1.27629, "vy":-0.02159, "omega":0.00435, "ax":3.99706, "ay":-0.07017, "alpha":-0.11741, "fx":[56.34467,56.34422,56.97071,56.9735], "fy":[-1.13234,0.02787,-1.74004,-1.13387]}, - {"t":0.35472, "x":4.67674, "y":7.5098, "heading":-3.13929, "vx":1.41807, "vy":-0.02408, "omega":0.00019, "ax":3.99673, "ay":-0.07201, "alpha":-0.10764, "fx":[56.32147,56.32222,56.98427,56.98671], "fy":[-1.19133,0.09448,-1.7941,-1.19179]}, - {"t":0.3902, "x":4.72956, "y":7.5089, "heading":-3.13929, "vx":1.55984, "vy":-0.02664, "omega":-0.00363, "ax":3.99631, "ay":-0.0746, "alpha":-0.13511, "fx":[56.30333,56.30621,56.99012,56.99098], "fy":[-1.19942,-0.00174,-1.83094,-1.19798]}, - {"t":0.42567, "x":4.78741, "y":7.50791, "heading":-3.13942, "vx":1.7016, "vy":-0.02928, "omega":-0.00842, "ax":3.99573, "ay":-0.07833, "alpha":-0.03921, "fx":[56.2517,56.25675,57.02577,57.02391], "fy":[-1.43815,0.56952,-2.13909,-1.43382]}, - {"t":0.46114, "x":4.85028, "y":7.50682, "heading":-3.13971, "vx":1.84334, "vy":-0.03206, "omega":-0.00982, "ax":3.99492, "ay":-0.0839, "alpha":-0.05501, "fx":[56.22666,56.23274,57.02776,57.02466], "fy":[-1.50742,0.48591,-2.23388,-1.50168]}, - {"t":0.49661, "x":4.91818, "y":7.50563, "heading":-3.14006, "vx":1.98505, "vy":-0.03504, "omega":-0.01177, "ax":3.99367, "ay":-0.09267, "alpha":0.00233, "fx":[56.20027,56.20832,57.01923,57.0131], "fy":[-1.72621,0.6458,-2.45642,-1.7173]}, - {"t":0.53209, "x":4.99111, "y":7.50433, "heading":-3.14048, "vx":2.12671, "vy":-0.03832, "omega":-0.01168, "ax":3.99152, "ay":-0.10787, "alpha":-0.03065, "fx":[56.23066,56.24297,56.92748,56.91815], "fy":[-1.83206,-0.02637,-2.43822,-1.8198]}, - {"t":0.56756, "x":5.06906, "y":7.5029, "heading":-3.14089, "vx":2.2683, "vy":-0.04215, "omega":-0.01277, "ax":3.98705, "ay":-0.13835, "alpha":0.10887, "fx":[56.2747,56.29027,56.75605,56.74465], "fy":[-2.36365,-0.36771,-2.76412,-2.349]}, - {"t":0.60303, "x":5.15203, "y":7.50132, "heading":-3.14135, "vx":2.40973, "vy":-0.04706, "omega":-0.00891, "ax":3.97428, "ay":-0.19137, "alpha":0.30747, "fx":[56.35568,56.37432,56.31179,56.29997], "fy":[-3.14768,-1.52107,-3.05022,-3.13155]}, - {"t":0.6385, "x":5.24001, "y":7.49953, "heading":3.14152, "vx":2.55071, "vy":-0.05385, "omega":0.002, "ax":2.61891, "ay":1.11149, "alpha":-0.05628, "fx":[37.97815,37.9972,36.26436,36.25257], "fy":[16.6953,11.23614,18.37785,16.71195]}, - {"t":0.67397, "x":5.33214, "y":7.49832, "heading":3.14159, "vx":2.64361, "vy":-0.01442, "omega":0.0, "ax":-0.00275, "ay":1.29064, "alpha":-0.00001, "fx":[1.20214,1.21744,-1.28097,-1.29457], "fy":[19.53607,12.12508,21.96708,19.55126]}, - {"t":0.70945, "x":5.42591, "y":7.49862, "heading":3.14159, "vx":2.64351, "vy":0.03136, "omega":0.0, "ax":-0.03922, "ay":1.68614, "alpha":0.0, "fx":[1.24219,1.2624,-2.35512,-2.37308], "fy":[25.70034,14.95426,29.23155,25.71827]}, - {"t":0.74492, "x":5.51966, "y":7.50079, "heading":3.14159, "vx":2.64212, "vy":0.09117, "omega":0.0, "ax":-0.12235, "ay":2.40447, "alpha":0.0, "fx":[0.93705,0.96871,-4.40751,-4.43549], "fy":[36.75777,20.83706,41.95517,36.78321]}, - {"t":0.78039, "x":5.6133, "y":7.50554, "heading":3.14159, "vx":2.63778, "vy":0.17647, "omega":0.0, "ax":-0.3332, "ay":3.55802, "alpha":0.00942, "fx":[-0.76036,-0.71686,-8.68891,-8.72658], "fy":[54.39006,30.9233,62.0055,54.42102]}, - {"t":0.81586, "x":5.70666, "y":7.51404, "heading":3.14159, "vx":2.62596, "vy":0.30268, "omega":0.00033, "ax":-0.43811, "ay":3.19558, "alpha":0.00255, "fx":[-2.79624,-2.79491,-9.62404,-9.62559], "fy":[48.70865,28.44147,55.33146,48.70759]}, - {"t":0.84504, "x":5.78308, "y":7.52423, "heading":-3.14158, "vx":2.61318, "vy":0.3959, "omega":0.00041, "ax":-0.32815, "ay":2.01564, "alpha":-0.00014, "fx":[-2.22749,-2.22841,-7.07584,-7.07452], "fy":[30.9967,16.58456,35.71131,30.99428]}, - {"t":0.87421, "x":5.85917, "y":7.53663, "heading":-3.14157, "vx":2.60361, "vy":0.4547, "omega":0.0004, "ax":-0.25108, "ay":1.3766, "alpha":-0.00004, "fx":[-1.80312,-1.78977,-5.31496,-5.32845], "fy":[21.26939,10.80227,24.69921,21.28254]}, - {"t":0.90338, "x":5.93502, "y":7.55048, "heading":-3.14156, "vx":2.59628, "vy":0.49486, "omega":0.0004, "ax":-0.16056, "ay":0.82255, "alpha":-0.00004, "fx":[-1.08796,-1.06333,-3.46389,-3.4887], "fy":[12.84777,5.72173,15.19693,12.87211]}, - {"t":0.93255, "x":6.01069, "y":7.56527, "heading":-3.14155, "vx":2.5916, "vy":0.51885, "omega":0.0004, "ax":-0.056, "ay":0.27771, "alpha":-0.00001, "fx":[-0.1183,-0.11322,-1.46918,-1.47435], "fy":[4.61218,0.58567,5.93158,4.61692]}, - {"t":0.96172, "x":6.08627, "y":7.58052, "heading":-3.14154, "vx":2.58997, "vy":0.52696, "omega":0.0004, "ax":0.061, "ay":-0.30265, "alpha":0.00001, "fx":[1.02023,1.02345,0.70903,0.70577], "fy":[-4.13385,-5.09161,-3.80297,-4.13201]}, - {"t":0.9909, "x":6.16185, "y":7.59577, "heading":-3.14152, "vx":2.59174, "vy":0.51813, "omega":0.0004, "ax":0.18982, "ay":-0.97841, "alpha":0.00005, "fx":[2.33124,2.24553,3.05009,3.13582], "fy":[-14.22801,-11.86615,-15.06706,-14.31438]}, - {"t":1.02007, "x":6.23753, "y":7.61047, "heading":-3.14151, "vx":2.59728, "vy":0.48958, "omega":0.0004, "ax":0.31778, "ay":-1.78614, "alpha":0.00003, "fx":[3.49286,3.43172,5.51618,5.57749], "fy":[-26.32883,-20.20587,-28.34695,-26.39257]}, - {"t":1.04924, "x":6.31344, "y":7.62399, "heading":-3.1415, "vx":2.60655, "vy":0.43748, "omega":0.0004, "ax":0.42422, "ay":-2.79864, "alpha":0.00026, "fx":[4.33664,4.08659,7.68979,7.94042], "fy":[-41.34721,-30.69431,-45.04361,-41.59782]}, - {"t":1.07841, "x":6.38966, "y":7.63556, "heading":-3.14149, "vx":2.61893, "vy":0.35584, "omega":0.00041, "ax":0.43334, "ay":-3.79769, "alpha":0.00024, "fx":[3.66639,3.52675,8.61809,8.75906], "fy":[-56.30631,-41.2631,-61.30966,-56.44975]}, - {"t":1.10759, "x":6.46624, "y":7.64432, "heading":-3.14148, "vx":2.63157, "vy":0.24505, "omega":0.00042, "ax":0.27668, "ay":-3.89553, "alpha":0.00033, "fx":[1.57751,1.10443,6.26575,6.73983], "fy":[-57.56356,-42.22477,-63.05194,-58.03614]}, - {"t":1.13676, "x":6.54313, "y":7.64981, "heading":-3.14146, "vx":2.63964, "vy":0.13141, "omega":0.00043, "ax":0.1079, "ay":-3.93642, "alpha":0.00058, "fx":[-0.9379,-1.16085,3.99671,4.21974], "fy":[-58.26407,-42.95358,-63.48594,-58.49136]}, - {"t":1.16593, "x":6.62018, "y":7.65197, "heading":-3.14145, "vx":2.64279, "vy":0.01658, "omega":0.00045, "ax":-0.06457, "ay":-3.9561, "alpha":0.0007, "fx":[-3.13728,-3.8809,1.30756,2.04926], "fy":[-58.30062,-43.00603,-63.96144,-59.04276]}, - {"t":1.1951, "x":6.69725, "y":7.65077, "heading":-3.14144, "vx":2.6409, "vy":-0.09883, "omega":0.00047, "ax":-0.23959, "ay":-3.96097, "alpha":0.00107, "fx":[-5.924,-6.25146,-0.86632,-0.54283], "fy":[-58.6724,-42.89331,-64.01555,-59.00572]}, - {"t":1.22427, "x":6.77418, "y":7.6462, "heading":-3.14143, "vx":2.63391, "vy":-0.21438, "omega":0.0005, "ax":-0.42731, "ay":-3.95274, "alpha":0.00472, "fx":[-8.17212,-9.22179,-3.93739,-2.89744], "fy":[-58.1535,-42.79944,-63.9691,-59.19811]}, - {"t":1.25345, "x":6.85084, "y":7.63827, "heading":-3.14141, "vx":2.62145, "vy":-0.32969, "omega":0.00064, "ax":-0.80743, "ay":-3.89825, "alpha":0.09699, "fx":[-13.77723,-14.29789,-9.10274,-8.60358], "fy":[-57.73352,-42.0046,-63.04026,-58.2522]}, - {"t":1.28262, "x":6.92697, "y":7.62699, "heading":-3.14139, "vx":2.59789, "vy":-0.44341, "omega":0.00346, "ax":-1.86266, "ay":-3.52332, "alpha":1.92631, "fx":[-26.5395,-27.97622,-26.24995,-24.84725], "fy":[-52.96031,-36.75563,-55.7019,-54.3545]}, - {"t":1.31179, "x":7.00196, "y":7.61256, "heading":-3.14129, "vx":2.54356, "vy":-0.5462, "omega":0.05966, "ax":-2.63269, "ay":-2.99692, "alpha":4.57847, "fx":[-35.18721,-36.10697,-39.43122,-38.5479], "fy":[-47.16892,-30.04035,-44.66789,-48.048]}, - {"t":1.34096, "x":7.07504, "y":7.59535, "heading":-3.13955, "vx":2.46676, "vy":-0.63362, "omega":0.19322, "ax":-3.04314, "ay":-2.58297, "alpha":6.97433, "fx":[-38.40975,-40.24361,-47.84935,-46.04319], "fy":[-42.27995,-23.62718,-36.52237,-44.02486]}, - {"t":1.37013, "x":7.14571, "y":7.57577, "heading":-3.13391, "vx":2.37798, "vy":-0.70897, "omega":0.39668, "ax":-3.27047, "ay":-2.29121, "alpha":6.46715, "fx":[-41.8774,-43.38423,-50.82771,-49.34634], "fy":[-37.60217,-21.6373,-31.62019,-39.05201]}, - {"t":1.39931, "x":7.21369, "y":7.55411, "heading":-3.12234, "vx":2.28257, "vy":-0.77581, "omega":0.58534, "ax":-3.40882, "ay":-2.082, "alpha":7.24258, "fx":[-42.88957,-44.97761,-53.73949,-51.67338], "fy":[-34.80579,-18.27234,-28.16321,-36.8083]}, - {"t":1.42848, "x":7.27883, "y":7.53059, "heading":-3.10527, "vx":2.18313, "vy":-0.83655, "omega":0.79662, "ax":-3.49912, "ay":-1.92811, "alpha":6.73219, "fx":[-44.43362,-46.68878,-54.75493,-52.52259], "fy":[-32.05748,-16.85915,-26.16079,-34.2463]}, - {"t":1.45765, "x":7.34102, "y":7.50537, "heading":-3.08203, "vx":2.08105, "vy":-0.8928, "omega":0.99301, "ax":-3.56211, "ay":-1.81046, "alpha":6.12702, "fx":[-45.68098,-47.78696,-55.29492,-53.20894], "fy":[-29.78693,-16.68601,-24.34148,-31.83856]}, - {"t":1.48682, "x":7.40022, "y":7.47855, "heading":-3.05306, "vx":1.97714, "vy":-0.94561, "omega":1.17175, "ax":-3.60845, "ay":-1.71737, "alpha":6.87166, "fx":[-45.15257,-48.30178,-57.13909,-54.00587], "fy":[-28.25979,-15.29526,-22.48067,-31.33896]}, - {"t":1.516, "x":7.45636, "y":7.45024, "heading":-3.01888, "vx":1.87187, "vy":-0.99571, "omega":1.37221, "ax":-3.64275, "ay":-1.64432, "alpha":5.24298, "fx":[-47.2248,-49.28543,-56.04084,-53.9926], "fy":[-26.4133,-16.69577,-21.67696,-28.44671]}, - {"t":1.54517, "x":7.50942, "y":7.42049, "heading":-2.97885, "vx":1.76561, "vy":-1.04368, "omega":1.52516, "ax":-3.67016, "ay":-1.58298, "alpha":7.14343, "fx":[-45.41668,-49.4672,-58.62375,-54.5904], "fy":[-25.83978,-13.45975,-20.66022,-29.79523]}, - {"t":1.57434, "x":7.55936, "y":7.38937, "heading":-2.93435, "vx":1.65854, "vy":-1.08986, "omega":1.73355, "ax":-3.69124, "ay":-1.53387, "alpha":5.0905, "fx":[-47.88751,-50.06599,-56.75406,-54.58552], "fy":[-24.48827,-15.84786,-19.99863,-26.63569]}, - {"t":1.60351, "x":7.60617, "y":7.35692, "heading":-2.88378, "vx":1.55086, "vy":-1.1346, "omega":1.88205, "ax":-3.70909, "ay":-1.49076, "alpha":7.80456, "fx":[-45.01724,-50.12406,-60.12606,-55.03819], "fy":[-24.15439,-12.00239,-19.25224,-29.11731]}, - {"t":1.63268, "x":7.64984, "y":7.32319, "heading":-2.82888, "vx":1.44266, "vy":-1.17809, "omega":2.10973, "ax":-3.72332, "ay":-1.45537, "alpha":5.68139, "fx":[-47.72822,-50.43049,-57.82215,-55.13166], "fy":[-23.45559,-14.11224,-18.87495,-26.07667]}, - {"t":1.66186, "x":7.69034, "y":7.2882, "heading":-2.76733, "vx":1.33404, "vy":-1.22055, "omega":2.27547, "ax":-3.73567, "ay":-1.42383, "alpha":8.13637, "fx":[-45.0152,-50.91027,-60.88057,-55.00655], "fy":[-22.81781,-10.62335,-18.78949,-28.50076]}, - {"t":1.69103, "x":7.72766, "y":7.25199, "heading":-2.70095, "vx":1.22506, "vy":-1.26208, "omega":2.51282, "ax":-3.74618, "ay":-1.39635, "alpha":4.01371, "fx":[-49.54871,-51.71877,-56.64835,-54.49265], "fy":[-21.63418,-14.87095,-18.94451,-23.72361]}, - {"t":1.7202, "x":7.76181, "y":7.21458, "heading":-2.62765, "vx":1.11578, "vy":-1.30282, "omega":2.62991, "ax":-3.75575, "ay":-1.37078, "alpha":4.48819, "fx":[-48.87277,-52.73602,-57.59389,-53.74823], "fy":[-20.391,-13.85453,-19.34675,-24.1309]}, - {"t":1.74937, "x":7.79276, "y":7.17599, "heading":-2.55093, "vx":1.00622, "vy":-1.34281, "omega":2.76084, "ax":-3.76402, "ay":-1.34823, "alpha":0.22195, "fx":[-53.27591,-53.07204,-53.42826,-53.64374], "fy":[-19.4818,-18.73908,-18.91177,-19.31182]}, - {"t":1.77854, "x":7.82051, "y":7.13624, "heading":-2.47039, "vx":0.89641, "vy":-1.38214, "omega":2.76731, "ax":-3.77151, "ay":-1.32743, "alpha":0.72771, "fx":[-52.72222,-53.53502,-54.1941,-53.3932], "fy":[-18.79713,-17.97198,-18.89218,-19.60416]}, - {"t":1.80772, "x":7.84506, "y":7.09536, "heading":-2.38966, "vx":0.78639, "vy":-1.42086, "omega":2.78854, "ax":-3.77792, "ay":-1.30935, "alpha":-0.82385, "fx":[-54.25201,-53.2732,-52.84732,-53.83538], "fy":[-18.65914,-19.78143,-18.1134,-17.68639]}, - {"t":1.83689, "x":7.86639, "y":7.05335, "heading":-2.30831, "vx":0.67618, "vy":-1.45906, "omega":2.76451, "ax":-3.78348, "ay":-1.29345, "alpha":-0.03857, "fx":[-53.03922,-53.44602,-54.22287,-53.81508], "fy":[-17.97089,-19.54176,-17.59408,-18.23162]}, - {"t":1.86606, "x":7.88451, "y":7.01024, "heading":-2.22767, "vx":0.56581, "vy":-1.49679, "omega":2.76338, "ax":-3.78832, "ay":-1.27943, "alpha":-0.31903, "fx":[-53.04297,-53.3294,-54.35775,-54.06745], "fy":[-17.71377,-20.17136,-16.96046,-17.69807]}, - {"t":1.89523, "x":7.8994, "y":6.96603, "heading":-2.14705, "vx":0.45529, "vy":-1.53412, "omega":2.75408, "ax":-3.79257, "ay":-1.26697, "alpha":-2.2262, "fx":[-54.85254,-52.88092,-52.6652,-54.64022], "fy":[-17.94822,-22.17604,-16.15516,-15.5576]}, - {"t":1.92441, "x":7.91107, "y":6.92074, "heading":-2.06671, "vx":0.34465, "vy":-1.57108, "omega":2.68914, "ax":-3.79664, "ay":-1.25491, "alpha":-11.80693, "fx":[-64.39828,-49.78318,-43.2123,-57.87594], "fy":[-20.58504,-31.8211,-12.74402,-6.00312]}, - {"t":1.95358, "x":7.91951, "y":6.87437, "heading":-1.98826, "vx":0.2339, "vy":-1.60768, "omega":2.3447, "ax":-3.80076, "ay":-1.24258, "alpha":-31.07654, "fx":[-83.22322,-41.72138,-24.41978,-66.13846], "fy":[-27.86055,-50.3604,-4.59158,12.3584]}, - {"t":1.98275, "x":7.92471, "y":6.82694, "heading":-1.91986, "vx":0.12302, "vy":-1.64393, "omega":1.43813, "ax":-3.72016, "ay":-1.4631, "alpha":-20.55814, "fx":[-71.41578,-43.92251,-34.00907,-61.58553], "fy":[-27.91146,-42.90113,-10.99973,-1.14567]}, - {"t":2.01892, "x":7.92673, "y":6.76652, "heading":-1.86784, "vx":-0.01155, "vy":-1.69686, "omega":0.69445, "ax":-3.23442, "ay":-2.34982, "alpha":-12.54608, "fx":[-57.00344,-40.03954,-34.67371,-51.67469], "fy":[-38.06779,-46.74405,-26.92543,-21.4975]}, - {"t":2.0551, "x":7.92419, "y":6.7036, "heading":-1.84272, "vx":-0.12856, "vy":-1.78186, "omega":0.24061, "ax":-1.63115, "ay":-3.64833, "alpha":-6.23621, "fx":[-30.04482,-19.34395,-16.17028,-26.92737], "fy":[-56.8734,-54.12248,-49.51633,-46.34809]}, - {"t":2.09127, "x":7.91848, "y":6.63675, "heading":-1.83401, "vx":-0.18756, "vy":-1.91384, "omega":0.01502, "ax":-0.44448, "ay":-3.97045, "alpha":-2.57113, "fx":[-10.33801,-4.02042,-2.25051,-8.59332], "fy":[-60.78931,-53.60757,-56.25005,-54.47774]}, - {"t":2.12745, "x":7.9114, "y":6.56492, "heading":-1.83347, "vx":-0.20364, "vy":-2.05747, "omega":-0.07799, "ax":-0.41764, "ay":-3.9719, "alpha":-0.69451, "fx":[-8.06244,-4.6627,-3.77185,-7.18297], "fy":[-59.68428,-52.28247,-57.08322,-56.15688]}, - {"t":2.16362, "x":7.90376, "y":6.48789, "heading":-1.83629, "vx":-0.21875, "vy":-2.20115, "omega":-0.10312, "ax":-0.4011, "ay":-3.97085, "alpha":0.56425, "fx":[-6.68375,-5.05676,-4.68468,-6.31742], "fy":[-59.09218,-51.0506,-57.71265,-57.29205]}, - {"t":2.1998, "x":7.89559, "y":6.40567, "heading":-1.84002, "vx":-0.23326, "vy":-2.34479, "omega":-0.08271, "ax":-0.38753, "ay":-3.96516, "alpha":1.19356, "fx":[-6.1589,-5.05499,-4.8257,-5.93317], "fy":[-59.13443,-49.69539,-58.13636,-57.85811]}, - {"t":2.23597, "x":7.88689, "y":6.31826, "heading":-1.84301, "vx":-0.24728, "vy":-2.48823, "omega":-0.03953, "ax":-0.30527, "ay":-3.91717, "alpha":1.0925, "fx":[-5.48061,-3.61567,-3.17313,-5.03938], "fy":[-59.14601,-48.03425,-57.69213,-57.23112]}, - {"t":2.27214, "x":7.87775, "y":6.22568, "heading":-1.84444, "vx":-0.25832, "vy":-2.62993, "omega":-0.00001, "ax":0.268, "ay":-0.05779, "alpha":0.00005, "fx":[5.22135,3.02675,2.37644,4.5712], "fy":[1.73254,-4.90311,0.25536,-0.36133]}, - {"t":2.30832, "x":7.86858, "y":6.13051, "heading":-1.84444, "vx":-0.24862, "vy":-2.63202, "omega":-0.00001, "ax":0.21739, "ay":-0.02015, "alpha":0.0, "fx":[4.03306,2.55235,2.12999,3.61062], "fy":[1.45764,-3.05137,0.42567,0.02569]}, - {"t":2.34449, "x":7.85973, "y":6.03528, "heading":-1.84444, "vx":-0.24076, "vy":-2.63275, "omega":-0.00001, "ax":0.16193, "ay":-0.01463, "alpha":0.0, "fx":[2.68426,2.03345,1.90652,2.55729], "fy":[0.65372,-1.47508,0.06405,-0.07193]}, - {"t":2.38067, "x":7.85113, "y":5.94004, "heading":-1.84444, "vx":-0.2349, "vy":-2.63328, "omega":-0.00001, "ax":0.13923, "ay":-0.01229, "alpha":0.0, "fx":[1.9371,1.95809,2.00994,1.98893], "fy":[-0.10424,-0.19214,-0.21444,-0.18577]}, - {"t":2.41684, "x":7.84272, "y":5.84477, "heading":-1.84444, "vx":-0.22987, "vy":-2.63372, "omega":-0.00001, "ax":0.15011, "ay":-0.01295, "alpha":0.0, "fx":[1.72067,2.31002,2.53504,1.94568], "fy":[-0.85086,0.91741,-0.50452,-0.29611]}, - {"t":2.45302, "x":7.8345, "y":5.74949, "heading":-1.84444, "vx":-0.22444, "vy":-2.63419, "omega":-0.00001, "ax":0.1978, "ay":-0.01606, "alpha":0.00005, "fx":[1.99076,3.20916,3.61674,2.39832], "fy":[-1.62227,2.03948,-0.85422,-0.47352]}, - {"t":2.48919, "x":7.82651, "y":5.65419, "heading":-1.84445, "vx":-0.21728, "vy":-2.63477, "omega":-0.00001, "ax":0.41169, "ay":1.55002, "alpha":-0.05316, "fx":[6.20401,5.55653,5.46724,6.11492], "fy":[22.83133,20.57854,22.28383,22.19255]}, - {"t":2.52537, "x":7.81892, "y":5.55989, "heading":-1.84445, "vx":-0.20239, "vy":-2.5787, "omega":-0.00193, "ax":0.38928, "ay":3.95987, "alpha":-1.38191, "fx":[6.9594,4.63933,4.07363,6.39957], "fy":[60.53547,46.83084,58.84683,58.31152]}, - {"t":2.56154, "x":7.81186, "y":5.4692, "heading":-1.84452, "vx":-0.18831, "vy":-2.43546, "omega":-0.05192, "ax":0.35766, "ay":3.97357, "alpha":-1.38659, "fx":[5.89326,4.55011,4.2422,5.59385], "fy":[59.61048,48.80557,58.58877,58.29663]}, - {"t":2.59771, "x":7.80528, "y":5.3837, "heading":-1.84639, "vx":-0.17537, "vy":-2.29171, "omega":-0.10208, "ax":0.34606, "ay":3.97811, "alpha":-1.55826, "fx":[5.30473,4.64482,4.5018,5.17008], "fy":[59.08496,49.51509,58.54763,58.41139]}, - {"t":2.63389, "x":7.79916, "y":5.3034, "heading":-1.85009, "vx":-0.16285, "vy":-2.14781, "omega":-0.15845, "ax":0.33776, "ay":3.98057, "alpha":-1.21798, "fx":[5.00112,4.67297,4.57103,4.90562], "fy":[58.25877,51.43348,58.04604,57.96005]}, - {"t":2.67006, "x":7.79349, "y":5.22831, "heading":-1.85582, "vx":-0.15063, "vy":-2.00381, "omega":-0.20251, "ax":0.32993, "ay":3.98227, "alpha":-1.44276, "fx":[4.84788,4.61136,4.50197,4.74581], "fy":[58.4198,50.83859,58.31165,58.22448]}, - {"t":2.70624, "x":7.78826, "y":5.15843, "heading":-1.86314, "vx":-0.1387, "vy":-1.85976, "omega":-0.2547, "ax":0.3216, "ay":3.98364, "alpha":-0.92981, "fx":[4.91587,4.4508,4.19825,4.66953], "fy":[58.06734,52.2785,57.87624,57.65024]}, - {"t":2.74241, "x":7.78345, "y":5.09376, "heading":-1.87236, "vx":-0.12706, "vy":-1.71565, "omega":-0.28833, "ax":0.31212, "ay":3.98488, "alpha":-1.24893, "fx":[4.88092,4.28434,3.96363,4.56849], "fy":[58.58502,50.94627,58.34724,58.06439]}, - {"t":2.77859, "x":7.77906, "y":5.0343, "heading":-1.88279, "vx":-0.11577, "vy":-1.5715, "omega":-0.33351, "ax":0.30087, "ay":3.98612, "alpha":-1.16502, "fx":[5.07312,3.98432,3.45179,4.55028], "fy":[59.03612,50.38754,58.53531,58.05387]}, - {"t":2.81476, "x":7.77507, "y":4.98006, "heading":-1.89485, "vx":-0.10489, "vy":-1.42731, "omega":-0.37566, "ax":0.28694, "ay":3.98743, "alpha":-0.85835, "fx":[4.65759,3.90013,3.47326,4.23879], "fy":[58.28807,52.13331,58.02013,57.64574]}, - {"t":2.85094, "x":7.77146, "y":4.93104, "heading":-1.90844, "vx":-0.09451, "vy":-1.28306, "omega":-0.40671, "ax":0.26877, "ay":3.98892, "alpha":-0.72903, "fx":[4.52277,3.60519,3.09256,4.0185], "fy":[58.31373,52.34558,57.98161,57.53109]}, - {"t":2.88711, "x":7.76822, "y":4.88723, "heading":-1.92315, "vx":-0.08479, "vy":-1.13877, "omega":-0.43308, "ax":0.24291, "ay":3.99077, "alpha":-1.08128, "fx":[3.84413,3.3847,3.03812,3.50602], "fy":[58.21023,52.00563,58.17086,57.88987]}, - {"t":2.92328, "x":7.76531, "y":4.84865, "heading":-1.93882, "vx":-0.076, "vy":-0.9944, "omega":-0.47219, "ax":0.19852, "ay":3.99338, "alpha":-1.55789, "fx":[2.94303,2.87379,2.68072,2.75852], "fy":[58.34365,51.1297,58.53767,58.41353]}, - {"t":2.95946, "x":7.76269, "y":4.81529, "heading":-1.9559, "vx":-0.06882, "vy":-0.84994, "omega":-0.52855, "ax":0.02495, "ay":3.99837, "alpha":-1.7658, "fx":[-0.60405,0.83291,1.30976,-0.12388], "fy":[56.98286,53.21108,57.99525,58.51821]}, - {"t":2.99563, "x":7.76022, "y":4.78716, "heading":-1.97502, "vx":-0.06791, "vy":-0.7053, "omega":-0.59243, "ax":-0.27005, "ay":3.98944, "alpha":-2.90858, "fx":[-5.94016,-2.94101,-1.71663,-4.71384], "fy":[56.44937,51.99601,58.25489,59.50094]}, - {"t":3.03181, "x":7.75758, "y":4.76426, "heading":-1.99645, "vx":-0.07768, "vy":-0.56099, "omega":-0.69764, "ax":-0.55626, "ay":3.9598, "alpha":-4.81759, "fx":[-11.87929,-6.38585,-3.89133,-9.38375], "fy":[55.49621,49.57652,58.49333,60.95474]}, - {"t":3.06798, "x":7.75441, "y":4.74656, "heading":-2.02169, "vx":-0.09781, "vy":-0.41775, "omega":-0.87191, "ax":-0.80172, "ay":3.91759, "alpha":-7.35538, "fx":[-18.31288,-9.00128,-4.41832,-13.72523], "fy":[53.68718,47.26385,58.36435,62.81208]}, - {"t":3.10416, "x":7.75035, "y":4.73401, "heading":-2.05323, "vx":-0.12681, "vy":-0.27603, "omega":-1.13799, "ax":-1.00985, "ay":3.86927, "alpha":-11.5649, "fx":[-25.80634,-10.80909,-2.8289,-17.81413], "fy":[51.72876,42.72588,58.6347,66.29805]}, - {"t":3.14033, "x":7.7451, "y":4.72655, "heading":-2.0944, "vx":-0.16334, "vy":-0.13606, "omega":-1.55634, "ax":-1.10093, "ay":3.84296, "alpha":-9.41421, "fx":[-25.54472,-12.96675,-5.67312,-18.23838], "fy":[51.59889,45.60623,56.82949,63.86125]}, - {"t":3.17304, "x":7.73917, "y":4.72416, "heading":-2.1453, "vx":-0.19935, "vy":-0.01035, "omega":-1.86429, "ax":-1.10041, "ay":3.84432, "alpha":-4.93271, "fx":[-20.92425,-14.39237,-10.27522,-16.80137], "fy":[53.02679,49.98803,55.46715,59.49086]}, - {"t":3.20575, "x":7.73206, "y":4.72588, "heading":-2.20629, "vx":-0.23535, "vy":0.1154, "omega":-2.02565, "ax":-1.09946, "ay":3.84452, "alpha":-2.12212, "fx":[-17.91249,-15.15429,-13.25823,-16.01424], "fy":[53.891,52.60356,54.79059,56.6989]}, - {"t":3.23846, "x":7.72377, "y":4.73171, "heading":-2.27255, "vx":-0.27131, "vy":0.24116, "omega":-2.09506, "ax":-1.09837, "ay":3.84474, "alpha":-0.65948, "fx":[-16.17497,-15.48354,-14.96292,-15.65599], "fy":[54.44254,53.68703,54.64384,55.22359]}, - {"t":3.27117, "x":7.71431, "y":4.74165, "heading":-2.34108, "vx":-0.30724, "vy":0.36692, "omega":-2.11664, "ax":-1.09716, "ay":3.845, "alpha":0.08945, "fx":[-15.07496,-15.61394,-16.02616,-15.49394], "fy":[54.89825,53.87122,54.7917,54.45008]}, - {"t":3.30389, "x":7.70367, "y":4.75571, "heading":-2.41032, "vx":-0.34313, "vy":0.49269, "omega":-2.11371, "ax":-1.09576, "ay":3.84529, "alpha":0.5527, "fx":[-14.46418,-15.63439,-16.5965,-15.43463], "fy":[55.05366,54.16699,54.8546,53.9526]}, - {"t":3.3366, "x":7.69186, "y":4.77389, "heading":-2.47946, "vx":-0.37897, "vy":0.61848, "omega":-2.09563, "ax":-1.09419, "ay":3.84562, "alpha":0.99889, "fx":[-13.81544,-15.60576,-17.19957,-15.41967], "fy":[55.20602,54.35826,55.01457,53.46757]}, - {"t":3.36931, "x":7.67888, "y":4.79618, "heading":-2.54801, "vx":-0.41476, "vy":0.74427, "omega":-2.06296, "ax":-1.09237, "ay":3.84599, "alpha":0.9176, "fx":[-13.11076,-15.52144,-17.8465,-15.45877], "fy":[55.90521,52.76631,55.84464,53.55161]}, - {"t":3.40202, "x":7.66473, "y":4.82258, "heading":-2.61549, "vx":-0.4505, "vy":0.87008, "omega":-2.03294, "ax":-1.09028, "ay":3.84642, "alpha":1.55466, "fx":[-12.40016,-15.33077,-18.49773,-15.59002], "fy":[55.76936,53.40844,56.0276,52.88686]}, - {"t":3.43473, "x":7.64941, "y":4.8531, "heading":-2.68199, "vx":-0.48616, "vy":0.9959, "omega":-1.98209, "ax":-1.08776, "ay":3.84695, "alpha":1.99796, "fx":[-11.83226,-15.05604,-18.99379,-15.79368], "fy":[55.63822,53.65452,56.37399,52.45511]}, - {"t":3.46744, "x":7.63292, "y":4.88773, "heading":-2.74683, "vx":-0.52174, "vy":1.12173, "omega":-1.91673, "ax":-1.08478, "ay":3.84756, "alpha":2.53485, "fx":[-10.82916,-14.58146,-19.91021,-16.18648], "fy":[55.71094,53.13413,57.29737,52.01393]}, - {"t":3.50015, "x":7.61528, "y":4.92648, "heading":-2.80952, "vx":-0.55723, "vy":1.24759, "omega":-1.83381, "ax":-1.08108, "ay":3.84832, "alpha":3.2427, "fx":[-10.14833,-14.04181,-20.48563,-16.62174], "fy":[55.25462,53.73184,57.79922,51.41382]}, - {"t":3.53286, "x":7.59647, "y":4.96935, "heading":-2.86951, "vx":-0.59259, "vy":1.37347, "omega":-1.72774, "ax":-1.07651, "ay":3.84925, "alpha":4.4663, "fx":[-9.24489,-13.29951,-21.25995,-17.23399], "fy":[54.31604,55.44392,58.17627,50.31607]}, - {"t":3.56557, "x":7.57651, "y":5.01634, "heading":-2.92603, "vx":-0.6278, "vy":1.49939, "omega":-1.58164, "ax":-1.07062, "ay":3.85044, "alpha":5.4803, "fx":[-8.60919,-12.50619,-21.72931,-17.85967], "fy":[53.33534,56.9223,58.57403,49.48833]}, - {"t":3.59828, "x":7.5554, "y":5.06745, "heading":-2.97776, "vx":-0.66282, "vy":1.62534, "omega":-1.40238, "ax":-1.06284, "ay":3.852, "alpha":6.3435, "fx":[-7.68912,-11.47361,-22.42644,-18.67411], "fy":[52.74793,56.87739,59.76311,49.01994]}, - {"t":3.631, "x":7.53315, "y":5.12267, "heading":-3.02364, "vx":-0.69759, "vy":1.75134, "omega":-1.19488, "ax":-1.05171, "ay":3.85423, "alpha":7.19945, "fx":[-7.1093,-10.49475,-22.69014,-19.3376], "fy":[51.85777,57.6982,60.45792,48.52071]}, - {"t":3.66371, "x":7.50977, "y":5.18202, "heading":-3.06272, "vx":-0.73199, "vy":1.87742, "omega":-0.95938, "ax":-1.03512, "ay":3.85748, "alpha":7.65256, "fx":[-6.486,-9.45062,-22.84131,-19.91344], "fy":[51.52281,56.89985,61.69308,48.60334]}, - {"t":3.69642, "x":7.48527, "y":5.2455, "heading":-3.0941, "vx":-0.76585, "vy":2.0036, "omega":-0.70905, "ax":-1.00688, "ay":3.86292, "alpha":7.65067, "fx":[-6.05832,-8.50038,-22.46756,-20.064], "fy":[51.5783,55.46669,62.81319,49.16926]}, - {"t":3.72913, "x":7.45968, "y":5.31311, "heading":-3.1173, "vx":-0.79879, "vy":2.12996, "omega":-0.45879, "ax":-0.94732, "ay":3.87401, "alpha":7.11954, "fx":[-5.83699,-7.73881,-21.0035,-19.13354], "fy":[51.90158,54.73751,62.9951,50.02215]}, - {"t":3.76184, "x":7.43304, "y":5.38485, "heading":-3.1323, "vx":-0.82978, "vy":2.25668, "omega":-0.2259, "ax":-0.75417, "ay":3.90526, "alpha":5.17282, "fx":[-4.50359,-5.92029,-16.86771,-15.47006], "fy":[53.83613,51.99546,63.15934,52.43703]}, - {"t":3.79455, "x":7.4055, "y":5.46076, "heading":-3.13969, "vx":-0.85445, "vy":2.38443, "omega":-0.0567, "ax":0.58069, "ay":3.76367, "alpha":1.73097, "fx":[11.61227,10.62655,4.85032,5.83573], "fy":[54.15014,47.15125,58.92223,53.1763]}, - {"t":3.82726, "x":7.37786, "y":5.54077, "heading":-3.14155, "vx":-0.83545, "vy":2.50754, "omega":-0.00007, "ax":1.26796, "ay":0.42297, "alpha":0.00071, "fx":[17.7638,17.03525,18.18273,18.91142], "fy":[5.78108,9.0649,4.07577,5.06058]}, - {"t":3.85997, "x":7.35121, "y":5.62302, "heading":-3.14155, "vx":-0.79398, "vy":2.52137, "omega":-0.00005, "ax":0.88975, "ay":0.27474, "alpha":0.00001, "fx":[12.42148,11.90533,12.80282,13.31923], "fy":[3.70143,6.46007,2.2264,3.18996]}, - {"t":3.89268, "x":7.32571, "y":5.70564, "heading":-3.14155, "vx":-0.76487, "vy":2.53036, "omega":-0.00005, "ax":0.56349, "ay":0.16821, "alpha":0.0, "fx":[7.8629,7.46291,8.11205,8.51216], "fy":[2.2582,4.35773,1.06016,1.86145]}, - {"t":3.92539, "x":7.30099, "y":5.7885, "heading":-3.14155, "vx":-0.74644, "vy":2.53586, "omega":-0.00005, "ax":0.26935, "ay":0.07884, "alpha":0.0, "fx":[3.726,3.46737,3.91,4.16853], "fy":[1.02545,2.52068,0.15683,0.76701]}, - {"t":3.9581, "x":7.27672, "y":5.8715, "heading":-3.14156, "vx":-0.73763, "vy":2.53844, "omega":-0.00005, "ax":-0.0381, "ay":-0.01104, "alpha":0.0, "fx":[-0.5979,-0.7903,-0.48209,-0.28998], "fy":[-0.21392,0.79922,-0.80428,-0.4072]}, - {"t":3.99082, "x":7.25257, "y":5.95453, "heading":-3.14156, "vx":-0.73888, "vy":2.53808, "omega":-0.00005, "ax":-0.40353, "ay":-0.1186, "alpha":0.0, "fx":[-5.77603,-5.8709,-5.66379,-5.5694], "fy":[-1.73564,-1.09941,-2.0561,-1.83356]}, - {"t":4.02353, "x":7.22819, "y":6.03749, "heading":-3.14156, "vx":-0.75207, "vy":2.5342, "omega":-0.00005, "ax":-0.88097, "ay":-0.267, "alpha":0.0, "fx":[-12.53264,-12.58851,-12.44242,-12.38723], "fy":[-3.82828,-3.39351,-4.03012,-3.88681]}, - {"t":4.05624, "x":7.20312, "y":6.12024, "heading":-3.14156, "vx":-0.78089, "vy":2.52547, "omega":-0.00005, "ax":-1.52877, "ay":-0.48969, "alpha":-0.00001, "fx":[-21.77198,-21.76042,-21.56818,-21.58071], "fy":[-7.04119,-6.48276,-7.20832,-7.0333]}, - {"t":4.08895, "x":7.17675, "y":6.20259, "heading":-3.14156, "vx":-0.8309, "vy":2.50945, "omega":-0.00005, "ax":-2.40027, "ay":-0.83732, "alpha":-0.00002, "fx":[-34.25129,-34.21146,-33.79553,-33.83694], "fy":[-12.09478,-10.88468,-12.43815,-12.0582]}, - {"t":4.12166, "x":7.14829, "y":6.28423, "heading":-3.14156, "vx":-0.90941, "vy":2.48206, "omega":-0.00005, "ax":-3.40315, "ay":-1.33563, "alpha":0.0, "fx":[-48.7098,-48.61904,-47.76797,-47.86197], "fy":[-19.40093,-16.91653,-20.09914,-19.31353]}, - {"t":4.15437, "x":7.11672, "y":6.3647, "heading":-3.14157, "vx":-1.02074, "vy":2.43837, "omega":-0.00005, "ax":-3.55289, "ay":-1.59052, "alpha":-0.00006, "fx":[-50.87711,-50.75949,-49.84443,-49.96805], "fy":[-23.05938,-20.381,-23.7984,-22.94369]}, - {"t":4.18708, "x":7.08143, "y":6.44361, "heading":-3.14157, "vx":-1.13695, "vy":2.38634, "omega":-0.00005, "ax":-3.51096, "ay":-1.78119, "alpha":-0.00006, "fx":[-50.28525,-50.12659,-49.24585,-49.41348], "fy":[-25.76523,-23.1957,-26.42496,-25.60752]}, - {"t":4.21979, "x":7.04236, "y":6.52072, "heading":-3.14157, "vx":-1.2518, "vy":2.32808, "omega":-0.00006, "ax":-3.43861, "ay":-1.96193, "alpha":-0.00009, "fx":[-49.22849,-49.04543,-48.25039,-48.44513], "fy":[-28.29657,-25.96551,-28.86624,-28.11302]}, - {"t":4.2525, "x":6.99958, "y":6.59582, "heading":-3.14157, "vx":-1.36428, "vy":2.2639, "omega":-0.00006, "ax":-3.34828, "ay":-2.13542, "alpha":-0.00011, "fx":[-47.90577,-47.69775,-47.01099,-47.23272], "fy":[-30.71391,-28.71772,-31.14186,-30.50478]}, - {"t":4.28521, "x":6.95316, "y":6.66873, "heading":-3.14157, "vx":-1.47381, "vy":2.19405, "omega":-0.00006, "ax":-3.24487, "ay":-2.30255, "alpha":-0.00014, "fx":[-46.38311,-46.17161,-45.60158,-45.82762], "fy":[-33.0262,-31.39399,-33.32098,-32.8134]}, - {"t":4.31793, "x":6.90321, "y":6.73927, "heading":-3.14158, "vx":-1.57995, "vy":2.11873, "omega":-0.00007, "ax":-3.13078, "ay":-2.46339, "alpha":-0.00019, "fx":[-44.75053,-44.55579,-43.99785,-44.21114], "fy":[-35.29142,-33.71185,-35.57728,-35.09392]}, - {"t":4.35064, "x":6.84985, "y":6.80726, "heading":-3.14158, "vx":-1.68236, "vy":2.03815, "omega":-0.00007, "ax":-3.00728, "ay":-2.6179, "alpha":-0.00024, "fx":[-43.0097,-42.86137,-42.23573,-42.40573], "fy":[-37.49255,-35.73827,-37.86505,-37.33892]}, - {"t":4.38335, "x":6.79321, "y":6.87253, "heading":-3.14158, "vx":-1.78073, "vy":1.95252, "omega":-0.00008, "ax":-2.87511, "ay":-2.76589, "alpha":-0.00032, "fx":[-41.11673,-41.03621,-40.38074,-40.48523], "fy":[-39.57321,-37.71965,-40.05083,-39.48234]}, - {"t":4.41606, "x":6.73343, "y":6.93492, "heading":-3.14158, "vx":-1.87478, "vy":1.86204, "omega":-0.00009, "ax":-2.73513, "ay":-2.90686, "alpha":-0.00041, "fx":[-38.98208,-38.98116,-38.54942,-38.56909], "fy":[-41.4239,-40.20147,-41.78675,-41.40664]}, - {"t":4.44877, "x":6.67064, "y":6.99427, "heading":-3.14159, "vx":-1.96425, "vy":1.76696, "omega":-0.0001, "ax":-2.58933, "ay":-3.03924, "alpha":-0.00041, "fx":[-36.66577,-36.72886,-36.74043,-36.68004], "fy":[-43.05387,-43.05698,-43.11963,-43.09444]}, - {"t":4.48148, "x":6.605, "y":7.05045, "heading":-3.14159, "vx":-2.04895, "vy":1.66754, "omega":-0.00012, "ax":-2.44722, "ay":-3.15614, "alpha":0.00362, "fx":[-34.54313,-34.64044,-34.84047,-34.73325], "fy":[-44.6249,-45.21526,-44.44553,-44.66723]}, - {"t":4.51419, "x":6.53667, "y":7.1033, "heading":3.14159, "vx":-2.129, "vy":1.5643, "omega":0.0, "ax":-2.27782, "ay":-3.27542, "alpha":0.0, "fx":[-32.1592,-32.1613,-32.42167,-32.41043], "fy":[-46.30695,-47.00005,-46.1139,-46.2954]}, - {"t":4.55411, "x":6.44987, "y":7.16314, "heading":-3.14159, "vx":-2.21992, "vy":1.43356, "omega":0.0, "ax":-2.07278, "ay":-3.41365, "alpha":0.0, "fx":[-29.42255,-29.42046,-29.34024,-29.34312], "fy":[-48.43563,-48.20064,-48.49652,-48.42118]}, - {"t":4.59402, "x":6.35961, "y":7.21764, "heading":-3.14159, "vx":-2.30266, "vy":1.29729, "omega":0.0, "ax":-1.86012, "ay":-3.53229, "alpha":0.0, "fx":[-26.67181,-26.68093,-26.0561,-26.05973], "fy":[-50.38041,-48.59041,-50.9315,-50.37842]}, - {"t":4.63394, "x":6.26621, "y":7.26661, "heading":-3.14159, "vx":-2.37691, "vy":1.1563, "omega":0.0, "ax":-1.64149, "ay":-3.63662, "alpha":0.0, "fx":[-23.88465,-23.91306,-22.63827,-22.63665], "fy":[-52.17412,-48.49773,-53.33915,-52.18556]}, - {"t":4.67386, "x":6.17002, "y":7.30987, "heading":-3.14159, "vx":-2.44243, "vy":1.01113, "omega":0.0, "ax":-1.41727, "ay":-3.72621, "alpha":0.0, "fx":[-20.97515,-21.00878,-19.1898,-19.18551], "fy":[-53.713,-48.41067,-55.42304,-53.72931]}, - {"t":4.71377, "x":6.0714, "y":7.34726, "heading":-3.14159, "vx":-2.499, "vy":0.8624, "omega":0.0, "ax":-1.18817, "ay":-3.79998, "alpha":0.0, "fx":[-17.66669,-17.69271,-16.00746,-16.00237], "fy":[-54.695,-49.74264,-56.312,-54.70922]}, - {"t":4.75369, "x":5.9707, "y":7.37866, "heading":-3.14159, "vx":-2.54643, "vy":0.71071, "omega":0.0, "ax":-0.95503, "ay":-3.85585, "alpha":0.0, "fx":[-14.49568,-14.51318,-12.57133,-12.57003], "fy":[-55.61913,-49.87971,-57.49987,-55.62794]}, - {"t":4.79361, "x":5.8683, "y":7.40395, "heading":-3.14159, "vx":-2.58455, "vy":0.5568, "omega":0.0, "ax":-0.71884, "ay":-3.88887, "alpha":0.0, "fx":[-11.36258,-11.37152,-9.01059,-9.01358], "fy":[-56.30086,-49.29865,-58.59578,-56.3035]}, - {"t":4.83352, "x":5.76456, "y":7.42308, "heading":-3.14159, "vx":-2.61325, "vy":0.40157, "omega":0.0, "ax":-0.48106, "ay":-3.88388, "alpha":0.0, "fx":[-8.20653,-8.20739,-5.42817,-5.43387], "fy":[-56.44328,-48.2177,-59.11448,-56.44076]}, - {"t":4.87344, "x":5.65986, "y":7.43602, "heading":-3.14159, "vx":-2.63245, "vy":0.24654, "omega":0.0, "ax":-0.24491, "ay":-3.75192, "alpha":0.0, "fx":[-4.98148,-4.97597,-1.96065,-1.96824], "fy":[-54.69384,-45.75028,-57.60243,-54.68743]}, - {"t":4.91336, "x":5.55459, "y":7.44287, "heading":-3.14159, "vx":-2.64223, "vy":0.09677, "omega":0.0, "ax":-0.04456, "ay":-2.31448, "alpha":0.0, "fx":[-1.28764,-1.27876,0.02447,0.01537], "fy":[-33.46368,-29.60359,-34.70886,-33.45503]}, - {"t":4.95328, "x":5.44908, "y":7.44489, "heading":-3.14159, "vx":-2.644, "vy":0.00439, "omega":0.0, "ax":0.00832, "ay":-1.16802, "alpha":0.0, "fx":[0.12681,0.13677,0.1092,0.09921], "fy":[-16.54766,-16.64178,-16.49922,-16.53815]}, - {"t":4.99319, "x":5.34355, "y":7.44413, "heading":-3.14159, "vx":-2.64367, "vy":-0.04224, "omega":0.0, "ax":0.00985, "ay":-0.49826, "alpha":0.0, "fx":[0.48598,0.49631,-0.20684,-0.21717], "fy":[-6.71631,-8.81618,-6.01268,-6.70624]}, - {"t":5.03311, "x":5.23803, "y":7.44205, "heading":-3.14159, "vx":-2.64328, "vy":-0.06212, "omega":0.0, "ax":0.00329, "ay":-0.13411, "alpha":0.0, "fx":[0.5235,0.53396,-0.43017,-0.44047], "fy":[-1.42406,-4.30137,-0.46467,-1.41387]}, - {"t":5.07303, "x":5.13252, "y":7.43946, "heading":-3.14159, "vx":-2.64315, "vy":-0.06748, "omega":0.0, "ax":-0.00141, "ay":0.05608, "alpha":0.0, "fx":[0.47445,0.48482,-0.51456,-0.52448], "fy":[1.28946,-1.69597,2.28657,1.29944]}, - {"t":5.11294, "x":5.02702, "y":7.43681, "heading":-3.14159, "vx":-2.6432, "vy":-0.06524, "omega":0.0, "ax":-0.00315, "ay":0.15916, "alpha":0.00002, "fx":[0.41786,0.42791,-0.50744,-0.51667], "fy":[2.71879,-0.0746,3.65192,2.7283]}, - {"t":5.15286, "x":4.9215, "y":7.43434, "heading":-3.14159, "vx":-2.64333, "vy":-0.05889, "omega":0.0, "ax":2.28872, "ay":0.28847, "alpha":-0.03496, "fx":[32.87835,32.88824,32.00597,31.99786], "fy":[4.57718,1.74146,5.45142,4.58615]}, - {"t":5.19278, "x":4.81781, "y":7.43222, "heading":-3.14159, "vx":-2.55197, "vy":-0.04737, "omega":-0.00139, "ax":3.98237, "ay":0.0783, "alpha":0.03975, "fx":[56.70282,56.71249,56.19594,56.18919], "fy":[1.30426,-0.00945,1.8322,1.3125]}, - {"t":5.23269, "x":4.71912, "y":7.43039, "heading":3.14154, "vx":-2.39301, "vy":-0.04425, "omega":0.00019, "ax":3.9909, "ay":0.0755, "alpha":0.03451, "fx":[56.74275,56.7513,56.39764,56.3921], "fy":[1.19149,0.32821,1.56252,1.1986]}, - {"t":5.27261, "x":4.62678, "y":7.42868, "heading":3.14154, "vx":-2.2337, "vy":-0.04123, "omega":0.00157, "ax":3.99371, "ay":0.07568, "alpha":0.0386, "fx":[56.7305,56.73759,56.48966,56.48537], "fy":[1.13585,0.61216,1.4015,1.14161]}, - {"t":5.31253, "x":4.5408, "y":7.4271, "heading":-3.14158, "vx":-2.07429, "vy":-0.03821, "omega":0.00311, "ax":3.99511, "ay":0.0757, "alpha":0.04984, "fx":[56.70741,56.71271,56.55277,56.54973], "fy":[1.07637,0.88661,1.24879,1.08055]}, - {"t":5.35244, "x":4.46118, "y":7.42563, "heading":-3.14145, "vx":-1.91482, "vy":-0.03519, "omega":0.0051, "ax":3.99595, "ay":0.07555, "alpha":0.03508, "fx":[56.68518,56.68865,56.59923,56.59734], "fy":[1.06177,0.99566,1.16167,1.0644]}, - {"t":5.39236, "x":4.38793, "y":7.42429, "heading":-3.14125, "vx":-1.75531, "vy":-0.03217, "omega":0.0065, "ax":3.99651, "ay":0.07528, "alpha":0.00316, "fx":[56.65865,56.65998,56.64217,56.64157], "fy":[1.07068,1.0333,1.09289,1.07158]}, - {"t":5.43228, "x":4.32105, "y":7.42306, "heading":-3.14099, "vx":-1.59578, "vy":-0.02917, "omega":0.00663, "ax":3.99692, "ay":0.07494, "alpha":-0.02318, "fx":[56.62207,56.62137,56.69062,56.69128], "fy":[1.06249,1.13345,0.99149,1.06178]}, - {"t":5.47219, "x":4.26054, "y":7.42196, "heading":-3.14073, "vx":-1.43624, "vy":-0.02618, "omega":0.0057, "ax":3.99722, "ay":0.07477, "alpha":-0.02748, "fx":[56.57718,56.57507,56.74414,56.74606], "fy":[1.01734,1.36681,0.8401,1.01534]}, - {"t":5.51211, "x":4.20639, "y":7.42097, "heading":-3.1405, "vx":-1.27668, "vy":-0.02319, "omega":0.0046, "ax":3.99746, "ay":0.07432, "alpha":-0.03862, "fx":[56.53885,56.53651,56.78884,56.79188], "fy":[0.98605,1.52423,0.72024,0.98335]}, - {"t":5.55203, "x":4.15861, "y":7.42011, "heading":-3.14031, "vx":-1.11712, "vy":-0.02023, "omega":0.00306, "ax":3.99766, "ay":0.07385, "alpha":-0.10484, "fx":[56.52343,56.52224,56.8088,56.81263], "fy":[1.06014,1.31697,0.75286,1.05747]}, - {"t":5.59194, "x":4.11721, "y":7.41936, "heading":-3.14019, "vx":-0.95755, "vy":-0.01728, "omega":-0.00112, "ax":3.99782, "ay":0.07338, "alpha":-0.05191, "fx":[56.50404,56.50434,56.8317,56.83611], "fy":[0.95382,1.64072,0.61427,0.95157]}, - {"t":5.63186, "x":4.08217, "y":7.41873, "heading":-3.14024, "vx":-0.79797, "vy":-0.01435, "omega":-0.00319, "ax":3.99795, "ay":0.07289, "alpha":-0.06318, "fx":[56.5231,56.52624,56.8151,56.81942], "fy":[0.98162,1.50418,0.66666,0.98051]}, - {"t":5.67178, "x":4.0535, "y":7.41821, "heading":-3.14036, "vx":-0.63838, "vy":-0.01144, "omega":-0.00572, "ax":3.99807, "ay":0.0724, "alpha":0.06449, "fx":[56.55644,56.56205,56.78408,56.78785], "fy":[0.81753,1.88717,0.5829,0.81759]}, - {"t":5.71169, "x":4.03121, "y":7.41781, "heading":-3.14059, "vx":-0.47879, "vy":-0.00855, "omega":-0.00314, "ax":3.99817, "ay":0.0719, "alpha":-0.10275, "fx":[56.68806,56.6904,56.65773,56.65992], "fy":[1.18655,0.49472,1.20854,1.18718]}, - {"t":5.75161, "x":4.01528, "y":7.41753, "heading":-3.14072, "vx":-0.3192, "vy":-0.00568, "omega":-0.00724, "ax":3.99826, "ay":0.0714, "alpha":-0.18577, "fx":[56.78283,56.78417,56.56636,56.56777], "fy":[1.3952,-0.35117,1.60808,1.39644]}, - {"t":5.79153, "x":4.00572, "y":7.41736, "heading":-3.14101, "vx":-0.1596, "vy":-0.00283, "omega":-0.01466, "ax":3.99833, "ay":0.0709, "alpha":0.36723, "fx":[56.66231,56.6595,56.54919,56.83458], "fy":[-0.57254,1.5655,1.56667,1.46019]}, - {"t":5.83144, "x":4.00254, "y":7.4173, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], - "splits":[0] - }, - "events":[] -} diff --git a/src/main/deploy/pathplanner/autos/Trench_NHFH_Shoot+PickupFuel+Park.auto b/src/main/deploy/pathplanner/autos/Trench_NHFH_Shoot+PickupFuel+Park.auto index 667167f2..094bdc02 100644 --- a/src/main/deploy/pathplanner/autos/Trench_NHFH_Shoot+PickupFuel+Park.auto +++ b/src/main/deploy/pathplanner/autos/Trench_NHFH_Shoot+PickupFuel+Park.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "AccelLimit_StartCenterShoot" + "pathName": "StartCenterShoot" } }, { diff --git a/src/main/java/com/team1816/lib/BaseRobot.java b/src/main/java/com/team1816/lib/BaseRobot.java index 20ab10ea..70f9090e 100644 --- a/src/main/java/com/team1816/lib/BaseRobot.java +++ b/src/main/java/com/team1816/lib/BaseRobot.java @@ -41,6 +41,17 @@ protected BaseRobot() { public void robotPeriodic() { // Update the pose of the robot on the field. FieldContainer.field.setRobotPose(BaseRobotState.robotPose); + // Update the path preview until the match starts. + if (!BaseRobotState.hasAutoStarted) { + baseRobotContainer.autoModeManager.updateAutoPathPreviewDisplay(); + } + } + + @Override + public void autonomousInit() { + BaseRobotState.hasAutoStarted = true; + // Clear the path preview at the start of the match. + baseRobotContainer.autoModeManager.clearAutoPathPreviewDisplay(); } /** diff --git a/src/main/java/com/team1816/lib/BaseRobotContainer.java b/src/main/java/com/team1816/lib/BaseRobotContainer.java index 54e1c8a6..5d89b2a4 100644 --- a/src/main/java/com/team1816/lib/BaseRobotContainer.java +++ b/src/main/java/com/team1816/lib/BaseRobotContainer.java @@ -1,7 +1,5 @@ package com.team1816.lib; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.util.FlippingUtil; import com.team1816.lib.auto.AutoModeManager; import com.team1816.lib.auto.PathfindManager; import com.team1816.lib.inputs.CommandButtonBoard; @@ -9,10 +7,6 @@ import com.team1816.lib.subsystems.LedManager; import com.team1816.lib.subsystems.Vision; import com.team1816.lib.subsystems.drivetrain.Swerve; -import com.team1816.lib.util.GreenLogger; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public abstract class BaseRobotContainer { @@ -23,7 +17,6 @@ public abstract class BaseRobotContainer { protected final Swerve swerve = new Swerve(driverController); protected final Vision vision = new Vision(); - private boolean poseInitialized; protected PathfindManager pathfindManager; public AutoModeManager autoModeManager; @@ -36,48 +29,7 @@ public void initializeLibSubSystems() { Singleton.CreateSubSystem(LedManager.class); pathfindManager = Singleton.get(PathfindManager.class); - autoModeManager = Singleton.get(AutoModeManager.class); - - if (autoModeManager != null) { - autoModeManager.onChange(this::updatePoseOnSelection); - } else { - GreenLogger.log("Failed to initialize AutoModeManager!"); - } - } - - public void updateInitialPose(){ - if(poseInitialized || DriverStation.getAlliance().isEmpty()) return; - forceUpdatePose(); - } - - /** - * Forces pose update regardless of poseInitialized state. - * Called from autonomousInit to ensure pose is always set before auto starts. - */ - public void forceUpdatePose(){ - updatePoseOnSelection(autoModeManager.getSelectedAuto()); - } - - private void updatePoseOnSelection(Command selectedAuto) { - if (selectedAuto != null) { - try { - Pose2d startingPose = selectedAuto instanceof PathPlannerAuto - ? ((PathPlannerAuto) selectedAuto).getStartingPose() - : Pose2d.kZero; - if (startingPose != null) { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - startingPose = FlippingUtil.flipFieldPose(startingPose); - } - // Reset odometry and update Field2d this is to give drivers clue that the - // proper auto is set prior to auto start - swerve.resetPose(startingPose); - poseInitialized = true; - } - } catch (Exception e) { - GreenLogger.log("Error loading auto pose: " + e.getMessage()); - } - } + autoModeManager = new AutoModeManager(swerve::resetPose); } /** diff --git a/src/main/java/com/team1816/lib/BaseRobotState.java b/src/main/java/com/team1816/lib/BaseRobotState.java index 9d20bab8..9a4eb170 100644 --- a/src/main/java/com/team1816/lib/BaseRobotState.java +++ b/src/main/java/com/team1816/lib/BaseRobotState.java @@ -43,4 +43,9 @@ public class BaseRobotState { * to be the pre-vision position estimate of the real robot. */ public static Pose2d simActualOrRawOdometryPose = Pose2d.kZero; + + /** + * If autonomous has ever been initialized. + */ + public static boolean hasAutoStarted = false; } diff --git a/src/main/java/com/team1816/lib/auto/AutoModeManager.java b/src/main/java/com/team1816/lib/auto/AutoModeManager.java index 716a2024..a3646f2c 100644 --- a/src/main/java/com/team1816/lib/auto/AutoModeManager.java +++ b/src/main/java/com/team1816/lib/auto/AutoModeManager.java @@ -2,83 +2,245 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.FlippingUtil; -import com.pathplanner.lib.util.PPLibTelemetry; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.team1816.lib.BaseRobotState; +import com.team1816.lib.util.FieldContainer; import com.team1816.lib.util.GreenLogger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import org.json.simple.parser.ParseException; -import java.util.Optional; +import java.io.IOException; +import java.util.*; import java.util.function.Consumer; import java.util.stream.Stream; /** - * Manages SmartDashboard and logging of autos. Does NOT interact directly with {@link CommandScheduler} or the drivetrain. + * Manages creating the auto chooser to select the auto routine. Also handles resetting the robot's + * pose based on the auto starting pose, and drawing the path preview. Does NOT interact directly + * with {@link CommandScheduler} or the drivetrain. */ public class AutoModeManager { private final SendableChooser autoChooser; - private Consumer listener; + /** + * A mapping of the auto names as displayed in the {@link #autoChooser} to the path preview for + * that auto to display on the dashboard. + */ + private final HashMap> autoPathPreviewMap = new HashMap<>(); + /** + * A consumer that will accept a {@link Pose2d} to reset the robot's pose to. + */ + private final Consumer resetPoseConsumer; /** - * Puts dropdown in SmartDashboard and adds all autos. + * Constructs the {@link AutoModeManager}, including putting the auto dropdown in + * SmartDashboard and handling the creation of mirrored versions of autos. + * + * @param resetPoseConsumer A consumer that will accept a {@link Pose2d} that is the auto + * starting pose to reset the robot's pose to. */ - public AutoModeManager() { + public AutoModeManager(Consumer resetPoseConsumer) { + this.resetPoseConsumer = resetPoseConsumer; + + // Build the auto chooser with all the autos from PathPlanner and mirrored versions of most + // autos. autoChooser = AutoBuilder.buildAutoChooserWithOptionsModifier( autos -> autos.flatMap(auto -> { // Get the name of the original auto for PathPlanner to look for. - String autoName = auto.getName(); + String originalAutoName = auto.getName(); // Don't create a mirrored version if marked with "[DM]" (Don't Mirror). - if (autoName.startsWith("[DM]")) { - // Remove the "[DM]" marker for the display name, and any spaces. - auto.setName(autoName.substring(5).stripLeading()); + if (originalAutoName.startsWith("[DM]")) { + // Remove the "[DM]" marker for the display name, and any leading spaces. + String modifiedAutoName = originalAutoName.substring(5).stripLeading(); + auto.setName(modifiedAutoName); + addAutoPathPreviewToMap(originalAutoName, modifiedAutoName, false); return Stream.of(auto); } // Rename the original auto to specify it is the left version. - auto.setName("Left " + autoName); + String leftAutoName = "Left " + originalAutoName; + auto.setName(leftAutoName); + addAutoPathPreviewToMap(originalAutoName, leftAutoName, false); // Create a mirrored version of the auto. - PathPlannerAuto mirroredAuto = new PathPlannerAuto(autoName, true); + PathPlannerAuto mirroredAuto = new PathPlannerAuto(originalAutoName, true); // Rename the mirrored auto to specify it is the right version. - mirroredAuto.setName("Right " + autoName); + String rightAutoName = "Right " + originalAutoName; + mirroredAuto.setName(rightAutoName); + addAutoPathPreviewToMap(originalAutoName, rightAutoName, true); // Return both the original and the mirrored auto. return Stream.of(auto, mirroredAuto); }) ); + + // Publish the auto chooser to the SmartDashboard. SmartDashboard.putData("Auto Mode", autoChooser); - listener = a -> {}; + // Display the active path on the main Field2d. This is the path that PathPlanner is + // actively running, not a path that is scheduled to run once auto starts. + PathPlannerLogging.setLogActivePathCallback(pose2ds -> + FieldContainer.field.getObject("activePath").setPoses(pose2ds) + ); + + // Create a trigger and use it to reset the pose to the auto start pose whenever the + // alliance color changes. + new Trigger(() -> + // If the alliance is blue, defaulting to true if the alliance is empty. + DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue + ) + .onChange(Commands.runOnce(() -> + resetRobotPoseToAutoStart(autoChooser.getSelected(), false) + ).ignoringDisable(true)); + + // Register a listener to reset the pose to the auto start pose whenever the selected auto + // changes. + autoChooser.onChange(autoCommand -> resetRobotPoseToAutoStart(autoCommand, false)); - autoChooser.onChange(a -> { - // log starting pose - Pose2d startingPose = a instanceof PathPlannerAuto - ? ((PathPlannerAuto) a).getStartingPose() + // Use the autonomous trigger to force reset the pose to the auto start pose whenever auto + // starts, to make sure the pose is set correctly at the beginning of auto. + RobotModeTriggers.autonomous().onTrue(Commands.runOnce(() -> + resetRobotPoseToAutoStart(autoChooser.getSelected(), true) + ).ignoringDisable(true)); + } + + /** + * Resets the robot pose using the {@link #resetPoseConsumer} to the start pose of the passed + * in auto command. + * + * @param autoCommand The auto command to use the start pose of. + * @param forceResetPose If the pose should be reset regardless of whether the match has + * already started. + */ + private void resetRobotPoseToAutoStart(Command autoCommand, boolean forceResetPose) { + // Only reset the pose if auto has not started, or if forceResetPose is true. This is to + // prevent resetting the pose in the middle of a match. This is based on if auto has ever + // started, rather than just checking if we are currently in teleop or auto, because we + // want to prevent the pose resetting even during the disabled period between auto and + // teleop. + if (!BaseRobotState.hasAutoStarted || forceResetPose) { + // Get the starting pose for the auto. If this is the default None auto, it will just + // be an empty Command rather than a PathPlannerAuto, so use Pose2d.kZero in that case. + Pose2d startingPose = autoCommand instanceof PathPlannerAuto + ? ((PathPlannerAuto) autoCommand).getStartingPose() : Pose2d.kZero; - Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - startingPose = FlippingUtil.flipFieldPose(startingPose); - } - GreenLogger.log("Auto Mode Manager- starting pose: " + startingPose); - // TODO: why is this not PPLibTelemetry.setCurrentPose? look into this after Pittsburgh - PPLibTelemetry.setTargetPose(startingPose); + // Flip the starting pose based on alliance, defaulting to blue if the alliance is empty. + startingPose = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue + ? startingPose + : FlippingUtil.flipFieldPose(startingPose); + // Call the consumer to reset the robot's pose. + resetPoseConsumer.accept(startingPose); + } + } + + /** + * Updates the display of the selected auto path on the field. + */ + public void updateAutoPathPreviewDisplay() { + String autoName = autoChooser.getSelected().getName(); + List path = autoPathPreviewMap.get(autoName); + if (path != null) { + // Flip the path based on alliance, defaulting to blue if the alliance is empty. + path = DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Blue + ? path + : path.stream().map(FlippingUtil::flipFieldPose).toList(); + // The number of Pose2ds to move through in the path per second. + double pose2dsPerSecond = 300; + // Zero-indexed progress through the path to display. Use the timer to have it count + // up, using modulus to loop back to the start when the path ends. + int pathProgress = (int) ((Timer.getFPGATimestamp() * pose2dsPerSecond) % (path.size())); + // Show the path on the field up to the pathProgress. + FieldContainer.field.getObject("autoPathPreview").setPoses( + path.stream().limit(pathProgress + 1).toList() + ); + } + else { + // Clear the poses if no valid path was found. + clearAutoPathPreviewDisplay(); + } + } - if (listener != null) listener.accept(a); - }); + /** + * Clear the display of the selected auto path on the field. + */ + public void clearAutoPathPreviewDisplay() { + FieldContainer.field.getObject("autoPathPreview").setPoses(); } /** - * Retrieves currently selected auto. + * Adds the paths from the auto to the {@link #autoPathPreviewMap} so we can look it up later for + * displaying on the field using the name stored in the {@link #autoChooser}. This is necessary + * because we modify the auto names when we build the auto chooser, and the only way to get the + * paths is to load the auto from the file, which requires having the original auto file name. + * + * @param originalAutoName The name of the auto file that contains the paths. + * @param modifiedAutoName The modified name of the auto that will be used later to find the + * paths from the map. + * @param mirror If the paths stored should be mirrored to the other side of the current + * alliance from the original paths stored in the auto file. */ - public Command getSelectedAuto() { - return autoChooser.getSelected(); + private void addAutoPathPreviewToMap(String originalAutoName, String modifiedAutoName, boolean mirror) { + try { + // Load the paths for the auto. + List pathPlannerPaths = PathPlannerAuto.getPathGroupFromAutoFile(originalAutoName); + + // Combine the paths into a single list of Pose2ds, with repeated Pose2ds at the end of + // each path and at the very end to create pauses when running through the path with + // the preview. + // The number of times to repeat the last Pose2d in each path. + int pathEndRepeats = 5; + // The number of times to repeat the last Pose2d in the whole auto. + int autoEndRepeats = 10; + List combinedPath = new ArrayList<>(); + for (int i = 0; i < pathPlannerPaths.size(); i++) { + PathPlannerPath path = pathPlannerPaths.get(i); + + // Mirror the path if necessary. + if (mirror) path = path.mirrorPath(); + + // Get the list of poses representing the path. + List pathPoses = path.getPathPoses(); + if (pathPoses.isEmpty()) continue; + + // Add all the poses from the individual path to the combined path. + combinedPath.addAll(pathPoses); + + // Repeat the last pose in the individual path to create a pause at the end of each + // path. + Pose2d lastPose = pathPoses.get(pathPoses.size() - 1); + combinedPath.addAll(Collections.nCopies(pathEndRepeats, lastPose)); + + // If this is the last path in the auto, repeat the last pose in the path to create + // a longer pause at the very end of the auto. + if (i == pathPlannerPaths.size() - 1) { + combinedPath.addAll(Collections.nCopies(autoEndRepeats, lastPose)); + } + } + + autoPathPreviewMap.put( + modifiedAutoName, + combinedPath + ); + } catch (IOException e) { + GreenLogger.log(e); + GreenLogger.log("IOException while getting path group. See above stack trace for details."); + } catch (ParseException e) { + GreenLogger.log(e); + GreenLogger.log("ParseException while getting path group. See above stack trace for details."); + } } /** - * Registers a single listener to changes to the auto choosers. + * Retrieves currently selected auto from the {@link #autoChooser}. */ - public void onChange(Consumer listener) { - this.listener = listener; + public Command getSelectedAuto() { + return autoChooser.getSelected(); } } diff --git a/src/main/java/com/team1816/season/Robot.java b/src/main/java/com/team1816/season/Robot.java index 5180123f..00d99c3c 100644 --- a/src/main/java/com/team1816/season/Robot.java +++ b/src/main/java/com/team1816/season/Robot.java @@ -53,23 +53,12 @@ public void disabledInit() { } } - @Override - public void disabledPeriodic() { - try { - robotContainer.updateInitialPose(); - } catch (Throwable t) { - robotStatusEvent.Publish(LedManager.RobotLEDStatus.ERROR); - GreenLogger.log(t); - } - } - @Override public void autonomousInit() { try { + super.autonomousInit(); RobotState.resetCameraQueue = true; robotContainer.autonomousInit(); - // Ensure pose is always initialized before scheduling auto - robotContainer.forceUpdatePose(); autonomousCommand = robotContainer.autoModeManager.getSelectedAuto(); // schedule the autonomous command if (autonomousCommand != null) { diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index fa548edc..6ede3cb1 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -19,13 +19,13 @@ shooterSettings: - 0.108 # 360 launchVelocitiesRPS: - 31 # 48 - - 39 # 96 - - 41 #120 - - 43 # 156 - - 48 # 204 + - 38 # 96 + - 40 #120 + - 42 # 156 + - 45 # 204 - 47 #210 - - 56.5 # 264 - - 100 # 360 + - 48 # 264 + - 49 # 360 subsystems: drivetrain: implemented: true @@ -218,7 +218,7 @@ subsystems: id: 10 deviceType: CANcoder constants: - intakeOnSpeed: .4 + intakeOnSpeed: .55 intakeSlowSpeed: .1 outtakeSpeed: -0.6 flipperMotorOutPosition: 0.164 #0.166