diff --git a/src/main/deploy/choreo/TestPath.traj b/src/main/deploy/choreo/TestPath.traj new file mode 100644 index 0000000..0bdffa8 --- /dev/null +++ b/src/main/deploy/choreo/TestPath.traj @@ -0,0 +1,166 @@ +{ + "name":"TestPath", + "version":"v2025.0.0", + "snapshot":{ + "waypoints":[ + {"x":1.267973780632019, "y":5.541214942932129, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.621983051300049, "y":5.532590866088867, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2593494653701782, "y":5.523966312408447, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.656480073928833, "y":4.1095871925354, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2679738998413086, "y":5.5153422355651855, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.330001354217529, "y":2.548595905303955, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2593495845794678, "y":5.532590866088867, "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}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.267973780632019 m", "val":1.267973780632019}, "y":{"exp":"5.541214942932129 m", "val":5.541214942932129}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.621983051300049 m", "val":2.621983051300049}, "y":{"exp":"5.532590866088867 m", "val":5.532590866088867}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2593494653701782 m", "val":1.2593494653701782}, "y":{"exp":"5.523966312408447 m", "val":5.523966312408447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.656480073928833 m", "val":2.656480073928833}, "y":{"exp":"4.1095871925354 m", "val":4.1095871925354}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2679738998413086 m", "val":1.2679738998413086}, "y":{"exp":"5.5153422355651855 m", "val":5.5153422355651855}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.330001354217529 m", "val":5.330001354217529}, "y":{"exp":"2.548595905303955 m", "val":2.548595905303955}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2593495845794678 m", "val":1.2593495845794678}, "y":{"exp":"5.532590866088867 m", "val":5.532590866088867}, "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}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0 m", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "waypoints":[0.0,0.5755879650145282,1.123301521989643,1.8890997868985295,2.63426533709414,4.249904060382652,5.866427869563064], + "samples":[ + {"t":0.0, "x":1.267973780632019, "y":5.541214942932129, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":18.865456009392055, "ay":-0.011724700521217655, "alpha":0.0, "fx":[165.0727400821805,165.0727400821805,165.0727400821805,165.0727400821805], "fy":[-0.10259112956065956,-0.10259112956064945,-0.10259112956064945,-0.10259112956065956]}, + {"t":0.04796566375121069, "x":1.2896757041642462, "y":5.541201455394164, "heading":0.0, "vx":0.9048941194597564, "vy":-0.0005623830427843708, "omega":0.0, "ax":18.855859918501604, "ay":-0.011383143465239998, "alpha":0.0, "fx":[164.98877428688905,164.98877428688905,164.98877428688905,164.98877428688905], "fy":[-0.09960250532084872,-0.09960250532085128,-0.0996025053208513,-0.09960250532084872]}, + {"t":0.09593132750242138, "x":1.3547704358742665, "y":5.541161385691266, "heading":0.0, "vx":1.8093279560505355, "vy":-0.001108383074669864, "omega":0.0, "ax":18.827664016737536, "ay":-0.01100112488926204, "alpha":0.0, "fx":[164.74206014645344,164.74206014645344,164.74206014645344,164.74206014645344], "fy":[-0.09625984278103791,-0.09625984278104777,-0.09625984278104777,-0.09625984278103791]}, + {"t":0.14389699125363206, "x":1.4632145016507556, "y":5.541095566190434, "heading":0.0, "vx":2.712409357498137, "vy":-0.001636059331993282, "omega":0.0, "ax":17.10112042667401, "ay":-0.007089698386984598, "alpha":0.0, "fx":[149.6348037333976,149.6348037333976,149.6348037333976,149.6348037333976], "fy":[-0.062034860886116386,-0.06203486088611408,-0.06203486088611408,-0.062034860886116386]}, + {"t":0.19186265500484276, "x":1.6129893326209732, "y":5.541008935866734, "heading":0.0, "vx":3.5326759496529436, "vy":-0.0019761214209208858, "omega":0.0, "ax":0.0019087268994704709, "ay":0.0027552334050446586, "alpha":0.0, "fx":[0.016701360370366618,0.016701360370366607,0.01670136037036663,0.016701360370366618], "fy":[0.024108292294140778,0.02410829229414075,0.02410829229414075,0.024108292294140778]}, + {"t":0.23982831875605343, "x":1.7824386750726795, "y":5.540917319380623, "heading":0.0, "vx":3.5327675030055965, "vy":-0.0018439648218584103, "omega":0.0, "ax":-3.726852443715553e-6, "ay":0.0022116266051688755, "alpha":0.0, "fx":[-0.00003260995888251109,-0.0000326099588825114,-0.000032609958882510766,-0.00003260995888251108], "fy":[0.01935173279522766,0.01935173279522766,0.01935173279522766,0.01935173279522766]}, + {"t":0.2877939825072641, "x":1.9518902089458563, "y":5.540831416534092, "heading":0.0, "vx":3.532767324244645, "vy":-0.0017378826837716487, "omega":0.0, "ax":-0.000011043820168810536, "ay":-0.010254064379973067, "alpha":0.0, "fx":[-0.00009663342647709224,-0.00009663342647709478,-0.0000966334264770896,-0.00009663342647709216], "fy":[-0.08972306332476435,-0.08972306332476435,-0.08972306332476435,-0.08972306332476435]}, + {"t":0.3357596462584748, "x":2.121341725827554, "y":5.540736262049567, "heading":0.0, "vx":3.53276679452048, "vy":-0.002229725687904704, "omega":0.0, "ax":-0.04654410883274017, "ay":-0.5456541018080981, "alpha":0.0, "fx":[-0.40726095228647646,-0.4072609522864763,-0.40726095228647646,-0.40726095228647646], "fy":[-4.774473390820859,-4.774473390820859,-4.774473390820859,-4.774473390820859]}, + {"t":0.3837253100096855, "x":2.290739687875358, "y":5.540001617244342, "heading":0.0, "vx":3.53053427544661, "vy":-0.028402386859700823, "omega":0.0, "ax":-18.74672968275005, "ay":-0.1907589536084768, "alpha":0.0, "fx":[-164.03388472406294,-164.03388472406294,-164.03388472406294,-164.03388472406294], "fy":[-1.6691408440741706,-1.669140844074173,-1.6691408440741728,-1.6691408440741706]}, + {"t":0.4316909737608962, "x":2.4385187613820056, "y":5.538419837876941, "heading":0.0, "vx":2.6313349430489796, "vy":-0.03755226668601782, "omega":0.0, "ax":-18.843514025536315, "ay":-0.05302180953447778, "alpha":0.0, "fx":[-164.88074772344277,-164.88074772344277,-164.88074772344277,-164.88074772344277], "fy":[-0.4639408334266865,-0.4639408334266746,-0.46394083342667464,-0.4639408334266865]}, + {"t":0.4796566375121069, "x":2.5430558059597534, "y":5.536557624711507, "heading":0.0, "vx":1.727493285408882, "vy":-0.04009549297362932, "omega":0.0, "ax":-18.860589386032505, "ay":-0.028653820383887793, "alpha":0.0, "fx":[-165.0301571277844,-165.0301571277844,-165.0301571277844,-165.0301571277844], "fy":[-0.250720928359015,-0.25072092835902143,-0.25072092835902143,-0.250720928359015]}, + {"t":0.5276223012633176, "x":2.6042198428200254, "y":5.534601455785127, "heading":0.0, "vx":0.8228325967687936, "vy":-0.04146989248735047, "omega":0.0, "ax":-18.8676984942043, "ay":-0.018654089379989965, "alpha":0.0, "fx":[-165.09236182428765,-165.09236182428765,-165.09236182428765,-165.09236182428765], "fy":[-0.16322328207491524,-0.16322328207490913,-0.16322328207490913,-0.1632232820749153]}, + {"t":0.5755879650145282, "x":2.621983051300049, "y":5.532590866088867, "heading":0.0, "vx":-0.0821690849634343, "vy":-0.04236464826613609, "omega":0.0, "ax":-18.789114212603124, "ay":1.7146450148820802, "alpha":0.0, "fx":[-164.40474936027735,-164.40474936027735,-164.40474936027735,-164.40474936027735], "fy":[15.003143880218202,15.003143880218204,15.003143880218204,15.003143880218202]}, + {"t":0.6212307614291211, "x":2.5986612737459795, "y":5.5324432549302145, "heading":0.0, "vx":-0.9397567997798124, "vy":0.03589654507142329, "omega":0.0, "ax":-18.75973320453252, "ay":-1.952303952348285, "alpha":0.0, "fx":[-164.14766553965953,-164.14766553965953,-164.14766553965953,-164.14766553965953], "fy":[-17.082659583047498,-17.082659583047498,-17.082659583047498,-17.082659583047498]}, + {"t":0.666873557843714, "x":2.5362273989277897, "y":5.532048090514479, "heading":0.0, "vx":-1.7960034832263685, "vy":-0.05321206676501455, "omega":0.0, "ax":-18.770250068928135, "ay":1.7147644122187895, "alpha":0.0, "fx":[-164.2396881031212,-164.2396881031212,-164.2396881031212,-164.2396881031212], "fy":[15.004188606914406,15.004188606914408,15.00418860691441,15.004188606914406]}, + {"t":0.712516354258307, "x":2.434701076349338, "y":5.531405497209796, "heading":0.0, "vx":-2.6527301857734535, "vy":0.025054576200876703, "omega":0.0, "ax":-18.70601033954735, "ay":-1.951963922763416, "alpha":0.0, "fx":[-163.67759047103934,-163.67759047103934,-163.67759047103934,-163.67759047103934], "fy":[-17.079684324179887,-17.079684324179894,-17.079684324179894,-17.079684324179887]}, + {"t":0.7581591506728999, "x":2.2941382654891473, "y":5.530515829202012, "heading":0.0, "vx":-3.5065248074306834, "vy":-0.06403851573444402, "omega":0.0, "ax":-0.5664997033707523, "ay":1.8299819026975657, "alpha":0.0, "fx":[-4.956872404494082,-4.9568724044940815,-4.956872404494082,-4.956872404494082], "fy":[16.012341648603698,16.012341648603698,16.012341648603698,16.012341648603698]}, + {"t":0.8038019470874928, "x":2.1335005831169656, "y":5.529499100765973, "heading":0.0, "vx":-3.5323814380605616, "vy":0.01948697569277029, "omega":0.0, "ax":0.01158430172720965, "ay":-1.8334941437866603, "alpha":0.0, "fx":[0.10136264011308443,0.10136264011308452,0.10136264011308437,0.10136264011308443], "fy":[-16.043073758133282,-16.043073758133282,-16.043073758133282,-16.043073758133282]}, + {"t":0.8494447435020857, "x":1.9722848828652648, "y":5.528478713865706, "heading":0.0, "vx":-3.5318526981352214, "vy":-0.06419882423943257, "omega":0.0, "ax":-0.011610753293089788, "ay":1.833493976366152, "alpha":0.0, "fx":[-0.10159409131453565,-0.10159409131453548,-0.10159409131453578,-0.10159409131453565], "fy":[16.043072293203828,16.043072293203828,16.043072293203828,16.043072293203828]}, + {"t":0.8950875399166786, "x":1.8110691550607552, "y":5.527458326791048, "heading":0.0, "vx":-3.532382645383998, "vy":0.019486968051230104, "omega":0.0, "ax":0.012037323011337598, "ay":-1.833491276467001, "alpha":0.0, "fx":[0.105326576349204,0.1053265763492039,0.10532657634920407,0.105326576349204], "fy":[-16.043048669086257,-16.043048669086257,-16.043048669086257,-16.043048669086257]}, + {"t":0.9407303363312716, "x":1.6498538715850983, "y":5.526437942528692, "heading":0.0, "vx":-3.531833228300415, "vy":-0.06419870100848528, "omega":0.0, "ax":10.774562807024031, "ay":1.901763196968494, "alpha":0.0, "fx":[94.27742456146028,94.27742456146028,94.27742456146028,94.27742456146028], "fy":[16.640427973474324,16.64042797347433,16.640427973474328,16.640427973474324]}, + {"t":0.9863731327458644, "x":1.4998742606388358, "y":5.525488672512943, "heading":0.0, "vx":-3.0400520516431726, "vy":0.022603089419513014, "omega":0.0, "ax":18.737839091829976, "ay":-1.7149695518950985, "alpha":0.0, "fx":[163.9560920535123,163.9560920535123,163.9560920535123,163.9560920535123], "fy":[-15.005983579082114,-15.005983579082102,-15.005983579082104,-15.005983579082114]}, + {"t":1.0320159291604571, "x":1.3806357246646652, "y":5.524733972816046, "heading":0.0, "vx":-2.184804676725377, "vy":-0.05567291669486055, "omega":0.0, "ax":18.749114063214556, "ay":1.9522367403300704, "alpha":0.0, "fx":[164.05474805312735,164.05474805312735,164.05474805312735,164.05474805312735], "fy":[17.082071477888114,17.08207147788812,17.08207147788812,17.082071477888114]}, + {"t":1.07765872557505, "x":1.3004448148838497, "y":5.5242264183177365, "heading":0.0, "vx":-1.3290426805840947, "vy":0.03343262739711329, "omega":0.0, "ax":18.783857315057457, "ay":-1.714678287508146, "alpha":0.0, "fx":[164.35875150675272,164.35875150675272,164.35875150675272,164.35875150675272], "fy":[-15.003435015696285,-15.003435015696276,-15.003435015696278,-15.003435015696285]}, + {"t":1.123301521989643, "x":1.2593494653701782, "y":5.523966312408447, "heading":0.0, "vx":-0.4716949052721658, "vy":-0.04483008459614381, "omega":0.0, "ax":14.27687208230492, "ay":-12.338494522657706, "alpha":0.0, "fx":[124.92263072016804,124.92263072016804,124.92263072016804,124.92263072016804], "fy":[-107.96182707325494,-107.96182707325492,-107.96182707325492,-107.96182707325492]}, + {"t":1.1743547396502354, "x":1.2538737639302393, "y":5.505597874826556, "heading":0.0, "vx":0.25718535265818304, "vy":-0.6747499310654155, "omega":0.0, "ax":14.31797565757378, "ay":-12.281575416970645, "alpha":0.0, "fx":[125.28228700377058,125.28228700377058,125.28228700377058,125.28228700377058], "fy":[-107.46378489849312,-107.46378489849312,-107.46378489849312,-107.46378489849312]}, + {"t":1.225407957310828, "x":1.2856633117640115, "y":5.455144180075873, "heading":0.0, "vx":0.9881640803633616, "vy":-1.301763874042999, "omega":0.0, "ax":14.412029317623784, "ay":-12.149708764420778, "alpha":0.0, "fx":[126.1052565292081,126.10525652920808,126.10525652920808,126.1052565292081], "fy":[-106.3099516886818,-106.30995168868182,-106.3099516886818,-106.3099516886818]}, + {"t":1.2764611749714203, "x":1.3548942478777632, "y":5.372851256685875, "heading":0.0, "vx":1.7239445500468482, "vy":-1.9220456001057808, "omega":0.0, "ax":14.83786207756512, "ay":-11.52066505020585, "alpha":0.0, "fx":[129.8312931786948,129.8312931786948,129.8312931786948,129.8312931786948], "fy":[-100.80581918930118,-100.8058191893012,-100.8058191893012,-100.80581918930118]}, + {"t":1.3275143926320128, "x":1.462244096320975, "y":5.259710734853384, "heading":0.0, "vx":2.481465152310631, "vy":-2.5102126205087205, "omega":0.0, "ax":3.238700403798349, "ay":3.3345653089046676, "alpha":0.0, "fx":[28.338628533235557,28.338628533235557,28.338628533235557,28.338628533235557], "fy":[29.177446452915845,29.177446452915845,29.177446452915845,29.177446452915845]}, + {"t":1.3785676102926052, "x":1.5931516014793994, "y":5.135901960816366, "heading":0.0, "vx":2.646811228963197, "vy":-2.3399723319897494, "omega":0.0, "ax":0.06325471542913676, "ay":0.07165034653139979, "alpha":0.0, "fx":[0.5534787600049467,0.5534787600049467,0.5534787600049467,0.5534787600049467], "fy":[0.6269405321497482,0.6269405321497483,0.6269405321497483,0.6269405321497482]}, + {"t":1.4296208279531977, "x":1.7283622657848123, "y":5.016532219874909, "heading":0.0, "vx":2.650040585718059, "vy":-2.336314351252825, "omega":0.0, "ax":-0.0007530549656576511, "ay":-0.0008476983388608786, "alpha":0.0, "fx":[-0.006589230949504448,-0.006589230949504449,-0.006589230949504447,-0.006589230949504448], "fy":[-0.007417360465032685,-0.007417360465032688,-0.007417360465032688,-0.007417360465032685]}, + {"t":1.48067404561379, "x":1.863654383223964, "y":4.897254750043204, "heading":0.0, "vx":2.6500021398389872, "vy":-2.33635762898063, "omega":0.0, "ax":-0.0019011581103465028, "ay":-0.0021504467490689938, "alpha":0.0, "fx":[-0.0166351334655319,-0.016635133465531894,-0.016635133465531905,-0.0166351334655319], "fy":[-0.018816409054353694,-0.01881640905435369,-0.01881640905435369,-0.018816409054353694]}, + {"t":1.5317272632743826, "x":1.9989430416514504, "y":4.7779733729822995, "heading":0.0, "vx":2.6499050796001726, "vy":-2.336467416206577, "omega":0.0, "ax":-0.001893407496065109, "ay":-0.002142029305962182, "alpha":0.0, "fx":[-0.016567315590569707,-0.01656731559056971,-0.016567315590569703,-0.016567315590569707], "fy":[-0.01874275642716909,-0.018742756427169095,-0.018742756427169095,-0.01874275642716909]}, + {"t":1.582780480934975, "x":2.134226754942159, "y":4.6586864018999945, "heading":0.0, "vx":2.649808415055156, "vy":-2.3365767736949694, "omega":0.0, "ax":-0.005457638948225878, "ay":-0.0061837055684424444, "alpha":0.0, "fx":[-0.04775434079697643,-0.04775434079697643,-0.04775434079697643,-0.04775434079697643], "fy":[-0.05410742372387139,-0.05410742372387138,-0.05410742372387138,-0.05410742372387139]}, + {"t":1.6338336985955677, "x":2.269500888235077, "y":4.539388580590812, "heading":0.0, "vx":2.649529785026019, "vy":-2.336892471761304, "omega":0.0, "ax":-0.18829498689761537, "ay":-0.2120178340059645, "alpha":0.0, "fx":[-1.6475811353541343,-1.6475811353541343,-1.6475811353541343,-1.6475811353541343], "fy":[-1.855156047552189,-1.855156047552189,-1.855156047552189,-1.855156047552189]}, + {"t":1.68488691625616, "x":2.4045225200995812, "y":4.419806395649478, "heading":0.0, "vx":2.639916720075537, "vy":-2.347716664388738, "omega":0.0, "ax":-13.248077177295125, "ay":2.588962068337596, "alpha":0.0, "fx":[-115.92067530133232,-115.92067530133232,-115.92067530133232,-115.92067530133232], "fy":[22.65341809795397,22.65341809795396,22.653418097953963,22.65341809795397]}, + {"t":1.7359401339167524, "x":2.5220336632708835, "y":4.303321881316774, "heading":0.0, "vx":1.963559752358762, "vy":-2.2155418203988813, "omega":0.0, "ax":-14.181757773920854, "ay":12.389581904603864, "alpha":0.0, "fx":[-124.09038052180748,-124.09038052180748,-124.09038052180748,-124.09038052180748], "fy":[108.40884166528382,108.40884166528384,108.40884166528382,108.40884166528382]}, + {"t":1.7869933515773448, "x":2.603797819911871, "y":4.206357637907929, "heading":0.0, "vx":1.2395353859169815, "vy":-1.5830137986994028, "omega":0.0, "ax":-14.173842240902951, "ay":12.439092392435006, "alpha":0.0, "fx":[-124.0211196079008,-124.02111960790084,-124.02111960790084,-124.0211196079008], "fy":[108.8420584338063,108.8420584338063,108.84205843380629,108.8420584338063]}, + {"t":1.8380465692379373, "x":2.6486085186267876, "y":4.141750508103315, "heading":0.0, "vx":0.5159151329052638, "vy":-0.9479581072881988, "omega":0.0, "ax":-14.170773778550853, "ay":12.456110142915543, "alpha":0.0, "fx":[-123.99427056231994,-123.99427056231994,-123.99427056231994,-123.99427056231994], "fy":[108.990963750511,108.990963750511,108.990963750511,108.990963750511]}, + {"t":1.8890997868985295, "x":2.656480073928833, "y":4.1095871925354, "heading":0.0, "vx":-0.20754846523010892, "vy":-0.3120336049576183, "omega":0.0, "ax":-14.161783530248304, "ay":12.465604363150828, "alpha":0.0, "fx":[-123.91560588967263,-123.91560588967263,-123.91560588967263,-123.91560588967263], "fy":[109.07403817756976,109.07403817756972,109.07403817756976,109.07403817756976]}, + {"t":1.938777490244904, "x":2.6286947926738113, "y":4.1094678514327265, "heading":0.0, "vx":-0.9110733463013496, "vy":0.3072289906282545, "omega":0.0, "ax":-14.134779997664468, "ay":12.482495087206695, "alpha":0.0, "fx":[-123.67932497956409,-123.6793249795641,-123.67932497956409,-123.67932497956409], "fy":[109.22183201305856,109.22183201305856,109.22183201305856,109.22183201305856]}, + {"t":1.9884551935912775, "x":2.565993331740958, "y":4.140132895938215, "heading":0.0, "vx":-1.6132567538915867, "vy":0.92733067859308, "omega":0.0, "ax":-14.054988112273543, "ay":12.531985196809796, "alpha":0.0, "fx":[-122.9811459823935,-122.9811459823935,-122.9811459823935,-122.9811459823935], "fy":[109.6548704720857,109.6548704720857,109.65487047208572,109.6548704720857]}, + {"t":2.038132896937652, "x":2.468507469959143, "y":4.201664235825569, "heading":0.0, "vx":-2.311476283869925, "vy":1.5498909215413477, "omega":0.0, "ax":-7.469669989098811, "ay":15.075100656521212, "alpha":0.0, "fx":[-65.3596124046146,-65.35961240461458,-65.3596124046146,-65.3596124046146], "fy":[131.90713074456062,131.90713074456062,131.90713074456062,131.90713074456062]}, + {"t":2.0878106002840258, "x":2.344461533876078, "y":4.297260983305097, "heading":0.0, "vx":-2.682552333683689, "vy":2.298787299872737, "omega":0.0, "ax":0.4398183750799681, "ay":0.5103816650449983, "alpha":0.0, "fx":[3.8484107819497217,3.8484107819497217,3.8484107819497217,3.8484107819497217], "fy":[4.465839569143735,4.465839569143735,4.465839569143735,4.465839569143735]}, + {"t":2.1374883036304, "x":2.2117412030446375, "y":4.41208923571874, "heading":0.0, "vx":-2.660703166920182, "vy":2.3241418888222714, "omega":0.0, "ax":0.0075517884104739155, "ay":0.008648952993987711, "alpha":0.0, "fx":[0.06607814859164676,0.06607814859164676,0.06607814859164676,0.06607814859164676], "fy":[0.07567833869739249,0.07567833869739249,0.07567833869739249,0.07567833869739249]}, + {"t":2.1871660069767738, "x":2.079572898857547, "y":4.527557939270552, "heading":0.0, "vx":-2.660328011415792, "vy":2.324571548943363, "omega":0.0, "ax":-0.0017829199578066714, "ay":-0.002035142018568675, "alpha":0.0, "fx":[-0.015600549630808376,-0.015600549630808374,-0.015600549630808376,-0.015600549630808376], "fy":[-0.017807492662475912,-0.017807492662475912,-0.017807492662475912,-0.017807492662475912]}, + {"t":2.236843710323148, "x":1.9474117130912931, "y":4.643034803849131, "heading":0.0, "vx":-2.6604165827845465, "vy":2.324470447761897, "omega":0.0, "ax":-0.002049101106542957, "ay":-0.0023394086393347274, "alpha":0.0, "fx":[-0.017929634682250874,-0.017929634682250874,-0.01792963468225087,-0.017929634682250874], "fy":[-0.02046982559417886,-0.020469825594178863,-0.020469825594178863,-0.02046982559417886]}, + {"t":2.2865214136695218, "x":1.8152457988520612, "y":4.758506270507336, "heading":0.0, "vx":-2.6605183774214436, "vy":2.324354231313506, "omega":0.0, "ax":0.000026952933190144705, "ay":0.00003743111947314196, "alpha":0.0, "fx":[0.0002358381654137661,0.00023583816541376256,0.0002358381654137698,0.00023583816541376625], "fy":[0.00032752229538999617,0.00032752229538998814,0.00032752229538998814,0.00032752229538999617]}, + {"t":2.336199117015896, "x":1.6830773894091666, "y":4.873974896670066, "heading":0.0, "vx":-2.660517038461625, "vy":2.324356090805555, "omega":0.0, "ax":0.10522822372057762, "ay":0.12017162097046738, "alpha":0.0, "fx":[0.9207469575550542,0.9207469575550542,0.9207469575550542,0.9207469575550542], "fy":[1.0515016834915896,1.0515016834915896,1.0515016834915896,1.0515016834915896]}, + {"t":2.38587682036227, "x":1.5510388582342265, "y":4.989591853242511, "heading":0.0, "vx":-2.655289541979968, "vy":2.330325940942779, "omega":0.0, "ax":4.682469079898556, "ay":4.844547954667019, "alpha":0.0, "fx":[40.97160444911238,40.97160444911238,40.97160444911238,40.97160444911238], "fy":[42.38979460333641,42.38979460333641,42.38979460333641,42.38979460333641]}, + {"t":2.435554523708644, "x":1.4249080444091806, "y":5.111334961514686, "heading":0.0, "vx":-2.4226752321001985, "vy":2.5709919570820103, "omega":0.0, "ax":18.29538428012582, "ay":2.30436960060017, "alpha":0.0, "fx":[160.08461245110092,160.08461245110092,160.08461245110092,160.08461245110092], "fy":[20.16323400525148,20.16323400525148,20.16323400525149,20.16323400525148]}, + {"t":2.485232227055018, "x":1.3271304564356792, "y":5.241899384418069, "heading":0.0, "vx":-1.5138025592241933, "vy":2.685467746501028, "omega":0.0, "ax":15.207752708009496, "ay":-11.11003334917073, "alpha":0.0, "fx":[133.0678361950831,133.06783619508312,133.0678361950831,133.0678361950831], "fy":[-97.21279180524388,-97.2127918052439,-97.21279180524388,-97.21279180524388]}, + {"t":2.534909930401392, "x":1.2706936323218885, "y":5.361598172088949, "heading":0.0, "vx":-0.7583163316306809, "vy":2.133546805612602, "omega":0.0, "ax":14.755175390402924, "ay":-11.743898320982982, "alpha":0.0, "fx":[129.10778466602557,129.10778466602557,129.10778466602557,129.10778466602557], "fy":[-102.7591103086011,-102.7591103086011,-102.75911030860112,-102.7591103086011]}, + {"t":2.584587633747766, "x":1.2512291769597357, "y":5.4530966454795164, "heading":0.0, "vx":-0.025313105762525705, "vy":1.5501369086928292, "omega":0.0, "ax":14.589252376922582, "ay":-11.963050103766829, "alpha":0.0, "fx":[127.6559582980726,127.6559582980726,127.6559582980726,127.6559582980726], "fy":[-104.67668840795974,-104.67668840795974,-104.67668840795974,-104.67668840795974]}, + {"t":2.63426533709414, "x":1.2679738998413086, "y":5.5153422355651855, "heading":0.0, "vx":0.6994474458636166, "vy":0.9558400545200916, "omega":0.0, "ax":14.475230158330158, "ay":-12.10049023547646, "alpha":0.0, "fx":[126.65826388538888,126.6582638853889,126.65826388538888,126.65826388538888], "fy":[-105.879289560419,-105.87928956041898,-105.879289560419,-105.879289560419]}, + {"t":2.6832240862847008, "x":1.3195662394030554, "y":5.547636778833588, "heading":0.0, "vx":1.408136608660946, "vy":0.3634151879985685, "omega":0.0, "ax":14.253675387397688, "ay":-12.348018276174209, "alpha":0.0, "fx":[124.71965963972976,124.71965963972978,124.71965963972976,124.71965963972976], "fy":[-108.04515991652433,-108.04515991652433,-108.04515991652433,-108.04515991652433]}, + {"t":2.7321828354752618, "x":1.4055895850756268, "y":5.550630284350123, "heading":0.0, "vx":2.1059787269962205, "vy":-0.2411283417851067, "omega":0.0, "ax":13.642535315214772, "ay":-12.98794971428502, "alpha":0.0, "fx":[119.37218400812924,119.37218400812924,119.37218400812924,119.37218400812924], "fy":[-113.64455999999392,-113.64455999999392,-113.6445599999939,-113.64455999999392]}, + {"t":2.7811415846658227, "x":1.5250459691088725, "y":5.523259150068087, "heading":0.0, "vx":2.7739001918171904, "vy":-0.877002114346404, "omega":0.0, "ax":6.152059015241305, "ay":-17.581463039935677, "alpha":0.0, "fx":[53.83051638336141,53.83051638336141,53.83051638336141,53.83051638336142], "fy":[-153.83780159943717,-153.83780159943717,-153.83780159943717,-153.83780159943717]}, + {"t":2.8301003338563837, "x":1.6682257698684675, "y":5.459251199403695, "heading":0.0, "vx":3.0750973061499187, "vy":-1.7377685537217311, "omega":0.0, "ax":-5.494174113747152, "ay":-8.342208342590064, "alpha":0.0, "fx":[-48.07402349528758,-48.07402349528757,-48.07402349528758,-48.07402349528758], "fy":[-72.99432299766306,-72.99432299766306,-72.99432299766306,-72.99432299766306]}, + {"t":2.8790590830469447, "x":1.812194032236094, "y":5.364174258437323, "heading":0.0, "vx":2.8061094137056997, "vy":-2.146192639662003, "omega":0.0, "ax":-0.19010704770465667, "ay":-0.24747411939444328, "alpha":0.0, "fx":[-1.6634366674157457,-1.663436667415746,-1.6634366674157457,-1.6634366674157457], "fy":[-2.165398544701379,-2.165398544701379,-2.165398544701379,-2.165398544701379]}, + {"t":2.9280178322375057, "x":1.9493497998118785, "y":5.2588027586034745, "heading":0.0, "vx":2.7968020104377693, "vy":-2.1583086630045902, "omega":0.0, "ax":-0.00544243107015352, "ay":-0.007044365478369092, "alpha":0.0, "fx":[-0.04762127186384331,-0.04762127186384329,-0.04762127186384331,-0.04762127186384331], "fy":[-0.06163819793572957,-0.06163819793572955,-0.06163819793572955,-0.06163819793572957]}, + {"t":2.976976581428066, "x":2.0862712053341568, "y":5.153126223567571, "heading":0.0, "vx":2.796535555820019, "vy":-2.1586535463272525, "omega":0.0, "ax":-0.0012240090566071483, "ay":-0.0015790499387939865, "alpha":0.0, "fx":[-0.010710079245312547,-0.010710079245312558,-0.010710079245312537,-0.010710079245312547], "fy":[-0.013816686964447367,-0.013816686964447392,-0.013816686964447392,-0.013816686964447367]}, + {"t":3.0259353306186276, "x":2.2231846212641977, "y":5.047439353544542, "heading":0.0, "vx":2.7964756298676097, "vy":-2.1587308546371657, "omega":0.0, "ax":-0.0010796378734730975, "ay":-0.0013924990496096805, "alpha":0.0, "fx":[-0.009446831392889606,-0.009446831392889606,-0.009446831392889604,-0.009446831392889606], "fy":[-0.012184366684084702,-0.012184366684084706,-0.012184366684084706,-0.012184366684084702]}, + {"t":3.0748940798091886, "x":2.360095276320477, "y":4.9417489221807855, "heading":0.0, "vx":2.796422772147746, "vy":-2.1587990296488835, "omega":0.0, "ax":-0.0010398196931065188, "ay":-0.0013413517611034014, "alpha":0.0, "fx":[-0.00909842231468204,-0.00909842231468204,-0.00909842231468204,-0.00909842231468204], "fy":[-0.011736827909654765,-0.01173682790965476,-0.01173682790965476,-0.011736827909654765]}, + {"t":3.1238528289997496, "x":2.4970033912501814, "y":4.836055214352711, "heading":0.0, "vx":2.7963718638761876, "vy":-2.1588647005533317, "omega":0.0, "ax":-0.0010132094430629357, "ay":-0.001307276091564062, "alpha":0.0, "fx":[-0.008865582626800686,-0.008865582626800681,-0.008865582626800692,-0.008865582626800686], "fy":[-0.011438665801185547,-0.011438665801185535,-0.011438665801185535,-0.011438665801185547]}, + {"t":3.1728115781903106, "x":2.6339090456664285, "y":4.730358332198287, "heading":0.0, "vx":2.7963222584091874, "vy":-2.1589287031556212, "omega":0.0, "ax":-0.0009959774351845996, "ay":-0.0012853156148322077, "alpha":0.0, "fx":[-0.008714802557865248,-0.008714802557865256,-0.008714802557865237,-0.008714802557865248], "fy":[-0.011246511629781807,-0.011246511629781826,-0.011246511629781826,-0.011246511629781807]}, + {"t":3.2217703273808715, "x":2.770812292113267, "y":4.624658342875695, "heading":0.0, "vx":2.796273496599739, "vy":-2.1589916306004384, "omega":0.0, "ax":-0.00098668440011179, "ay":-0.0012736096915502426, "alpha":0.0, "fx":[-0.008633488500978162,-0.008633488500978155,-0.008633488500978167,-0.008633488500978162], "fy":[-0.011144084801064632,-0.011144084801064618,-0.011144084801064618,-0.011144084801064632]}, + {"t":3.2707290765714325, "x":2.9077131623804195, "y":4.518955286733425, "heading":0.0, "vx":2.796225189765664, "vy":-2.159053984937894, "omega":0.0, "ax":-0.0009842641624195625, "ay":-0.0012707803154332865, "alpha":0.0, "fx":[-0.008612311421171172,-0.00861231142117117,-0.008612311421171174,-0.008612311421171172], "fy":[-0.011119327760041256,-0.011119327760041252,-0.011119327760041252,-0.011119327760041256]}, + {"t":3.3196878257619935, "x":3.044611670506004, "y":4.413249181191735, "heading":0.0, "vx":2.7961770014233984, "vy":-2.159116200752633, "omega":0.0, "ax":-0.0009879019200471574, "ay":-0.0012757745560958123, "alpha":0.0, "fx":[-0.008644141800412628,-0.00864414180041263,-0.008644141800412626,-0.008644141800412628], "fy":[-0.01116302736583836,-0.01116302736583836,-0.01116302736583836,-0.01116302736583836]}, + {"t":3.3686465749525545, "x":3.1815078150308467, "y":4.307540023656079, "heading":0.0, "vx":2.79612863498107, "vy":-2.159178661079149, "omega":0.0, "ax":-0.000996977123416569, "ay":-0.001287790655050978, "alpha":0.0, "fx":[-0.00872354982989498,-0.008723549829894981,-0.008723549829894978,-0.00872354982989498], "fy":[-0.011268168231696057,-0.01126816823169606,-0.01126816823169606,-0.011268168231696057]}, + {"t":3.4176053241431155, "x":3.3184015807187253, "y":4.201827793739915, "heading":0.0, "vx":2.7960798242281357, "vy":-2.159241709698839, "omega":0.0, "ax":-0.0010110230511126148, "ay":-0.0013062257291048618, "alpha":0.0, "fx":[-0.00884645169723538,-0.008846451697235382,-0.008846451697235377,-0.00884645169723538], "fy":[-0.011429475129667537,-0.011429475129667544,-0.011429475129667544,-0.011429475129667537]}, + {"t":3.4665640733336764, "x":3.455292939859435, "y":4.096112454948134, "heading":0.0, "vx":2.7960303258041503, "vy":-2.1593056608766967, "omega":0.0, "ax":-0.0010296975251083576, "ay":-0.0013306378867469029, "alpha":0.0, "fx":[-0.00900985334469813,-0.009009853344698124,-0.009009853344698136,-0.00900985334469813], "fy":[-0.011643081509035404,-0.011643081509035397,-0.011643081509035397,-0.011643081509035404]}, + {"t":3.5155228225242374, "x":3.592181853238245, "y":3.990393955929203, "heading":0.0, "vx":2.795979913101277, "vy":-2.159370807243258, "omega":0.0, "ax":-0.001052761665981217, "ay":-0.0013607187469092546, "alpha":0.0, "fx":[-0.009211664577335646,-0.009211664577335653,-0.009211664577335636,-0.009211664577335646], "fy":[-0.011906289035455968,-0.011906289035455986,-0.011906289035455986,-0.011906289035455968]}, + {"t":3.5644815717147984, "x":3.7290682708322778, "y":3.8846722313743545, "heading":0.0, "vx":2.7959283712069145, "vy":-2.159437426331107, "omega":0.0, "ax":-0.0010800645954020763, "ay":-0.001396273649659147, "alpha":0.0, "fx":[-0.009450565209768167,-0.009450565209768164,-0.009450565209768173,-0.009450565209768167], "fy":[-0.01221739443451754,-0.01221739443451753,-0.01221739443451753,-0.01221739443451754]}, + {"t":3.6134403209053594, "x":3.865952132277627, "y":3.778947202620469, "heading":0.0, "vx":2.7958754925952785, "vy":-2.159505786142522, "omega":0.0, "ax":-0.001111532669455948, "ay":-0.0014372077277624022, "alpha":0.0, "fx":[-0.009725910857739543,-0.009725910857739536,-0.009725910857739549,-0.009725910857739543], "fy":[-0.012575567617921024,-0.012575567617921012,-0.012575567617921012,-0.012575567617921024]}, + {"t":3.66239907009592, "x":4.00283336713845, "y":3.673218777997065, "heading":0.0, "vx":2.7958210733460978, "vy":-2.1595761500352006, "omega":0.0, "ax":-0.0011471623006121866, "ay":-0.001483516617562025, "alpha":0.0, "fx":[-0.010037670130356632,-0.010037670130356632,-0.010037670130356635,-0.010037670130356632], "fy":[-0.01298077040366772,-0.012980770403667718,-0.012980770403667718,-0.01298077040366772]}, + {"t":3.711357819286482, "x":4.1397118949995155, "y":3.5674868529452293, "heading":0.0, "vx":2.795764909714741, "vy":-2.1596487811531997, "omega":0.0, "ax":-0.0011870157885547532, "ay":-0.0015352810601879344, "alpha":0.0, "fx":[-0.01038638814985409,-0.0103863881498541,-0.010386388149854084,-0.01038638814985409], "fy":[-0.013433709276644416,-0.013433709276644432,-0.013433709276644432,-0.013433709276644416]}, + {"t":3.7603165684770423, "x":4.276587625395849, "y":3.461751309926078, "heading":0.0, "vx":2.795706794906464, "vy":-2.1597239465935627, "omega":0.0, "ax":-0.0012312236569159414, "ay":-0.0015926699179964964, "alpha":0.0, "fx":[-0.010773206998014487,-0.01077320699801448,-0.010773206998014493,-0.010773206998014487], "fy":[-0.01393586178246935,-0.013935861782469337,-0.013935861782469337,-0.01393586178246935]}, + {"t":3.8092753176676033, "x":4.413460457581633, "y":3.3560120181216115, "heading":0.0, "vx":2.7956465157362476, "vy":-2.159801921720621, "omega":0.0, "ax":-0.0012801651497510711, "ay":-0.0016561737793626657, "alpha":0.0, "fx":[-0.011201445060321872,-0.011201445060321877,-0.011201445060321872,-0.011201445060321872], "fy":[-0.014491520569423322,-0.014491520569423324,-0.014491520569423324,-0.014491520569423322]}, + {"t":3.858234066858164, "x":4.550330279919263, "y":3.250268832644376, "heading":0.0, "vx":2.7955838404517586, "vy":-2.159883005917301, "omega":0.0, "ax":-0.0013424923801207357, "ay":-0.001736990027943751, "alpha":0.0, "fx":[-0.011746808326056436,-0.01174680832605644,-0.01174680832605643,-0.011746808326056436], "fy":[-0.015198662744507816,-0.015198662744507823,-0.015198662744507823,-0.015198662744507816]}, + {"t":3.9071928160487257, "x":4.687196959055447, "y":3.1445215805296693, "heading":0.0, "vx":2.79551811370403, "vy":-2.1599680467764255, "omega":0.0, "ax":-0.001792828379695196, "ay":-0.002319930663031605, "alpha":0.0, "fx":[-0.015687248322332967,-0.015687248322332963,-0.01568724832233297,-0.015687248322332967], "fy":[-0.020299393301526546,-0.02029939330152654,-0.02029939330152654,-0.020299393301526546]}, + {"t":3.9561515652392862, "x":4.824059880573783, "y":3.038769466278434, "heading":0.0, "vx":2.7954303390690467, "vy":-2.1600816276798964, "omega":0.0, "ax":-0.019327259961354953, "ay":-0.025000190882196675, "alpha":0.0, "fx":[-0.16911352466185584,-0.16911352466185584,-0.1691135246618558,-0.16911352466185584], "fy":[-0.2187516702192209,-0.21875167021922093,-0.21875167021922093,-0.2187516702192209]}, + {"t":4.005110314429847, "x":4.960897490097912, "y":2.932984609419918, "heading":0.0, "vx":2.794484100596058, "vy":-2.1613056057550137, "omega":0.0, "ax":-0.8059592384037121, "ay":-1.0190658027892905, "alpha":0.0, "fx":[-7.052143336032481,-7.052143336032481,-7.05214333603248,-7.052143336032481], "fy":[-8.916825774406291,-8.916825774406291,-8.916825774406291,-8.916825774406291]}, + {"t":4.054069063620408, "x":5.096746010621656, "y":2.825948460807493, "heading":0.0, "vx":2.7550253443852353, "vy":-2.2111977928024524, "omega":0.0, "ax":-17.137893215426843, "ay":4.00691171475562, "alpha":0.0, "fx":[-149.95656563498486,-149.95656563498486,-149.95656563498486,-149.95656563498486], "fy":[35.060477504111674,35.06047750411166,35.06047750411166,35.060477504111674]}, + {"t":4.103027812810969, "x":5.211089190731154, "y":2.722493184452432, "heading":0.0, "vx":1.915975528796537, "vy":-2.015024407131012, "omega":0.0, "ax":-15.102232262966464, "ay":11.245282497665515, "alpha":0.0, "fx":[-132.14453230095654,-132.14453230095657,-132.14453230095657,-132.14453230095654], "fy":[98.39622185457328,98.39622185457326,98.39622185457328,98.39622185457328]}, + {"t":4.15198656200153, "x":5.286793239405819, "y":2.63731735112368, "heading":0.0, "vx":1.1765891272163649, "vy":-1.4644694417508015, "omega":0.0, "ax":-15.025841838884675, "ay":11.392552721685878, "alpha":0.0, "fx":[-131.47611609024094,-131.47611609024094,-131.47611609024094,-131.47611609024094], "fy":[99.68483631475144,99.68483631475142,99.68483631475142,99.68483631475144]}, + {"t":4.200945311192091, "x":5.326389407052538, "y":2.5792725006140502, "heading":0.0, "vx":0.4409427052493736, "vy":-0.9067043104095408, "omega":0.0, "ax":-14.999051073878473, "ay":11.443260325659724, "alpha":0.0, "fx":[-131.24169689643662,-131.24169689643662,-131.24169689643662,-131.24169689643662], "fy":[100.12852784952258,100.12852784952258,100.1285278495226,100.1285278495226]}, + {"t":4.249904060382652, "x":5.330001354217529, "y":2.548595905303955, "heading":0.0, "vx":-0.29339207437305564, "vy":-0.3464565982032701, "omega":0.0, "ax":-14.9818799301223, "ay":11.465727948948766, "alpha":0.0, "fx":[-131.09144938857014,-131.09144938857014,-131.09144938857014,-131.09144938857014], "fy":[100.32511955330168,100.32511955330168,100.32511955330168,100.32511955330168]}, + {"t":4.298889630357816, "x":5.297654221068327, "y":2.5453810318795393, "heading":0.0, "vx":-1.0272880020495716, "vy":0.2151986205561571, "omega":0.0, "ax":-14.949152770353576, "ay":11.492881346322967, "alpha":0.0, "fx":[-130.80508674059382,-130.8050867405938,-130.8050867405938,-130.80508674059382], "fy":[100.56271178032596,100.56271178032598,100.56271178032596,100.56271178032596]}, + {"t":4.34787520033298, "x":5.229396043417714, "y":2.569711737932569, "heading":0.0, "vx":-1.7595807711511484, "vy":0.7781839639627217, "omega":0.0, "ax":-14.852485850036848, "ay":11.57227163795689, "alpha":0.0, "fx":[-129.95925118782242,-129.95925118782242,-129.95925118782242,-129.95925118782242], "fy":[101.25737683212276,101.2573768321228,101.2573768321228,101.25737683212276]}, + {"t":4.396860770308144, "x":5.125382067381477, "y":2.621715853838815, "heading":0.0, "vx":-2.487138256063267, "vy":1.3450582860554685, "omega":0.0, "ax":-7.276729652638048, "ay":15.33332096235697, "alpha":0.0, "fx":[-63.671384460582914,-63.671384460582914,-63.671384460582914,-63.671384460582914], "fy":[134.1665584206235,134.1665584206235,134.16655842062346,134.1665584206235]}, + {"t":4.445846340283308, "x":4.99481761276168, "y":2.706001112292851, "heading":0.0, "vx":-2.843593005652921, "vy":2.0961697530086596, "omega":0.0, "ax":0.5545062419367349, "ay":0.7446709767474283, "alpha":0.0, "fx":[4.85192961694643,4.851929616946431,4.851929616946431,4.85192961694643], "fy":[6.515871046539998,6.515871046539998,6.515871046539998,6.515871046539998]}, + {"t":4.494831910258472, "x":4.856187881328154, "y":2.8095766334583816, "heading":0.0, "vx":-2.8164302013368636, "vy":2.132647885248595, "omega":0.0, "ax":0.011000672723273692, "ay":0.014524171701441428, "alpha":0.0, "fx":[0.09625588632864482,0.09625588632864485,0.09625588632864482,0.09625588632864482], "fy":[0.12708650238761246,0.1270865023876125,0.1270865023876125,0.12708650238761246]}, + {"t":4.543817480233636, "x":4.718236641150892, "y":2.914063031673629, "heading":0.0, "vx":-2.815891327113404, "vy":2.133359360077807, "omega":0.0, "ax":-0.001227251474132137, "ay":-0.0016196997558399805, "alpha":0.0, "fx":[-0.010738450398656198,-0.010738450398656191,-0.010738450398656204,-0.010738450398656198], "fy":[-0.014172372863599838,-0.014172372863599824,-0.014172372863599824,-0.014172372863599838]}, + {"t":4.5928030502088, "x":4.580297127056351, "y":3.01856491258441, "heading":0.0, "vx":-2.815951444726367, "vy":2.1332800181620786, "omega":0.0, "ax":-0.0014408334318671628, "ay":-0.0019014827466442993, "alpha":0.0, "fx":[-0.012607292528837674,-0.01260729252883768,-0.01260729252883767,-0.012607292528837674], "fy":[-0.016637974033137614,-0.01663797403313762,-0.01663797403313762,-0.016637974033137614]}, + {"t":4.641788620183964, "x":4.442354411812129, "y":3.1230625688049565, "heading":0.0, "vx":-2.8160220247732664, "vy":2.1331868729459367, "omega":0.0, "ax":-0.0013919510259475228, "ay":-0.001836820339249938, "alpha":0.0, "fx":[-0.012179571477040824,-0.012179571477040824,-0.012179571477040829,-0.012179571477040824], "fy":[-0.01607217796843696,-0.016072177968436952,-0.016072177968436952,-0.01607217796843696]}, + {"t":4.690774190159128, "x":4.304408297812851, "y":3.227555739835506, "heading":0.0, "vx":-2.8160902102876504, "vy":2.1330968952546763, "omega":0.0, "ax":-0.001342531070884347, "ay":-0.0017714350321481843, "alpha":0.0, "fx":[-0.011747146870238036,-0.011747146870238038,-0.011747146870238036,-0.011747146870238036], "fy":[-0.01550005653129661,-0.015500056531296613,-0.015500056531296613,-0.01550005653129661]}, + {"t":4.739759760134292, "x":4.166458903001005, "y":3.3320445817063997, "heading":0.0, "vx":-2.8161559749373666, "vy":2.1330101204999528, "omega":0.0, "ax":-0.0012981504804318088, "ay":-0.0017126891118853145, "alpha":0.0, "fx":[-0.011358816703778328,-0.011358816703778326,-0.011358816703778328,-0.011358816703778328], "fy":[-0.014986029728996503,-0.014986029728996502,-0.014986029728996502,-0.014986029728996503]}, + {"t":4.788745330109456, "x":4.028506339917832, "y":3.43652924334942, "heading":0.0, "vx":-2.816219565578564, "vy":2.1329262234476167, "omega":0.0, "ax":-0.001258744996513245, "ay":-0.0016604982736770876, "alpha":0.0, "fx":[-0.011014018719490894,-0.011014018719490893,-0.011014018719490896,-0.011014018719490894], "fy":[-0.01452935989467452,-0.014529359894674515,-0.014529359894674515,-0.01452935989467452]}, + {"t":4.83773090008462, "x":3.8905507090892786, "y":3.541009857865716, "heading":0.0, "vx":-2.816281225919672, "vy":2.132844882993238, "omega":0.0, "ax":-0.001224200346111438, "ay":-0.0016147119637453585, "alpha":0.0, "fx":[-0.010711753028475084,-0.01071175302847508,-0.010711753028475086,-0.010711753028475084], "fy":[-0.014128729682771887,-0.014128729682771884,-0.014128729682771884,-0.014128729682771887]}, + {"t":4.886716470059784, "x":3.752592099240202, "y":3.645486542807588, "heading":0.0, "vx":-2.81634119407139, "vy":2.132765785407348, "omega":0.0, "ax":-0.001194476831760012, "ay":-0.0015752778729050151, "alpha":0.0, "fx":[-0.010451672277900104,-0.010451672277900106,-0.0104516722779001,-0.010451672277900104], "fy":[-0.01378368138791888,-0.013783681387918884,-0.013783681387918884,-0.01378368138791888]}, + {"t":4.935702040034948, "x":3.6146305874791, "y":3.7499594004218793, "heading":0.0, "vx":-2.8163997061998156, "vy":2.1326886195228747, "omega":0.0, "ax":-0.0011696148718010348, "ay":-0.001542249265081888, "alpha":0.0, "fx":[-0.010234130128259054,-0.01023413012825906,-0.010234130128259048,-0.010234130128259054], "fy":[-0.01349468106946651,-0.013494681069466528,-0.013494681069466528,-0.01349468106946651]}, + {"t":4.984687610010112, "x":3.4766662392972423, "y":3.85442851764883, "heading":0.0, "vx":-2.8164570004509626, "vy":2.132613071563581, "omega":0.0, "ax":-0.0011497410907868476, "ay":-0.0015157930280517609, "alpha":0.0, "fx":[-0.010060234544384917,-0.010060234544384912,-0.010060234544384917,-0.010060234544384917], "fy":[-0.01326318899545291,-0.013263188995452905,-0.013263188995452905,-0.01326318899545291]}, + {"t":5.033673179985276, "x":3.33869910836826, "y":3.958893965857944, "heading":0.0, "vx":-2.8165133211736184, "vy":2.132538819578137, "omega":0.0, "ax":-0.0011350782064731642, "ay":-0.0014962027299356897, "alpha":0.0, "fx":[-0.009931934306640189,-0.009931934306640184,-0.00993193430664019,-0.009931934306640189], "fy":[-0.013091773886937288,-0.013091773886937282,-0.013091773886937282,-0.013091773886937288]}, + {"t":5.08265874996044, "x":3.2007292361290034, "y":4.063355800295532, "heading":0.0, "vx":-2.8165689236265288, "vy":2.1324655272346127, "omega":0.0, "ax":-0.0011259596464752463, "ay":-0.0014839179167356064, "alpha":0.0, "fx":[-0.009852146906658404,-0.009852146906658414,-0.0098521469066584,-0.009852146906658404], "fy":[-0.012984281771436549,-0.012984281771436566,-0.012984281771436566,-0.012984281771436549]}, + {"t":5.131644319935604, "x":3.0627566511122835, "y":4.167814059205131, "heading":0.0, "vx":-2.8166240794015804, "vy":2.1323928366696654, "omega":0.0, "ax":-0.0011228502964270308, "ay":-0.001479551499799608, "alpha":0.0, "fx":[-0.00982494009373652,-0.009824940093736514,-0.009824940093736526,-0.00982494009373652], "fy":[-0.012946075623246575,-0.012946075623246563,-0.012946075623246563,-0.012946075623246575]}, + {"t":5.180629889910768, "x":2.9247813679890617, "y":4.272268762564771, "heading":0.0, "vx":-2.816679082863348, "vy":2.13232035999614, "omega":0.0, "ax":-0.0011263753740847853, "ay":-0.001483927869593419, "alpha":0.0, "fx":[-0.009855784523241873,-0.009855784523241876,-0.009855784523241873,-0.009855784523241873], "fy":[-0.012984368858942417,-0.012984368858942422,-0.012984368858942422,-0.012984368858942417]}, + {"t":5.229615459885932, "x":2.786803386260551, "y":4.376719910362509, "heading":0.0, "vx":-2.816734259003053, "vy":2.132247668943646, "omega":0.0, "ax":-0.0011373604901648033, "ay":-0.0014981357748893485, "alpha":0.0, "fx":[-0.00995190428894203,-0.009951904288942033,-0.009951904288942028,-0.00995190428894203], "fy":[-0.013108688030281796,-0.013108688030281803,-0.013108688030281803,-0.013108688030281796]}, + {"t":5.278601029861096, "x":2.6488226885175226, "y":4.481167480301065, "heading":0.0, "vx":-2.8167899732549313, "vy":2.1321742819088128, "omega":0.0, "ax":-0.0011568874397396031, "ay":-0.001523601970598285, "alpha":0.0, "fx":[-0.010122765097721528,-0.010122765097721522,-0.01012276509772153,-0.010122765097721528], "fy":[-0.013331517242734998,-0.013331517242734986,-0.013331517242734986,-0.013331517242734998]}, + {"t":5.32758659983626, "x":2.510839238151812, "y":4.585611424779725, "heading":0.0, "vx":-2.816846644045564, "vy":2.132099647397868, "omega":0.0, "ax":-0.0011863727119371325, "ay":-0.0015621948609168162, "alpha":0.0, "fx":[-0.010380761229449907,-0.010380761229449902,-0.010380761229449914,-0.010380761229449907], "fy":[-0.01366920503302215,-0.013669205033022136,-0.013669205033022136,-0.01366920503302215]}, + {"t":5.376572169811424, "x":2.372852976358897, "y":4.690051666940848, "heading":0.0, "vx":-2.816904759189061, "vy":2.1320231223921935, "omega":0.0, "ax":-0.0012276796630690443, "ay":-0.001616372594334555, "alpha":0.0, "fx":[-0.010742197051854136,-0.010742197051854144,-0.01074219705185413,-0.010742197051854138], "fy":[-0.014143260200427346,-0.014143260200427362,-0.014143260200427362,-0.014143260200427346]}, + {"t":5.425557739786588, "x":2.234863818202762, "y":4.794488095478881, "heading":0.0, "vx":-2.8169648977771033, "vy":2.1319439434593677, "omega":0.0, "ax":-0.0012832819155782131, "ay":-0.001689398807408379, "alpha":0.0, "fx":[-0.011228716761309364,-0.01122871676130936,-0.011228716761309366,-0.011228716761309364], "fy":[-0.01478223956482332,-0.01478223956482331,-0.01478223956482331,-0.01478223956482332]}, + {"t":5.474543309761752, "x":2.0968716474124176, "y":4.898920557775419, "heading":0.0, "vx":-2.817027760073177, "vy":2.1318611872958715, "omega":0.0, "ax":-0.0013565081472641685, "ay":-0.001785665863249571, "alpha":0.0, "fx":[-0.011869446288561476,-0.011869446288561476,-0.011869446288561476,-0.011869446288561476], "fy":[-0.015624576303433746,-0.015624576303433746,-0.015624576303433746,-0.015624576303433746]}, + {"t":5.523528879736916, "x":1.958876309420348, "y":5.003348850713576, "heading":0.0, "vx":-2.8170942093979465, "vy":2.131773715435775, "omega":0.0, "ax":-0.0014519642910901895, "ay":-0.001911252456460027, "alpha":0.0, "fx":[-0.012704687547039158,-0.012704687547039168,-0.01270468754703915,-0.012704687547039158], "fy":[-0.016723458994025227,-0.016723458994025248,-0.016723458994025248,-0.016723458994025227]}, + {"t":5.57251444971208, "x":1.820877601842614, "y":5.107772708114891, "heading":0.0, "vx":-2.817165334696329, "vy":2.131680091644829, "omega":0.0, "ax":-0.001578269590107037, "ay":-0.0020775676539525265, "alpha":0.0, "fx":[-0.013809858913436576,-0.013809858913436562,-0.013809858913436586,-0.013809858913436576], "fy":[-0.018178716972084626,-0.01817871697208459,-0.01817871697208459,-0.018178716972084626]}, + {"t":5.621500019687244, "x":1.6828752586113815, "y":5.212191779757625, "heading":0.0, "vx":-2.817242647131775, "vy":2.131578320809138, "omega":0.0, "ax":-0.0011123657874409516, "ay":-0.002985272937986941, "alpha":0.0, "fx":[-0.009733200640108325,-0.009733200640108328,-0.009733200640108325,-0.009733200640108325], "fy":[-0.02612113820738573,-0.026121138207385736,-0.026121138207385736,-0.02612113820738573]}, + {"t":5.670485589662408, "x":1.544869687174569, "y":5.316604777039492, "heading":0.0, "vx":-2.8172971370038935, "vy":2.1314320855127393, "omega":0.0, "ax":12.412725783079908, "ay":-9.395402825481163, "alpha":0.0, "fx":[108.6113506019492,108.6113506019492,108.6113506019492,108.6113506019492], "fy":[-82.20977472296016,-82.20977472296018,-82.20977472296018,-82.20977472296016]}, + {"t":5.719471159637572, "x":1.4217554830428207, "y":5.409741653760425, "heading":0.0, "vx":-2.2092526895743063, "vy":1.671192922960275, "omega":0.0, "ax":15.015524078909255, "ay":-11.35914425879452, "alpha":0.0, "fx":[131.38583569045596,131.38583569045596,131.385835690456,131.38583569045596], "fy":[-99.39251226445204,-99.39251226445208,-99.39251226445204,-99.39251226445204]}, + {"t":5.768456729612736, "x":1.331549502000014, "y":5.477977369488735, "heading":0.0, "vx":-1.4737086840931315, "vy":1.1147587670131098, "omega":0.0, "ax":15.038332192108658, "ay":-11.375771917263773, "alpha":0.0, "fx":[131.58540668095074,131.58540668095074,131.58540668095074,131.58540668095074], "fy":[-99.53800427605802,-99.538004276058,-99.538004276058,-99.53800427605802]}, + {"t":5.8174422995879, "x":1.277401928322828, "y":5.518935891185538, "heading":0.0, "vx":-0.7370474101868266, "vy":0.557510095738476, "omega":0.0, "ax":15.046214845729253, "ay":-11.38110868202888, "alpha":0.0, "fx":[131.65437990013092,131.65437990013092,131.65437990013095,131.65437990013092], "fy":[-99.58470096775272,-99.58470096775268,-99.5847009677527,-99.58470096775272]}, + {"t":5.866427869563064, "x":1.2593495845794678, "y":5.532590866088867, "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/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f6343b9..32bd922 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,7 +23,6 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -33,20 +32,16 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveIO; import frc.robot.subsystems.drive.DriveIOCTRE; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterIO; -import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOSim; import frc.robot.subsystems.vision.VisionSource; +import frc.robot.util.AutoChooser; import java.io.File; import java.util.Set; -import java.util.function.Supplier; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; @@ -62,7 +57,6 @@ public class Robot extends LoggedRobot { private final CommandXboxController driver = new CommandXboxController(0); private final Drive drive; - private final Shooter shooter; private final VisionSource limelight_back; private final VisionSource limelight_front; @@ -71,8 +65,7 @@ public class Robot extends LoggedRobot { .withDriveRequestType( DriveRequestType.Velocity); // Use closed-loop control for drive motors - private final LoggedDashboardChooser> autoChooser = - new LoggedDashboardChooser<>("Autos"); + private final AutoChooser autoChooser; private final AutoFactory autoFactory; public Robot() { @@ -120,8 +113,6 @@ public Robot() { limelight_back = new VisionSource(new VisionIOLimelight("limelight-back")); limelight_front = new VisionSource(new VisionIOLimelight("limelight-front")); - shooter = - new Shooter(new ShooterIO() {}); // no op since we dont have a shooter on the real robot break; case SIM: @@ -140,7 +131,6 @@ public Robot() { limelight_back = new VisionSource(new VisionIOSim("limelight-back")); limelight_front = new VisionSource(new VisionIOSim("limelight-front")); - shooter = new Shooter(new ShooterIOSim()); break; // in replay @@ -155,7 +145,6 @@ public Robot() { drive = new Drive(new DriveIO() {}); limelight_back = new VisionSource(new VisionIO() {}); limelight_front = new VisionSource(new VisionIO() {}); - shooter = new Shooter(new ShooterIO() {}); break; } @@ -206,7 +195,6 @@ public Robot() { drive .moveToPosition(new Pose2d(2, 5.6, new Rotation2d())) .until(drive::isTeleopAtSetpoint)); - driver.b().whileTrue(shooter.maintain(500)); // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. @@ -223,11 +211,13 @@ public Robot() { () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, new AutoFactory.AutoBindings()); + autoChooser = new AutoChooser(autoFactory, "AutoChooser"); + autoChooser.addOption( "Simple Path", - () -> { - final AutoLoop routine = autoFactory.newLoop("simple"); - final AutoTrajectory path = autoFactory.trajectory("SimplePath", routine); + (factory) -> { + final AutoLoop routine = factory.newLoop("simple"); + final AutoTrajectory path = factory.trajectory("SimplePath", routine); routine .enabled() @@ -244,9 +234,9 @@ public Robot() { autoChooser.addOption( "Bad Idea TM", - () -> { - final AutoLoop routine = autoFactory.newLoop("jadesadumbass"); - final AutoTrajectory path = autoFactory.trajectory("VeryBadIdeaTm", routine); + (factory) -> { + final AutoLoop routine = factory.newLoop("jadesadumbass"); + final AutoTrajectory path = factory.trajectory("VeryBadIdeaTm", routine); routine .enabled() @@ -263,9 +253,9 @@ public Robot() { autoChooser.addOption( "Move To Side", - () -> { - final AutoLoop routine = autoFactory.newLoop("side"); - final AutoTrajectory path = autoFactory.trajectory("MoveToSide", routine); + (factory) -> { + final AutoLoop routine = factory.newLoop("side"); + final AutoTrajectory path = factory.trajectory("MoveToSide", routine); routine .enabled() @@ -282,9 +272,9 @@ public Robot() { autoChooser.addOption( "Test Path", - () -> { - final AutoLoop routine = autoFactory.newLoop("test"); - final AutoTrajectory path = autoFactory.trajectory("TestPath", routine); + (factory) -> { + final AutoLoop routine = factory.newLoop("test"); + final AutoTrajectory path = factory.trajectory("TestPath", routine); routine .enabled() @@ -299,19 +289,20 @@ public Robot() { return routine.cmd(); }); - autoChooser.addDefaultOption( + autoChooser.addOption( "Set Pose to Vision Pose", - () -> { + (factory) -> { return Commands.runOnce( () -> drive.resetPose(limelight_back.getPose().toPose2d()), drive); }); - autonomous().whileTrue(Commands.defer(() -> autoChooser.get().get().asProxy(), Set.of())); + autonomous().whileTrue(Commands.defer(() -> autoChooser.getSelected().asProxy(), Set.of())); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + autoChooser.periodic(); if (limelight_back.inField()) { drive.addVisionMeasurement( diff --git a/src/main/java/frc/robot/generated/BuildConstants.java b/src/main/java/frc/robot/generated/BuildConstants.java index 8e1ef8c..2df5382 100644 --- a/src/main/java/frc/robot/generated/BuildConstants.java +++ b/src/main/java/frc/robot/generated/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025-Reefscape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 11; - public static final String GIT_SHA = "438d14fa33e145a166cad585d0deaf523ef636ab"; - public static final String GIT_DATE = "2024-12-04 13:29:11 AWST"; + public static final int GIT_REVISION = 12; + public static final String GIT_SHA = "c02cec69070359a2db261b4202656e68ae713eab"; + public static final String GIT_DATE = "2024-12-04 14:14:33 AWST"; public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2024-12-04 14:12:23 AWST"; - public static final long BUILD_UNIX_TIME = 1733292743200L; + public static final String BUILD_DATE = "2024-12-04 16:19:17 AWST"; + public static final long BUILD_UNIX_TIME = 1733300357546L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index b7d080e..148f46c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -30,21 +30,22 @@ public VisionIOLimelight(String camera) { @Override public void updateInputs(VisionIOInputs inputs) { + inputs.camera = camera; + inputs.cameraOffset = cameraOffset; + inputs.timestamp = Util.now().in(Milliseconds); + inputs.validIds = validIds; + var pose = botpose.get(); if (pose != null && pose.length != 0) { var rotation = new Rotation3d( degreesToRadians(pose[3]), degreesToRadians(pose[4]), degreesToRadians(pose[5])); inputs.estimatedPose = new Pose3d(pose[0], pose[1], pose[2], rotation); - inputs.timestamp = Util.now().in(Milliseconds); inputs.latency = pose[6]; inputs.tagCount = pose[7]; inputs.inField = Util.inField(inputs.estimatedPose); - inputs.camera = camera; - inputs.validIds = validIds; inputs.averageTagDistance = pose[9]; inputs.averageTagArea = pose[10]; - inputs.cameraOffset = cameraOffset; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSource.java b/src/main/java/frc/robot/subsystems/vision/VisionSource.java index a6668b9..b094565 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSource.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSource.java @@ -57,7 +57,7 @@ public Matrix getStdDevs() { if (inputs.tagCount <= 0) { return VecBuilder.fill(9999999, 9999999, 9999999); } - return VecBuilder.fill(0.7, 0.7, 9999999); + return VecBuilder.fill(0.15, 0.15, 9999999); } public void setCameraOffset(Transform3d translation) { diff --git a/src/main/java/frc/robot/util/AutoChooser.java b/src/main/java/frc/robot/util/AutoChooser.java new file mode 100644 index 0000000..83e114c --- /dev/null +++ b/src/main/java/frc/robot/util/AutoChooser.java @@ -0,0 +1,155 @@ +package frc.robot.util; + +import choreo.auto.AutoFactory; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringArrayEntry; +import edu.wpi.first.networktables.StringEntry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.IterativeRobotBase; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Function; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; +import org.littletonrobotics.junction.networktables.LoggedDashboardInput; + +/** + * An auto chooser that allows for the selection of auto routines at runtime. + * + *

This chooser takes a lazy loading approach to auto routines, only generating the auto routine + * when it is selected. This approach has the benefit of not loading all autos on startup, but also + * not loading the auto during auto start causing a delay. + * + *

Once the {@link AutoChooser} is made you can add auto routines to it using the {@link + * #addAutoRoutine(String, AutoRoutineGenerator)} method. Unlike {@code SendableChooser} this + * chooser has to be updated every cycle by calling the {@link #update()} method in your {@link + * IterativeRobotBase#robotPeriodic()}. + * + *

You can retrieve the auto routine {@link Command} that is currently selected by calling the + * {@link #getSelectedAutoRoutine()} method. + */ +public class AutoChooser implements LoggedDashboardInput { + /** A function that generates an auto routine {@link Command} from an {@link AutoFactory}. */ + public static interface AutoRoutineGenerator extends Function { + /** A generator that returns a command that does nothing */ + static final AutoRoutineGenerator NONE = factory -> Commands.none().withName("Do Nothing Auto"); + } + + private static final String NONE_NAME = "Nothing"; + private String selectedValue; + + private final HashMap autoRoutines = + new HashMap<>(Map.of(NONE_NAME, AutoRoutineGenerator.NONE)); + + private final StringEntry selected, active; + private final StringArrayEntry options; + + private final AutoFactory factory; + private final String path; + + private final LoggableInputs inputs = + new LoggableInputs() { + public void toLog(LogTable table) { + table.put(path, selectedValue); + } + + public void fromLog(LogTable table) { + selectedValue = table.get(path, selectedValue); + } + }; + + private String lastAutoRoutineName = NONE_NAME; + private Command lastAutoRoutine = AutoRoutineGenerator.NONE.apply(null); + + /** + * Create a new auto chooser. + * + * @param factory The auto factory to use for auto routine generation. + * @param tableName The name of the network table to use for the chooser, passing in an empty + * string will put this chooser at the root of the network tables. + */ + public AutoChooser(AutoFactory factory, String tableName) { + this.factory = factory; + + path = NetworkTable.normalizeKey(tableName, true) + "/AutoChooser"; + NetworkTable table = NetworkTableInstance.getDefault().getTable(path); + + selected = table.getStringTopic("selected").getEntry(NONE_NAME); + table.getStringTopic(".type").publish().set("String Chooser"); + table.getStringTopic("default").publish().set(NONE_NAME); + active = table.getStringTopic("active").getEntry(NONE_NAME); + options = + table.getStringArrayTopic("options").getEntry(autoRoutines.keySet().toArray(new String[0])); + } + + /** + * Update the auto chooser. + * + *

This method should be called every cycle in the {@link IterativeRobotBase#robotPeriodic()}. + * It will check if the selected auto routine has changed and update the active auto routine. + */ + public void update() { + if (DriverStation.isDisabled() || IterativeRobotBase.isSimulation()) { + String selectStr = selected.get(); + if (selectStr.equals(lastAutoRoutineName)) return; + if (!autoRoutines.containsKey(selectStr)) { + selected.set(NONE_NAME); + selectStr = NONE_NAME; + DriverStation.reportError("Selected an auto that isn't an option", false); + } + lastAutoRoutineName = selectStr; + lastAutoRoutine = autoRoutines.get(lastAutoRoutineName).apply(this.factory); + active.set(lastAutoRoutineName); + } + } + + /** + * Add an auto routine to the chooser. + * + *

An auto routine is a function that takes an AutoFactory and returns a Command. These + * functions can be static, a lambda or belong to a local variable. + * + *

A good paradigm is making an `AutoRoutines` class that has a reference to all your + * subsystems and has helper methods for auto commands inside it. Then you crate methods inside + * that class that take an `AutoFactory` and return a `Command`. + * + * @param name The name of the auto routine. + * @param generator The function that generates the auto routine. + */ + public void addOption(String name, AutoRoutineGenerator generator) { + autoRoutines.put(name, generator); + options.set(autoRoutines.keySet().toArray(new String[0])); + } + + @Override + public void periodic() { + update(); + if (!Logger.hasReplaySource()) { + selectedValue = selected.get(); + } + Logger.processInputs(prefix + path, inputs); + } + + /** + * Choose an auto routine by name. + * + * @param choice The name of the auto routine to choose. + */ + public void choose(String choice) { + selected.set(choice); + update(); + } + + /** + * Get the currently selected auto routine. + * + * @return The currently selected auto routine. + */ + public Command getSelected() { + return lastAutoRoutine; + } +}