diff --git a/src/main/deploy/choreo/Spinny Path.traj b/src/main/deploy/choreo/Spinny Path.traj new file mode 100644 index 00000000..805785d2 --- /dev/null +++ b/src/main/deploy/choreo/Spinny Path.traj @@ -0,0 +1,223 @@ +{ + "name":"Spinny Path", + "version":"v2025.0.0", + "snapshot":{ + "waypoints":[ + {"x":0.0, "y":0.0, "heading":0.0, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.328159809112549, "y":1.8797401189804075, "heading":1.9215241981719864, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.332931041717529, "y":1.074251651763916, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.744625091552734, "y":2.9537248611450195, "heading":0.0, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.268489360809326, "y":1.4322465658187866, "heading":3.113822016996372, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.079949140548706, "y":3.2222208976745605, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.328159809112549 m", "val":2.328159809112549}, "y":{"exp":"1.8797401189804077 m", "val":1.8797401189804075}, "heading":{"exp":"1.9215241981719866 rad", "val":1.9215241981719864}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.332931041717529 m", "val":4.332931041717529}, "y":{"exp":"1.074251651763916 m", "val":1.074251651763916}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.744625091552734 m", "val":4.744625091552734}, "y":{"exp":"2.9537248611450195 m", "val":2.9537248611450195}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.268489360809326 m", "val":7.268489360809326}, "y":{"exp":"1.4322465658187866 m", "val":1.4322465658187866}, "heading":{"exp":"3.113822016996372 rad", "val":3.113822016996372}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.079949140548706 m", "val":3.079949140548706}, "y":{"exp":"3.2222208976745605 m", "val":3.2222208976745605}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "waypoints":[0.0,1.1901207758286565,2.051646391922014,2.7624109469050144,4.090378690411133,5.917899521471614], + "samples":[ + {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.275380998237953, "ay":3.307722104326171, "alpha":30.908246495481706, "fx":[-13.516260048745425,-151.32450824986734,154.45896259442148,104.87223310164472], "fy":[153.8943048419133,-20.35648425049452,-1.9750355631325172,113.49973023703106]}, + {"t":0.031318967784964645, "x":0.000625496417582301, "y":0.0016222354962726394, "heading":0.0, "vx":0.0399436163973705, "vy":0.10359444202700682, "omega":0.968014376281738, "ax":1.280220835517808, "ay":3.633546195001896, "alpha":29.851705697989143, "fx":[-12.096492755233044,-152.16300963215812,154.4477516606671,104.6607519885674], "fy":[153.99100296895224,1.4153863717079829,0.12257048610186946,113.67321066853836]}, + {"t":0.06263793556992929, "x":0.00250435931466199, "y":0.006648738784447873, "heading":0.0303172110661504, "vx":0.08003881150259326, "vy":0.21739335825345207, "omega":1.902938985363306, "ax":1.2879429792680508, "ay":3.93633099175066, "alpha":28.63114241936016, "fx":[-12.251947710372502,-149.23551552975286,154.31111208888527,102.59747059925144], "fy":[153.94745892604473,16.36693979501934,5.8093737681733515,115.5111180275855]}, + {"t":0.09395690335489391, "x":0.005642749575042184, "y":0.01538780409792563, "heading":0.08991529584549708, "vx":0.12037585617916072, "vy":0.3406751817750489, "omega":2.7996368124419813, "ax":3.691037726884133, "ay":5.405349240535846, "alpha":16.098453979588687, "fx":[-13.48574830425154,34.507058558083685,153.67694261890455,98.76335023665494], "fy":[153.79832389054812,113.10882938669288,14.79475488416626,118.76960637141237]}, + {"t":0.12527587113985858, "x":0.011223025514454964, "y":0.028708392522979045, "heading":0.17759703098396865, "vx":0.23597534784053403, "vy":0.5099651405058742, "omega":3.3038237740164553, "ax":5.12458567258243, "ay":4.3022734144134, "alpha":16.284623418407232, "fx":[-15.222785331393023,149.56541886920309,152.06156240088313,93.26610737159392], "fy":[153.56709714142698,15.6411276802225,26.45399036462553,123.08461754078488]}, + {"t":0.15659483892482323, "x":0.021126825845981356, "y":0.04678997644843588, "heading":0.2810693813295903, "vx":0.3964720814314346, "vy":0.6447079029739973, "omega":3.813841370247832, "ax":5.011461239103741, "ay":4.677524580194257, "alpha":15.740583075650534, "fx":[-14.780819896819777,150.1310893819156,148.96470054700316,86.97417025061904], "fy":[153.50338484987748,25.342993662992267,40.16434117988846,127.53772140467387]}, + {"t":0.18791380670978788, "x":0.03600173758693273, "y":0.06927560236918487, "heading":0.4005149563413477, "vx":0.5534258745345236, "vy":0.7912031446144815, "omega":4.306820184510691, "ax":4.872048273965295, "ay":5.2506042261158585, "alpha":14.15890365437622, "fx":[-8.275404310987389,145.51850050437335,143.3472257441346,80.36999058402017], "fy":[153.8138360926826,46.722908997256155,56.784405831875574,131.68561498265743]}, + {"t":0.21923277449475256, "x":0.05572390658050297, "y":0.09663036857838142, "heading":0.5354001189556735, "vx":0.7060133974736352, "vy":0.9556466492238032, "omega":4.750262431932518, "ax":4.76674155734787, "ay":5.994168422401809, "alpha":10.66776328296938, "fx":[13.809222496139569,131.72641515020678,132.47594010131905,75.14677075312356], "fy":[153.1062213409574,77.96398295128088,78.51001023374045,134.51573555292643]}, + {"t":0.25055174227971716, "x":0.08017331278212723, "y":0.12950000839624193, "heading":0.6841734350314957, "vx":0.8553028227474654, "vy":1.143377816942658, "omega":5.084365766529466, "ax":4.713246354185454, "ay":6.736139749059915, "alpha":3.2274860173327156, "fx":[66.86232928766199,99.4189628298747,104.11926394221834,78.7944398291369], "fy":[137.9460429517108,116.53391853688234,112.78733979850628,131.7998204412515]}, + {"t":0.2818707100646818, "x":0.109272073557465, "y":0.1686130861882967, "heading":0.8434105226804093, "vx":1.0029168334768015, "vy":1.3543467607384854, "omega":5.185447297132733, "ax":3.6372555009285903, "ay":6.207130626625637, "alpha":-14.025881151276092, "fx":[118.06782525188368,42.907542875066675,-27.400935902635815,135.90255332848284], "fy":[97.544014493442,147.15139368233,149.6436951216839,65.53479056798422]}, + {"t":0.3131896778496464, "x":0.14246624503956695, "y":0.21407404689767093, "heading":1.005813379529941, "vx":1.1168319213360696, "vy":1.5487476848708412, "omega":4.746171177200175, "ax":3.382830975367996, "ay":4.912500394466925, "alpha":-21.50841364196183, "fx":[125.77957641394244,35.11054755423694,-60.98112088739167,150.71817822227635], "fy":[87.18228352443118,148.9153235252005,137.55885010332773,-9.699127927693867]}, + {"t":0.34450864563461103, "x":0.1791033398113416, "y":0.2649885068971691, "heading":1.1544585617306011, "vx":1.2227786956756004, "vy":1.702602126468777, "omega":4.0725498632418775, "ax":3.7496910420236063, "ay":4.900357256992043, "alpha":-19.774351994155055, "fx":[126.20440668977716,37.72105390216379,-36.961842871185915,150.84349220068987], "fy":[86.22459661783716,147.8210313926637,142.19543693402989,-13.18339648850425]}, + {"t":0.37582761341957566, "x":0.21923850063268743, "y":0.320715573730007, "heading":1.282006619700136, "vx":1.340215148624308, "vy":1.856076257535329, "omega":3.453237570168384, "ax":4.044816581303205, "ay":4.793411928929155, "alpha":-18.145447828373435, "fx":[125.50016607649412,35.20125199849659,-11.482416106727092,150.45336890732818], "fy":[86.82708195223178,147.9051709970932,137.7008820374246,-17.298831996246353]}, + {"t":0.4071465812045403, "x":0.26319639097716635, "y":0.3811968417835289, "heading":1.390158455914069, "vx":1.466894628830234, "vy":2.006200971317526, "omega":2.8849408741878, "ax":4.529337801934489, "ay":4.355904038434729, "alpha":-16.68167874602874, "fx":[123.02676311925713,28.407158047372665,34.00570627631833,150.12995162677433], "fy":[89.73494859809108,148.69075924352802,102.39255450342962,-18.098043945496535]}, + {"t":0.4384655489895049, "x":0.3113593799219236, "y":0.4461652900356118, "heading":1.4805118262142845, "vx":1.6087488135362429, "vy":2.1426233895716607, "omega":2.3624879149417963, "ax":5.389767919428755, "ay":3.5476355917422357, "alpha":-15.524518097063696, "fx":[118.68340071988524,24.565816105410427,105.73869361951203,150.3292151698299], "fy":[94.5452834500584,148.44199770097762,29.42055945993404,-9.570614889971283]}, + {"t":0.46978451677446953, "x":0.3643870838835279, "y":0.5150099413452307, "heading":1.5545025091147149, "vx":1.777550781373268, "vy":2.25373167438223, "omega":1.876276032782758, "ax":5.433060714763641, "ay":3.6024342757353116, "alpha":-15.111477381603848, "fx":[113.73829725710712,22.42377702599908,116.80037751219828,149.56215044010412], "fy":[99.10262801330164,147.3643592832671,19.512045504761748,0.918117819347266]}, + {"t":0.5011034845594342, "x":0.4227227237075501, "y":0.5873612648522153, "heading":1.6132655377411391, "vx":1.9477086348727073, "vy":2.3665561974114366, "omega":1.403000159485085, "ax":5.237540651636733, "ay":3.7602392001997793, "alpha":-14.873414455454917, "fx":[108.46108415536824,19.474503850946157,112.85470121812943,147.24862257401844], "fy":[102.54681024560132,145.32843494232438,21.179124109608455,9.534232566867024]}, + {"t":0.5324224523443989, "x":0.4862916412245529, "y":0.6633235296303857, "heading":1.6572060545383531, "vx":2.111743001813761, "vy":2.4843230077862546, "omega":0.9371801713022648, "ax":4.86436291820375, "ay":3.865443514746681, "alpha":-14.46501196963671, "fx":[102.40978252171044,17.582061380380193,98.520489723859,141.87858625792975], "fy":[103.47430826918968,140.06958498177343,26.018205083929864,16.820880785659075]}, + {"t":0.5637414201293636, "x":0.5548149249289341, "y":0.7430257256341334, "heading":1.6865575701320763, "vx":2.2640898273433607, "vy":2.6053847086992064, "omega":0.4841509274160849, "ax":4.042640900981286, "ay":3.4830070414894276, "alpha":-12.546556702251376, "fx":[90.5246546338199,21.872140255159454,65.62061879542254,121.49376538749952], "fy":[93.03710852945386,116.83106225402194,25.76763061478981,22.413224291603072]}, + {"t":0.5950603879143282, "x":0.6277065495353578, "y":0.8263318874363789, "heading":1.7017206774308815, "vx":2.390701167487374, "vy":2.7144688940264188, "omega":0.09120572224604172, "ax":0.7062586703266118, "ay":0.14576029723965575, "alpha":-1.3723442336426286, "fx":[16.456927198524927,8.655122443820597,9.70792249474212,17.505320230070367], "fy":[7.148104014885282,6.164942290724699,-1.7837998155422246,-0.7301575881761443]}, + {"t":0.6263793556992929, "x":0.7029272190886755, "y":0.9114177377973768, "heading":1.7045771465077098, "vx":2.412820460031185, "vy":2.7190339560799943, "omega":0.04822531740270625, "ax":0.10100085623523757, "ay":-0.07340286029439083, "alpha":-0.028750682948716865, "fx":[1.9419559961803667,1.7774283302233165,1.7995217366514134,1.9640453737011832], "fy":[-1.2662151572804714,-1.288376859211036,-1.4529231745710425,-1.4307559224282784]}, + {"t":0.6576983234842576, "x":0.7785438000932535, "y":0.9965390750581002, "heading":1.7060875136698648, "vx":2.4159837025938704, "vy":2.71673505426311, "omega":0.04732487568963965, "ax":0.04666208859720116, "ay":-0.04140765731455801, "alpha":0.00003891243587867593, "fx":[0.864182355026334,0.8644046747682931,0.8643681161238964,0.8641456740709157], "fy":[-0.7670810770422459,-0.7670455266974623,-0.7668243300253065,-0.7668595813559584]}, + {"t":0.6890172912692223, "x":0.8542328007458673, "y":1.0816041047781229, "heading":1.707569679927016, "vx":2.4174451110434254, "vy":2.715438209177625, "omega":0.047326094386965376, "ax":0.09263382460747067, "ay":-0.0830267438961762, "alpha":0.0011453792797180603, "fx":[1.7129382820750194,1.7194902545958717,1.718589120642341,1.7120371402050545], "fy":[-1.5415492277880836,-1.54064535773624,-1.534093474275239,-1.5349973419803402]}, + {"t":0.720336259054187, "x":0.9299901175289648, "y":1.1666081070308336, "heading":1.7090518843525095, "vx":2.4203463068121045, "vy":2.7128378972602496, "omega":0.04736196648372843, "ax":0.3220955701052671, "ay":-0.2904295023778933, "alpha":0.004186684508457721, "fx":[5.9555116162242605,5.979458840620938,5.976196933337704,5.95224920777613], "fy":[-5.393004557446222,-5.389608417125746,-5.365666101717531,-5.369061895883857]}, + {"t":0.7516552268391516, "x":1.0059508337284082, "y":1.2514289518235453, "heading":1.710535212255046, "vx":2.430434007595911, "vy":2.703741945031473, "omega":0.04749308912097463, "ax":1.1453681937839872, "ay":-1.0570867924218186, "alpha":0.026006554322554113, "fx":[21.147852625207612,21.2968507927299,21.281188564732467,21.13214675839806], "fy":[-19.666634907493588,-19.640482673131967,-19.49210047451195,-19.51822822181019]}, + {"t":0.7829741946241163, "x":1.0826312512002645, "y":1.3355889222452295, "heading":1.7120226467832342, "vx":2.4663057571589553, "vy":2.6706350778337025, "omega":0.04830758755800064, "ax":3.0497066870429026, "ay":-3.0762579815678532, "alpha":0.2716544933688496, "fx":[55.582279423489254,57.19448954535781,57.39489817907868,55.7750018817088], "fy":[-58.04232004631098,-57.404534805219306,-55.91418648747348,-56.5527599993953]}, + {"t":0.814293162409081, "x":1.161369096462775, "y":1.4177217397201989, "heading":1.7135355905617329, "vx":2.561819422644043, "vy":2.5742898532107383, "omega":0.05681552588446053, "ax":3.8713093391334463, "ay":-5.465162376236233, "alpha":2.327602247173598, "fx":[60.48005340032964,77.24606399466356,83.47689645544104,65.61455246728451], "fy":[-110.28324685680184,-101.44451926023184,-91.3832179213074,-101.79196609224891]}, + {"t":0.8456121301940457, "x":1.2435012770151956, "y":1.4956655126333862, "heading":1.715314994186594, "vx":2.6830648351219963, "vy":2.403126608809795, "omega":0.12971362567990177, "ax":2.862451914848389, "ay":-6.902634976071243, "alpha":5.595531961091559, "fx":[25.96951500620769,70.63944520500625,87.80881176427178,27.6555654918017], "fy":[-141.1787341227277,-125.77548352400596,-109.24949134132204,-135.19871111911056]}, + {"t":0.8769310979790104, "x":1.3289359558383602, "y":1.5675436369694509, "heading":1.7193774910505335, "vx":2.7727138744291433, "vy":2.1869432063628493, "omega":0.3049599109090683, "ax":2.5561857220041415, "ay":-7.3370741954473635, "alpha":5.969625335436646, "fx":[18.781485852103582,67.75273237957076,86.23107273130815,16.61739680886033], "fy":[-147.5847904179002,-132.7866063631697,-118.558795632248,-144.6589605789864]}, + {"t":0.908250065763975, "x":1.4170281451895217, "y":1.6324380544081258, "heading":1.7289285206760006, "vx":2.852770972708978, "vy":1.957153615999738, "omega":0.49192241447791746, "ax":2.636598781491906, "ay":-7.510147312470924, "alpha":5.167153523026362, "fx":[22.91403915829864,67.18804965001178,82.97742680920845,22.26081490565347], "fy":[-149.12841806783092,-135.28283692122454,-124.52514059628643,-147.47539850100378]}, + {"t":0.9395690335489396, "x":1.5076670779128232, "y":1.690050817284687, "heading":1.7443350229277363, "vx":2.9353465250084, "vy":1.7219435542601225, "omega":0.6537523292055466, "ax":2.8052863332184854, "ay":-7.594108052698323, "alpha":3.967053366263392, "fx":[30.85644868947279,66.62319555359203,78.11288454734715,32.245525065079164], "fy":[-148.80206959306048,-136.6995734111466,-129.56227986052176,-147.56835454358458]}, + {"t":0.9708880013339044, "x":1.6009749226309289, "y":1.7402558662042336, "heading":1.7648098710654705, "vx":3.0232051973060714, "vy":1.484103928802123, "omega":0.7779963457847854, "ax":2.9587897313599125, "ay":-7.638381261065568, "alpha":2.6047427342024156, "fx":[40.192741818044496,64.97871899478577,71.67191421460025,42.36743858956265], "fy":[-147.23086851589264,-138.15632840540349,-134.35925375277975,-146.16594019574987]}, + {"t":1.002206969118869, "x":1.6971096943097213, "y":1.7829903102537212, "heading":1.7891759135559244, "vx":3.1158714375850165, "vy":1.244877712157533, "omega":0.8595741995653917, "ax":3.0559561097644083, "ay":-7.665516643685514, "alpha":1.123508853746757, "fx":[50.02124880727672,61.343491889135485,63.620135851111534,51.42479971270172], "fy":[-144.63546153998263,-140.23045046883794,-139.06381191594215,-143.99307317260957]}, + {"t":1.0335259369038337, "x":1.7961943311515405, "y":1.818219127884367, "heading":1.8160968902208996, "vx":3.211580828538994, "vy":1.004801643338836, "omega":0.894761337162009, "ax":2.765612735041673, "ay":-7.78676983476084, "alpha":-1.665907230891256, "fx":[61.47104798184442,42.59846753695972,42.0167621808133,58.812438614149976], "fy":[-140.4118487447157,-147.2204033701706,-147.55729292652822,-141.71665847634654]}, + {"t":1.0648449046887982, "x":1.8981340916483032, "y":1.845869543571535, "heading":1.8441198917147088, "vx":3.298196964693452, "vy":0.7609280497350269, "omega":0.842586842264986, "ax":2.353255714741206, "ay":-7.845494448384622, "alpha":-4.2763555310605215, "fx":[70.13828895862973,17.394145663777618,23.869992344595268,62.945582426743854], "fy":[-136.52355844628184,-152.48859310226894,-151.9245174553141,-140.3203236880551]}, + {"t":1.0961638724737628, "x":2.0025843442112303, "y":1.865853289203783, "heading":1.8705088418836409, "vx":3.371898504613216, "vy":0.51521526184895, "omega":0.7086558011506462, "ax":2.2461266758155825, "ay":-7.854394294717284, "alpha":-4.908604934878396, "fx":[71.60972191786124,10.041321282043398,20.54473329266142,64.21525666525879], "fy":[-135.99158274157847,-153.37515166532737,-152.60301360583082,-139.94661649427738]}, + {"t":1.1274828402587274, "x":2.109290312683645, "y":1.878137199117281, "heading":1.892703210090506, "vx":3.442244873614034, "vy":0.2692237399622892, "omega":0.5549233613260711, "ax":1.9363988977580788, "ay":-7.872590841176772, "alpha":-6.054032434917486, "fx":[72.12489182197997,-5.736566383773367,12.811608212656347,64.26398788623759], "fy":[-135.85674170610267,-153.75089524034595,-153.58730991739304,-140.06956337726308]}, + {"t":1.158801808043692, "x":2.218047554278625, "y":1.8827079841877243, "heading":1.9100828369670015, "vx":3.5028908883117604, "vy":0.022662321023266135, "omega":0.36531731452775934, "ax":0.8264596714245628, "ay":-7.498642384718694, "alpha":-11.957760669368062, "fx":[84.9966665243035,-74.37405616964031,-15.777648511907882,66.3857822937477], "fy":[-128.03611889085929,-134.83650262141006,-153.4671095015687,-139.21968598520047]}, + {"t":1.1901207758286565, "x":2.328159809112549, "y":1.8797401189804075, "heading":1.9215241981719864, "vx":3.5287747521366786, "vy":-0.2121874182547091, "omega":-0.009187406656496274, "ax":-0.18370376905598024, "ay":-8.064017386505045, "alpha":-6.5279313480970735, "fx":[25.28416171902864,-55.77700188561233,-19.788458178860736,36.67105350362497], "fy":[-151.5530032709477,-143.42252353126827,-152.84448276521815,-149.62691056395164]}, + {"t":1.2154597645372849, "x":2.4175164178918083, "y":1.8717746953492465, "heading":1.921291398578456, "vx":3.5241198844068364, "vy":-0.41652146375754073, "omega":-0.17459858537662712, "ax":-1.099267996856237, "ay":-8.1849335623837, "alpha":-2.8587781389679163, "fx":[-12.474383305172651,-43.06241821582218,-26.47779857554211,0.5720327454520414], "fy":[-153.20104650471333,-147.6647827348387,-151.65381981001838,-153.88570872031303]}, + {"t":1.2407987532459128, "x":2.5064611514553787, "y":1.8585928356634391, "heading":1.9168672469950552, "vx":3.49626554504674, "vy":-0.6239194028756521, "omega":-0.24703713236040784, "ax":-1.7676117356090384, "ay":-8.108385033469139, "alpha":-0.2704707990981753, "fx":[-32.25377362370833,-34.83509211409512,-33.215361941437116,-30.65459058856186], "fy":[-150.2831044763362,-149.71646551138355,-150.10033559385764,-150.6341247780841]}, + {"t":1.266137741954541, "x":2.5944855243847273, "y":1.8401802964808096, "heading":1.910607575887563, "vx":3.451476051236904, "vy":-0.8293776796839353, "omega":-0.2538905888847701, "ax":-2.02456413422507, "ay":-8.038491138452429, "alpha":0.4523568398818919, "fx":[-38.19689398975543,-34.02549313759541,-36.778036083858005,-40.99548436525812], "fy":[-148.7601313179565,-149.74913021813248,-149.06428329635915,-147.98218663321538]}, + {"t":1.2914767306631691, "x":2.6812924868489207, "y":1.8165840905311343, "heading":1.9041742451225847, "vx":3.4001756434998818, "vy":-1.033064915875588, "omega":-0.24242832402673223, "ax":-2.279009705969272, "ay":-7.954372886604066, "alpha":0.9528784929819945, "fx":[-43.47234572772927,-34.94933718444376,-40.874678214156944,-49.55090996952143], "fy":[-147.13011366416913,-149.33418653284977,-147.73872094774637,-145.12055727795675]}, + {"t":1.3168157193717973, "x":2.766717863645558, "y":1.7878536606688034, "heading":1.89803135655742, "vx":3.342427842293472, "vy":-1.234620680633466, "omega":-0.21828334665236693, "ax":-2.573131484337025, "ay":-7.843615034302334, "alpha":1.3246230936380246, "fx":[-49.09078173062552,-37.64182395936272,-46.14024213487837,-57.76531758669488], "fy":[-145.10942093872336,-148.40609933650137,-145.8452256030883,-141.75700478307817]}, + {"t":1.3421547080804257, "x":2.8505855470054335, "y":1.7540515683932691, "heading":1.8925002773013135, "vx":3.277227292666041, "vy":-1.4333699534224775, "omega":-0.18471873703948508, "ax":-2.926254210566554, "ay":-7.694005318262266, "alpha":1.5250258216277075, "fx":[-55.40846908375713,-42.78355403257418,-52.97499962930457,-65.63329920681896], "fy":[-142.4665808582393,-146.62552580804808,-143.05625397583742,-137.88510537728988]}, + {"t":1.3674936967890536, "x":2.9326877506178755, "y":1.7152614000711357, "heading":1.8878196913091985, "vx":3.20307897026592, "vy":-1.6283282673060495, "omega":-0.14607612496489436, "ax":-3.3605383200185637, "ay":-7.48448472721241, "alpha":1.489407157126316, "fx":[-62.885934614215685,-51.29019428117445,-61.706226630887706,-73.0932075272575], "fy":[-138.79653955233934,-143.31450709188312,-138.9014035364605,-133.49805428903014]}, + {"t":1.3928326854976818, "x":3.012771691554298, "y":1.6715984480857773, "heading":1.8841182700281127, "vx":3.1179263277200575, "vy":-1.817977541298784, "omega":-0.10833605382792084, "ax":-3.89484170397625, "ay":-7.1804314166296095, "alpha":1.1370858305947005, "fx":[-72.10435725742872,-64.07438587690726,-72.32156238965207,-80.06072664020432], "fy":[-133.37968313161034,-137.21929042859438,-132.82425985731578,-128.56056937773408]}, + {"t":1.41817167420631, "x":3.090526422065606, "y":1.6232275861835437, "heading":1.8813731439834296, "vx":3.01923497776111, "vy":-1.99992241188784, "omega":-0.07952344880574078, "ax":-4.543193780356145, "ay":-6.7216941221989535, "alpha":0.34479460737038076, "fx":[-83.93314955432437,-81.88137473283618,-84.38682553310902,-86.3947909787565], "fy":[-124.75274404316636,-126.0071269955389,-124.25762178624947,-122.97938130052128]}, + {"t":1.443510662914938, "x":3.165572271697816, "y":1.570393694690965, "heading":1.8793581002120696, "vx":2.9041150418595563, "vy":-2.1702433433530905, "omega":-0.07078670214278686, "ax":-5.431983692687361, "ay":-5.8745748029428215, "alpha":-1.5213920251813646, "fx":[-102.9280873314586,-108.84901216134104,-98.88567626080068,-91.78203207022077], "fy":[-106.2585994371678,-101.1478284582949,-111.30318996938708,-116.52588013557796]}, + {"t":1.4688496516235665, "x":3.237415768415948, "y":1.5135159955961717, "heading":1.877564436765753, "vx":2.7664740684050995, "vy":-2.3190991279528492, "omega":-0.10933723749025424, "ax":-6.772813055402408, "ay":-3.467807264108836, "alpha":-6.8958601557333274, "fx":[-139.99972076339196,-143.06721079289383,-119.47809103075498,-99.23915106161272], "fy":[-29.1717619241248,-35.836669525176966,-87.06787679504653,-104.84659633894712]}, + {"t":1.4941886403321945, "x":3.305341132696078, "y":1.45363909127243, "heading":1.8747939417395547, "vx":2.594857834868609, "vy":-2.4069698570618017, "omega":-0.2840713601126591, "ax":-6.823446347028112, "ay":0.5896413635315972, "alpha":-13.242887632592383, "fx":[-99.72500448911484,-143.33481146636535,-139.43755783014328,-123.03811917299522], "fy":[105.49719068333182,40.077667878715026,-48.82237191928541,-53.06713730143245]}, + {"t":1.5195276290408226, "x":3.3689016602567023, "y":1.3928382030913768, "heading":1.8675958607532157, "vx":2.4219586049273345, "vy":-2.3920289412091345, "omega":-0.6196327403045476, "ax":-4.883071616502484, "ay":4.1675766931150005, "alpha":-15.59522667952396, "fx":[-44.89385042767069,-128.09327634250266,-146.8134500873826,-41.97643306587999], "fy":[142.24541714884296,79.03155386003105,-22.22923470854179,109.71968573917198]}, + {"t":1.5448666177494508, "x":3.4287040189009823, "y":1.3335645349670862, "heading":1.851894993743142, "vx":2.298226508373356, "vy":-2.286426762439952, "omega":-1.0148000130455002, "ax":-3.8380830700931554, "ay":5.514193200824726, "alpha":-15.67990172612591, "fx":[-28.54010798759179,-113.6203761735684,-148.1448611209127,5.949446785011203], "fy":[148.198994769053,100.19536110084576,18.28515653978488,141.85603345301868]}, + {"t":1.570205606458079, "x":3.485706606293051, "y":1.2773990264837718, "heading":1.8261809876710664, "vx":2.2009733647974894, "vy":-2.1467026831870606, "omega":-1.4121128658362023, "ax":-3.1941137723678934, "ay":6.137327081818421, "alpha":-15.168650978889028, "fx":[-16.28119380973713,-102.04447497430888,-142.91608361814178,24.596251234995336], "fy":[150.9944318465706,112.86320294210904,45.9103279907269,144.93432605835667]}, + {"t":1.595544595166707, "x":3.5404516322420596, "y":1.2249740308917196, "heading":1.7903994757083348, "vx":2.120037751985386, "vy":-1.9911890215597063, "omega":-1.7964711417153914, "ax":-2.6883750343146287, "ay":6.531453885288277, "alpha":-14.602075839825144, "fx":[-6.4172473536128365,-92.36854260161682,-135.99907777599188,35.60853818891933], "fy":[152.31668002494837,121.49478574281407,65.24512196929952,144.8457677161758]}, + {"t":1.6208835838753353, "x":3.593308190018646, "y":1.1766161216003832, "heading":1.7448787137330324, "vx":2.05191704734633, "vy":-1.8256885853094624, "omega":-2.166472976543251, "ax":-2.259362207239526, "ay":6.794138645406195, "alpha":-14.168575277628186, "fx":[2.2795945147964676,-83.89323631197406,-129.3483170538542,43.570331641069856], "fy":[152.82478931143308,127.90002439958629,78.76525097643832,143.87407927339655]}, + {"t":1.6462225725839634, "x":3.644576364950277, "y":1.1325361562541838, "heading":1.689982479442855, "vx":1.9946670938883864, "vy":-1.6535319828886612, "omega":-2.5254903455204185, "ax":-1.873951986098538, "ay":6.96060492240041, "alpha":-13.980154710679027, "fx":[10.663585653479144,-76.23697843821223,-123.9964946138761,50.73253265254067], "fy":[152.74803357873998,132.90287838212487,87.79438616363032,142.25199936630648]}, + {"t":1.6715615612925916, "x":3.6945176130389905, "y":1.0928719061437064, "heading":1.625989108093964, "vx":1.9471830456721249, "vy":-1.4771572933547363, "omega":-2.8797333278791877, "ax":-1.507893454268184, "ay":7.044110284682223, "alpha":-14.116932988761246, "fx":[19.375536250425352,-69.1538778560287,-120.51228686186734,58.57381822764946], "fy":[152.09833838596165,136.9488084644617,93.1637918803387,139.6731040407745]}, + {"t":1.6969005500012198, "x":3.743373179932539, "y":1.0577036202078436, "heading":1.5530195778149734, "vx":1.908974550460609, "vy":-1.2986666623888423, "omega":-3.237442133481868, "ax":-1.1385993212586332, "ay":7.045393792744488, "alpha":-14.64425369791096, "fx":[28.926880164912763,-62.42061086139514,-119.12670820109471,68.26389238416749], "fy":[150.7377650209723,140.32494832911405,95.443800107308,135.47262185945917]}, + {"t":1.722239538709848, "x":3.791379137495859, "y":1.0270585184020051, "heading":1.4709860681498397, "vx":1.880123595115585, "vy":-1.120143508626652, "omega":-3.6085127125795182, "ax":-0.7419691605735265, "ay":6.953716199584469, "alpha":-15.620663528500693, "fx":[39.7067054194356,-55.781199565509944,-119.74888588166304,80.85236885916598], "fy":[148.39548526737775,143.23914917719296,95.09711287251427,128.45517847772916]}, + {"t":1.747578527418476, "x":3.83878137207037, "y":1.0009075813165011, "heading":1.3795500052708465, "vx":1.8613228469336625, "vy":-0.9439433723623772, "omega":-4.004324529349476, "ax":-0.32307738520165485, "ay":6.781943430954864, "alpha":-16.85551633626037, "fx":[51.281284635076545,-48.93702839407846,-121.32396440732732,95.04355085150902], "fy":[144.91012053022513,145.8378531844552,93.47107034549872,118.2415808524048]}, + {"t":1.7729175161271042, "x":3.8858416924364665, "y":0.9791662329088424, "heading":1.2780844712359778, "vx":1.8531363927180244, "vy":-0.7720957843428576, "omega":-4.431426267472072, "ax":-1.5763695789397614, "ay":7.786250701804865, "alpha":-7.654071295653216, "fx":[20.193560510104355,-44.024557887673375,-89.9787587053536,-2.980313281566424], "fy":[152.3570347764236,147.40518890619452,124.33148890254034,152.77402941016024]}, + {"t":1.7982565048357324, "x":3.9322922292135303, "y":0.96210174354053, "heading":1.165796611081385, "vx":1.8131927817566451, "vy":-0.5748000657272769, "omega":-4.625372693607663, "ax":-2.359469696451834, "ay":7.943235080625554, "alpha":-1.5619776266486625, "fx":[-31.62494886371116,-44.82806283974691,-55.817790609939394,-42.53758855732594], "fy":[150.46380551978012,147.13389024799508,143.2467717339591,147.6539331516517]}, + {"t":1.8235954935443608, "x":3.977479234949973, "y":0.950086925194956, "heading":1.048594344624864, "vx":1.753406205759902, "vy":-0.3735265217093278, "omega":-4.664951627052443, "ax":-2.064167895183802, "ay":8.044844438336224, "alpha":-0.6514341153748311, "fx":[-33.09724365300569,-38.31002227068179,-43.34973861648004,-38.17306647821001], "fy":[150.27837056360713,149.06077408276383,147.64693585032222,149.04035425076086]}, + {"t":1.8489344822529887, "x":4.021246110691669, "y":0.9432047947816424, "heading":0.930389188020686, "vx":1.701102278771127, "vy":-0.16967829932365702, "omega":-4.6814583087463415, "ax":-1.4623253887095429, "ay":8.167193228629257, "alpha":-1.6597005565813585, "fx":[-13.721216325097243,-26.752523185757376,-40.271458304788595,-27.59556558306939], "fy":[153.3640250829015,151.69022461834123,148.62094482858794,151.41581739285357]}, + {"t":1.874273470961617, "x":4.063880868626471, "y":0.9415272500718148, "heading":0.8117657687954495, "vx":1.6640484322582758, "vy":0.03727011767776321, "omega":-4.723513442409261, "ax":-1.014022705583017, "ay":8.22263843012684, "alpha":-2.355109209624901, "fx":[0.4344534029338248,-17.220423675938658,-37.40083651950123,-20.940107418728527], "fy":[154.05023155014896,153.15570771897904,149.45649784110935,152.53639890099996]}, + {"t":1.899612459670245, "x":4.105720639148022, "y":0.945111368657251, "heading":0.6920767150131885, "vx":1.6383541223712152, "vy":0.24562346001387833, "omega":-4.783189528079532, "ax":-1.4045864785999913, "ay":8.200800680550897, "alpha":0.362816932323448, "fx":[-28.81010624591303,-26.786758247602485,-23.189422037964057,-25.276716495036567], "fy":[151.40734582923145,151.76524420176642,152.36404013025296,152.04429065940397]}, + {"t":1.9249514483788732, "x":4.14678395830418, "y":0.9539679396113164, "heading":0.5708755295699531, "vx":1.6027633214496784, "vy":0.4534234558600668, "omega":-4.773996113928089, "ax":-1.806370574158891, "ay":8.071201148589145, "alpha":3.0265170249499156, "fx":[-53.49492669595375,-45.24016914222464,-10.28028912843534,-24.81499813167016], "fy":[144.6361422656001,147.30735975730988,153.80574149192682,152.2299071818356]}, + {"t":1.9502904370875016, "x":4.186816456935824, "y":0.9680483466942366, "heading":0.4499072959440951, "vx":1.5569917178674682, "vy":0.6579395306292329, "omega":-4.697307233206413, "ax":-2.1198542044508972, "ay":7.880824483465136, "alpha":5.391820810166321, "fx":[-67.80625049193583,-68.07209227555903,-0.11175108608306558,-21.06566444578011], "fy":[138.5913389772652,138.25802406558563,154.16227679059622,152.86288449751788]}, + {"t":1.9756294257961295, "x":4.225588511089654, "y":0.9872498672517074, "heading":0.330882281000921, "vx":1.503276756116949, "vy":0.8576316532304354, "omega":-4.560683946578663, "ax":-2.340405079371691, "ay":7.659512346068233, "alpha":7.592602036388723, "fx":[-74.96678544699097,-90.1938596302622,7.3484441876405935,-15.583730630877277], "fy":[134.92190670382723,125.0049454141539,153.98632150957522,153.56477706794678]}, + {"t":2.0009684145047575, "x":4.262928678507265, "y":1.0114403359322854, "heading":0.215319161974943, "vx":1.4439732582371338, "vy":1.0517159500810556, "omega":-4.368295089309503, "ax":-2.4613154415399627, "ay":7.449087027396003, "alpha":9.433942134526031, "fx":[-77.70769918417267,-107.37774596168086,11.93816822049922,-9.206661507458454], "fy":[133.415131667847,110.67689750763158,153.68494458963224,154.11098592060432]}, + {"t":2.0263074032133854, "x":4.298727339145245, "y":1.0404811511216838, "heading":0.10463098203097412, "vx":1.381606014055581, "vy":1.2404682821578306, "omega":-4.129248536084897, "ax":-2.506903300817357, "ay":7.28365887837048, "alpha":10.719657729381607, "fx":[-77.8172365865822,-118.88503035026476,13.385167755783396,-2.4143525698927726], "fy":[133.39160855712828,98.31094788802764,153.5387249909106,154.3904375446456]}, + {"t":2.051646391922014, "x":4.332931041717529, "y":1.074251651763916, "heading":0.0, "vx":1.3180836196225476, "vy":1.4250288322343587, "omega":-3.857623249919739, "ax":-2.645613038390952, "ay":7.353570238943828, "alpha":9.599589615513336, "fx":[-73.83874704188217,-117.26583305228225,-3.3431905021963533,-1.5604081919480983], "fy":[135.7055472047292,100.42999104085592,154.21378088659873,154.4619927306864]}, + {"t":2.0871846196711634, "x":4.378102738413164, "y":1.129538304193455, "heading":-0.1370930936260636, "vx":1.2240632209280895, "vy":1.6863616861553152, "omega":-3.5164708478652513, "ax":-2.6916896317746315, "ay":7.335247892673094, "alpha":9.413602377735897, "fx":[-69.0762593742162,-119.58270389607428,-16.08610785851674,5.323169689888358], "fy":[138.17129998016694,97.63967624885596,153.29689000609594,154.34597963724536]}, + {"t":2.1227228474203135, "x":4.419904020189998, "y":1.1941006928571576, "heading":-0.26206223549074514, "vx":1.1284053417640572, "vy":1.9470433963616036, "omega":-3.181928102625334, "ax":-2.7981537444549955, "ay":7.280583538968938, "alpha":9.344412632411151, "fx":[-64.62385626768814,-121.73531123726714,-33.390152637072624,12.43970552284622], "fy":[140.2841561354197,94.91084743689916,150.31550474309338,153.89336491971846]}, + {"t":2.1582610751694635, "x":4.458238560213446, "y":1.2678927279082095, "heading":-0.3751423210832648, "vx":1.0289639167164788, "vy":2.205782432316194, "omega":-2.849844238312673, "ax":-2.990063195319406, "ay":7.147643324358566, "alpha":9.645295818713045, "fx":[-60.51559532833551,-124.0962143530614,-57.15187348341145,20.23588114998423], "fy":[142.07209710721526,91.75532164177456,142.72234152025808,153.00483834582954]}, + {"t":2.1937993029186136, "x":4.492917940705448, "y":1.3507959402855594, "heading":-0.4764207346740232, "vx":0.9227023698968668, "vy":2.4597970086469396, "omega":-2.5070675187993254, "ax":-3.3015809562178324, "ay":6.8397345159426255, "alpha":10.817947397612636, "fx":[-56.80428432136906,-126.95894783587173,-90.16086852315468,29.316570796128676], "fy":[143.55160444220945,87.68060198232887,124.0680661338327,151.44197825878612]}, + {"t":2.2293375306677636, "x":4.523624256032781, "y":1.4425319414068145, "heading":-0.5655174711396099, "vx":0.8053700339425414, "vy":2.7028690516182308, "omega":-2.122616840404644, "ax":-3.6714671335246782, "ay":6.2143483280840135, "alpha":13.66160128974023, "fx":[-53.52030964881343,-129.95676470659464,-127.9949470421868,39.46036440901854], "fy":[144.74214994083135,83.05936621730449,83.61963915343793,148.9874836195145]}, + {"t":2.2648757584169137, "x":4.549927211317867, "y":1.542511371519779, "heading":-0.6409515118380912, "vx":0.6748925987778227, "vy":2.923715977814229, "omega":-1.6371077423517753, "ax":-3.700593534764338, "ay":6.140322287617396, "alpha":14.208852091173013, "fx":[-50.54677434012402,-127.15690071147918,-134.19674534181982,37.73084658980274], "fy":[145.68801846590827,87.0753034746146,72.94240341554872,149.21847228892594]}, + {"t":2.3004139861660637, "x":4.571574836974076, "y":1.6502925638211212, "heading":-0.6991314196356853, "vx":0.5433800629323359, "vy":3.1419321497247577, "omega":-1.132150320681684, "ax":-3.803977293166819, "ay":6.153069257627698, "alpha":13.87999005548106, "fx":[-50.70649241532819,-124.861803413628,-135.4244824740191,29.163708606831932], "fy":[145.3997675857073,89.93695362050362,69.81704497718884,150.71482897572113]}, + {"t":2.335952213915214, "x":4.588483455112679, "y":1.7658368216308826, "heading":-0.739366035578344, "vx":0.4081934515351786, "vy":3.3606013263586236, "omega":-0.6388800729340618, "ax":-4.300823966988186, "ay":5.910539147706592, "alpha":13.254976117437296, "fx":[-58.846031928281015,-126.63756131197012,-139.91525579916802,6.75940297319856], "fy":[141.66267294535953,86.36066883204727,57.646096380906094,152.23058621697302]}, + {"t":2.371490441664364, "x":4.600274030530221, "y":1.888999040844854, "heading":-0.7620707011146682, "vx":0.25534978988735, "vy":3.570651412710087, "omega":-0.1678217128630323, "ax":-7.525492554613674, "ay":2.288184418055294, "alpha":4.807306499251395, "fx":[-129.534727930162,-145.4900141885617,-147.75789761562066,-134.76605265187342], "fy":[73.20801146043738,34.09272163877103,5.499303648435849,56.72697041723635]}, + {"t":2.407028669413514, "x":4.604596490290451, "y":2.0173386131018956, "heading":-0.7680347873676471, "vx":-0.012092878443042744, "vy":3.651969431690992, "omega":0.003021440367332382, "ax":-2.9594424420139154, "ay":-3.872680215323626, "alpha":9.126426692537876, "fx":[-73.31958611215494,-89.86974073570845,-46.3409064285123,-9.72893836755128], "fy":[-33.19330177047202,-64.40199382825939,-102.05875360871391,-87.26508258545145]}, + {"t":2.442566897162664, "x":4.602297893775792, "y":2.1446776034912935, "heading":-0.7679274107317424, "vx":-0.11726621795783368, "vy":3.514341240199194, "omega":0.3273584707026644, "ax":3.2053844332858215, "ay":-6.19233592579991, "alpha":14.831737077079108, "fx":[129.53077972158889,-47.532318940521755,37.52672821742137,117.9553328947914], "fy":[-70.48660877817333,-143.7258003262051,-148.05442125157094,-96.51095371471438]}, + {"t":2.478105124911814, "x":4.600154605402299, "y":2.265660709147072, "heading":-0.7562936708442977, "vx":-0.0033525359441423136, "vy":3.2942765957688733, "omega":0.8544521208634129, "ax":3.394926578516229, "ay":-6.211298641881072, "alpha":14.740130648640076, "fx":[133.93092051095203,-43.56336544530447,40.82813099578621,120.3276342876766], "fy":[-69.99994461182398,-146.66026629859226,-148.16754972947953,-95.35493313978913]}, + {"t":2.513643352660964, "x":4.60217930001153, "y":2.378811132720205, "heading":-0.7259279567723095, "vx":0.11729713799480974, "vy":3.073538050015718, "omega":1.378290240907009, "ax":3.519284220464332, "ay":-6.169057496318694, "alpha":14.735009933353789, "fx":[135.7658745898439,-41.739255114192446,43.93666879887766,122.77344105123233], "fy":[-68.85160365378172,-147.77911933018075,-147.61570571530035,-92.80670308799652]}, + {"t":2.549181580410114, "x":4.608570199924965, "y":2.484143574138887, "heading":-0.6769459642865254, "vx":0.2423662621356607, "vy":2.854300679713943, "omega":1.9019463798045224, "ax":3.655002159802379, "ay":-6.045229958815227, "alpha":14.985955295912492, "fx":[138.1746684299295,-41.51817651391262,47.78830700323786,126.34700109618387], "fy":[-64.93812067576361,-148.1474295862223,-146.58874679080222,-88.20470013591435]}, + {"t":2.584719808159264, "x":4.619491538402989, "y":2.581762902922714, "heading":-0.6093541606743611, "vx":0.3722585613143525, "vy":2.6394639206415835, "omega":2.43452067214924, "ax":3.826843921992447, "ay":-5.798200855430968, "alpha":15.633004261839302, "fx":[142.2596717400351,-42.750335546719704,52.7062502307304,131.30762606853062], "fy":[-55.74229077996758,-147.98818541226134,-145.0013991025741,-80.84522968236656]}, + {"t":2.620258035908414, "x":4.635137534111942, "y":2.67190330866752, "heading":-0.5228356105675079, "vx":0.5082578121745701, "vy":2.433406138105962, "omega":2.990089938009916, "ax":4.025017453712825, "ay":-5.499748313613285, "alpha":16.22861662815044, "fx":[145.71053563008465,-43.19004866055243,58.90590770028688,136.7790984408566], "fy":[-45.45123777186703,-147.99455255388872,-142.66917297068647,-71.3503897625388]}, + {"t":2.6557962636575643, "x":4.655741845351501, "y":2.7549092536585738, "heading":-0.4165731133600699, "vx":0.65129979913892, "vy":2.23795482997377, "omega":3.566826211794768, "ax":3.5379973590343115, "ay":-6.797296620964259, "alpha":10.304461111709973, "fx":[88.16096191090614,-19.010041072631388,63.70106146541354,129.27116603244573], "fy":[-125.64026754754966,-153.08849999469564,-140.6306141394949,-84.23873037225978]}, + {"t":2.6913344914067143, "x":4.681122070480764, "y":2.8301498260886255, "heading":-0.2898144311036698, "vx":0.7770339550601723, "vy":1.9963909545794147, "omega":3.933028497614976, "ax":3.291093060116733, "ay":-7.211494732017498, "alpha":8.130629894328743, "fx":[56.48268539221704,0.5135212806510844,67.50453462981154,119.32976133524885], "fy":[-143.22889701930256,-154.33394306947,-138.8726407001671,-97.84974091677262]}, + {"t":2.7268727191558644, "x":4.710814748857096, "y":2.896544087509489, "heading":-0.1500415686115318, "vx":0.8939935697742475, "vy":1.7401072123811825, "omega":4.2219766745436775, "ax":3.2295346061358523, "ay":-7.378748351671399, "alpha":6.859434545005323, "fx":[44.11071278010181,14.04269647095388,70.83561214391723,110.2807385044201], "fy":[-147.72336446481302,-153.74534146934644,-137.22719632695137,-107.98080561751968]}, + {"t":2.7624109469050144, "x":4.744625091552734, "y":2.9537248611450195, "heading":0.0, "vx":1.0087655061308645, "vy":1.4778795729558196, "omega":4.465748821634463, "ax":3.5833111854122293, "ay":-7.177538158822725, "alpha":7.272416986313004, "fx":[42.95593373598406,23.530783484017523,81.62622148346104,117.36742040135864], "fy":[-148.0022338890234,-152.55578451197678,-131.0621312694197,-100.14929744043812]}, + {"t":2.7948004040637, "x":4.779178043254554, "y":2.997827683392594, "heading":0.14464318013978175, "vx":1.124827010257014, "vy":1.2454030082557972, "omega":4.701298460052748, "ax":3.743353668816579, "ay":-6.8739397351001665, "alpha":9.381584050980596, "fx":[28.770533837104168,25.363132904701576,92.99718062562378,130.2067392478532], "fy":[-151.23791665586458,-152.2271611017775,-123.20998034666307,-82.60138898979594]}, + {"t":2.827189861222386, "x":4.817574112511022, "y":3.034559964959339, "heading":0.2969156852018565, "vx":1.2460722035429586, "vy":1.0227598316943811, "omega":5.005162874752595, "ax":3.5513370472736994, "ay":-7.243400222083573, "alpha":6.74372192809176, "fx":[30.066190945203147,35.93803512023805,88.43783806151038,108.66939503146216], "fy":[-151.02880576289687,-150.00441347289978,-126.40247187695942,-109.21334454097163]}, + {"t":2.859579318381072, "x":4.85979652765672, "y":3.0638871586595235, "heading":0.45903019370590115, "vx":1.3610980826916843, "vy":0.7881500305179889, "omega":5.223588367232614, "ax":2.809993445102866, "ay":-7.821540798599404, "alpha":1.0597470730428926, "fx":[45.467454410447765,47.24480004324753,58.1695991438668,57.30494076321898], "fy":[-147.1727013880527,-146.66086740129964,-142.6754748254911,-142.97327107178904]}, + {"t":2.8919687755397576, "x":4.90535570535026, "y":3.0853122112834828, "heading":0.6282193853409923, "vx":1.4521122449980322, "vy":0.5348145699068392, "omega":5.25791299965398, "ax":1.6716511348778242, "ay":-7.942198211039526, "alpha":-5.788240574600284, "fx":[67.82811126516404,53.27927958455666,-15.893960445946352,18.6358588770539], "fy":[-138.28253587855392,-144.11867954926373,-153.0576188022267,-152.9627468294519]}, + {"t":2.9243582326984434, "x":4.953265678023739, "y":3.0984685764049846, "heading":0.7985203331873829, "vx":1.506256117815426, "vy":0.2775710812045822, "omega":5.070435029538797, "ax":2.5137392108524024, "ay":-7.805343098204728, "alpha":-4.198799214329456, "fx":[74.8590450423599,58.90397481099989,13.33922449587254,39.13566630440044], "fy":[-134.41803601124806,-141.8091253641398,-153.17064418718763,-148.88445389721647]}, + {"t":2.956747689857129, "x":5.003371048935128, "y":3.103364750340972, "heading":0.9627489713525303, "vx":1.587674766293439, "vy":0.024760255316434977, "omega":4.934438202268349, "ax":3.510694186916514, "ay":-7.458159104885671, "alpha":-2.8851437487708025, "fx":[84.31414122005557,71.6939845499142,43.49062243162991,60.601562718670934], "fy":[-128.42611762993295,-135.60705388170496,-147.2588827503577,-141.26803750077397]}, + {"t":2.989137147015815, "x":5.056636466908643, "y":3.100254630222406, "heading":1.1225727461070845, "vx":1.7013842452578192, "vy":-0.21680546949392307, "omega":4.8409899624208865, "ax":4.646945885988116, "ay":-6.798747043666986, "alpha":-2.5570638424342365, "fx":[101.67739524799433,91.36893453188432,68.6594017269401,82.57719529426878], "fy":[-114.72258332283432,-122.75286524127256,-136.95122017949302,-129.27890222759973]}, + {"t":3.021526604174501, "x":5.114180880904556, "y":3.0896662144011415, "heading":1.2793697831005444, "vx":1.851896299950763, "vy":-0.4370131955975177, "omega":4.758168052644338, "ax":4.286125431801061, "ay":-4.704868969378815, "alpha":-17.20940581291958, "fx":[146.77567982934497,127.27953117463252,-34.01015236338595,77.50540235068549], "fy":[-42.9923504177109,-24.2124259953678,-148.98756970218153,-132.38198608807738]}, + {"t":3.0539160613331866, "x":5.176411034439723, "y":3.073043709465488, "heading":1.4334842633954965, "vx":1.990721576000838, "vy":-0.5894013475184438, "omega":4.200764740340338, "ax":2.190377909136885, "ay":-5.967707632462643, "alpha":-17.519311815881366, "fx":[148.16276692773786,-47.02139226206317,-23.963845663977217,85.10318953043608], "fy":[-36.34202658393806,-127.45374625807317,-150.88898933012405,-127.45076090175704]}, + {"t":3.0863055184918724, "x":5.242038363112217, "y":3.050823027554645, "heading":1.5695447529864683, "vx":2.061666727450159, "vy":-0.7826921582156557, "omega":3.633323740830188, "ax":2.8317857501655137, "ay":-6.1033369383626015, "alpha":-15.898841182263572, "fx":[147.5635014815891,-25.811969785073003,-5.460263401711003,93.51007436345746], "fy":[-36.67352610204604,-141.91657379587912,-152.4810308335337,-121.11289635794957]}, + {"t":3.118694975650558, "x":5.31030000981418, "y":3.02227061842329, "heading":1.6872261366337242, "vx":2.153386730687723, "vy":-0.9803759285057768, "omega":3.11836890548451, "ax":3.3467387122963905, "ay":-6.031513025897403, "alpha":-14.997110286980297, "fx":[147.2225965058906,-10.657087174299368,10.253483809680802,101.1341845753429], "fy":[-35.374274240887914,-145.20526893761797,-151.90543482016724,-114.37775906401362]}, + {"t":3.151084432809244, "x":5.381802530269201, "y":2.9873530136881308, "heading":1.7882284127028931, "vx":2.2617857808309627, "vy":-1.175733361260137, "omega":2.6326206443402733, "ax":3.7476963885802466, "ay":-5.840717914872852, "alpha":-14.637913996903642, "fx":[147.40476254926898,-0.8299859813321159,22.951210971939123,108.13334249725732], "fy":[-30.57963540097661,-145.0377253611266,-149.99968308942928,-107.11006502556732]}, + {"t":3.18347388996793, "x":5.457026354839936, "y":2.94620796712929, "heading":1.8734975662778248, "vx":2.3831716324526444, "vy":-1.3649110439398808, "omega":2.158506556045033, "ax":4.071105164258642, "ay":-5.551552165349201, "alpha":-14.668027330982708, "fx":[147.61206296694053,6.100111553392598,33.3698112177096,114.53805367155157], "fy":[-22.137453981586287,-142.8368333392581,-147.19993991341798,-99.1291695921292]}, + {"t":3.2158633471266156, "x":5.536351441595018, "y":2.8990872366810283, "heading":1.943410421901588, "vx":2.515032518758905, "vy":-1.5447228049636692, "omega":1.683417113205734, "ax":4.3320632747652015, "ay":-5.165599666996156, "alpha":-14.895246819709168, "fx":[146.76974200172145,11.968174811404,42.15835538004782,120.05763170763096], "fy":[-10.88931387424412,-138.0265285207142,-143.44717237898055,-90.34593335447232]}, + {"t":3.2482528042853014, "x":5.620084313445395, "y":2.8463449478344787, "heading":1.997935388369964, "vx":2.6553456966056292, "vy":-1.7120337740767635, "omega":1.2009681544707107, "ax":4.5181816510714405, "ay":-4.656015976260598, "alpha":-15.033816569156462, "fx":[143.11308453282174,18.310872229664653,49.53360586623978,123.78547953585468], "fy":[1.2660790775738255,-127.56092363174405,-137.94359208929558,-80.71647500572941]}, + {"t":3.280642261443987, "x":5.708459479206321, "y":2.7884508437698727, "heading":2.0368340949581394, "vx":2.8016871476281686, "vy":-1.8628396040700137, "omega":0.7140309967724744, "ax":4.508887488793746, "ay":-3.866875269289132, "alpha":-14.389102664912526, "fx":[131.39070318342618,26.060487593009732,54.54153317976346,122.0617323135516], "fy":[9.740214316124218,-100.21888110207638,-126.53444487775914,-69.4759432873819]}, + {"t":3.313031718602673, "x":5.801569689980044, "y":2.7260861553925104, "heading":2.059961171338075, "vx":2.947727565779789, "vy":-1.9880855949426364, "omega":0.24797577245535615, "ax":1.9453412460621775, "ay":-1.396458750109767, "alpha":-5.928327462366833, "fx":[45.65372678482174,12.975258863942011,28.327066221155267,57.1703903683356], "fy":[-1.4613388819694262,-20.6182057204169,-49.892424687734774,-31.48886658801131]}, + {"t":3.345421175761359, "x":5.898065392003353, "y":2.6609606458545816, "heading":2.06799297199641, "vx":3.0107361127281447, "vy":-2.033316135803189, "omega":0.05596046409036422, "ax":0.04626813596910488, "ay":-0.045319721633327065, "alpha":-0.15769357524509522, "fx":[1.040056863173625,0.2396508544158182,0.6739440660408568,1.4742618740487428], "fy":[-0.22208180954951473,-0.6564415667876261,-1.456732155441653,-1.0223920045911417]}, + {"t":3.377810632920045, "x":5.995605769759807, "y":2.595078868046585, "heading":2.069805501050645, "vx":3.012234712535928, "vy":-2.0347840169854745, "omega":0.05085285479076319, "ax":-0.003198191811209887, "ay":-0.006633780156865188, "alpha":-0.0018090199471106976, "fx":[-0.05714698433486013,-0.06632205977859154,-0.061326623010562,-0.052151967784904434], "fy":[-0.11578659732091735,-0.12078156622128725,-0.1299549356434707,-0.12496040507615272]}, + {"t":3.4102000900787304, "x":6.093168739358765, "y":2.529169838628378, "heading":2.0714525974122875, "vx":3.0121311248392737, "vy":-2.034998881523666, "omega":0.050794261616687035, "ax":-0.010340129839113636, "ay":-0.014641055172364558, "alpha":0.0018174585207499363, "fx":[-0.19360382449992017,-0.18439771280809164,-0.1894359441834603,-0.19864205802877885], "fy":[-0.27830381739184346,-0.2732655798859515,-0.26405942346829875,-0.26909767486405156]}, + {"t":3.442589547237416, "x":6.1907246075872315, "y":2.4632496497406504, "heading":2.073097795972828, "vx":3.0117962136468344, "vy":-2.035473097352929, "omega":0.05085312811158255, "ax":-0.0378550348751424, "ay":-0.05426650834603097, "alpha":0.003516363185571433, "fx":[-0.7051613629593814,-0.6873645858922441,-0.6971405640923761,-0.7149373108855483], "fy":[-1.0189095111085364,-1.0091364462576562,-0.9913389531518384,-1.0011121598227115]}, + {"t":3.474979004396102, "x":6.2882551955978565, "y":2.3972933161851544, "heading":2.0747449011871835, "vx":3.0105701096165056, "vy":-2.037230760100154, "omega":0.050967021206336, "ax":-0.14907829925610666, "ay":-0.2104919965742577, "alpha":0.015042264867481331, "fx":[-2.7782409281420852,-2.7021931949792712,-2.7442160943663265,-2.8202628177987448], "fy":[-3.957699722566209,-3.915843239642255,-3.83976370382905,-3.881624376156089]}, + {"t":3.507368461554788, "x":6.385687729883869, "y":2.3311981066092256, "heading":2.0763956953370517, "vx":3.00574154442946, "vy":-2.0440484816054423, "omega":0.05145423199983089, "ax":-0.6564729225205183, "ay":-0.7623191633561975, "alpha":0.28089577542838995, "fx":[-12.460969091931451,-11.049780604778476,-11.857780838409049,-13.268235348581172], "fy":[-15.21407428026885,-14.45255656557126,-13.023276215670466,-13.788795113223388]}, + {"t":3.5397579187134736, "x":6.482697721566505, "y":2.2645926201582833, "heading":2.0780622699800433, "vx":2.984478742829644, "vy":-2.068739585488213, "omega":0.0605522936841246, "ax":-3.832366641875282, "ay":-0.7637233372243534, "alpha":6.7840517258177995, "fx":[-75.83874564898986,-47.92102290239174,-68.18387682009678,-91.98873439177748], "fy":[-43.17968820133038,-27.95053357808493,19.1183492298768,-4.570862058739378]}, + {"t":3.5721473758721594, "x":6.577353144223101, "y":2.1971866657127204, "heading":2.0800235259021855, "vx":2.8603504676662475, "vy":-2.0934761698003297, "omega":0.28028404641980975, "ax":-5.902029266546009, "ay":1.001418453972039, "alpha":13.642473932366125, "fx":[-125.01823505896438,-86.69588893549401,-87.84272281245742,-137.71269749294495], "fy":[-56.18349705251325,-4.955179519685644,103.39669702408048,31.935069965998828]}, + {"t":3.604536833030845, "x":6.666902501767688, "y":2.129905391499433, "heading":2.089101774015963, "vx":2.6691869435881452, "vy":-2.0610407696874846, "omega":0.7221563713906723, "ax":-6.133011335727601, "ay":1.2636603439638785, "alpha":14.807859337595476, "fx":[-130.69929081282763,-95.20010894799375,-85.07364546098397,-143.4094986195811], "fy":[-63.66995361220788,3.6578631883299337,117.48078788618042,36.15337010129332]}, + {"t":3.636926290189531, "x":6.750139017558252, "y":2.06381223824762, "heading":2.1124920268689933, "vx":2.4705420356758605, "vy":-2.020111497113536, "omega":1.201774897017569, "ax":-6.053093132211434, "ay":1.5523836861597011, "alpha":15.726418382179917, "fx":[-131.20759039432684,-88.50762905335189,-82.9991134007123,-145.74723113088973], "fy":[-69.44717945991545,24.866488752302164,123.105492489203,36.488200758610226]}, + {"t":3.6693157473482167, "x":6.826983452786199, "y":1.9991962084158288, "heading":2.151416863410328, "vx":2.2744856349925624, "vy":-1.969830632216824, "omega":1.7111450514667566, "ax":-5.629005525546932, "ay":2.436015113112761, "alpha":17.23428882749336, "fx":[-131.57648083050265,-56.30050838094842,-82.2332969398778,-146.93147522539218], "fy":[-71.8951192526627,89.95836230651918,125.78239734302323,36.63384730341851]}, + {"t":3.7017052045069025, "x":6.89770017788682, "y":1.936672247178056, "heading":2.206839922747108, "vx":2.0921652016768535, "vy":-1.8909294250727464, "omega":2.269354311105273, "ax":-5.238706668109856, "ay":3.0919102181156415, "alpha":18.896699330577142, "fx":[-134.1539949040505,-24.84045054585896,-82.01502154835096,-147.1158326286626], "fy":[-68.65797629174554,131.67084526665596,127.30257214435484,38.75800312048637]}, + {"t":3.734094661665588, "x":6.962716369887964, "y":1.8770478954220744, "heading":2.280343076984531, "vx":1.9224863364831868, "vy":-1.7907841315245865, "omega":2.881408144513571, "ax":-4.891374399374843, "ay":3.4212172390756614, "alpha":20.605224877676104, "fx":[-137.03883624926115,0.8970122373890719,-79.6330552085662,-146.61726728044508], "fy":[-63.64491283129205,144.9519245588817,129.77295022039087,42.39118086065707]}, + {"t":3.766484118824274, "x":7.022418944690146, "y":1.8208399295612323, "heading":2.3736703226379428, "vx":1.764057374927542, "vy":-1.6799727623289875, "omega":3.5488001929341513, "ax":-6.336689764097178, "ay":3.945339823288059, "alpha":11.72752046309348, "fx":[-151.44398172379525,-77.18498834485176,-96.86094403811734,-143.98275713566738], "fy":[-6.269998179926195,129.03565121546708,118.09629767290822,51.4403861193166]}, + {"t":3.79887357598296, "x":7.076231967922833, "y":1.76849600625276, "heading":2.48861403445172, "vx":1.5588154332854327, "vy":-1.552185347146141, "omega":3.9286482145511306, "ax":-7.583015518886768, "ay":3.2125562441549924, "alpha":2.016560944093252, "fx":[-145.85127710431723,-136.93304500773587,-135.26587323718908,-143.76025841404058], "fy":[45.845468856874014,67.82275753118185,71.64023427834452,52.70340635055468]}, + {"t":3.8312630331416457, "x":7.122743570278095, "y":1.719906674778152, "heading":2.6158608174884717, "vx":1.313205677002799, "vy":-1.4481323943062143, "omega":3.993963528857718, "ax":-6.911136233724117, "ay":3.572279366575783, "alpha":-9.748649842229504, "fx":[-114.94495066202607,-149.08907113762413,-153.28654001853792,-94.7116994659643], "fy":[102.33072302620204,38.21662823137458,3.6360704180251817,120.4796120352648]}, + {"t":3.863652490300331, "x":7.16165243248508, "y":1.6748762505771522, "heading":2.745223128099763, "vx":1.0893577260427496, "vy":-1.3324282048036502, "omega":3.6782100524377945, "ax":-6.12907317308849, "ay":3.218732092399014, "alpha":-16.57039528264165, "fx":[-109.2503440762093,-152.5935477469835,-148.26091851863723,-43.98596290594998], "fy":[108.82364105764016,22.067472362077996,-39.71488164682962,147.2931914887696]}, + {"t":3.8960419474590178, "x":7.193721203233713, "y":1.6334079731197348, "heading":2.864358355013845, "vx":0.8908403730805484, "vy":-1.2281752195916047, "omega":3.141503944328181, "ax":-5.706815147826275, "ay":2.6844868584098927, "alpha":-20.10178092797852, "fx":[-110.68137303520248,-154.04445482940406,-136.5880207854806,-21.49267202206592], "fy":[107.5626248767725,8.918414823595304,-70.12284940553268,152.530072071037]}, + {"t":3.928431404617703, "x":7.219581595260789, "y":1.5950361610841932, "heading":2.9661099624325056, "vx":0.7059997283374893, "vy":-1.141226147498082, "omega":2.49041817214813, "ax":-5.422880920397783, "ay":2.1669811083538217, "alpha":-22.337168785871057, "fx":[-113.75049205630847,-154.3555412998949,-120.37446398141935,-13.289904292808217], "fy":[104.42280907369474,-2.242090637485707,-95.27382976427448,153.6404076837834]}, + {"t":3.960820861776389, "x":7.239604033568306, "y":1.5592091306211469, "heading":3.0467732551265105, "vx":0.5303555590896097, "vy":-1.071038805725374, "omega":1.7669294007118224, "ax":-5.112768735735911, "ay":1.6923538675130108, "alpha":-24.33131216815751, "fx":[-116.71935512425408,-153.98850991160674,-97.8169012103353,-10.27004384700598], "fy":[101.16597940103972,-11.506078937156396,-118.25371774794512,153.97693062036572]}, + {"t":3.9932103189350743, "x":7.254100118350897, "y":1.5254064698119767, "heading":3.1040031392532885, "vx":0.36475575516122255, "vy":-1.016224382636225, "omega":0.9788514076266704, "ax":-4.703065360905343, "ay":1.299199214719112, "alpha":-26.44302786522583, "fx":[-118.9280057624502,-153.30690714163774,-67.65437803723218,-8.551415517434943], "fy":[98.61128436380444,-18.768875300522794,-137.73587651947716,154.14853887630505]}, + {"t":4.0255997760937605, "x":7.263447420559034, "y":1.4931729936720566, "heading":3.135707604985332, "vx":0.212426021139679, "vy":-0.9741440253304818, "omega":0.12237608943999947, "ax":-4.245193599764187, "ay":1.057824201359948, "alpha":-28.418295365832687, "fx":[-120.06007478196464,-152.63221070663164,-35.50548774194497,-6.3201301887878705], "fy":[97.2676824525794,-23.811056801388546,-149.38500777105466,154.3004615502196]}, + {"t":4.057989233252447, "x":7.268101016724993, "y":1.4621758669827092, "heading":3.1396713000914964, "vx":0.07492650490978903, "vy":-0.9398816736791128, "omega":-0.798077070834522, "ax":-3.8862445039375206, "ay":0.9779188214148647, "alpha":-29.786600835048333, "fx":[-119.8857553822562,-152.23903831537163,-13.270357642555052,-2.5289314675402035], "fy":[97.51120734350832,-26.339859425743,-153.1587135001611,154.43941522338028]}, + {"t":4.090378690411133, "x":7.268489360809326, "y":1.4322465658187866, "heading":3.113822016996372, "vx":-0.05094684495867406, "vy":-0.9082074139082232, "omega":-1.7628489024841991, "ax":-5.325650461314898, "ay":1.7017410602321612, "alpha":-23.113006111758075, "fx":[-119.26925640446284,-153.89294564799025,-105.16114776628102,-16.243441559163983], "fy":[98.26945957547844,-13.75167331825946,-112.04490361272832,153.60570902598968]}, + {"t":4.126929107032343, "x":7.26306987642102, "y":1.4001879124437466, "heading":3.0493891551703327, "vx":-0.2456015880986709, "vy":-0.8460080691753188, "omega":-2.607638905237522, "ax":-5.669583306666556, "ay":2.311379801233252, "alpha":-20.548025256906858, "fx":[-116.09235918089747,-154.43715688887514,-131.08936815070285,-18.429203803836337], "fy":[101.9685619720382,-3.724300811237335,-80.31343550971495,153.31468106268326]}, + {"t":4.163479523653552, "x":7.250305944462414, "y":1.3708098892746907, "heading":2.954078866786226, "vx":-0.45282722002598896, "vy":-0.7615261744703947, "omega":-3.358677789120606, "ax":-5.858802545794502, "ay":2.9743607719785548, "alpha":-18.29095167675147, "fx":[-111.6064080090692,-154.14979376710053,-145.89272654865442,-22.41803468799888], "fy":[106.81262138507978,9.624837493987572,-48.78744939262334,152.71443138790312]}, + {"t":4.200029940274762, "x":7.229841437213619, "y":1.3449625636177878, "heading":2.831317794297465, "vx":-0.6669688939761818, "vy":-0.6528120490727958, "omega":-4.027219693304285, "ax":-6.000144519564702, "ay":3.689270578861953, "alpha":-15.613032903561596, "fx":[-106.50274765607492,-152.10351076313896,-153.09422863159955,-32.83822011469615], "fy":[111.83173143847972,26.54366377817603,-15.754895615689843,150.71017904575848]}, + {"t":4.2365803568959715, "x":7.201455550865625, "y":1.3235663203222776, "heading":2.684121236682053, "vx":-0.8862766759537394, "vy":-0.5179676723870201, "omega":-4.597882550650116, "ax":-6.187166532765327, "ay":4.451488339761466, "alpha":-11.43039881512841, "fx":[-102.5577859962174,-146.66754376691694,-152.15844340833442,-57.01102090804876], "fy":[115.3407804031709,47.994136915653286,23.306796900162684,143.1601538972606]}, + {"t":4.273130773517181, "x":7.1649289492826895, "y":1.3076078310865664, "heading":2.516066713880401, "vx":-1.1124201904311173, "vy":-0.35526391898428167, "omega":-5.01566838948964, "ax":-6.354972829019909, "ay":5.1535000084828955, "alpha":-4.880327308841389, "fx":[-105.19805121704728,-133.40550576977057,-136.35778949069828,-95.86588047891084], "fy":[112.68260909683362,77.23205939758391,71.43265307324175,120.46518706082146]}, + {"t":4.309681190138391, "x":7.1200246190487935, "y":1.2980651520851438, "heading":2.332741944610723, "vx":-1.3446970949482622, "vy":-0.1669013466168244, "omega":-5.19404638587566, "ax":-5.910639343095462, "ay":5.616901135318317, "alpha":5.14626760246928, "fx":[-132.5026712709241,-86.59216123965972,-93.0290808005314,-125.7835343401413], "fy":[77.80800614630076,126.90480186946276,122.65848667399683,88.77367662370304]}, + {"t":4.3462316067596, "x":7.06692727105678, "y":1.295716739997999, "heading":2.1428973852570796, "vx":-1.5607334254361145, "vy":0.03839872999920552, "omega":-5.005948160961174, "ax":-5.370054593545923, "ay":5.157021179171574, "alpha":12.515577360278405, "fx":[-152.27988449478536,-45.56997765690911,-68.49411310989618,-131.51262946503968], "fy":[18.475278100042352,145.80054890496322,137.7809946109481,80.01656350650981]}, + {"t":4.38278202338081, "x":7.0062947976711145, "y":1.3005649468491138, "heading":1.9599278943897704, "vx":-1.7570111581088583, "vy":0.2268900026223284, "omega":-4.548498594188019, "ax":-5.420755567566237, "ay":4.897962429525061, "alpha":12.937522536202083, "fx":[-153.40881706420066,-59.17002890129868,-58.82566635430056,-130.20842617004746], "fy":[-0.4254718337886448,139.31582918186697,141.99338324705937,81.99649988351493]}, + {"t":4.4193324400020195, "x":6.938454424831767, "y":1.312129545683575, "heading":1.7936783757712118, "vx":-1.955142032505146, "vy":0.4059125700165015, "omega":-4.075626755443545, "ax":-5.439146538350128, "ay":4.67170255266005, "alpha":13.042752426340725, "fx":[-152.9735407349005,-81.82359625807126,-41.8498783324463,-126.3284734078662], "fy":[-11.172023725041482,121.92712203998516,147.66181937637265,87.70018103016142]}, + {"t":4.455882856623229, "x":6.863360001435737, "y":1.330086359927922, "heading":1.6447125198672017, "vx":-2.1539451045456537, "vy":0.5766652446465951, "omega":-3.598908720373498, "ax":-5.980241521272659, "ay":3.733673895941496, "alpha":13.608970272957771, "fx":[-153.17555022583826,-139.1078397788295,-30.482475808130314,-120.29826801525073], "fy":[-5.206821369636912,36.175353637391545,150.0942071556423,95.55769217911656]}, + {"t":4.492433273244439, "x":6.780637809621264, "y":1.353657683871476, "heading":1.5131709067558456, "vx":-2.372525423643626, "vy":0.7131325810709916, "omega":-3.101495187111234, "ax":-5.92753304430308, "ay":3.729276589937726, "alpha":14.263780338411037, "fx":[-152.70277369695637,-145.8465226304346,-27.214497210677205,-113.39527464825838], "fy":[9.751348508449208,13.019926499158752,150.2424646505549,103.28090433714333]}, + {"t":4.528983689865648, "x":6.6899616235742645, "y":1.382214008563526, "heading":1.3998099655182534, "vx":-2.589179225948891, "vy":0.8494391941289394, "omega":-2.580148073148892, "ax":-5.625523427862897, "ay":3.9140323736885407, "alpha":15.20722589439043, "fx":[-151.03725719792962,-145.17236650827036,-16.037704244172108,-104.5364517731342], "fy":[21.62339318703024,5.479231060265246,151.16619566710455,111.71401058743648]}, + {"t":4.565534106486858, "x":6.591568383090131, "y":1.415875807421157, "heading":1.30550447850025, "vx":-2.794794450949655, "vy":0.9924987080561576, "omega":-2.0243176310560744, "ax":-5.120610287429682, "ay":4.183189784565888, "alpha":16.265232632954408, "fx":[-148.10494725525473,-140.5941955318708,2.2026782917140326,-92.87931047967876], "fy":[33.11145297738128,5.099944649390576,150.9542732788671,120.75849385327854]}, + {"t":4.602084523108068, "x":6.48599708552046, "y":1.454946279242117, "heading":1.2315148257114903, "vx":-2.981954890310062, "vy":1.145396037487629, "omega":-1.429816601880696, "ax":-4.328820693448195, "ay":4.586570306957516, "alpha":16.86177945304754, "fx":[-142.82099360972526,-124.55838631127936,24.501730042152403,-77.83601765733768], "fy":[45.74847115348659,17.58497834291961,147.05017675820753,129.42619464725476]}, + {"t":4.638634939729277, "x":6.374113884823244, "y":1.4998746568207393, "heading":1.1792544332208286, "vx":-3.140175090134107, "vy":1.3130370930693958, "omega":-0.8135115378968559, "ax":-3.4147952722234978, "ay":4.874526225607953, "alpha":16.33462534360992, "fx":[-133.55803635753733,-87.44586702788015,33.907370039982375,-65.8988187830594], "fy":[55.07373100479427,35.77414406019073,139.01599312974875,131.28003080810825]}, + {"t":4.675185356350487, "x":6.257058208245613, "y":1.5511227297243768, "heading":1.1495202475845376, "vx":-3.2649872800100135, "vy":1.491203057446379, "omega":-0.2164741762365437, "ax":0.5201418194517319, "ay":4.6030711717298916, "alpha":5.098430601641018, "fx":[-16.614837950221457,22.42028622193916,32.66493146222954,0.06588738559266445], "fy":[81.76937972260583,70.68853838459003,89.53259923794258,99.0418196259857]}, + {"t":4.711735772971696, "x":6.138069000197285, "y":1.6087015199775494, "heading":1.1416080262553587, "vx":-3.2459758798069385, "vy":1.6594472265101865, "omega":-0.030124413632239926, "ax":0.5859980045116094, "ay":1.1709693526022704, "alpha":0.06820038115571374, "fx":[10.590864912008232,10.957620336554216,11.116696294883663,10.75023861481], "fy":[21.598173319690915,21.429616464118332,21.779278437232385,21.947709174555374]}, + {"t":4.748286189592906, "x":6.019818656470893, "y":1.6701371757412335, "heading":1.1405069663866307, "vx":-3.2245574086028417, "vy":1.7022466441984674, "omega":-0.027631661287273297, "ax":0.12491967279626928, "ay":0.23753791242581965, "alpha":0.0030284383466292596, "fx":[2.3021665512163634,2.3180776995074917,2.3253577308493836,2.3094467365567586], "fy":[4.395386646252065,4.388087881436044,4.403967808381387,4.411266519734631]}, + {"t":4.784836606214116, "x":5.90204318192127, "y":1.732513667141401, "heading":1.1394970176546446, "vx":-3.2199915425179526, "vy":1.7109287538609634, "omega":-0.027520970603992344, "ax":0.025331427992635058, "ay":0.04769393303570426, "alpha":-0.00003349174847867335, "fx":[0.4693179254915037,0.4691438597786145,0.4690594930388404,0.4692335588093875], "fy":[0.8834318777218086,0.883516239775184,0.883342177541657,0.8832578157106076]}, + {"t":4.821387022835325, "x":5.7843680700701965, "y":1.7950806838526787, "heading":1.1384911147132486, "vx":-3.2190656682712113, "vy":1.7126719869837226, "omega":-0.027522194741352615, "ax":0.005316809966483198, "ay":0.009770271465586646, "alpha":-0.0005678748599770586, "fx":[0.10065340491650832,0.09767566900188328,0.09630250337789802,0.09928023950051756], "fy":[0.18176724432712776,0.18314041688352675,0.18016269188197037,0.17878951924975853]}, + {"t":4.857937439456535, "x":5.666713430214677, "y":1.8576860847262249, "heading":1.1374851670291222, "vx":-3.2188713366518407, "vy":1.713029094476292, "omega":-0.027542950804073485, "ax":0.0013158084975382213, "ay":0.002199377217658691, "alpha":-0.0006677535253824767, "fx":[0.026930997124644057,0.02343215208554807,0.021811812742978196,0.025310658012441423], "fy":[0.04167611698013565,0.04329645704840002,0.03979761269092306,0.03817727258243852]}, + {"t":4.8944878560777445, "x":5.549063220725949, "y":1.9202994809238392, "heading":1.1364784607022558, "vx":-3.218823243303062, "vy":1.7131094826299047, "omega":-0.027567357473626496, "ax":0.0005175884278461064, "ay":0.0006913306358557552, "alpha":-0.0006822682395105949, "fx":[0.01220232703797248,0.00862799069942406,0.006971218638125246,0.010545555066740538], "fy":[0.013763607975675793,0.015420380284748942,0.01184604409765614,0.0101892717912003]}, + {"t":4.931038272698954, "x":5.431414235884907, "y":1.982914808017397, "heading":1.135470862301449, "vx":-3.2188043252303857, "vy":1.7131347510526682, "omega":-0.02759229466202803, "ax":0.0003593536451425205, "ay":0.00039446718966104047, "alpha":-0.0006797305015336793, "fx":[0.00926326132966128,0.0057048004935933636,0.004048635069484408,0.007607095968580008], "fy":[0.008257468961584753,0.00991363445703844,0.0063551736182904045,0.004699008110693562]}, + {"t":4.967588689320164, "x":5.313765836811774, "y":2.0455308603875033, "heading":1.134462352436017, "vx":-3.2187911907049416, "vy":1.7131491689927938, "omega":-0.027617139095049228, "ax":0.00032894691113602485, "ay":0.00033957289739082886, "alpha":-0.0006736293270012593, "fx":[0.008677113642995003,0.005151375381758636,0.0035083957172227643,0.007034134010269404], "fy":[0.007230948487248901,0.008873928190087157,0.005348189921411176,0.0037052102231444938]}, + {"t":5.004139105941373, "x":5.196117897500338, "y":2.108147403071781, "heading":1.1334529344962068, "vx":-3.218779167558293, "vy":1.7131615805236666, "omega":-0.027641760527599386, "ax":0.00032403369196382797, "ay":0.00033301683061028774, "alpha":-0.000666628784941654, "fx":[0.008560618953725901,0.005073912702347087,0.0034428851186649847,0.006929591395478112], "fy":[0.0070959770607889865,0.008727004668387018,0.005240298414565539,0.0036092708025134497]}, + {"t":5.040689522562583, "x":5.078470394358057, "y":2.17076439502345, "heading":1.1324426166327792, "vx":-3.2187673239918526, "vy":1.7131737524275674, "omega":-0.027666126087420695, "ax":0.00032413772658060867, "ay":0.0003360600299073584, "alpha":-0.0006592538278627815, "fx":[0.008534787911798755,0.005087625206591354,0.0034725700211582547,0.006919732747355766], "fy":[0.007140557628402025,0.008755612831200124,0.005308450118307906,0.003693394917866307]}, + {"t":5.077239939183793, "x":4.960823324172554, "y":2.2333818338960336, "heading":1.131431408197989, "vx":-3.2187554766229036, "vy":1.7131860355616702, "omega":-0.027690222089488205, "ax":0.00032501865686646167, "ay":0.00034068204678440033, "alpha":-0.0006516096413962251, "fx":[0.008522992167738986,0.0051179872858879835,0.0035169989466904807,0.006922003849604964], "fy":[0.007212121134675375,0.008813109489707505,0.00540810461038149,0.00380711624739828]}, + {"t":5.113790355805002, "x":4.843176687601754, "y":2.2959997248096378, "heading":1.1304193190442844, "vx":-3.2187435970555853, "vy":1.7131984876324156, "omega":-0.027714038693355637, "ax":0.00032508369735532777, "ay":0.0003438718650270591, "alpha":-0.0006437326170617617, "fx":[0.008494596056190493,0.005131920774360432,0.0035478044155470164,0.006910479723563581], "fy":[0.007258474141104007,0.00884259051401183,0.005479915238774248,0.003895798842234665]}, + {"t":5.150340772426212, "x":4.725530485277534, "y":2.358618072982307, "heading":1.129406359383786, "vx":-3.21873171511101, "vy":1.7132110562923466, "omega":-0.027737567388701905, "ax":0.0003202889367633631, "ay":0.00033798558321407866, "alpha":-0.0006357144345242873, "fx":[0.008376104184844928,0.005057323448323044,0.003488679165241345,0.006807459948514729], "fy":[0.007135237181191929,0.008703881496726915,0.005385100817011405,0.003816456394234408]}, + {"t":5.186891189047421, "x":4.6078847140405985, "y":2.421236876612894, "heading":1.12839253973967, "vx":-3.218720008416932, "vy":1.7132234098062251, "omega":-0.027760803016135885, "ax":0.0002911723972002435, "ay":0.00028623580089624795, "alpha":-0.0006279971819385335, "fx":[0.007807917848964188,0.004530819119543473,0.002978272365089333,0.006255371230174641], "fy":[0.006163935323334386,0.007716482269528051,0.004439383993111766,0.002886836430827014]}, + {"t":5.223441605668631, "x":4.490239351239337, "y":2.483856097202441, "heading":1.127377870823691, "vx":-3.218709365944506, "vy":1.713233871844, "omega":-0.027783756574772684, "ax":0.00014042862784177543, "ay":4.704255094570976e-6, "alpha":-0.0006227699246440347, "fx":[0.004996725522578982,0.0017486802525772073,0.0002053124131493589,0.003453357991235906], "fy":[0.000939468724687092,0.0024828381629960314,-0.0007652017859634644,-0.002308576250273095]}, + {"t":5.259992022289841, "x":4.372594276733091, "y":2.546475512130192, "heading":1.1263623629455808, "vx":-3.2187042332196523, "vy":1.7132340437864837, "omega":-0.027806519074977585, "ax":-0.0006189741218402034, "ay":-0.0014277752657438157, "alpha":-0.0006318529478600754, "fx":[-0.009033371407488172,-0.01232769136870316,-0.01389590375108789,-0.010601588211617762], "fy":[-0.02558222462610885,-0.024014002497404787,-0.027308260580109173,-0.02887652618480497]}, + {"t":5.29654243891105, "x":4.254948882574498, "y":2.6090939764942127, "heading":1.1253460230886048, "vx":-3.2187268569816836, "vy":1.713181858005679, "omega":-0.027829613563465208, "ax":-0.004376793928766944, "ay":-0.008650860343268745, "alpha":-0.0008958713651022587, "fx":[-0.07761975471644494,-0.08229069054104757,-0.08451417522290076,-0.07984328811409204], "fy":[-0.15900755351834223,-0.15678414365603674,-0.16145491597077935,-0.1636783279669365]}, + {"t":5.33309285553226, "x":4.137300151410316, "y":2.6717057086675067, "heading":1.1243288391184527, "vx":-3.218886830623245, "vy":1.7128656654560006, "omega":-0.02786235803509871, "ax":-0.01440834166849137, "ay":-0.04948239986466822, "alpha":-0.03481053625748369, "fx":[-0.1328248216386172,-0.31414204778599614,-0.4009174620683813,-0.21960088604219405], "fy":[-0.8692464893773102,-0.7824702391042971,-0.9637853300405668,-1.050549982651365]}, + {"t":5.3696432721534695, "x":4.019638872405278, "y":2.734278609771749, "heading":1.1233104583242206, "vx":-3.219413461514049, "vy":1.7110570631255295, "omega":-0.02913469763811746, "ax":1.3135939619852124, "ay":-0.9574735213641088, "alpha":-5.206578722658849, "fx":[44.21270553396191,18.275181208866773,4.09200830836278,30.741654404368933], "fy":[-10.42739730862158,2.871495120946405,-26.144771992554386,-37.23662407059452]}, + {"t":5.406193688774679, "x":3.9028454058427826, "y":2.796178898076197, "heading":1.1222455729874146, "vx":-3.171401054932384, "vy":1.6760610070158948, "omega":-0.21943731912242392, "ax":5.326274568539827, "ay":-2.571856182630029, "alpha":-18.50697360367384, "fx":[145.41403864836445,120.59332954274927,26.524764050457588,102.0808979924074], "fy":[-14.792945837058696,61.84572785842924,-132.97677558048522,-104.61968729957889]}, + {"t":5.442744105395889, "x":3.790487148874296, "y":2.855721712450008, "heading":1.1142250475512483, "vx":-2.9767235004133, "vy":1.5820585920509334, "omega":-0.8958749147344325, "ax":5.809361871753621, "ay":-2.214085917671863, "alpha":-19.12322545002307, "fx":[151.01595527778898,123.60652379853846,43.46945132490885,112.31207195324596], "fy":[-5.184189201304082,80.13333371397535,-138.18858417440745,-100.79775780673675]}, + {"t":5.479294522017098, "x":3.6855671237545766, "y":2.9120676779371206, "heading":1.0814804461772145, "vx":-2.7643889036973346, "vy":1.5011328293248736, "omega":-1.594836772074095, "ax":6.258418973594686, "ay":-2.071396682095774, "alpha":-17.557390570057958, "fx":[152.53471882661705,127.72097062393934,66.17539538176099,117.24266008336566], "fy":[-3.4685284975719215,79.92056072685938,-132.59345342596896,-97.3242161864302]}, + {"t":5.515844938638308, "x":3.5887079716984776, "y":2.965551084707285, "heading":1.0231884977150811, "vx":-2.535641082822366, "vy":1.4254224176064818, "omega":-2.236566712191011, "ax":6.1962606430741305, "ay":-3.200181017503927, "alpha":-15.636188192786715, "fx":[150.5474501907055,145.82623298655682,56.63432431193217,106.06055103488168], "fy":[-27.864139420149662,40.999276285563965,-139.88759242125664,-110.34255566898857]}, + {"t":5.5523953552595176, "x":3.500168128115461, "y":3.015513254290094, "heading":0.9414410525833704, "vx":-2.309165174824402, "vy":1.3084544681534265, "omega":-2.808075905005005, "ax":5.994646287772561, "ay":-4.013087065216704, "alpha":-14.185223459282584, "fx":[146.3546917489734,151.72054224506442,50.051045398444685,96.00507477601089], "fy":[-46.11638530340294,11.98733118724043,-143.5147799621653,-119.67776040944725]}, + {"t":5.588945771880727, "x":3.419771401693694, "y":3.0606572025997627, "heading":0.8388047083514573, "vx":-2.090058355509527, "vy":1.1617744639825685, "omega":-3.3265517323067395, "ax":6.162026316202383, "ay":-4.672859557519127, "alpha":-10.009843572585517, "fx":[139.85318058096266,150.61566023583373,72.04667898322725,94.01668591477844], "fy":[-63.67279233545562,-26.367460422898148,-134.6535417485524,-121.50902439057091]}, + {"t":5.625496188501937, "x":3.3474949250504435, "y":3.09999922973929, "heading":0.7172178566236393, "vx":-1.8648337264214725, "vy":0.990979500342843, "omega":-3.6924156851978776, "ax":6.8642245173271155, "ay":-4.528108796745834, "alpha":3.772572900272888, "fx":[117.67051543989967,113.95203837971354,135.07285704966813,141.86125517044988], "fy":[-98.83190183193555,-103.46260096487552,-73.87559673356054,-59.30842500293373]}, + {"t":5.662046605123146, "x":3.2839195472926956, "y":3.133195318456784, "heading":0.5822585249909678, "vx":-1.6139434605316447, "vy":0.8254752373156184, "omega":-3.554526573959018, "ax":6.571198782569459, "ay":-4.232547703099863, "alpha":9.326112169415255, "fx":[101.01944390930676,92.54323979916671,139.4857255956822,153.7985660988504], "fy":[-115.7460204441208,-123.33130701617978,-65.87531588707857,-8.628350879883513]}, + {"t":5.698597021744356, "x":3.229318581911537, "y":3.1605395823106823, "heading":0.4523390978216046, "vx":-1.3737634073279452, "vy":0.6707738553981745, "omega":-3.2136532887107574, "ax":6.287545483114642, "ay":-4.15165541584455, "alpha":11.454395337401175, "fx":[96.93712781420678,76.8885551560826,138.37724493159482,153.62874185111338], "fy":[-119.01590335543828,-133.78920904563824,-68.45225192614997,13.669517878135425]}, + {"t":5.735147438365566, "x":3.183306826643783, "y":3.1822834795398034, "heading":0.33487873124310585, "vx":-1.1439510003953002, "vy":0.5190291202813548, "omega":-2.794990366984703, "ax":6.248020898110479, "ay":-4.033001792408792, "alpha":11.836446959993667, "fx":[110.4832804381123,63.7990873751338,135.78308746557985,152.83791702038326], "fy":[-106.18011352292038,-140.57406523876978,-73.60201057960312,21.559152545310756]}, + {"t":5.771697854986775, "x":3.145668409496318, "y":3.1985603001232294, "heading":0.23272066887754736, "vx":-0.9155832335113376, "vy":0.3716212245347283, "omega":-2.362363299282084, "ax":6.4655262569306995, "ay":-3.516753183878631, "alpha":12.185641274161672, "fx":[140.90690257940938,52.71708405444152,132.7152554982368,152.6786671913939], "fy":[-59.467694883478714,-145.13724913253037,-79.07851579232486,23.13424992113392]}, + {"t":5.808248271607985, "x":3.116522215659705, "y":3.209794137468462, "heading":0.14637530607813184, "vx":-0.6792655551451505, "vy":0.2430824305099988, "omega":-1.916973033914867, "ax":6.458732589778783, "ay":-2.614511032052903, "alpha":14.907903579811572, "fx":[152.19615837614597,43.75112347093083,129.64891861674857,152.91837964770502], "fy":[16.655979907628016,-148.11575883999015,-84.06055312515679,21.816438714783477]}, + {"t":5.8447986882291945, "x":3.096008993480415, "y":3.216932495852193, "heading":0.07630914303691917, "vx":-0.4431961881437518, "vy":0.1475209630277164, "omega":-1.3720829471239313, "ax":6.166967450500098, "ay":-2.0955596276791457, "alpha":17.958433903780897, "fx":[139.774507213893,36.943004293586334,126.90548566029608,153.27528730487586], "fy":[63.509448523667785,-149.97972024719127,-88.18082202202034,19.3952720500513]}, + {"t":5.881349104850404, "x":3.083929315684164, "y":3.220924684928133, "heading":0.026158939680682333, "vx":-0.2177909585385341, "vy":0.07092738558145664, "omega":-0.7156947060762832, "ax":5.958645035311399, "ay":-1.9405356255309707, "alpha":19.581027310670283, "fx":[130.584794915421,32.46016111495695,124.84757430832197,153.57156303745106], "fy":[81.26890931819443,-151.03020203128906,-91.09494725841492,17.085836547170977]}, + {"t":5.917899521471614, "x":3.079949140548706, "y":3.2222208976745605, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto b/src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto new file mode 100644 index 00000000..ec5f6f58 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Spinny Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java new file mode 100644 index 00000000..95f2dacc --- /dev/null +++ b/src/main/java/frc/robot/Auto.java @@ -0,0 +1,123 @@ +package frc.robot; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +public class Auto { + private final LoggedDashboardChooser autoChooser; + private Command previousAuto = Commands.none(); + private RobotConfig config; // PathPlanner robot configuration + + // *NEW + private final Field2d m_trajectoryField = new Field2d(); + + public Auto(CommandSwerveDrivetrain drivetrain) { + setUpPathPlanner(drivetrain); + autoChooser = new LoggedDashboardChooser<>("Auto Routine", AutoBuilder.buildAutoChooser()); + SmartDashboard.putData("Auto Path", m_trajectoryField); + } + + public void logAutoInformation() { + if (previousAuto == autoChooser.get()) { + return; + } + + previousAuto = autoChooser.get(); + + Command command = previousAuto; + { + try + { + var paths = PathPlannerAuto.getPathGroupFromAutoFile(command.getName()); // A list of all paths contained in this auto + List poses = new ArrayList<>(); // This will be a list of all points during the auto + + for (PathPlannerPath path : paths) { // For each path assigned, split into segments + List points = path.getAllPathPoints(); + for (PathPoint point : points) { // For each segment, split into points + Pose2d newPose2d = new Pose2d(point.position, new Rotation2d()); + poses.add(newPose2d); + } + } + + // Generate a trajectory from the "poses" list. This is our entire path + // "config" is used for unit conversions; Reference Field2d Widget + var m_trajectory = TrajectoryGenerator.generateTrajectory(poses, new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))); + + // Log the trajectory + m_trajectoryField.getObject("traj").setTrajectory(m_trajectory); + // Log the start and end positions + m_trajectoryField.getObject("start_and_end").setPoses(poses.get(0), poses.get(poses.size() -1)); + } + catch (Exception e) + { + // Fallback in case the path is set to none, or the path file referenced does not exist + e.printStackTrace(); + System.out.println("Pathplanner file not found! Skipping..."); + m_trajectoryField.getObject("traj").setPoses(); + m_trajectoryField.getObject("start_and_end").setPoses(); + } + } + } + + public Command getAuto() { + if (AutoBuilder.isConfigured()) { + return autoChooser.get(); + } else { + return Commands.none(); + } + } + + public void setUpPathPlanner(CommandSwerveDrivetrain drivetrain) { + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + e.printStackTrace(); + } + + AutoBuilder.configure( + drivetrain::getRobotPose, + drivetrain::resetPose, + drivetrain::getChassisSpeeds, + (speeds, feedforwards) -> drivetrain.setControl(drivetrain.driveWithFeedforwards(speeds, feedforwards)), + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + drivetrain + ); + } +} + diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 386c043a..4a9d73b0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,16 +5,13 @@ package frc.robot; -import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -33,7 +30,8 @@ public Robot() { Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables PowerDistribution distribution = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging } else { - setUseTiming(false); // Run as fast as possible + setUseTiming(true); // Run as fast as possible + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables } Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. @@ -42,8 +40,8 @@ public Robot() { @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); - + CommandScheduler.getInstance().run(); + m_robotContainer.auto.logAutoInformation(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b29f1d5e..5c1f83f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,26 +6,10 @@ import static edu.wpi.first.units.Units.*; -import java.lang.reflect.Array; -import java.util.ArrayList; -import java.util.List; - -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; - - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPoint; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.lib.controller.LogitechController; @@ -50,55 +34,17 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - private final LoggedDashboardChooser autoChooser; + public Auto auto = new Auto(drivetrain); // Use open-loop control for drive motors private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Field2d m_trajectoryField = new Field2d(); // *NEW public RobotContainer() { configureBindings(); drivetrain.setUpPathPlanner(); - SmartDashboard.putData("Trajectory Field", m_trajectoryField); // Establish the "Trajectory Field" Field2d into the dashboard - var tempAutoChooser = AutoBuilder.buildAutoChooser(); - tempAutoChooser.onChange((Command command) -> { - try - { - var paths = PathPlannerAuto.getPathGroupFromAutoFile(command.getName()); // A list of all paths contained in this auto - List poses = new ArrayList<>(); // This will be a list of all points during the auto - - for (PathPlannerPath path : paths) { // For each path assigned, split into segments - List points = path.getAllPathPoints(); - for (PathPoint point : points) { // For each segment, split into points - Pose2d newPose2d = new Pose2d(point.position, new Rotation2d()); - poses.add(newPose2d); - } - } - - // Generate a trajectory from the "poses" list. This is our entire path - // "config" is used for unit conversions; Reference Field2d Widget - var m_trajectory = TrajectoryGenerator.generateTrajectory(poses, new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))); - - // Log the trajectory - m_trajectoryField.getObject("traj").setTrajectory(m_trajectory); - // Log the start and end positions - m_trajectoryField.getObject("start_and_end").setPoses(poses.get(0), poses.get(poses.size() -1)); - } - catch (Exception e) - { - // Fallback in case the path is set to none, or the path file referenced does not exist - e.printStackTrace(); - System.out.println("Pathplanner file not found! Skipping..."); - m_trajectoryField.getObject("traj").setPoses(); - m_trajectoryField.getObject("start_and_end").setPoses(); - } - - }); - SmartDashboard.putData(tempAutoChooser); - - autoChooser = new LoggedDashboardChooser<>("Auto Routine", tempAutoChooser); + // Establish the "Trajectory Field" Field2d into the dashboard } private void configureBindings() { @@ -155,8 +101,7 @@ private double sps(double value) { } public Command getAutonomousCommand() { - - return autoChooser.get(); - + return auto.getAuto(); } + } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 2fa6c5e3..471bc230 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -40,6 +40,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.RobotContainer; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements @@ -309,6 +310,12 @@ public SwerveRequest driveFieldRelative(ChassisSpeeds speeds) { return driveRobotRelative(speeds); } + public SwerveRequest driveWithFeedforwards(ChassisSpeeds speeds, DriveFeedforwards feedforwards) { + return m_applyRobotSpeeds.withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()); + } + /** * Returns a command that applies the specified control request to this swerve drivetrain. *