diff --git a/src/main/deploy/choreo/D to Station.traj b/src/main/deploy/choreo/D to Station.traj new file mode 100644 index 00000000..4ae3e1c4 --- /dev/null +++ b/src/main/deploy/choreo/D to Station.traj @@ -0,0 +1,60 @@ +{ + "name":"D to Station", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.7218778133392334, "y":2.0161004066467285, "heading":1.0379526372170715, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.607, "y":0.754, "heading":0.8110335871211297, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.7218778133392334 m", "val":3.7218778133392334}, "y":{"exp":"2.0161004066467285 m", "val":2.0161004066467285}, "heading":{"exp":"1.0379526372170715 rad", "val":1.0379526372170715}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.607 m", "val":1.607}, "y":{"exp":"0.754 m", "val":0.754}, "heading":{"exp":"0.8110335871211297 rad", "val":0.8110335871211297}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.04669], + "samples":[ + {"t":0.0, "x":3.72188, "y":2.0161, "heading":1.03795, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.74042, "ay":-4.61926, "alpha":-0.83805, "fx":[-130.74832,-134.7716,-132.59811,-128.53101], "fy":[-80.19805,-73.24538,-77.12532,-83.72059]}, + {"t":0.04187, "x":3.71509, "y":2.01205, "heading":1.03795, "vx":-0.32407, "vy":-0.1934, "omega":-0.03509, "ax":-7.73972, "ay":-4.61885, "alpha":-0.83703, "fx":[-130.73724,-134.75564,-132.58554,-128.52347], "fy":[-80.18849,-73.24535,-77.12035,-83.70704]}, + {"t":0.08374, "x":3.69474, "y":1.99991, "heading":1.03648, "vx":-0.64812, "vy":-0.38678, "omega":-0.07013, "ax":-7.73889, "ay":-4.61835, "alpha":-0.83582, "fx":[-130.71909,-134.73542,-132.57481,-128.516], "fy":[-80.18493,-73.24728,-77.10692,-83.68837]}, + {"t":0.1256, "x":3.66082, "y":1.97967, "heading":1.03355, "vx":-0.97213, "vy":-0.58014, "omega":-0.10513, "ax":-7.73787, "ay":-4.61775, "alpha":-0.83435, "fx":[-130.69317,-134.70986,-132.56502,-128.50809], "fy":[-80.18669,-73.25113,-77.08474,-83.66369]}, + {"t":0.16747, "x":3.61334, "y":1.95133, "heading":1.02915, "vx":-1.2961, "vy":-0.77347, "omega":-0.14006, "ax":-7.7366, "ay":-4.61699, "alpha":-0.83254, "fx":[-130.65841,-134.67737,-132.55489,-128.49897], "fy":[-80.19271,-73.2569,-77.05338,-83.63166]}, + {"t":0.20934, "x":3.5523, "y":1.9149, "heading":1.02328, "vx":-1.62001, "vy":-0.96678, "omega":-0.17491, "ax":-7.73497, "ay":-4.61601, "alpha":-0.83024, "fx":[-130.61315,-134.63545,-132.54235,-128.4874], "fy":[-80.20138,-73.26454,-77.01216,-83.59023]}, + {"t":0.25121, "x":3.47769, "y":1.87038, "heading":1.01596, "vx":-1.94386, "vy":-1.16004, "omega":-0.20967, "ax":-7.73279, "ay":-4.61471, "alpha":-0.82722, "fx":[-130.55458,-134.57999,-132.52402,-128.47131], "fy":[-80.21003,-73.27386,-76.95989,-83.53599]}, + {"t":0.29307, "x":3.38953, "y":1.81776, "heading":1.00718, "vx":-2.26761, "vy":-1.35325, "omega":-0.24431, "ax":-7.72973, "ay":-4.61289, "alpha":-0.82308, "fx":[-130.47759,-134.50366,-132.49383,-128.44681], "fy":[-80.21395,-73.28434,-76.8944,-83.46304]}, + {"t":0.33494, "x":3.28781, "y":1.75706, "heading":0.99695, "vx":-2.59124, "vy":-1.54638, "omega":-0.27877, "ax":-7.72514, "ay":-4.61015, "alpha":-0.81709, "fx":[-130.37189,-134.39211,-132.43973,-128.40587], "fy":[-80.20391,-73.29451,-76.81113,-83.35994]}, + {"t":0.37681, "x":3.17255, "y":1.68828, "heading":0.98528, "vx":-2.91467, "vy":-1.73939, "omega":-0.31298, "ax":-7.71748, "ay":-4.60558, "alpha":-0.80765, "fx":[-130.2131,-134.21249,-132.33384,-128.32904], "fy":[-80.15896,-73.29963,-76.69903,-83.20109]}, + {"t":0.41868, "x":3.04376, "y":1.61142, "heading":0.97218, "vx":-3.23779, "vy":-1.93222, "omega":-0.34679, "ax":-7.70214, "ay":-4.59644, "alpha":-0.79073, "fx":[-129.92696,-133.86909,-132.09357,-128.15498], "fy":[-80.01843,-73.28048,-76.52306,-82.91421]}, + {"t":0.46055, "x":2.90145, "y":1.52649, "heading":0.95766, "vx":-3.56026, "vy":-2.12466, "omega":-0.3799, "ax":-7.65605, "ay":-4.56895, "alpha":-0.75217, "fx":[-129.13339,-132.90659,-131.31468,-127.55424], "fy":[-79.49719,-73.10141,-76.09329,-82.17421]}, + {"t":0.50241, "x":2.74568, "y":1.43353, "heading":0.94175, "vx":-3.8808, "vy":-2.31595, "omega":-0.41139, "ax":0.00003, "ay":0.00002, "alpha":-0.00497, "fx":[0.01907,-0.00249,-0.01817,0.00338], "fy":[0.0032,0.01888,-0.00267,-0.01836]}, + {"t":0.54428, "x":2.5832, "y":1.33657, "heading":0.92453, "vx":-3.8808, "vy":-2.31595, "omega":-0.4116, "ax":7.65605, "ay":4.56895, "alpha":0.75228, "fx":[129.04047,132.87373,131.39852,127.59615], "fy":[79.6497,73.15664,75.94692,82.11283]}, + {"t":0.58615, "x":2.42743, "y":1.24361, "heading":0.90729, "vx":-3.56026, "vy":-2.12466, "omega":-0.3801, "ax":7.70214, "ay":4.59643, "alpha":0.79088, "fx":[129.73344,133.80283,132.26713,128.24114], "fy":[80.33364,73.39674,76.22112,82.78467]}, + {"t":0.62802, "x":2.28512, "y":1.15868, "heading":0.89138, "vx":-3.23778, "vy":-1.93222, "omega":-0.34699, "ax":7.71748, "ay":4.60558, "alpha":0.80787, "fx":[129.92594,134.11522,132.59072,128.45652], "fy":[80.6254,73.47285,76.25255,83.00787]}, + {"t":0.66988, "x":2.15632, "y":1.08182, "heading":0.87685, "vx":-2.91467, "vy":-1.73939, "omega":-0.31317, "ax":7.72514, "ay":4.61015, "alpha":0.81738, "fx":[129.99971,134.2665,132.77219,128.57109], "fy":[80.80755,73.51989,76.2335,83.1085]}, + {"t":0.71175, "x":2.04106, "y":1.01304, "heading":0.86374, "vx":-2.59124, "vy":-1.54638, "omega":-0.27894, "ax":7.72973, "ay":4.61289, "alpha":0.82345, "fx":[130.02953,134.35251,132.89373,128.64599], "fy":[80.93997,73.55679,76.19972,83.15918]}, + {"t":0.75362, "x":1.93935, "y":0.95234, "heading":0.85206, "vx":-2.26761, "vy":-1.35325, "omega":-0.24447, "ax":7.73278, "ay":4.61471, "alpha":0.82765, "fx":[130.03996,134.40615,132.98306,128.70056], "fy":[81.04332,73.58812,76.16256,83.18567]}, + {"t":0.79549, "x":1.85119, "y":0.89972, "heading":0.84183, "vx":-1.94386, "vy":-1.16004, "omega":-0.20982, "ax":7.73497, "ay":4.61601, "alpha":0.83073, "fx":[130.0414,134.44188,133.05214,128.74276], "fy":[81.12672,73.61524,76.12669,83.19955]}, + {"t":0.83736, "x":1.77658, "y":0.8552, "heading":0.83304, "vx":-1.62001, "vy":-0.96678, "omega":-0.17504, "ax":7.7366, "ay":4.61699, "alpha":0.83307, "fx":[130.03899,134.46704,133.10702,128.77641], "fy":[81.1948,73.63857,76.09435,83.20681]}, + {"t":0.87922, "x":1.71554, "y":0.81877, "heading":0.82571, "vx":-1.2961, "vy":-0.77347, "omega":-0.14016, "ax":7.73787, "ay":4.61775, "alpha":0.83492, "fx":[130.03552,134.48579,133.15107,128.80355], "fy":[81.25024,73.65821,76.06675,83.21092]}, + {"t":0.92109, "x":1.66805, "y":0.79044, "heading":0.81985, "vx":-0.97213, "vy":-0.58014, "omega":-0.1052, "ax":7.73889, "ay":4.61835, "alpha":0.8364, "fx":[130.03269,134.50069,133.18636,128.82537], "fy":[81.29465,73.67413,76.04458,83.21401]}, + {"t":0.96296, "x":1.63414, "y":0.77019, "heading":0.81544, "vx":-0.64812, "vy":-0.38678, "omega":-0.07018, "ax":7.73972, "ay":4.61885, "alpha":0.83763, "fx":[130.03154,134.51338,133.21418,128.84259], "fy":[81.3291,73.68626,76.02826,83.21749]}, + {"t":1.00483, "x":1.61378, "y":0.75805, "heading":0.8125, "vx":-0.32407, "vy":-0.1934, "omega":-0.03511, "ax":7.74041, "ay":4.61926, "alpha":0.83865, "fx":[130.03276,134.52496,133.23541,128.85569], "fy":[81.3543,73.69454,76.01806,83.2223]}, + {"t":1.04669, "x":1.607, "y":0.754, "heading":0.81103, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/E to Station.traj b/src/main/deploy/choreo/E to Station.traj new file mode 100644 index 00000000..b1578648 --- /dev/null +++ b/src/main/deploy/choreo/E to Station.traj @@ -0,0 +1 @@ +{"name":"E to Station","version":1,"snapshot":{"waypoints":[{"x":5.455968856811523,"y":2.1711997985839844,"heading":2.2318394733980336,"intervals":38,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":1.607,"y":0.754,"heading":0.8110335871211297,"intervals":38,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false}],"constraints":[{"from":"first","to":null,"data":{"type":"StopPoint","props":{}},"enabled":true},{"from":"last","to":null,"data":{"type":"StopPoint","props":{}},"enabled":true},{"from":"first","to":"last","data":{"type":"KeepInRectangle","props":{"x":0.0,"y":0.0,"w":17.548,"h":8.052}},"enabled":false}],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"5.455968856811523 m","val":5.455968856811523},"y":{"exp":"2.1711997985839844 m","val":2.1711997985839844},"heading":{"exp":"2.2318394733980336 rad","val":2.2318394733980336},"intervals":38,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"1.607 m","val":1.607},"y":{"exp":"0.754 m","val":0.754},"heading":{"exp":"0.8110335871211297 rad","val":0.8110335871211297},"intervals":38,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false}],"constraints":[{"from":"first","to":null,"data":{"type":"StopPoint","props":{}},"enabled":true},{"from":"last","to":null,"data":{"type":"StopPoint","props":{}},"enabled":true},{"from":"first","to":"last","data":{"type":"KeepInRectangle","props":{"x":{"exp":"0 m","val":0.0},"y":{"exp":"0 m","val":0.0},"w":{"exp":"17.548 m","val":17.548},"h":{"exp":"8.052 m","val":8.052}}},"enabled":false}],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,1.39413],"samples":[{"t":0.0,"x":5.45597,"y":2.1712,"heading":2.23184,"vx":0.0,"vy":0.0,"omega":0.0,"ax":-8.24342,"ay":-3.06767,"alpha":-6.37409,"fx":[-153.17224,-148.87807,-127.68424,-131.13823],"fy":[-7.31843,-36.99196,-85.00563,-79.40498]},{"t":0.03669,"x":5.45042,"y":2.16914,"heading":2.23184,"vx":-0.30243,"vy":-0.11255,"omega":-0.23385,"ax":-8.25319,"ay":-3.0685,"alpha":-6.22206,"fx":[-153.09876,-148.79705,-128.058,-131.58399],"fy":[-8.45632,-37.26955,-84.41926,-78.6319]},{"t":0.07338,"x":5.43377,"y":2.16294,"heading":2.22326,"vx":-0.60522,"vy":-0.22512,"omega":-0.46212,"ax":-8.26372,"ay":-3.06907,"alpha":-6.05725,"fx":[-152.99802,-148.77065,-128.60487,-131.8806],"fy":[-9.8419,-37.31904,-83.55786,-78.09721]},{"t":0.11006,"x":5.40601,"y":2.15262,"heading":2.20631,"vx":-0.9084,"vy":-0.33772,"omega":-0.68435,"ax":-8.27542,"ay":-3.06954,"alpha":-5.87009,"fx":[-152.85789,-148.78751,-129.32551,-132.07916],"fy":[-11.53641,-37.18435,-82.40729,-77.71972]},{"t":0.14675,"x":5.36711,"y":2.13816,"heading":2.1812,"vx":-1.212,"vy":-0.45033,"omega":-0.89971,"ax":-8.28879,"ay":-3.07006,"alpha":-5.64774,"fx":[-152.65933,-148.83124,-130.22348,-132.24555],"fy":[-13.62619,-36.92572,-80.94328,-77.38851]},{"t":0.18344,"x":5.31707,"y":2.11957,"heading":2.14819,"vx":-1.5161,"vy":-0.56296,"omega":-1.10691,"ax":-8.30439,"ay":-3.07089,"alpha":-5.37285,"fx":[-152.37243,-148.87867,-131.30567,-132.46418],"fy":[-16.22522,-36.62901,-79.12854,-76.95716]},{"t":0.22013,"x":5.25586,"y":2.09685,"heading":2.10758,"vx":-1.82076,"vy":-0.67563,"omega":-1.30402,"ax":-8.32279,"ay":-3.07235,"alpha":-5.02164,"fx":[-151.95028,-148.89622,-132.58357,-132.84331],"fy":[-19.47804,-36.42206,-76.90675,-76.23201]},{"t":0.25681,"x":5.18346,"y":2.07,"heading":2.05974,"vx":-2.12611,"vy":-0.78834,"omega":-1.48826,"ax":-8.3445,"ay":-3.07481,"alpha":-4.56068,"fx":[-151.31938,-148.83178,-134.07654,-133.52263],"fy":[-23.56288,-36.50513,-74.18967,-74.94878]},{"t":0.2935,"x":5.09984,"y":2.03901,"heading":2.00514,"vx":-2.43224,"vy":-0.90115,"omega":-1.65558,"ax":-8.36969,"ay":-3.07852,"alpha":-3.94042,"fx":[-150.36459,-148.59529,-135.81956,-134.68498],"fy":[-28.69505,-37.21287,-70.82749,-72.72371]},{"t":0.33019,"x":5.00497,"y":2.00387,"heading":1.9444,"vx":-2.73931,"vy":-1.0141,"omega":-1.80014,"ax":-8.39766,"ay":-3.08304,"alpha":-3.07961,"fx":[-148.9051,-148.00556,-137.88227,-136.57454],"fy":[-35.13252,-39.15485,-66.53358,-68.94535]},{"t":0.36688,"x":4.89882,"y":1.96459,"heading":1.87836,"vx":-3.0474,"vy":-1.1272,"omega":-1.91312,"ax":-8.42471,"ay":-3.08578,"alpha":-1.82153,"fx":[-146.65219,-146.61849,-140.41823,-139.51844],"fy":[-43.19113,-43.58302,-60.66338,-62.51563]},{"t":0.40356,"x":4.78135,"y":1.92116,"heading":1.80817,"vx":-3.35648,"vy":-1.24041,"omega":-1.97995,"ax":-8.43394,"ay":-3.07751,"alpha":0.21456,"fx":[-143.123,-143.01567,-143.80232,-143.89468],"fy":[-53.29111,-53.54348,-51.38915,-51.1668]},{"t":0.44025,"x":4.65253,"y":1.87358,"heading":1.73553,"vx":-3.6659,"vy":-1.35332,"omega":-1.97208,"ax":-8.32379,"ay":-3.01094,"alpha":4.33365,"fx":[-137.42658,-130.47643,-148.87016,-149.56798],"fy":[-66.08573,-78.29263,-30.9929,-29.48996]},{"t":0.47694,"x":4.51244,"y":1.82191,"heading":1.66318,"vx":-3.97128,"vy":-1.46378,"omega":-1.81309,"ax":-6.98227,"ay":-2.02939,"alpha":18.20757,"fx":[-128.09831,-62.68689,-132.95874,-151.32187],"fy":[-82.03177,-137.21217,67.24163,13.925]},{"t":0.51363,"x":4.36204,"y":1.76684,"heading":1.59666,"vx":-4.22744,"vy":-1.53824,"omega":-1.1451,"ax":-6.649,"ay":-1.83644,"alpha":19.36984,"fx":[-127.61794,-53.54698,-122.15298,-149.07247],"fy":[-80.30773,-138.6061,74.19325,19.77121]},{"t":0.55031,"x":4.20248,"y":1.70917,"heading":1.55465,"vx":-4.47138,"vy":-1.60561,"omega":-0.43447,"ax":-3.64521,"ay":-2.21615,"alpha":11.64066,"fx":[-84.22305,-29.40344,-38.68828,-95.70103],"fy":[-62.81778,-81.65746,-4.50349,-1.80564]},{"t":0.587,"x":4.03598,"y":1.64877,"heading":1.53871,"vx":-4.60511,"vy":-1.68692,"omega":-0.0074,"ax":0.11727,"ay":-0.33783,"alpha":0.01879,"fx":[1.94255,2.04345,2.04679,1.94583],"fy":[-5.7952,-5.79834,-5.69759,-5.69446]},{"t":0.62369,"x":3.86711,"y":1.58666,"heading":1.53844,"vx":-4.60081,"vy":-1.69931,"omega":-0.00671,"ax":0.03168,"ay":-0.0857,"alpha":0.00005,"fx":[0.53864,0.53891,0.53892,0.53865],"fy":[-1.45778,-1.45779,-1.45752,-1.45751]},{"t":0.66038,"x":3.69834,"y":1.52425,"heading":1.53819,"vx":-4.59965,"vy":-1.70246,"omega":-0.00671,"ax":0.00563,"ay":-0.01522,"alpha":0.00001,"fx":[0.09577,0.09584,0.09584,0.09578],"fy":[-0.25887,-0.25887,-0.25881,-0.25881]},{"t":0.69706,"x":3.52959,"y":1.46179,"heading":1.53795,"vx":-4.59944,"vy":-1.70301,"omega":-0.00671,"ax":-0.00864,"ay":0.02335,"alpha":0.0,"fx":[-0.14698,-0.14698,-0.14698,-0.14698],"fy":[0.3971,0.3971,0.39709,0.39709]},{"t":0.73375,"x":3.36084,"y":1.39932,"heading":1.5377,"vx":-4.59976,"vy":-1.70216,"omega":-0.00671,"ax":-0.04096,"ay":0.11087,"alpha":-0.00005,"fx":[-0.69658,-0.69684,-0.69685,-0.69659],"fy":[1.88595,1.88596,1.8857,1.88569]},{"t":0.77044,"x":3.19206,"y":1.33695,"heading":1.53746,"vx":-4.60126,"vy":-1.69809,"omega":-0.00671,"ax":-0.15049,"ay":0.43286,"alpha":-0.02269,"fx":[-2.49669,-2.61862,-2.62289,-2.5009],"fy":[7.42165,7.42551,7.30396,7.30011]},{"t":0.80713,"x":3.02315,"y":1.27494,"heading":1.53721,"vx":-4.60678,"vy":-1.68221,"omega":-0.00754,"ax":3.89806,"ay":2.43822,"alpha":-12.41716,"fx":[89.33789,31.07841,42.32187,102.48134],"fy":[66.94025,90.12795,7.12723,1.69813]},{"t":0.84381,"x":2.85676,"y":1.21487,"heading":1.53693,"vx":-4.46377,"vy":-1.59276,"omega":-0.4631,"ax":6.73828,"ay":1.81401,"alpha":-18.70951,"fx":[129.58341,56.21343,124.10285,148.56539],"fy":[77.17913,137.83577,-68.54726,-23.04427]},{"t":0.8805,"x":2.69753,"y":1.15765,"heading":1.51994,"vx":-4.21656,"vy":-1.52621,"omega":-1.1495,"ax":7.00885,"ay":1.79491,"alpha":-18.01699,"fx":[132.30368,62.67512,131.96439,149.93105],"fy":[75.12918,137.52667,-66.16117,-24.37095]},{"t":0.91719,"x":2.54755,"y":1.10287,"heading":1.47777,"vx":-3.95942,"vy":-1.46035,"omega":-1.8105,"ax":8.22544,"ay":2.95508,"alpha":-6.16119,"fx":[138.81445,120.55205,148.56226,151.72057],"fy":[63.19501,92.82068,30.80161,14.24263]},{"t":0.95388,"x":2.40783,"y":1.05128,"heading":1.41135,"vx":-3.65765,"vy":-1.35194,"omega":-2.03654,"ax":8.42751,"ay":3.07576,"alpha":-0.94337,"fx":[142.81486,140.89239,144.07406,145.61671],"fy":[54.1013,58.80453,50.41927,45.94631]},{"t":0.99056,"x":2.27931,"y":1.00375,"heading":1.33663,"vx":-3.34847,"vy":-1.2391,"omega":-2.07115,"ax":8.42715,"ay":3.0874,"alpha":1.60884,"fx":[144.40267,147.19306,142.82628,138.95144],"fy":[50.10895,41.44097,54.72076,63.79278]},{"t":1.02725,"x":2.16213,"y":0.96037,"heading":1.26065,"vx":-3.0393,"vy":-1.12583,"omega":-2.01213,"ax":8.39591,"ay":3.08225,"alpha":3.09478,"fx":[144.71691,149.92174,142.82976,133.77948],"fy":[49.48331,30.7853,55.13632,74.30751]},{"t":1.06394,"x":2.05628,"y":0.92114,"heading":1.18683,"vx":-2.73127,"vy":-1.01275,"omega":-1.89859,"ax":8.36351,"ay":3.07476,"alpha":4.0453,"fx":[144.26191,151.33712,143.33422,130.1104],"fy":[51.00679,23.38908,54.0678,80.73926]},{"t":1.10063,"x":1.96171,"y":0.88605,"heading":1.11717,"vx":-2.42443,"vy":-0.89994,"omega":-1.75017,"ax":8.33465,"ay":3.06919,"alpha":4.70456,"fx":[143.29306,152.14418,144.02908,127.6134],"fy":[53.82804,17.87854,52.36314,84.75454]},{"t":1.13731,"x":1.87837,"y":0.8551,"heading":1.05296,"vx":-2.11866,"vy":-0.78734,"omega":-1.57758,"ax":8.30906,"ay":3.06625,"alpha":5.19969,"fx":[141.97861,152.62717,144.76756,125.96555],"fy":[57.32872,13.60721,50.41361,87.27446]},{"t":1.174,"x":1.80623,"y":0.82828,"heading":0.99509,"vx":-1.81382,"vy":-0.67485,"omega":-1.38681,"ax":8.28595,"ay":3.06528,"alpha":5.59839,"fx":[140.45866,152.92274,145.4754,124.90973],"fy":[61.05585,10.22386,48.43226,88.8464]},{"t":1.21069,"x":1.74526,"y":0.80558,"heading":0.94421,"vx":-1.50983,"vy":-0.56239,"omega":-1.18142,"ax":8.26486,"ay":3.06537,"alpha":5.93526,"fx":[138.86118,153.105,146.11424,124.25134],"fy":[64.6859,7.51201,46.54903,89.817]},{"t":1.24738,"x":1.69543,"y":0.78701,"heading":0.90086,"vx":-1.20661,"vy":-0.44993,"omega":-0.96367,"ax":8.2457,"ay":3.06575,"alpha":6.22638,"fx":[137.30129,153.21728,146.66478,123.84484],"fy":[67.99924,5.32293,44.85002,90.41798]},{"t":1.28406,"x":1.65672,"y":0.77257,"heading":0.86551,"vx":-0.90409,"vy":-0.33746,"omega":-0.73524,"ax":8.22857,"ay":3.06603,"alpha":6.47818,"fx":[135.87697,153.28606,147.11799,123.58136],"fy":[70.85657,3.54571,43.39577,90.81104]},{"t":1.32075,"x":1.62909,"y":0.76225,"heading":0.83853,"vx":-0.60221,"vy":-0.22497,"omega":-0.49757,"ax":8.2136,"ay":3.06609,"alpha":6.69257,"fx":[134.66669,153.32782,147.47025,123.37933],"fy":[73.17598,2.09387,42.23065,91.11252]},{"t":1.35744,"x":1.61252,"y":0.75606,"heading":0.82028,"vx":-0.30087,"vy":-0.11248,"omega":-0.25204,"ax":8.20092,"ay":3.066,"alpha":6.86984,"fx":[133.73035,153.35283,147.72043,123.17767],"fy":[74.91179,0.8998,41.3881,91.40742]},{"t":1.39413,"x":1.607,"y":0.754,"heading":0.81103,"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/choreo/K to Station.traj b/src/main/deploy/choreo/K to Station.traj new file mode 100644 index 00000000..2af231c2 --- /dev/null +++ b/src/main/deploy/choreo/K to Station.traj @@ -0,0 +1 @@ +{"name":"K to Station","version":1,"snapshot":{"waypoints":[{"x":3.683604478836059,"y":5.922018051147461,"heading":-1.0563449640135358,"intervals":24,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":1.6078,"y":7.268,"heading":5.34070751,"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":17.548,"h":8.052}},"enabled":false}],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"3.6836044788360596 m","val":3.683604478836059},"y":{"exp":"5.922018051147461 m","val":5.922018051147461},"heading":{"exp":"-1.0563449640135358 rad","val":-1.0563449640135358},"intervals":24,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"1.6078 m","val":1.6078},"y":{"exp":"7.268 m","val":7.268},"heading":{"exp":"5.34070751 rad","val":5.34070751},"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":"17.548 m","val":17.548},"h":{"exp":"8.052 m","val":8.052}}},"enabled":false}],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,1.04805],"samples":[{"t":0.0,"x":3.6836,"y":5.92202,"heading":-1.05634,"vx":0.0,"vy":0.0,"omega":0.0,"ax":-7.56589,"ay":4.90583,"alpha":0.42342,"fx":[-127.02677,-129.22104,-130.35828,-128.16836],"fy":[85.99456,82.66425,80.85183,84.27652]},{"t":0.04367,"x":3.67639,"y":5.9267,"heading":-1.05634,"vx":-0.33039,"vy":0.21423,"omega":0.01849,"ax":-7.5652,"ay":4.90539,"alpha":0.42244,"fx":[-127.01917,-129.20833,-130.34255,-128.15771],"fy":[85.98099,82.65866,80.85051,84.26676]},{"t":0.08734,"x":3.65475,"y":5.94073,"heading":-1.05554,"vx":-0.66076,"vy":0.42845,"omega":0.03694,"ax":-7.56437,"ay":4.90485,"alpha":0.42126,"fx":[-127.01042,-129.19425,-130.32312,-128.14343],"fy":[85.96389,82.64984,80.8495,84.25703]},{"t":0.13101,"x":3.61868,"y":5.96411,"heading":-1.05392,"vx":-0.99109,"vy":0.64263,"omega":0.05533,"ax":-7.56335,"ay":4.90418,"alpha":0.41981,"fx":[-126.99993,-129.17782,-130.29885,-128.12476],"fy":[85.9423,82.6374,80.84867,84.2466]},{"t":0.17468,"x":3.56819,"y":5.99685,"heading":-1.05151,"vx":-1.32137,"vy":0.85679,"omega":0.07367,"ax":-7.56204,"ay":4.90334,"alpha":0.41798,"fx":[-126.9868,-129.15759,-130.26795,-128.10049],"fy":[85.91468,82.62074,80.84785,84.23431]},{"t":0.21834,"x":3.50328,"y":6.03894,"heading":-1.04829,"vx":-1.6516,"vy":1.07092,"omega":0.09192,"ax":-7.56034,"ay":4.90224,"alpha":0.4156,"fx":[-126.96959,-129.13115,-130.22755,-128.06868],"fy":[85.87861,82.59885,80.84672,84.21832]},{"t":0.26201,"x":3.42395,"y":6.09038,"heading":-1.04428,"vx":-1.98175,"vy":1.28499,"omega":0.11007,"ax":-7.55802,"ay":4.90073,"alpha":0.41238,"fx":[-126.94574,-129.09442,-130.17274,-128.026],"fy":[85.82991,82.57,80.84467,84.19546]},{"t":0.30568,"x":3.3302,"y":6.15117,"heading":-1.03947,"vx":-2.3118,"vy":1.499,"omega":0.12808,"ax":-7.55466,"ay":4.89855,"alpha":0.40779,"fx":[-126.91041,-129.03964,-130.09426,-127.96606],"fy":[85.76077,82.53088,80.84049,84.15976]},{"t":0.34935,"x":3.22204,"y":6.2213,"heading":-1.03388,"vx":-2.6417,"vy":1.71292,"omega":0.14588,"ax":-7.54938,"ay":4.89513,"alpha":0.40073,"fx":[-126.85299,-128.95026,-129.97242,-127.87517],"fy":[85.6547,82.47426,80.83132,84.09858]},{"t":0.39302,"x":3.09948,"y":6.30077,"heading":-1.02751,"vx":-2.97137,"vy":1.92668,"omega":0.16338,"ax":-7.53985,"ay":4.88895,"alpha":0.38845,"fx":[-126.74528,-128.78336,-129.75658,-127.71743],"fy":[85.46959,82.38098,80.80874,83.97938]},{"t":0.43669,"x":2.96254,"y":6.38957,"heading":-1.02037,"vx":-3.30063,"vy":2.14018,"omega":0.18035,"ax":-7.51757,"ay":4.87451,"alpha":0.36168,"fx":[-126.48017,-128.38227,-129.26417,-127.36003],"fy":[85.05579,82.17865,80.73661,83.685]},{"t":0.48036,"x":2.81123,"y":6.48768,"heading":-1.0125,"vx":-3.62892,"vy":2.35304,"omega":0.19614,"ax":-7.40575,"ay":4.80203,"alpha":0.25,"fx":[-125.03278,-126.34672,-126.90757,-125.59176],"fy":[83.14481,81.17925,80.201,82.19965]},{"t":0.52403,"x":2.6457,"y":6.59501,"heading":-1.00393,"vx":-3.95232,"vy":2.56274,"omega":0.20706,"ax":7.40575,"ay":-4.80203,"alpha":-0.25012,"fx":[125.0357,126.35478,126.90488,125.58341],"fy":[-83.141,-81.16649,-80.20459,-82.21259]},{"t":0.5677,"x":2.48017,"y":6.70234,"heading":-0.99489,"vx":-3.62892,"vy":2.35304,"omega":0.19614,"ax":7.51757,"ay":-4.87451,"alpha":-0.36169,"fx":[126.49449,128.41675,129.25154,127.32386],"fy":[-85.03535,-82.12443,-80.7559,-83.74036]},{"t":0.61136,"x":2.32887,"y":6.80045,"heading":-0.98632,"vx":-3.30063,"vy":2.14018,"omega":0.18034,"ax":7.53985,"ay":-4.88895,"alpha":-0.38843,"fx":[126.77017,128.84333,129.73485,127.65431],"fy":[-85.43355,-82.28684,-80.84266,-84.07566]},{"t":0.65503,"x":2.19192,"y":6.88925,"heading":-0.97845,"vx":-2.97137,"vy":1.92668,"omega":0.16338,"ax":7.54938,"ay":-4.89513,"alpha":-0.40071,"fx":[126.88757,129.03358,129.94239,127.7873],"fy":[-85.60434,-82.34351,-80.87863,-84.23239]},{"t":0.6987,"x":2.06936,"y":6.96872,"heading":-0.97131,"vx":-2.6417,"vy":1.71292,"omega":0.14588,"ax":7.55466,"ay":-4.89855,"alpha":-0.40778,"fx":[126.95371,129.14394,130.05675,127.85597],"fy":[-85.69751,-82.36726,-80.89988,-84.32726]},{"t":0.74237,"x":1.96121,"y":7.03885,"heading":-0.96494,"vx":-2.3118,"vy":1.499,"omega":0.12807,"ax":7.55802,"ay":-4.90073,"alpha":-0.41236,"fx":[126.99677,129.21719,130.12862,127.89632],"fy":[-85.75521,-82.37742,-80.91476,-84.39264]},{"t":0.78604,"x":1.86746,"y":7.09963,"heading":-0.95935,"vx":-1.98175,"vy":1.28499,"omega":0.11006,"ax":7.56034,"ay":-4.90224,"alpha":-0.41558,"fx":[127.02731,129.26986,130.17769,127.92212],"fy":[-85.79398,-82.3813,-80.92612,-84.4411]},{"t":0.82971,"x":1.78813,"y":7.15107,"heading":-0.95455,"vx":-1.6516,"vy":1.07092,"omega":0.09192,"ax":7.56204,"ay":-4.90334,"alpha":-0.41796,"fx":[127.05019,129.30965,130.21324,127.93975],"fy":[-85.82166,-82.38226,-80.93513,-84.47855]},{"t":0.87338,"x":1.72321,"y":7.19316,"heading":-0.95053,"vx":-1.32137,"vy":0.85679,"omega":0.07366,"ax":7.56335,"ay":-4.90418,"alpha":-0.4198,"fx":[127.06792,129.34065,130.24019,127.9526],"fy":[-85.84243,-82.38205,-80.94237,-84.50813]},{"t":0.91705,"x":1.67272,"y":7.2259,"heading":-0.94731,"vx":-0.99109,"vy":0.64263,"omega":0.05533,"ax":7.56437,"ay":-4.90485,"alpha":-0.42125,"fx":[127.08196,129.36523,130.26142,127.96262],"fy":[-85.85875,-82.3817,-80.94815,-84.53166]},{"t":0.96072,"x":1.63666,"y":7.24929,"heading":-0.9449,"vx":-0.66076,"vy":0.42845,"omega":0.03694,"ax":7.5652,"ay":-4.90539,"alpha":-0.42243,"fx":[127.09318,129.38486,130.27872,127.97099],"fy":[-85.87216,-82.38183,-80.95264,-84.55029]},{"t":1.00438,"x":1.61501,"y":7.26332,"heading":-0.94329,"vx":-0.33039,"vy":0.21423,"omega":0.01849,"ax":7.56589,"ay":-4.90583,"alpha":-0.42341,"fx":[127.10216,129.4005,130.29327,127.97852],"fy":[-85.88365,-82.38283,-80.95594,-84.56476]},{"t":1.04805,"x":1.6078,"y":7.268,"heading":-0.94248,"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/choreo/L to Station.traj b/src/main/deploy/choreo/L to Station.traj new file mode 100644 index 00000000..119b7f8b --- /dev/null +++ b/src/main/deploy/choreo/L to Station.traj @@ -0,0 +1 @@ +{"name":"L to Station","version":1,"snapshot":{"waypoints":[{"x":3.415688991546631,"y":5.612884521484375,"heading":-1.0240078529133427,"intervals":23,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":1.706,"y":7.268,"heading":5.34070751,"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":17.548,"h":8.052}},"enabled":false}],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"3.415688991546631 m","val":3.415688991546631},"y":{"exp":"5.612884521484375 m","val":5.612884521484375},"heading":{"exp":"-1.0240078529133427 rad","val":-1.0240078529133427},"intervals":23,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"1.706 m","val":1.706},"y":{"exp":"7.268 m","val":7.268},"heading":{"exp":"5.34070751 rad","val":5.34070751},"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":"17.548 m","val":17.548},"h":{"exp":"8.052 m","val":8.052}}},"enabled":false}],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,1.02875],"samples":[{"t":0.0,"x":3.41569,"y":5.61288,"heading":-1.02401,"vx":0.0,"vy":0.0,"omega":0.0,"ax":-6.47916,"ay":6.27234,"alpha":0.31057,"fx":[-108.80431,-111.03145,-111.62857,-109.3702],"fy":[108.13696,105.85004,105.21578,107.56026]},{"t":0.04473,"x":3.40921,"y":5.61916,"heading":-1.02401,"vx":-0.2898,"vy":0.28055,"omega":0.01389,"ax":-6.47855,"ay":6.27176,"alpha":0.31031,"fx":[-108.79539,-111.02056,-111.61691,-109.36056],"fy":[108.12577,105.84097,105.20711,107.54937]},{"t":0.08946,"x":3.38977,"y":5.63798,"heading":-1.02339,"vx":-0.57958,"vy":0.56107,"omega":0.02777,"ax":-6.47782,"ay":6.27104,"alpha":0.31,"fx":[-108.78499,-111.00811,-111.60216,-109.34787],"fy":[108.11159,105.82897,105.19702,107.53697]},{"t":0.13418,"x":3.35736,"y":5.66935,"heading":-1.02214,"vx":-0.86932,"vy":0.84157,"omega":0.04164,"ax":-6.47689,"ay":6.27015,"alpha":0.30962,"fx":[-108.77236,-110.9932,-111.58337,-109.33132],"fy":[108.09347,105.81328,105.18478,107.52215]},{"t":0.17891,"x":3.312,"y":5.71326,"heading":-1.02028,"vx":-1.15902,"vy":1.12202,"omega":0.05549,"ax":-6.4757,"ay":6.269,"alpha":0.30914,"fx":[-108.75632,-110.97441,-111.55903,-109.30966],"fy":[108.06999,105.79271,105.16923,107.50349]},{"t":0.22364,"x":3.25368,"y":5.76972,"heading":-1.0178,"vx":-1.44866,"vy":1.40242,"omega":0.06931,"ax":-6.47412,"ay":6.26746,"alpha":0.30851,"fx":[-108.73485,-110.94936,-111.52664,-109.28076],"fy":[108.03874,105.76526,105.14843,107.47863]},{"t":0.26837,"x":3.18241,"y":5.83872,"heading":-1.0147,"vx":-1.73824,"vy":1.68275,"omega":0.08311,"ax":-6.4719,"ay":6.26532,"alpha":0.30766,"fx":[-108.70435,-110.9138,-111.4817,-109.24079],"fy":[107.99542,105.72732,105.11884,107.44333]},{"t":0.3131,"x":3.09819,"y":5.92025,"heading":-1.01098,"vx":-2.02771,"vy":1.96299,"omega":0.09687,"ax":-6.46857,"ay":6.26209,"alpha":0.30644,"fx":[-108.65751,-110.85926,-111.41531,-109.18202],"fy":[107.93152,105.67161,105.07334,107.38913]},{"t":0.35783,"x":3.00102,"y":6.01432,"heading":-1.00665,"vx":-2.31704,"vy":2.24308,"omega":0.11058,"ax":-6.46302,"ay":6.25672,"alpha":0.30455,"fx":[-108.57712,-110.76612,-111.30687,-109.08637],"fy":[107.82731,105.58108,104.99515,107.29649]},{"t":0.40255,"x":2.89092,"y":6.1209,"heading":-1.0017,"vx":-2.60612,"vy":2.52293,"omega":0.1242,"ax":-6.45192,"ay":6.24597,"alpha":0.30128,"fx":[-108.41078,-110.576,-111.09542,-108.89903],"fy":[107.62449,105.40402,104.8331,107.10728]},{"t":0.44728,"x":2.7679,"y":6.24,"heading":-0.99615,"vx":-2.8947,"vy":2.8023,"omega":0.13768,"ax":-6.41867,"ay":6.21379,"alpha":0.29435,"fx":[-107.89143,-110.00148,-110.48312,-108.34307],"fy":[107.03829,104.87885,104.32587,106.53597]},{"t":0.49201,"x":2.632,"y":6.37156,"heading":-0.98999,"vx":-3.1818,"vy":3.08024,"omega":0.15084,"ax":0.0,"ay":0.0,"alpha":0.00007,"fx":[0.00005,-0.00027,-0.00006,0.00027],"fy":[0.00026,0.00005,-0.00027,-0.00006]},{"t":0.53674,"x":2.48969,"y":6.50933,"heading":-0.98324,"vx":-3.1818,"vy":3.08024,"omega":0.15085,"ax":6.41867,"ay":-6.21379,"alpha":-0.29435,"fx":[107.90249,110.01788,110.47257,108.32617],"fy":[-107.02748,-104.86142,-104.33668,-106.55339]},{"t":0.58147,"x":2.35379,"y":6.64089,"heading":-0.9765,"vx":-2.8947,"vy":2.8023,"omega":0.13768,"ax":6.45192,"ay":-6.24597,"alpha":-0.30128,"fx":[108.43286,110.60905,111.07439,108.86493],"fy":[-107.6026,-105.36912,-104.855,-107.14217]},{"t":0.62619,"x":2.23077,"y":6.75998,"heading":-0.97034,"vx":-2.60612,"vy":2.52293,"omega":0.1242,"ax":6.46302,"ay":-6.25672,"alpha":-0.30455,"fx":[108.60927,110.81435,111.27627,109.03659],"fy":[-107.79526,-105.53024,-105.02722,-107.3473]},{"t":0.67092,"x":2.12067,"y":6.86657,"heading":-0.96478,"vx":-2.31704,"vy":2.24308,"omega":0.11058,"ax":6.46857,"ay":-6.26209,"alpha":-0.30644,"fx":[108.69869,110.92106,111.37613,109.11822],"fy":[-107.89036,-105.60654,-105.11452,-107.45418]},{"t":0.71565,"x":2.0235,"y":6.96063,"heading":-0.95984,"vx":-2.02771,"vy":1.96299,"omega":0.09688,"ax":6.4719,"ay":-6.26532,"alpha":-0.30767,"fx":[108.75348,110.9875,111.43497,109.16468],"fy":[-107.94625,-105.64974,-105.16804,-107.52086]},{"t":0.76038,"x":1.93928,"y":7.04217,"heading":-0.9555,"vx":-1.73824,"vy":1.68275,"omega":0.08311,"ax":6.47412,"ay":-6.26746,"alpha":-0.30852,"fx":[108.79084,111.03329,111.47339,109.19409],"fy":[-107.98265,-105.67696,-105.20456,-107.56689]},{"t":0.80511,"x":1.86801,"y":7.11116,"heading":-0.95179,"vx":-1.44866,"vy":1.40242,"omega":0.06931,"ax":6.4757,"ay":-6.269,"alpha":-0.30915,"fx":[108.81805,111.06688,111.50033,109.21417],"fy":[-108.0081,-105.69545,-105.23118,-107.60069]},{"t":0.84984,"x":1.80969,"y":7.16762,"heading":-0.94869,"vx":-1.15902,"vy":1.12202,"omega":0.05549,"ax":6.47689,"ay":-6.27015,"alpha":-0.30963,"fx":[108.83872,111.09251,111.52026,109.22877],"fy":[-108.02691,-105.70885,-105.25142,-107.62651]},{"t":0.89456,"x":1.76433,"y":7.21153,"heading":-0.9462,"vx":-0.86932,"vy":0.84157,"omega":0.04164,"ax":6.47782,"ay":-6.27104,"alpha":-0.31001,"fx":[108.85486,111.11256,111.53571,109.23999],"fy":[-108.04148,-105.71914,-105.26721,-107.64671]},{"t":0.93929,"x":1.73192,"y":7.2429,"heading":-0.94434,"vx":-0.57958,"vy":0.56107,"omega":0.02777,"ax":6.47855,"ay":-6.27176,"alpha":-0.31032,"fx":[108.86764,111.12845,111.54819,109.24913],"fy":[-108.05325,-105.72754,-105.27972,-107.66271]},{"t":0.98402,"x":1.71248,"y":7.26173,"heading":-0.9431,"vx":-0.2898,"vy":0.28055,"omega":0.01389,"ax":6.47916,"ay":-6.27234,"alpha":-0.31058,"fx":[108.87782,111.14109,111.55866,109.25697],"fy":[-108.06316,-105.73478,-105.28968,-107.67542]},{"t":1.02875,"x":1.706,"y":7.268,"heading":-0.94248,"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/choreo/Low to E.traj b/src/main/deploy/choreo/Low to E.traj new file mode 100644 index 00000000..a2150f10 --- /dev/null +++ b/src/main/deploy/choreo/Low to E.traj @@ -0,0 +1,58 @@ +{ + "name":"Low to E", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.3107686042785645, "y":0.7491863965988159, "heading":2.325900348187067, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.455968856811523, "y":2.1711997985839844, "heading":2.2318394733980336, "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":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"7.3107686042785645 m", "val":7.3107686042785645}, "y":{"exp":"0.7491863965988159 m", "val":0.7491863965988159}, "heading":{"exp":"2.325900348187067 rad", "val":2.325900348187067}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.455968856811523 m", "val":5.455968856811523}, "y":{"exp":"2.1711997985839844 m", "val":2.1711997985839844}, "heading":{"exp":"2.2318394733980336 rad", "val":2.2318394733980336}, "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":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.01956], + "samples":[ + {"t":0.0, "x":7.31077, "y":0.74919, "heading":2.3259, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.15641, "ay":5.48658, "alpha":-0.36451, "fx":[-120.37604,-122.70172,-123.1034,-120.73282], "fy":[95.08584,92.06613,91.5223,94.62651]}, + {"t":0.04433, "x":7.30374, "y":0.75458, "heading":2.3259, "vx":-0.31723, "vy":0.24321, "omega":-0.01616, "ax":-7.15574, "ay":5.48607, "alpha":-0.36424, "fx":[-120.3658,-122.68965,-123.09069,-120.72201], "fy":[95.07574,92.05848,91.51491,94.61652]}, + {"t":0.08866, "x":7.28264, "y":0.77075, "heading":2.32518, "vx":-0.63444, "vy":0.4864, "omega":-0.0323, "ax":-7.15491, "ay":5.48543, "alpha":-0.36393, "fx":[-120.35253,-122.67395,-123.07586,-120.70979], "fy":[95.06432,92.05039,91.50493,94.60305]}, + {"t":0.13299, "x":7.24749, "y":0.7977, "heading":2.32375, "vx":-0.95161, "vy":0.72956, "omega":-0.04844, "ax":-7.15388, "ay":5.48465, "alpha":-0.36354, "fx":[-120.33539,-122.65362,-123.05785,-120.69523], "fy":[95.05075,92.04121,91.49176,94.58529]}, + {"t":0.17731, "x":7.19828, "y":0.83543, "heading":2.3216, "vx":-1.26873, "vy":0.97269, "omega":-0.06455, "ax":-7.15256, "ay":5.48363, "alpha":-0.36304, "fx":[-120.31302,-122.62709,-123.03501,-120.67691], "fy":[95.03371,92.02991,91.4744,94.56194]}, + {"t":0.22164, "x":7.13501, "y":0.88393, "heading":2.31874, "vx":-1.58579, "vy":1.21577, "omega":-0.08065, "ax":-7.1508, "ay":5.48228, "alpha":-0.3624, "fx":[-120.28314,-122.59177,-123.00458,-120.65242], "fy":[95.01103,92.01478,91.4512,94.53086]}, + {"t":0.26597, "x":7.05769, "y":0.94321, "heading":2.31517, "vx":-1.90278, "vy":1.4588, "omega":-0.09671, "ax":-7.14832, "ay":5.48038, "alpha":-0.36153, "fx":[-120.24165,-122.54298,-122.96166,-120.61744], "fy":[94.97883,91.9927,91.41915,94.48824]}, + {"t":0.3103, "x":6.96632, "y":1.01327, "heading":2.31088, "vx":-2.21965, "vy":1.70173, "omega":-0.11274, "ax":-7.14461, "ay":5.47754, "alpha":-0.36029, "fx":[-120.18027,-122.47141,-122.89647,-120.56324], "fy":[94.92942,91.9574,91.37216,94.42649]}, + {"t":0.35463, "x":6.8609, "y":1.09408, "heading":2.30588, "vx":-2.53636, "vy":1.94454, "omega":-0.12871, "ax":-7.13843, "ay":5.4728, "alpha":-0.3584, "fx":[-120.07949,-122.35545,-122.78641,-120.46941], "fy":[94.84509,91.8941,91.29575,94.32804]}, + {"t":0.39896, "x":6.74146, "y":1.18566, "heading":2.30018, "vx":-2.8528, "vy":2.18715, "omega":-0.1446, "ax":-7.12607, "ay":5.46332, "alpha":-0.35516, "fx":[-119.88016,-122.13078,-122.56444,-120.27423], "fy":[94.67367,91.75789,91.14562,94.14093]}, + {"t":0.44329, "x":6.60799, "y":1.28798, "heading":2.29377, "vx":-3.16869, "vy":2.42933, "omega":-0.16034, "ax":-7.08905, "ay":5.43494, "alpha":-0.34847, "fx":[-119.28216,-121.48005,-121.90182,-119.66674], "fy":[94.16177,91.32055,90.69392,93.61077]}, + {"t":0.48762, "x":6.46057, "y":1.40101, "heading":2.28666, "vx":-3.48294, "vy":2.67025, "omega":-0.17579, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[-0.00003,0.00049,0.00003,-0.00049], "fy":[-0.00048,-0.00002,0.0005,0.00005]}, + {"t":0.53194, "x":6.30617, "y":1.51938, "heading":2.27887, "vx":-3.48294, "vy":2.67025, "omega":-0.17578, "ax":7.08905, "ay":-5.43494, "alpha":0.34847, "fx":[119.26841,121.46083,121.91478,119.68676], "fy":[-94.17866,-91.34648,-90.67708,-93.5848]}, + {"t":0.57627, "x":6.15874, "y":1.63241, "heading":2.27108, "vx":-3.16869, "vy":2.42933, "omega":-0.16033, "ax":7.12607, "ay":-5.46332, "alpha":0.35515, "fx":[119.85292,122.09214,122.59005,120.31451], "fy":[-94.70763,-91.80965,-91.11176,-94.08907]}, + {"t":0.6206, "x":6.02528, "y":1.73473, "heading":2.26397, "vx":-2.8528, "vy":2.18715, "omega":-0.14459, "ax":7.13843, "ay":-5.4728, "alpha":0.35839, "fx":[120.03999,122.29909,122.8235,120.52817], "fy":[-94.89456,-91.96943,-91.24642,-94.25257]}, + {"t":0.66493, "x":5.90583, "y":1.8263, "heading":2.25756, "vx":-2.53636, "vy":1.94454, "omega":-0.1287, "ax":7.14461, "ay":-5.47754, "alpha":0.36029, "fx":[120.12983,122.3992,122.94379,120.63856], "fy":[-94.99274,-92.05382,-91.30902,-94.32989]}, + {"t":0.70926, "x":5.80042, "y":1.90712, "heading":2.25185, "vx":-2.21965, "vy":1.70173, "omega":-0.11273, "ax":7.14832, "ay":-5.48038, "alpha":0.36152, "fx":[120.18165,122.45684,123.01793,120.70731], "fy":[-95.05427,-92.10765,-91.34393,-94.37307]}, + {"t":0.75359, "x":5.70905, "y":1.97717, "heading":2.24686, "vx":-1.90278, "vy":1.4588, "omega":-0.09671, "ax":7.1508, "ay":-5.48228, "alpha":0.36239, "fx":[120.21494,122.49363,123.06852,120.75483], "fy":[-95.09686,-92.14568,-91.36563,-94.3997]}, + {"t":0.79792, "x":5.63173, "y":2.03645, "heading":2.24257, "vx":-1.58579, "vy":1.21577, "omega":-0.08064, "ax":7.15256, "ay":-5.48363, "alpha":0.36303, "fx":[120.23799,122.51892,123.10532,120.7898], "fy":[-95.12819,-92.17416,-91.38022,-94.41739]}, + {"t":0.84224, "x":5.56846, "y":2.08496, "heading":2.239, "vx":-1.26873, "vy":0.97269, "omega":-0.06455, "ax":7.15388, "ay":-5.48465, "alpha":0.36352, "fx":[120.25492,122.53737,123.13325,120.81656], "fy":[-95.15214,-92.19618,-91.3907,-94.42998]}, + {"t":0.88657, "x":5.51925, "y":2.12269, "heading":2.23613, "vx":-0.95161, "vy":0.72956, "omega":-0.04843, "ax":7.15491, "ay":-5.48543, "alpha":0.36391, "fx":[120.26799,122.55159,123.15505,120.83749], "fy":[-95.17089,-92.21347,-91.39873,-94.43962]}, + {"t":0.9309, "x":5.48409, "y":2.14964, "heading":2.23399, "vx":-0.63444, "vy":0.4864, "omega":-0.0323, "ax":7.15574, "ay":-5.48607, "alpha":0.36423, "fx":[120.27855,122.56317,123.17239,120.85403], "fy":[-95.18574,-92.22702,-91.4053,-94.44759]}, + {"t":0.97523, "x":5.463, "y":2.16581, "heading":2.23256, "vx":-0.31723, "vy":0.24321, "omega":-0.01616, "ax":7.15641, "ay":-5.48658, "alpha":0.36449, "fx":[120.28748,122.5731,123.18632,120.86708], "fy":[-95.19754,-92.2375,-91.41101,-94.45473]}, + {"t":1.01956, "x":5.45597, "y":2.1712, "heading":2.23184, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/Station to A.traj b/src/main/deploy/choreo/Station to A.traj new file mode 100644 index 00000000..6e702257 --- /dev/null +++ b/src/main/deploy/choreo/Station to A.traj @@ -0,0 +1 @@ +{"name":"Station to A","version":1,"snapshot":{"waypoints":[{"x":1.607,"y":7.268,"heading":5.34070751,"intervals":28,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":2.544352769851685,"y":5.399655342102051,"heading":-0.06241831633700658,"intervals":17,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":2.657106399536133,"y":4.159363269805908,"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":17.548,"h":8.052}},"enabled":false}],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"1.607 m","val":1.607},"y":{"exp":"7.268 m","val":7.268},"heading":{"exp":"5.34070751 rad","val":5.34070751},"intervals":28,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"2.5443527698516846 m","val":2.544352769851685},"y":{"exp":"5.399655342102051 m","val":5.399655342102051},"heading":{"exp":"-62.41831633700657 mrad","val":-0.06241831633700658},"intervals":17,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"2.657106399536133 m","val":2.657106399536133},"y":{"exp":"4.159363269805908 m","val":4.159363269805908},"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":"17.548 m","val":17.548},"h":{"exp":"8.052 m","val":8.052}}},"enabled":false}],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,0.7139,1.24173],"samples":[{"t":0.0,"x":1.607,"y":7.268,"heading":-0.94248,"vx":0.0,"vy":0.0,"omega":0.0,"ax":4.6205,"ay":-7.24681,"alpha":8.94376,"fx":[122.46967,27.31395,54.76465,109.82532],"fy":[-92.04117,-150.76602,-143.23741,-107.02006]},{"t":0.0255,"x":1.6085,"y":7.26564,"heading":-0.94248,"vx":0.11781,"vy":-0.18477,"omega":0.22803,"ax":4.61745,"ay":-7.26934,"alpha":8.73767,"fx":[121.42052,28.42057,55.01176,109.31321],"fy":[-93.39427,-150.54574,-143.1309,-107.52698]},{"t":0.05099,"x":1.61301,"y":7.25857,"heading":-0.93666,"vx":0.23553,"vy":-0.37011,"omega":0.45081,"ax":4.61492,"ay":-7.29348,"alpha":8.50598,"fx":[120.10673,29.50086,55.50071,108.88545],"fy":[-95.04774,-150.32158,-142.92899,-107.94182]},{"t":0.07649,"x":1.62051,"y":7.24676,"heading":-0.92517,"vx":0.3532,"vy":-0.55607,"omega":0.66769,"ax":4.61239,"ay":-7.31971,"alpha":8.24497,"fx":[118.49866,30.58759,56.21994,108.51538],"fy":[-97.01131,-150.0872,-142.63307,-108.29292]},{"t":0.10199,"x":1.63102,"y":7.23021,"heading":-0.90815,"vx":0.4708,"vy":-0.74269,"omega":0.8779,"ax":4.60926,"ay":-7.34858,"alpha":7.9499,"fx":[116.55797,31.72581,57.15637,108.16834],"fy":[-99.29694,-149.83261,-142.244,-108.61548]},{"t":0.12748,"x":1.64452,"y":7.20888,"heading":-0.88576,"vx":0.58832,"vy":-0.93006,"omega":1.0806,"ax":4.6048,"ay":-7.38069,"alpha":7.61537,"fx":[114.23966,32.97143,58.2947,107.79956],"fy":[-101.91403,-149.54383,-141.76254,-108.95362]},{"t":0.15298,"x":1.66101,"y":7.18277,"heading":-0.85821,"vx":0.70572,"vy":-1.11824,"omega":1.27476,"ax":4.59816,"ay":-7.41663,"alpha":7.23575,"fx":[111.49566,34.39027,59.61638,107.35093],"fy":[-104.86355,-149.20233,-141.1901,-109.36312]},{"t":0.17848,"x":1.6805,"y":7.15185,"heading":-0.82571,"vx":0.82296,"vy":-1.30734,"omega":1.45925,"ax":4.5883,"ay":-7.45691,"alpha":6.80537,"fx":[108.28058,36.05762,61.0984,106.74612],"fy":[-108.13108,-148.78392,-140.52959,-109.91524]},{"t":0.20397,"x":1.70298,"y":7.11609,"heading":-0.7885,"vx":0.93995,"vy":-1.49746,"omega":1.63276,"ax":4.57406,"ay":-7.50196,"alpha":6.31838,"fx":[104.56049,38.0592,62.71148,105.88279],"fy":[-111.67966,-148.25671,-139.78665,-110.70169]},{"t":0.22947,"x":1.72843,"y":7.07547,"heading":-0.74687,"vx":1.05657,"vy":-1.68873,"omega":1.79386,"ax":4.55411,"ay":-7.55207,"alpha":5.76774,"fx":[100.32494,40.49413,64.41755,104.62004],"fy":[-115.4436,-147.57759,-138.97141,-111.84141]},{"t":0.25496,"x":1.75685,"y":7.02996,"heading":-0.70114,"vx":1.17268,"vy":-1.88129,"omega":1.94091,"ax":4.52691,"ay":-7.60745,"alpha":5.14273,"fx":[95.60151,43.48122,66.16553,102.75752],"fy":[-119.32561,-146.68605,-138.10118,-113.48933]},{"t":0.28046,"x":1.78822,"y":6.97952,"heading":-0.65165,"vx":1.2881,"vy":-2.07525,"omega":2.07203,"ax":4.49045,"ay":-7.66832,"alpha":4.42409,"fx":[90.4709,47.17068,67.88411,99.99966],"fy":[-123.19929,-145.49319,-137.20484,-115.84669]},{"t":0.30596,"x":1.82252,"y":6.92412,"heading":-0.59882,"vx":1.40259,"vy":-2.27076,"omega":2.18483,"ax":4.44164,"ay":-7.735,"alpha":3.57566,"fx":[85.07874,51.76478,69.46753,95.89315],"fy":[-126.91851,-143.86139,-136.33073,-119.16997]},{"t":0.33145,"x":1.85972,"y":6.86371,"heading":-0.54312,"vx":1.51584,"vy":-2.46798,"omega":2.276,"ax":4.37479,"ay":-7.80767,"alpha":2.53123,"fx":[79.6397,57.55401,70.74545,89.71662],"fy":[-130.3327,-141.56432,-135.56219,-123.7656]},{"t":0.35695,"x":1.8998,"y":6.79825,"heading":-0.48509,"vx":1.62738,"vy":-2.66705,"omega":2.34054,"ax":4.27864,"ay":-7.88502,"alpha":1.17545,"fx":[74.43051,64.98047,71.41125,80.29126],"fy":[-133.30441,-138.20315,-135.05156,-129.92894]},{"t":0.38245,"x":1.94268,"y":6.72768,"heading":-0.42541,"vx":1.73647,"vy":-2.86809,"omega":2.37051,"ax":4.13057,"ay":-7.95938,"alpha":-0.68017,"fx":[69.77136,74.74758,70.82212,65.69843],"fy":[-135.72303,-133.0165,-135.10482,-137.70245]},{"t":0.40794,"x":1.98829,"y":6.65197,"heading":-0.36497,"vx":1.84179,"vy":-3.07102,"omega":2.35317,"ax":3.88554,"ay":-8.00286,"alpha":-3.31289,"fx":[65.99757,87.98187,67.29591,43.09249],"fy":[-137.50983,-124.41576,-136.43084,-146.14896]},{"t":0.43344,"x":2.03652,"y":6.57107,"heading":-0.30497,"vx":1.94085,"vy":-3.27507,"omega":2.2687,"ax":3.43651,"ay":-7.94072,"alpha":-7.08011,"fx":[63.41937,106.23,54.46368,9.70306],"fy":[-138.61446,-108.83527,-140.98684,-151.84067]},{"t":0.45894,"x":2.08712,"y":6.48499,"heading":-0.24713,"vx":2.02847,"vy":-3.47753,"omega":2.08818,"ax":2.12649,"ay":-7.55169,"alpha":-13.60346,"fx":[62.18511,129.14004,-14.8633,-31.77806],"fy":[-139.04027,-79.50516,-146.64348,-148.61959]},{"t":0.48443,"x":2.13953,"y":6.39387,"heading":-0.19389,"vx":2.08269,"vy":-3.67007,"omega":1.74134,"ax":1.12944,"ay":-7.25704,"alpha":-17.11632,"fx":[58.96699,128.88657,-70.222,-40.78547],"fy":[-140.13102,-78.69663,-128.78822,-146.14513]},{"t":0.50993,"x":2.193,"y":6.29793,"heading":-0.14949,"vx":2.11149,"vy":-3.8551,"omega":1.30494,"ax":0.51639,"ay":-7.38584,"alpha":-16.98338,"fx":[49.9575,115.92173,-85.09002,-45.65443],"fy":[-143.07897,-95.02631,-120.1515,-144.26737]},{"t":0.53543,"x":2.247,"y":6.19724,"heading":-0.11622,"vx":2.12465,"vy":-4.04341,"omega":0.87192,"ax":-0.77842,"ay":-7.48122,"alpha":-16.08579,"fx":[29.74934,84.16227,-106.47129,-60.40343],"fy":[-147.75659,-121.22864,-101.95791,-138.07052]},{"t":0.56092,"x":2.30092,"y":6.09172,"heading":-0.09399,"vx":2.10481,"vy":-4.23415,"omega":0.46179,"ax":-3.78333,"ay":-6.95962,"alpha":-11.97004,"fx":[-15.65777,-17.50077,-132.90169,-91.35338],"fy":[-148.45528,-141.57516,-64.61601,-118.87806]},{"t":0.58642,"x":2.35335,"y":5.9815,"heading":-0.08221,"vx":2.00834,"vy":-4.4116,"omega":0.1566,"ax":-7.1653,"ay":-4.77224,"alpha":-4.10074,"fx":[-103.71193,-124.40489,-137.09865,-122.30332],"fy":[-105.60489,-77.64333,-56.42818,-85.02158]},{"t":0.61191,"x":2.40223,"y":5.86747,"heading":-0.07822,"vx":1.82565,"vy":-4.53327,"omega":0.05204,"ax":-8.30545,"ay":-2.76226,"alpha":0.96531,"fx":[-143.34418,-140.70664,-139.02523,-142.01739],"fy":[-40.85607,-49.50312,-53.29757,-44.28452]},{"t":0.63741,"x":2.44608,"y":5.75099,"heading":-0.0769,"vx":1.61389,"vy":-4.6037,"omega":0.07665,"ax":-8.63759,"ay":-0.96961,"alpha":4.71312,"fx":[-150.14168,-146.88723,-141.67922,-148.98379],"fy":[7.91773,-32.71782,-47.61696,6.44608]},{"t":0.66291,"x":2.48442,"y":5.6333,"heading":-0.07494,"vx":1.39367,"vy":-4.62842,"omega":0.19682,"ax":-8.69318,"ay":1.26051,"alpha":3.82401,"fx":[-146.41546,-151.05522,-150.35592,-143.64738],"fy":[37.41526,3.08343,0.60415,44.66086]},{"t":0.6884,"x":2.51713,"y":5.5157,"heading":-0.06992,"vx":1.17202,"vy":-4.59629,"omega":0.29432,"ax":-8.17322,"ay":3.52997,"alpha":-1.11514,"fx":[-140.49967,-135.85637,-137.81571,-141.9249],"fy":[56.54258,67.0636,63.27466,53.29447]},{"t":0.7139,"x":2.54435,"y":5.39966,"heading":-0.06242,"vx":0.96363,"vy":-4.50628,"omega":0.26589,"ax":-7.05422,"ay":5.50133,"alpha":-0.91218,"fx":[-120.8151,-115.74057,-119.34501,-124.0607],"fy":[92.53849,98.88957,94.61221,88.26389]},{"t":0.74495,"x":2.57087,"y":5.26239,"heading":-0.05416,"vx":0.74461,"vy":-4.33547,"omega":0.23757,"ax":-5.41573,"ay":7.13898,"alpha":-0.80035,"fx":[-91.71815,-87.31187,-92.56056,-96.88932],"fy":[121.75664,125.01242,121.22289,117.73627]},{"t":0.776,"x":2.59138,"y":5.13122,"heading":-0.04679,"vx":0.57646,"vy":-4.11382,"omega":0.21272,"ax":-4.12156,"ay":7.97064,"alpha":-0.70269,"fx":[-68.85106,-65.58227,-71.31678,-74.6761],"fy":[136.23864,137.88413,135.02986,133.16088]},{"t":0.80705,"x":2.60729,"y":5.00734,"heading":-0.04018,"vx":0.44849,"vy":-3.86634,"omega":0.1909,"ax":-3.17858,"ay":8.40128,"alpha":-0.62683,"fx":[-52.3622,-50.00136,-55.69494,-58.20856],"fy":[143.55347,144.42406,142.3378,141.29806]},{"t":0.8381,"x":2.61969,"y":4.89134,"heading":-0.03426,"vx":0.3498,"vy":-3.60549,"omega":0.17144,"ax":-2.49066,"ay":8.63738,"alpha":-0.56916,"fx":[-40.44402,-38.72473,-44.20286,-46.08985],"fy":[147.47532,147.96022,146.42438,145.81781]},{"t":0.86914,"x":2.62935,"y":4.78356,"heading":-0.02893,"vx":0.27247,"vy":-3.33731,"omega":0.15377,"ax":-1.97688,"ay":8.77452,"alpha":-0.52487,"fx":[-31.60713,-30.33712,-35.56263,-36.99754],"fy":[149.7069,149.98866,148.84092,148.47184]},{"t":0.90019,"x":2.63685,"y":4.68417,"heading":-0.02416,"vx":0.21109,"vy":-3.06488,"omega":0.13747,"ax":-1.58263,"ay":8.85815,"alpha":-0.4902,"fx":[-24.86385,-23.91484,-28.89808,-30.00332],"fy":[151.04083,151.20977,150.34005,150.10781]},{"t":0.93124,"x":2.64264,"y":4.59328,"heading":-0.01989,"vx":0.16195,"vy":-2.78984,"omega":0.12225,"ax":-1.27236,"ay":8.91117,"alpha":-0.46251,"fx":[-19.58011,-18.86656,-23.63184,-24.49166],"fy":[151.86912,151.97266,151.30696,151.15704]},{"t":0.96229,"x":2.64706,"y":4.51095,"heading":-0.01609,"vx":0.12244,"vy":-2.51316,"omega":0.10789,"ax":-1.02273,"ay":8.9458,"alpha":-0.43997,"fx":[-15.34347,-14.80712,-19.38074,-20.05377],"fy":[152.39795,152.46234,151.9501,151.85148]},{"t":0.99334,"x":2.65037,"y":4.43723,"heading":-0.01274,"vx":0.09069,"vy":-2.23541,"omega":0.09423,"ax":-0.81799,"ay":8.96892,"alpha":-0.42131,"fx":[-11.87869,-11.47875,-15.885,-16.41293],"fy":[152.74188,152.78233,152.38821,152.32245]},{"t":1.02439,"x":2.65279,"y":4.37215,"heading":-0.00982,"vx":0.06529,"vy":-1.95693,"omega":0.08115,"ax":-0.64731,"ay":8.98458,"alpha":-0.40563,"fx":[-8.99696,-8.70407,-12.96415,-13.37733],"fy":[152.96768,152.99332,152.69203,152.64773]},{"t":1.05544,"x":2.6545,"y":4.31572,"heading":-0.0073,"vx":0.04519,"vy":-1.67797,"omega":0.06855,"ax":-0.503,"ay":8.99528,"alpha":-0.3923,"fx":[-6.56519,-6.35769,-10.48968,-10.81075],"fy":[153.11584,153.13235,152.90536,152.87527]},{"t":1.08648,"x":2.65566,"y":4.26796,"heading":-0.00517,"vx":0.02958,"vy":-1.39868,"omega":0.05637,"ax":-0.37947,"ay":9.0026,"alpha":-0.38082,"fx":[-4.48734,-4.34881,-8.36807,-8.61426],"fy":[153.2118,153.22278,153.05621,153.03559]},{"t":1.11753,"x":2.6564,"y":4.22887,"heading":-0.00342,"vx":0.01779,"vy":-1.11916,"omega":0.04455,"ax":-0.27259,"ay":9.00755,"alpha":-0.37084,"fx":[-2.69249,-2.61025,-6.52978,-6.7145],"fy":[153.27201,153.27977,153.16307,153.14876]},{"t":1.14858,"x":2.65682,"y":4.19846,"heading":-0.00204,"vx":0.00933,"vy":-0.83949,"omega":0.03303,"ax":-0.17926,"ay":9.01084,"alpha":-0.36209,"fx":[-1.1273,-1.09135,-4.92218,-5.05607],"fy":[153.30737,153.31343,153.2384,153.22826]},{"t":1.17963,"x":2.65702,"y":4.17674,"heading":-0.00101,"vx":0.00376,"vy":-0.55972,"omega":0.02179,"ax":-0.09708,"ay":9.01294,"alpha":-0.35436,"fx":[0.24909,0.2468,-3.50476,-3.5964],"fy":[153.32529,153.33062,153.29082,153.28337]},{"t":1.21068,"x":2.65709,"y":4.16371,"heading":-0.00033,"vx":0.00075,"vy":-0.27988,"omega":0.01079,"ax":-0.02418,"ay":9.01417,"alpha":-0.34749,"fx":[1.46849,1.43452,-2.24587,-2.30228],"fy":[153.33086,153.33611,153.32638,153.32062]},{"t":1.24173,"x":2.65711,"y":4.15936,"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/choreo/Station to D.traj b/src/main/deploy/choreo/Station to D.traj new file mode 100644 index 00000000..83019fc5 --- /dev/null +++ b/src/main/deploy/choreo/Station to D.traj @@ -0,0 +1,60 @@ +{ + "name":"Station to D", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":1.607, "y":0.754, "heading":0.8110335871211297, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.7218778133392334, "y":2.0161004066467285, "heading":1.0379526372170715, "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":17.548, "h":8.052}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.607 m", "val":1.607}, "y":{"exp":"0.754 m", "val":0.754}, "heading":{"exp":"0.8110335871211297 rad", "val":0.8110335871211297}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.7218778133392334 m", "val":3.7218778133392334}, "y":{"exp":"2.0161004066467285 m", "val":2.0161004066467285}, "heading":{"exp":"1.0379526372170715 rad", "val":1.0379526372170715}, "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":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.04669], + "samples":[ + {"t":0.0, "x":1.607, "y":0.754, "heading":0.81103, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.74041, "ay":4.61926, "alpha":0.8386, "fx":[130.02846,134.52266,133.23925,128.85848], "fy":[81.36118,73.69871,76.01134,83.21799]}, + {"t":0.04187, "x":1.61378, "y":0.75805, "heading":0.81103, "vx":0.32407, "vy":0.1934, "omega":0.03511, "ax":7.73972, "ay":4.61885, "alpha":0.83758, "fx":[130.0184,134.50691,133.22587,128.85052], "fy":[81.35013,73.698,76.00773,83.20524]}, + {"t":0.08374, "x":1.63414, "y":0.77019, "heading":0.8125, "vx":0.64812, "vy":0.38678, "omega":0.07018, "ax":7.73889, "ay":4.61835, "alpha":0.83637, "fx":[130.01073,134.49013,133.20591,128.83837], "fy":[81.32984,73.6933,76.01028,83.19396]}, + {"t":0.1256, "x":1.66805, "y":0.79044, "heading":0.81544, "vx":0.97213, "vy":0.58014, "omega":0.10519, "ax":7.73787, "ay":4.61775, "alpha":0.8349, "fx":[130.00473,134.47124,133.17848,128.8215], "fy":[81.29959,73.6846,76.01868,83.18325]}, + {"t":0.16747, "x":1.71554, "y":0.81877, "heading":0.81985, "vx":1.2961, "vy":0.77347, "omega":0.14015, "ax":7.7366, "ay":4.61699, "alpha":0.83306, "fx":[129.99936,134.44866,133.14229,128.79915], "fy":[81.25836,73.67188,76.03251,83.17179]}, + {"t":0.20934, "x":1.77658, "y":0.8552, "heading":0.82571, "vx":1.62001, "vy":0.96678, "omega":0.17503, "ax":7.73497, "ay":4.61601, "alpha":0.83073, "fx":[129.99295,134.41988,133.09528,128.77008], "fy":[81.2045,73.65507,76.05109,83.15752]}, + {"t":0.25121, "x":1.85119, "y":0.89972, "heading":0.83304, "vx":1.94386, "vy":1.16004, "omega":0.20981, "ax":7.73278, "ay":4.61471, "alpha":0.82768, "fx":[129.98272,134.38078,133.03404,128.73219], "fy":[81.13533,73.63398,76.07327,83.13708]}, + {"t":0.29307, "x":1.93935, "y":0.95234, "heading":0.84183, "vx":2.26761, "vy":1.35325, "omega":0.24446, "ax":7.72973, "ay":4.61289, "alpha":0.8235, "fx":[129.96359,134.32407,132.9525,128.68159], "fy":[81.04612,73.60807,76.09687,83.10458]}, + {"t":0.33494, "x":2.04106, "y":1.01304, "heading":0.85206, "vx":2.59124, "vy":1.54638, "omega":0.27894, "ax":7.72514, "ay":4.61015, "alpha":0.81745, "fx":[129.9253,134.23538,132.83857,128.61023], "fy":[80.92757,73.5758,76.1174,83.04864]}, + {"t":0.37681, "x":2.15632, "y":1.08182, "heading":0.86374, "vx":2.91467, "vy":1.73939, "omega":0.31317, "ax":7.71748, "ay":4.60558, "alpha":0.80798, "fx":[129.84356,134.0819,132.66432,128.49859], "fy":[80.75862,73.53232,76.12391,82.94379]}, + {"t":0.41868, "x":2.28512, "y":1.15868, "heading":0.87685, "vx":3.23778, "vy":1.93222, "omega":0.34699, "ax":7.70214, "ay":4.59643, "alpha":0.79105, "fx":[129.64432,133.768,132.34696,128.2852], "fy":[80.47834,73.45805,76.0816,82.71812]}, + {"t":0.46055, "x":2.42743, "y":1.24361, "heading":0.89138, "vx":3.56026, "vy":2.12466, "omega":0.38011, "ax":7.65605, "ay":4.56895, "alpha":0.75261, "fx":[128.94844,132.83863,131.48147,127.64019], "fy":[79.80048,73.21588,75.80162,82.04802]}, + {"t":0.50241, "x":2.5832, "y":1.33657, "heading":0.90729, "vx":3.8808, "vy":2.31595, "omega":0.41162, "ax":0.00002, "ay":0.00002, "alpha":-0.00496, "fx":[0.01911,-0.00187,-0.01827,0.00271], "fy":[0.0026,0.019,-0.00198,-0.01838]}, + {"t":0.54428, "x":2.74568, "y":1.43353, "heading":0.92453, "vx":3.8808, "vy":2.31595, "omega":0.41142, "ax":-7.65605, "ay":-4.56895, "alpha":-0.75249, "fx":[-129.04011,-132.87444,-131.39882,-127.5954], "fy":[-79.65025,-73.15533,-75.94643,-82.11402]}, + {"t":0.58615, "x":2.90145, "y":1.52649, "heading":0.94175, "vx":3.56026, "vy":2.12466, "omega":0.37991, "ax":-7.70214, "ay":-4.59643, "alpha":-0.7909, "fx":[-129.83542,-133.83984,-132.1757,-128.19357], "fy":[-80.16768,-73.3317,-76.38043,-82.85633]}, + {"t":0.62802, "x":3.04376, "y":1.61142, "heading":0.95766, "vx":3.23778, "vy":1.93222, "omega":0.3468, "ax":-7.71748, "ay":-4.60558, "alpha":-0.80776, "fx":[-130.12745,-134.18667,-132.41053,-128.36378], "fy":[-80.29842,-73.34552,-76.56615,-83.1486]}, + {"t":0.66988, "x":3.17255, "y":1.68828, "heading":0.97218, "vx":2.91467, "vy":1.73939, "omega":0.31298, "ax":-7.72514, "ay":-4.61015, "alpha":-0.81716, "fx":[-130.29367,-134.3697,-132.5097,-128.4365], "fy":[-80.33124,-73.33466,-76.69008,-83.31351]}, + {"t":0.71175, "x":3.28781, "y":1.75706, "heading":0.98528, "vx":2.59124, "vy":1.54638, "omega":0.27877, "ax":-7.72973, "ay":-4.61289, "alpha":-0.82313, "fx":[-130.4076,-134.48449,-132.55642,-128.47336], "fy":[-80.32791,-73.31886,-76.78626,-83.42271]}, + {"t":0.75362, "x":3.38953, "y":1.81776, "heading":0.99695, "vx":2.26761, "vy":1.35325, "omega":0.2443, "ax":-7.73279, "ay":-4.61471, "alpha":-0.82724, "fx":[-130.49329,-134.56384,-132.57883,-128.49393], "fy":[-80.30986,-73.30301,-76.86531,-83.5016]}, + {"t":0.79549, "x":3.47769, "y":1.87038, "heading":1.00718, "vx":1.94386, "vy":1.16004, "omega":0.20967, "ax":-7.73497, "ay":-4.61601, "alpha":-0.83025, "fx":[-130.56088,-134.62214,-132.58909,-128.50624], "fy":[-80.28655,-73.28864,-76.93156,-83.56155]}, + {"t":0.83736, "x":3.5523, "y":1.9149, "heading":1.01596, "vx":1.62001, "vy":0.96678, "omega":0.17491, "ax":-7.7366, "ay":-4.61699, "alpha":-0.83253, "fx":[-130.61538,-134.6667,-132.59337,-128.51419], "fy":[-80.26285,-73.27626,-76.98708,-83.60848]}, + {"t":0.87922, "x":3.61334, "y":1.95133, "heading":1.02328, "vx":1.2961, "vy":0.77347, "omega":0.14005, "ax":-7.73787, "ay":-4.61775, "alpha":-0.83433, "fx":[-130.65955,-134.70168,-132.59509,-128.51983], "fy":[-80.24151,-73.266,-77.03296,-83.6458]}, + {"t":0.92109, "x":3.66082, "y":1.97967, "heading":1.02915, "vx":0.97213, "vy":0.58014, "omega":0.10512, "ax":-7.73889, "ay":-4.61835, "alpha":-0.83578, "fx":[-130.695,-134.7296,-132.59636,-128.52438], "fy":[-80.22423,-73.25786,-77.06983,-83.6756]}, + {"t":0.96296, "x":3.69474, "y":1.99991, "heading":1.03355, "vx":0.64812, "vy":0.38678, "omega":0.07013, "ax":-7.73972, "ay":-4.61885, "alpha":-0.83698, "fx":[-130.72277,-134.7521,-132.59849,-128.52856], "fy":[-80.2121,-73.2518,-77.09808,-83.69928]}, + {"t":1.00483, "x":3.71509, "y":2.01205, "heading":1.03648, "vx":0.32407, "vy":0.1934, "omega":0.03508, "ax":-7.74042, "ay":-4.61926, "alpha":-0.83799, "fx":[-130.74354,-134.77028,-132.60239,-128.53285], "fy":[-80.20586,-73.24778,-77.11795,-83.71778]}, + {"t":1.04669, "x":3.72188, "y":2.0161, "heading":1.03795, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/Station to L.traj b/src/main/deploy/choreo/Station to L.traj new file mode 100644 index 00000000..cd368ea7 --- /dev/null +++ b/src/main/deploy/choreo/Station to L.traj @@ -0,0 +1 @@ +{"name":"Station to L","version":1,"snapshot":{"waypoints":[{"x":1.607,"y":7.268,"heading":5.34070751,"intervals":23,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":3.3618175983428955,"y":5.681539058685303,"heading":-0.9917219873088342,"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":17.548,"h":8.052}},"enabled":false}],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"1.607 m","val":1.607},"y":{"exp":"7.268 m","val":7.268},"heading":{"exp":"5.34070751 rad","val":5.34070751},"intervals":23,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"3.3618175983428955 m","val":3.3618175983428955},"y":{"exp":"5.681539058685303 m","val":5.681539058685303},"heading":{"exp":"-0.9917219873088342 rad","val":-0.9917219873088342},"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":"17.548 m","val":17.548},"h":{"exp":"8.052 m","val":8.052}}},"enabled":false}],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,1.02571],"samples":[{"t":0.0,"x":1.607,"y":7.268,"heading":-0.94248,"vx":0.0,"vy":0.0,"omega":0.0,"ax":6.68963,"ay":-6.04783,"alpha":-0.18864,"fx":[112.98869,114.31071,114.5949,113.2605],"fy":[-103.75592,-102.29816,-101.97693,-103.45639]},{"t":0.0446,"x":1.61365,"y":7.26199,"heading":-0.94248,"vx":0.29833,"vy":-0.26971,"omega":-0.00841,"ax":6.689,"ay":-6.04726,"alpha":-0.18849,"fx":[112.97879,114.29969,114.58349,113.25023],"fy":[-103.74557,-102.2891,-101.96804,-103.44616]},{"t":0.08919,"x":1.63361,"y":7.24394,"heading":-0.94285,"vx":0.59664,"vy":-0.53939,"omega":-0.01682,"ax":6.68824,"ay":-6.04657,"alpha":-0.18831,"fx":[112.96648,114.28592,114.56974,113.23797],"fy":[-103.73314,-102.27835,-101.95694,-103.43334]},{"t":0.13379,"x":1.66687,"y":7.21388,"heading":-0.9436,"vx":0.89491,"vy":-0.80905,"omega":-0.02522,"ax":6.68728,"ay":-6.04571,"alpha":-0.18809,"fx":[112.95092,114.26849,114.5527,113.22286],"fy":[-103.71778,-102.26515,-101.94289,-103.41707]},{"t":0.17838,"x":1.71343,"y":7.17178,"heading":-0.94473,"vx":1.19313,"vy":-1.07866,"omega":-0.0336,"ax":6.68605,"ay":-6.04459,"alpha":-0.18782,"fx":[112.93081,114.24596,114.53089,113.20354],"fy":[-103.69813,-102.24831,-101.92471,-103.39602]},{"t":0.22298,"x":1.77328,"y":7.11767,"heading":-0.94623,"vx":1.49131,"vy":-1.34823,"omega":-0.04198,"ax":6.68441,"ay":-6.04311,"alpha":-0.18745,"fx":[112.90397,114.21595,114.50184,113.17776],"fy":[-103.67196,-102.22581,-101.90044,-103.36798]},{"t":0.26758,"x":1.84644,"y":7.05153,"heading":-0.9481,"vx":1.7894,"vy":-1.61773,"omega":-0.05034,"ax":6.68211,"ay":-6.04103,"alpha":-0.18696,"fx":[112.86643,114.17415,114.46111,113.14142],"fy":[-103.63526,-102.19405,-101.8665,-103.32897]},{"t":0.31217,"x":1.93288,"y":6.97338,"heading":-0.95034,"vx":2.0874,"vy":-1.88714,"omega":-0.05868,"ax":6.67866,"ay":-6.03791,"alpha":-0.18626,"fx":[112.81027,114.11199,114.39985,113.08633],"fy":[-103.58002,-102.14578,-101.81576,-103.27108]},{"t":0.35677,"x":2.03261,"y":6.88322,"heading":-0.95296,"vx":2.38524,"vy":-2.1564,"omega":-0.06698,"ax":6.67291,"ay":-6.03271,"alpha":-0.18518,"fx":[112.71687,114.00953,114.29756,112.99333],"fy":[-103.4877,-102.06403,-101.73141,-103.17587]},{"t":0.40137,"x":2.14562,"y":6.78105,"heading":-0.95595,"vx":2.68283,"vy":-2.42544,"omega":-0.07524,"ax":6.66142,"ay":-6.02232,"alpha":-0.18331,"fx":[112.52998,113.80725,114.09317,112.80468],"fy":[-103.3032,-101.8976,-101.56258,-102.98847]},{"t":0.44596,"x":2.27189,"y":6.6669,"heading":-0.9593,"vx":2.9799,"vy":-2.69401,"omega":-0.08342,"ax":6.62699,"ay":-5.9912,"alpha":-0.1794,"fx":[111.96597,113.21047,113.48573,112.23066],"fy":[-102.75542,-101.38913,-101.05214,-102.43763]},{"t":0.49056,"x":2.41137,"y":6.5408,"heading":-0.96302,"vx":3.27544,"vy":-2.9612,"omega":-0.09142,"ax":0.0,"ay":0.0,"alpha":0.00001,"fx":[0.00001,-0.00004,-0.00001,0.00004],"fy":[0.00004,0.00001,-0.00004,-0.00001]},{"t":0.53515,"x":2.55744,"y":6.40874,"heading":-0.9671,"vx":3.27544,"vy":-2.9612,"omega":-0.09142,"ax":-6.62699,"ay":5.9912,"alpha":0.1794,"fx":[-111.96212,-113.20458,-113.48947,-112.23666],"fy":[102.75949,101.39578,101.04808,102.43097]},{"t":0.57975,"x":2.69693,"y":6.28264,"heading":-0.97118,"vx":2.9799,"vy":-2.69401,"omega":-0.08342,"ax":-6.66142,"ay":6.02232,"alpha":0.18331,"fx":[-112.52232,-113.79541,-114.1006,-112.81676],"fy":[103.31141,101.91091,101.55439,102.97515]},{"t":0.62435,"x":2.8232,"y":6.16849,"heading":-0.9749,"vx":2.68283,"vy":-2.42544,"omega":-0.07524,"ax":-6.67291,"ay":6.03271,"alpha":0.18518,"fx":[-112.70574,-113.99225,-114.30834,-113.01096],"fy":[103.49969,102.08341,101.71943,103.15647]},{"t":0.66894,"x":2.9362,"y":6.06632,"heading":-0.97825,"vx":2.38524,"vy":-2.1564,"omega":-0.06698,"ax":-6.67866,"ay":6.03791,"alpha":0.18626,"fx":[-112.79604,-114.08984,-114.41362,-113.10894],"fy":[103.59539,102.1706,101.80042,103.24623]},{"t":0.71354,"x":3.03593,"y":5.97616,"heading":-0.98124,"vx":2.0874,"vy":-1.88714,"omega":-0.05868,"ax":-6.68211,"ay":6.04103,"alpha":0.18696,"fx":[-112.84949,-114.14772,-114.4775,-113.1684],"fy":[103.65359,102.22365,101.8482,103.29935]},{"t":0.75813,"x":3.12238,"y":5.898,"heading":-0.98386,"vx":1.7894,"vy":-1.61773,"omega":-0.05034,"ax":-6.68441,"ay":6.04311,"alpha":0.18745,"fx":[-112.8847,-114.18583,-114.52049,-113.2085],"fy":[103.69283,102.25952,101.8796,103.33424]},{"t":0.80273,"x":3.19553,"y":5.83187,"heading":-0.9861,"vx":1.49131,"vy":-1.34823,"omega":-0.04198,"ax":-6.68605,"ay":6.04459,"alpha":0.18781,"fx":[-112.9096,-114.21277,-114.55142,-113.23743],"fy":[103.72112,102.28545,101.90176,103.35884]},{"t":0.84733,"x":3.25539,"y":5.77775,"heading":-0.98797,"vx":1.19313,"vy":-1.07866,"omega":-0.0336,"ax":-6.68728,"ay":6.04571,"alpha":0.18809,"fx":[-112.92815,-114.23282,-114.57473,-113.25927],"fy":[103.74246,102.30506,101.91824,103.37713]},{"t":0.89192,"x":3.30195,"y":5.73566,"heading":-0.98947,"vx":0.89491,"vy":-0.80905,"omega":-0.02522,"ax":-6.68824,"ay":6.04657,"alpha":0.18831,"fx":[-112.94254,-114.24838,-114.59289,-113.27629],"fy":[103.7591,102.32034,101.93102,103.39131]},{"t":0.93652,"x":3.33521,"y":5.70559,"heading":-0.9906,"vx":0.59664,"vy":-0.53939,"omega":-0.01682,"ax":-6.689,"ay":6.04726,"alpha":0.18849,"fx":[-112.95408,-114.26089,-114.60739,-113.28983],"fy":[103.77238,102.3325,101.94127,103.40273]},{"t":0.98112,"x":3.35517,"y":5.68755,"heading":-0.99135,"vx":0.29833,"vy":-0.26971,"omega":-0.00841,"ax":-6.68963,"ay":6.04783,"alpha":0.18864,"fx":[-112.9636,-114.27126,-114.61918,-113.30077],"fy":[103.78316,102.34228,101.94973,103.41223]},{"t":1.02571,"x":3.36182,"y":5.68154,"heading":-0.99172,"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/choreo/YSM.traj b/src/main/deploy/choreo/YSM.traj new file mode 100644 index 00000000..886ddc7c --- /dev/null +++ b/src/main/deploy/choreo/YSM.traj @@ -0,0 +1 @@ +{"name":"YSM","version":1,"snapshot":{"waypoints":[{"x":5.902292251586914,"y":5.7291340827941895,"heading":-2.1224515836921896,"intervals":39,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":1.6631046533584597,"y":7.332815170288086,"heading":-0.8652284387671768,"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":17.548,"h":8.052}},"enabled":false}],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"5.902292251586914 m","val":5.902292251586914},"y":{"exp":"5.7291340827941895 m","val":5.7291340827941895},"heading":{"exp":"-2.1224515836921896 rad","val":-2.1224515836921896},"intervals":39,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"1.6631046533584595 m","val":1.6631046533584597},"y":{"exp":"7.332815170288086 m","val":7.332815170288086},"heading":{"exp":"-0.8652284387671768 rad","val":-0.8652284387671768},"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":"17.548 m","val":17.548},"h":{"exp":"8.052 m","val":8.052}}},"enabled":false}],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,1.47869],"samples":[{"t":0.0,"x":5.90229,"y":5.72913,"heading":-2.12245,"vx":0.0,"vy":0.0,"omega":0.0,"ax":-8.25516,"ay":3.14475,"alpha":5.8204,"fx":[-129.82496,-130.13802,-149.10959,-152.5992],"fy":[81.57396,81.20973,36.04776,15.13362]},{"t":0.03792,"x":5.89636,"y":5.73139,"heading":-2.12245,"vx":-0.31299,"vy":0.11923,"omega":0.22068,"ax":-8.2635,"ay":3.14656,"alpha":5.66861,"fx":[-130.30321,-130.45367,-149.01002,-152.47201],"fy":[80.77765,80.6785,36.40782,16.22409]},{"t":0.07583,"x":5.87855,"y":5.73818,"heading":-2.11408,"vx":-0.62631,"vy":0.23853,"omega":0.43561,"ax":-8.27276,"ay":3.14824,"alpha":5.49768,"fx":[-130.69245,-130.92788,-148.94972,-152.29926],"fy":[80.11205,79.87938,36.59501,17.61643]},{"t":0.11375,"x":5.84886,"y":5.74948,"heading":-2.09757,"vx":-0.93997,"vy":0.3579,"omega":0.64405,"ax":-8.28323,"ay":3.15001,"alpha":5.29867,"fx":[-131.03922,-131.56068,-148.91642,-152.06549],"fy":[79.50397,78.80007,36.6592,19.36]},{"t":0.15166,"x":5.80727,"y":5.76532,"heading":-2.07315,"vx":-1.25403,"vy":0.47733,"omega":0.84495,"ax":-8.29524,"ay":3.1521,"alpha":5.05941,"fx":[-131.405,-132.35434,-148.89206,-151.74746],"fy":[78.8513,77.41925,36.6703,21.52442]},{"t":0.18958,"x":5.75376,"y":5.78568,"heading":-2.04111,"vx":-1.56854,"vy":0.59685,"omega":1.03678,"ax":-8.30914,"ay":3.15479,"alpha":4.76329,"fx":[-131.86969,-133.31397,-148.85017,-151.31085],"fy":[78.01539,75.70349,36.72944,24.19994]},{"t":0.22749,"x":5.68831,"y":5.81058,"heading":-2.0018,"vx":-1.88358,"vy":0.71646,"omega":1.21738,"ax":-8.32527,"ay":3.1584,"alpha":4.38725,"fx":[-132.53709,-134.44922,-148.7504,-150.7052],"fy":[76.80596,73.60063,36.98913,27.49799]},{"t":0.26541,"x":5.61091,"y":5.84001,"heading":-1.95565,"vx":-2.19923,"vy":0.83621,"omega":1.38372,"ax":-8.34383,"ay":3.16319,"alpha":3.89754,"fx":[-133.54396,-135.77819,-148.52619,-149.85657],"fy":[74.95052,71.0251,37.69201,31.55211]},{"t":0.30332,"x":5.52153,"y":5.87399,"heading":-1.90318,"vx":-2.51559,"vy":0.95614,"omega":1.53149,"ax":-8.36467,"ay":3.16915,"alpha":3.24047,"fx":[-135.07534,-137.33696,-148.05424,-148.65591],"fy":[72.02908,67.82311,39.2524,36.52098]},{"t":0.34124,"x":5.42014,"y":5.91252,"heading":-1.84512,"vx":-2.83274,"vy":1.0763,"omega":1.65436,"ax":-8.38639,"ay":3.17543,"alpha":2.31892,"fx":[-137.38944,-139.20352,-147.06814,-146.93897],"fy":[67.32486,63.67963,42.44948,42.59886]},{"t":0.37915,"x":5.30671,"y":5.95561,"heading":-1.78239,"vx":-3.15071,"vy":1.1967,"omega":1.74228,"ax":-8.40281,"ay":3.17871,"alpha":0.92423,"fx":[-140.84505,-141.56399,-144.86304,-144.4452],"fy":[59.44774,57.82254,48.95685,50.04843]},{"t":0.41707,"x":5.18121,"y":6.00327,"heading":-1.71633,"vx":-3.4693,"vy":1.31722,"omega":1.77732,"ax":-8.38357,"ay":3.16592,"alpha":-1.50794,"fx":[-145.81761,-144.92012,-138.95651,-140.71442],"fy":[45.2617,47.75491,63.09224,59.29694]},{"t":0.45498,"x":5.04365,"y":6.05549,"heading":-1.64895,"vx":-3.78716,"vy":1.43725,"omega":1.72015,"ax":-8.11734,"ay":3.02006,"alpha":-7.20784,"fx":[-151.43302,-150.26401,-115.82202,-134.77567],"fy":[16.50728,19.48917,98.28112,71.20418]},{"t":0.4929,"x":4.89422,"y":6.11215,"heading":-1.58373,"vx":-4.09493,"vy":1.55176,"omega":1.44686,"ax":-6.76709,"ay":1.86554,"alpha":-19.24173,"fx":[-150.23328,-126.83286,-54.6124,-128.74644],"fy":[-20.10665,-73.71685,140.25677,80.49593]},{"t":0.53081,"x":4.7341,"y":6.17233,"heading":-1.52887,"vx":-4.35151,"vy":1.62249,"omega":0.71731,"ax":-6.46503,"ay":1.98894,"alpha":-18.4094,"fx":[-146.13845,-116.70019,-50.73875,-126.29555],"fy":[-20.72309,-58.33353,136.1039,78.27806]},{"t":0.56873,"x":4.56447,"y":6.23527,"heading":-1.50167,"vx":-4.59663,"vy":1.6979,"omega":0.01932,"ax":0.2002,"ay":0.89044,"alpha":-0.35635,"fx":[2.51225,4.4401,4.29613,2.37282],"fy":[14.13869,14.26206,16.15273,16.03097]},{"t":0.60664,"x":4.39033,"y":6.30029,"heading":-1.50094,"vx":-4.58904,"vy":1.73166,"omega":0.00581,"ax":0.07945,"ay":0.21027,"alpha":-0.0003,"fx":[1.35064,1.35227,1.35216,1.35052],"fy":[3.57581,3.57592,3.57755,3.57744]},{"t":0.64456,"x":4.21639,"y":6.3661,"heading":-1.50072,"vx":-4.58602,"vy":1.73964,"omega":0.0058,"ax":0.01958,"ay":0.05158,"alpha":-0.00002,"fx":[0.33297,0.33305,0.33304,0.33296],"fy":[0.87733,0.87733,0.87741,0.87741]},{"t":0.68247,"x":4.04253,"y":6.43209,"heading":-1.5005,"vx":-4.58528,"vy":1.74159,"omega":0.0058,"ax":0.00451,"ay":0.01188,"alpha":-0.00001,"fx":[0.07675,0.07678,0.07678,0.07675],"fy":[0.20208,0.20208,0.20212,0.20211]},{"t":0.72039,"x":3.86868,"y":6.49813,"heading":-1.50028,"vx":-4.58511,"vy":1.74204,"omega":0.0058,"ax":-0.00015,"ay":-0.0004,"alpha":0.0,"fx":[-0.00262,-0.0026,-0.0026,-0.00262],"fy":[-0.00688,-0.00688,-0.00686,-0.00686]},{"t":0.7583,"x":3.69483,"y":6.56418,"heading":-1.50006,"vx":-4.58512,"vy":1.74203,"omega":0.0058,"ax":-0.00517,"ay":-0.01362,"alpha":0.0,"fx":[-0.08801,-0.08801,-0.08801,-0.08801],"fy":[-0.23169,-0.23169,-0.23169,-0.23169]},{"t":0.79622,"x":3.52099,"y":6.63022,"heading":-1.49984,"vx":-4.58531,"vy":1.74151,"omega":0.0058,"ax":-0.02211,"ay":-0.05827,"alpha":0.00001,"fx":[-0.37611,-0.37616,-0.37616,-0.37611],"fy":[-0.99109,-0.99109,-0.99114,-0.99114]},{"t":0.83413,"x":3.34712,"y":6.69621,"heading":-1.49962,"vx":-4.58615,"vy":1.7393,"omega":0.0058,"ax":-0.08959,"ay":-0.23732,"alpha":0.00042,"fx":[-1.52279,-1.52502,-1.52486,-1.52263],"fy":[-4.03553,-4.03569,-4.03792,-4.03776]},{"t":0.87205,"x":3.17317,"y":6.76198,"heading":-1.4994,"vx":-4.58955,"vy":1.7303,"omega":0.00581,"ax":-0.18753,"ay":-1.01596,"alpha":0.51149,"fx":[-1.90688,-4.68388,-4.46743,-1.70117],"fy":[-15.83675,-16.01912,-18.72387,-18.54486]},{"t":0.90996,"x":2.99902,"y":6.82686,"heading":-1.49918,"vx":-4.59666,"vy":1.69178,"omega":0.02521,"ax":6.54918,"ay":-1.93567,"alpha":18.35861,"fx":[146.59073,118.68571,52.26185,128.06028],"fy":[23.34207,58.74418,-136.96444,-76.82245]},{"t":0.94788,"x":2.82945,"y":6.88961,"heading":-1.49822,"vx":-4.34835,"vy":1.61839,"omega":0.72127,"ax":6.89785,"ay":-1.80641,"alpha":18.34752,"fx":[149.47351,129.83423,58.30807,131.70567],"fy":[25.45203,66.53531,-139.12446,-75.76911]},{"t":0.98579,"x":2.66954,"y":6.94967,"heading":-1.47088,"vx":-4.08681,"vy":1.5499,"omega":1.41692,"ax":8.02843,"ay":-2.95301,"alpha":8.36164,"fx":[152.25956,149.5538,107.48625,136.94586],"fy":[-4.17991,-22.18228,-107.47788,-67.07968]},{"t":1.02371,"x":2.52036,"y":7.00632,"heading":-1.41715,"vx":-3.78242,"vy":1.43794,"omega":1.73395,"ax":8.37046,"ay":-3.16739,"alpha":2.08591,"fx":[147.33254,144.25193,136.49488,141.43712],"fy":[-40.01044,-49.60083,-68.27829,-57.61611]},{"t":1.06162,"x":2.38296,"y":7.05856,"heading":-1.35141,"vx":-3.46505,"vy":1.31785,"omega":1.81304,"ax":8.40275,"ay":-3.18351,"alpha":-0.72885,"fx":[140.95524,142.61025,144.78963,143.35849],"fy":[-59.21438,-55.18291,-49.1282,-53.07671]},{"t":1.09954,"x":2.25763,"y":7.10624,"heading":-1.28267,"vx":-3.14646,"vy":1.19714,"omega":1.7854,"ax":8.38508,"ay":-3.1786,"alpha":-2.31121,"fx":[135.91338,142.3784,148.26031,143.95915],"fy":[-70.32579,-56.24818,-37.97048,-51.7241]},{"t":1.13745,"x":2.14435,"y":7.14934,"heading":-1.21498,"vx":-2.82854,"vy":1.07663,"omega":1.69777,"ax":8.36066,"ay":-3.17034,"alpha":-3.30553,"fx":[132.27256,142.70578,150.06585,143.80579],"fy":[-77.14795,-55.68375,-30.509,-52.36563]},{"t":1.17537,"x":2.04312,"y":7.18788,"heading":-1.15061,"vx":-2.51154,"vy":0.95642,"omega":1.57244,"ax":8.33777,"ay":-3.16311,"alpha":-3.98271,"fx":[129.72299,143.26005,151.12808,143.18091],"fy":[-81.49222,-54.42105,-25.08875,-54.21244]},{"t":1.21328,"x":1.95389,"y":7.22187,"heading":-1.09099,"vx":-2.19542,"vy":0.83649,"omega":1.42144,"ax":8.31746,"ay":-3.1579,"alpha":-4.47763,"fx":[127.96748,143.88712,151.80035,142.2557],"fy":[-84.31526,-52.86939,-20.95554,-56.71965]},{"t":1.2512,"x":1.87663,"y":7.25132,"heading":-1.03709,"vx":-1.88006,"vy":0.71676,"omega":1.25167,"ax":8.29943,"ay":-3.15449,"alpha":-4.86243,"fx":[126.77309,144.50833,152.24592,141.15613],"fy":[-86.17016,-51.2447,-17.71238,-59.50071]},{"t":1.28911,"x":1.81131,"y":7.27623,"heading":-0.98964,"vx":-1.56539,"vy":0.59716,"omega":1.06731,"ax":8.28326,"ay":-3.15233,"alpha":-5.17628,"fx":[125.96469,145.08164,152.55085,139.98627],"fy":[-87.40116,-49.67558,-15.1229,-62.28145]},{"t":1.32703,"x":1.75791,"y":7.2966,"heading":-0.94917,"vx":-1.25133,"vy":0.47764,"omega":0.87105,"ax":8.26874,"ay":-3.1509,"alpha":-5.44006,"fx":[125.41138,145.5843,152.76483,138.83489],"fy":[-88.23548,-48.24544,-13.03095,-64.87148]},{"t":1.36494,"x":1.71641,"y":7.31245,"heading":-0.91614,"vx":-0.93782,"vy":0.35817,"omega":0.66479,"ax":8.25578,"ay":-3.14981,"alpha":-5.66451,"fx":[125.01499,146.00414,152.9184,137.77662],"fy":[-88.83015,-47.01196,-11.32471,-67.14287]},{"t":1.40286,"x":1.68679,"y":7.32376,"heading":-0.89094,"vx":-0.6248,"vy":0.23875,"omega":0.45002,"ax":8.2444,"ay":-3.14889,"alpha":-5.85501,"fx":[124.70159,146.33478,153.03103,136.87222],"fy":[-89.29726,-46.01681,-9.92014,-69.01235]},{"t":1.44077,"x":1.66902,"y":7.33055,"heading":-0.87387,"vx":-0.31222,"vy":0.11936,"omega":0.22803,"ax":8.23461,"ay":-3.14804,"alpha":-6.01427,"fx":[124.41551,146.57274,153.11544,136.16973],"fy":[-89.71829,-45.29117,-8.7533,-70.42604]},{"t":1.47869,"x":1.6631,"y":7.33282,"heading":-0.86523,"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/choreo/finish.traj b/src/main/deploy/choreo/finish.traj new file mode 100644 index 00000000..36f5b343 --- /dev/null +++ b/src/main/deploy/choreo/finish.traj @@ -0,0 +1 @@ +{"name":"finish","version":1,"snapshot":{"waypoints":[{"x":7.170319080352783,"y":6.300989151000977,"heading":-2.192828477365389,"intervals":18,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":5.815270900726318,"y":5.7291340827941895,"heading":-2.166995300595666,"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":17.548,"h":8.052}},"enabled":false}],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"7.170319080352783 m","val":7.170319080352783},"y":{"exp":"6.300989151000977 m","val":6.300989151000977},"heading":{"exp":"-2.1928284773653886 rad","val":-2.192828477365389},"intervals":18,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"5.815270900726318 m","val":5.815270900726318},"y":{"exp":"5.7291340827941895 m","val":5.7291340827941895},"heading":{"exp":"-2.166995300595666 rad","val":-2.166995300595666},"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":"17.548 m","val":17.548},"h":{"exp":"8.052 m","val":8.052}}},"enabled":false}],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,0.80803],"samples":[{"t":0.0,"x":7.17032,"y":6.30099,"heading":-2.19283,"vx":0.0,"vy":0.0,"omega":0.0,"ax":-8.30859,"ay":-3.50638,"alpha":0.15836,"fx":[-141.21755,-141.77941,-141.43798,-140.87175],"fy":[-59.90354,-58.56355,-59.38689,-60.71576]},{"t":0.04489,"x":7.16195,"y":6.29746,"heading":-2.19283,"vx":-0.37298,"vy":-0.1574,"omega":0.00711,"ax":-8.30754,"ay":-3.50593,"alpha":0.15834,"fx":[-141.19968,-141.7615,-141.42032,-140.85414],"fy":[-59.89591,-58.55632,-59.37952,-60.708]},{"t":0.08978,"x":7.13683,"y":6.28686,"heading":-2.19251,"vx":-0.7459,"vy":-0.31478,"omega":0.01422,"ax":-8.30618,"ay":-3.50536,"alpha":0.15831,"fx":[-141.17646,-141.73811,-141.39709,-140.83107],"fy":[-59.8856,-58.54678,-59.37022,-60.69794]},{"t":0.13467,"x":7.09498,"y":6.2692,"heading":-2.19187,"vx":-1.11877,"vy":-0.47214,"omega":0.02132,"ax":-8.30431,"ay":-3.50457,"alpha":0.15828,"fx":[-141.14489,-141.70626,-141.36533,-140.79958],"fy":[-59.87133,-58.5337,-59.35775,-60.68431]},{"t":0.17956,"x":7.03639,"y":6.24447,"heading":-2.19091,"vx":-1.49155,"vy":-0.62946,"omega":0.02843,"ax":-8.30163,"ay":-3.50344,"alpha":0.15823,"fx":[-141.0993,-141.66027,-141.31944,-140.75409],"fy":[-59.85069,-58.51478,-59.33976,-60.66465]},{"t":0.22445,"x":6.96107,"y":6.21268,"heading":-2.18964,"vx":-1.86422,"vy":-0.78673,"omega":0.03553,"ax":-8.2974,"ay":-3.50165,"alpha":0.15817,"fx":[-141.02759,-141.58803,-141.24747,-140.68263],"fy":[-59.81853,-58.48503,-59.31125,-60.63378]},{"t":0.26934,"x":6.86903,"y":6.17384,"heading":-2.18804,"vx":-2.23669,"vy":-0.94392,"omega":0.04263,"ax":-8.2898,"ay":-3.49845,"alpha":0.15809,"fx":[-140.89823,-141.45806,-141.11832,-140.55408],"fy":[-59.76147,-58.43156,-59.25918,-60.5782]},{"t":0.31423,"x":6.76027,"y":6.12794,"heading":-2.18613,"vx":-2.60882,"vy":-1.10097,"omega":0.04973,"ax":-8.2721,"ay":-3.49098,"alpha":0.15796,"fx":[-140.59595,-141.15523,-140.81824,-140.25458],"fy":[-59.63058,-58.30696,-59.1358,-60.44866]},{"t":0.35912,"x":6.63482,"y":6.075,"heading":-2.1839,"vx":-2.98016,"vy":-1.25768,"omega":0.05682,"ax":-8.18411,"ay":-3.45385,"alpha":0.15766,"fx":[-139.09038,-139.65077,-139.33055,-138.766],"fy":[-58.9887,-57.68709,-58.51432,-59.80557]},{"t":0.40401,"x":6.49279,"y":6.01506,"heading":-2.18135,"vx":-3.34755,"vy":-1.41273,"omega":0.0639,"ax":8.18411,"ay":3.45385,"alpha":-0.15766,"fx":[139.09152,139.65107,139.32943,138.76567],"fy":[58.98598,57.68649,58.517,59.8062]},{"t":0.4489,"x":6.35077,"y":5.95512,"heading":-2.17848,"vx":-2.98016,"vy":-1.25768,"omega":0.05682,"ax":8.2721,"ay":3.49098,"alpha":-0.15796,"fx":[140.59944,141.15603,140.81483,140.2537],"fy":[59.62232,58.30515,59.14395,60.45057]},{"t":0.49379,"x":6.22532,"y":5.90218,"heading":-2.17593,"vx":-2.60882,"vy":-1.10097,"omega":0.04973,"ax":8.2898,"ay":3.49845,"alpha":-0.15809,"fx":[140.90378,141.4593,141.1129,140.55272],"fy":[59.74836,58.42868,59.27212,60.58125]},{"t":0.53868,"x":6.11656,"y":5.85629,"heading":-2.17369,"vx":-2.23669,"vy":-0.94392,"omega":0.04263,"ax":8.2974,"ay":3.50165,"alpha":-0.15817,"fx":[141.0349,141.58964,141.24032,140.68085],"fy":[59.80126,58.48124,59.3283,60.6378]},{"t":0.58357,"x":6.02452,"y":5.81744,"heading":-2.17178,"vx":-1.86422,"vy":-0.78673,"omega":0.03553,"ax":8.30163,"ay":3.50344,"alpha":-0.15823,"fx":[141.10809,141.66219,141.31085,140.75197],"fy":[59.82994,58.51022,59.36024,60.66948]},{"t":0.62846,"x":5.9492,"y":5.78565,"heading":-2.17019,"vx":-1.49155,"vy":-0.62946,"omega":0.02843,"ax":8.30431,"ay":3.50457,"alpha":-0.15828,"fx":[141.15486,141.70843,141.35558,140.79719],"fy":[59.8478,58.52854,59.38097,60.68977]},{"t":0.67336,"x":5.89061,"y":5.76093,"heading":-2.16891,"vx":-1.11877,"vy":-0.47214,"omega":0.02132,"ax":8.30618,"ay":3.50536,"alpha":-0.15831,"fx":[141.18731,141.74047,141.38648,140.82847],"fy":[59.85999,58.54117,59.39551,60.70389]},{"t":0.71825,"x":5.84876,"y":5.74327,"heading":-2.16795,"vx":-0.7459,"vy":-0.31478,"omega":0.01422,"ax":8.30754,"ay":3.50593,"alpha":-0.15834,"fx":[141.21113,141.76397,141.40913,140.8514],"fy":[59.8689,58.55041,59.40619,60.71426]},{"t":0.76314,"x":5.82364,"y":5.73267,"heading":-2.16731,"vx":-0.37298,"vy":-0.1574,"omega":0.00711,"ax":8.30859,"ay":3.50638,"alpha":-0.15836,"fx":[141.2293,141.78194,141.4265,140.86895],"fy":[59.87582,58.55749,59.41425,60.72218]},{"t":0.80803,"x":5.81527,"y":5.72913,"heading":-2.167,"vx":0.0,"vy":0.0,"omega":0.0,"ax":0.0,"ay":0.0,"alpha":0.0,"fx":[0.0,0.0,0.0,0.0],"fy":[0.0,0.0,0.0,0.0]}],"splits":[0]},"events":[]} diff --git a/src/main/deploy/choreo/station.traj b/src/main/deploy/choreo/station.traj new file mode 100644 index 00000000..9b1ad7c1 --- /dev/null +++ b/src/main/deploy/choreo/station.traj @@ -0,0 +1 @@ +{"name":"station","version":1,"snapshot":{"waypoints":[{"x":1.6845420598983765,"y":7.261595726013184,"heading":-0.750929447106067,"intervals":24,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":3.45690655708313,"y":5.901409149169922,"heading":-0.9827935961184284,"intervals":40,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false}],"constraints":[],"targetDt":0.05},"params":{"waypoints":[{"x":{"exp":"1.6631046533584595 m","val":1.6631046533584597},"y":{"exp":"7.332815170288086 m","val":7.332815170288086},"heading":{"exp":"-0.8652284387671768 rad","val":-0.8652284387671768},"intervals":24,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false},{"x":{"exp":"3.45690655708313 m","val":3.45690655708313},"y":{"exp":"5.901409149169922 m","val":5.901409149169922},"heading":{"exp":"-0.9827935961184285 rad","val":-0.9827935961184284},"intervals":40,"split":false,"fixTranslation":true,"fixHeading":true,"overrideIntervals":false}],"constraints":[],"targetDt":{"exp":"0.05 s","val":0.05}},"trajectory":{"sampleType":"Swerve","waypoints":[0.0,0.46961],"samples":[{"t":0.0,"x":1.68454,"y":7.2616,"heading":-0.75093,"vx":3.2397,"vy":-2.74661,"omega":-2.14266,"ax":4.54318,"ay":-2.97116,"alpha":19.58424,"fx":[109.77684,-3.80227,72.35577,130.7822],"fy":[49.09411,-102.39101,-112.88162,-35.97556]},{"t":0.01957,"x":1.7488,"y":7.20728,"heading":-0.79285,"vx":3.3286,"vy":-2.80475,"omega":-1.75946,"ax":4.74628,"ay":-2.39058,"alpha":17.91174,"fx":[106.01325,12.65726,75.53333,128.7278],"fy":[51.71764,-80.20482,-104.35448,-29.8106]},{"t":0.03913,"x":1.81484,"y":7.15195,"heading":-0.82728,"vx":3.42147,"vy":-2.85153,"omega":-1.40898,"ax":4.82928,"ay":-1.78374,"alpha":16.08097,"fx":[101.02538,26.03467,76.72701,124.79194],"fy":[51.34361,-53.59105,-94.49599,-24.62033]},{"t":0.0587,"x":1.88271,"y":7.09581,"heading":-0.85485,"vx":3.51596,"vy":-2.88643,"omega":-1.09433,"ax":4.71451,"ay":-1.23289,"alpha":14.14762,"fx":[93.97547,33.36813,75.38494,118.04105],"fy":[47.88665,-29.67058,-82.17329,-19.92694]},{"t":0.07827,"x":1.95241,"y":7.03909,"heading":-0.87626,"vx":3.60821,"vy":-2.91055,"omega":-0.8175,"ax":4.31801,"ay":-0.74479,"alpha":11.89371,"fx":[82.95695,34.10857,70.07367,106.65334],"fy":[41.80918,-12.24173,-65.60606,-14.63577]},{"t":0.09783,"x":2.02384,"y":6.982,"heading":-0.89226,"vx":3.6927,"vy":-2.92513,"omega":-0.58478,"ax":3.54335,"ay":-0.26671,"alpha":8.96337,"fx":[65.86344,29.38287,58.24853,87.59065],"fy":[33.66813,-0.69022,-43.44574,-7.67856]},{"t":0.1174,"x":2.09678,"y":6.92471,"heading":-0.9037,"vx":3.76203,"vy":-2.93034,"omega":-0.40939,"ax":2.424,"ay":0.15774,"alpha":5.47012,"fx":[43.88426,21.42357,39.7421,59.87657],"fy":[24.44342,5.77886,-19.58295,0.09288]},{"t":0.13697,"x":2.17085,"y":6.86741,"heading":-0.91171,"vx":3.80946,"vy":-2.92726,"omega":-0.30236,"ax":1.37264,"ay":0.35913,"alpha":2.61836,"fx":[24.53706,13.59921,22.32683,32.93],"fy":[16.11235,7.56053,-3.99939,4.7614]},{"t":0.15654,"x":2.24565,"y":6.8102,"heading":-0.91763,"vx":3.83632,"vy":-2.92023,"omega":-0.25112,"ax":0.71896,"ay":0.34538,"alpha":1.10638,"fx":[12.76252,8.07515,11.71121,16.36807],"fy":[10.04832,6.46338,1.68774,5.29965]},{"t":0.1761,"x":2.32086,"y":6.75312,"heading":-0.92254,"vx":3.85039,"vy":-2.91347,"omega":-0.22947,"ax":0.38235,"ay":0.25657,"alpha":0.47317,"fx":[6.74544,4.72479,6.26335,8.28122],"fy":[6.14392,4.61511,2.58283,4.11491]},{"t":0.19567,"x":2.39627,"y":6.69616,"heading":-0.92703,"vx":3.85787,"vy":-2.90845,"omega":-0.22021,"ax":0.21323,"ay":0.17147,"alpha":0.22535,"fx":[3.74711,2.7801,3.50716,4.47383],"fy":[3.76355,3.03835,2.06959,2.79526]},{"t":0.21524,"x":2.4718,"y":6.63929,"heading":-0.93134,"vx":3.86204,"vy":-2.9051,"omega":-0.21581,"ax":0.12563,"ay":0.10996,"alpha":0.12681,"fx":[2.20673,1.66057,2.06704,2.61314],"fy":[2.34663,1.94057,1.39406,1.80022]},{"t":0.2348,"x":2.54739,"y":6.58246,"heading":-0.93556,"vx":3.8645,"vy":-2.90295,"omega":-0.21332,"ax":0.07859,"ay":0.07019,"alpha":0.08524,"fx":[1.38509,1.01676,1.28835,1.65666],"fy":[1.51384,1.24236,0.87395,1.14545]},{"t":0.25437,"x":2.62302,"y":6.52568,"heading":-0.93974,"vx":3.86604,"vy":-2.90157,"omega":-0.21166,"ax":0.05273,"ay":0.04595,"alpha":0.06624,"fx":[0.93549,0.64837,0.85821,1.14532],"fy":[1.03009,0.82029,0.53314,0.74295]},{"t":0.27394,"x":2.69868,"y":6.46891,"heading":-0.94388,"vx":3.86707,"vy":-2.90067,"omega":-0.21036,"ax":0.03852,"ay":0.03189,"alpha":0.05688,"fx":[0.68934,0.44203,0.62119,0.86849],"fy":[0.75565,0.5765,0.32918,0.50833]},{"t":0.2935,"x":2.77435,"y":6.41216,"heading":-0.948,"vx":3.86783,"vy":-2.90005,"omega":-0.20925,"ax":0.03119,"ay":0.0244,"alpha":0.05223,"fx":[0.56269,0.33493,0.4985,0.72625],"fy":[0.61067,0.44712,0.21935,0.38292]},{"t":0.31307,"x":2.85004,"y":6.35542,"heading":-0.95209,"vx":3.86844,"vy":-2.89957,"omega":-0.20822,"ax":0.0285,"ay":0.02136,"alpha":0.05071,"fx":[0.51675,0.29499,0.45288,0.67463],"fy":[0.55319,0.39531,0.17355,0.33143]},{"t":0.33264,"x":2.92574,"y":6.29869,"heading":-0.95617,"vx":3.86899,"vy":-2.89915,"omega":-0.20723,"ax":0.02996,"ay":0.02166,"alpha":0.05307,"fx":[0.54392,0.31117,0.47545,0.7082],"fy":[0.56688,0.4026,0.16984,0.33413]},{"t":0.35221,"x":3.00145,"y":6.24196,"heading":-0.96022,"vx":3.86958,"vy":-2.89873,"omega":-0.20619,"ax":0.03709,"ay":0.02445,"alpha":0.06408,"fx":[0.67319,0.39131,0.58857,0.87044],"fy":[0.65554,0.45829,0.1764,0.37365]},{"t":0.37177,"x":3.07717,"y":6.18525,"heading":-0.96425,"vx":3.87031,"vy":-2.89825,"omega":-0.20494,"ax":0.05536,"ay":0.02755,"alpha":0.09981,"fx":[1.00914,0.56888,0.87435,1.31459],"fy":[0.84151,0.53608,0.09577,0.40123]},{"t":0.39134,"x":3.15291,"y":6.12854,"heading":-0.96826,"vx":3.87139,"vy":-2.89771,"omega":-0.20299,"ax":0.10105,"ay":0.02187,"alpha":0.21197,"fx":[1.86517,0.92765,1.57263,2.50998],"fy":[1.16337,0.51848,-0.41931,0.2257]},{"t":0.41091,"x":3.22868,"y":6.07185,"heading":-0.97224,"vx":3.87337,"vy":-2.89728,"omega":-0.19884,"ax":0.22387,"ay":-0.02775,"alpha":0.57076,"fx":[4.21124,1.67954,3.40613,5.93487],"fy":[1.65822,-0.06937,-2.60251,-0.87416]},{"t":0.43047,"x":3.30452,"y":6.01515,"heading":-0.97613,"vx":3.87775,"vy":-2.89783,"omega":-0.18767,"ax":0.58909,"ay":-0.25465,"alpha":1.77082,"fx":[11.33514,3.42639,8.74658,16.57264],"fy":[2.29714,-3.07764,-10.95861,-5.58667]},{"t":0.45004,"x":3.38051,"y":5.9584,"heading":-0.9798,"vx":3.88927,"vy":-2.90281,"omega":-0.15302,"ax":1.56033,"ay":-1.0059,"alpha":7.66204,"fx":[34.21352,-2.42604,20.96945,53.40642],"fy":[12.33925,-12.79616,-45.99029,-21.99323]},{"t":0.46961,"x":3.45691,"y":5.90141,"heading":-0.98279,"vx":3.9198,"vy":-2.92249,"omega":-0.0031,"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/CHOREO TEST.auto b/src/main/deploy/pathplanner/autos/CHOREO TEST.auto new file mode 100644 index 00000000..dc6cd4fc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/CHOREO TEST.auto @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Low to E" + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "E to Station" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "isCollected" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Station to D" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto new file mode 100644 index 00000000..6a60d7af --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New New Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Low to E" + } + }, + { + "type": "path", + "data": { + "pathName": "E to Station" + } + }, + { + "type": "path", + "data": { + "pathName": "Station to D" + } + }, + { + "type": "path", + "data": { + "pathName": "D to Station" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C to RSF.path b/src/main/deploy/pathplanner/paths/C to RSF.path index 0f2dc862..ee10bfb7 100644 --- a/src/main/deploy/pathplanner/paths/C to RSF.path +++ b/src/main/deploy/pathplanner/paths/C to RSF.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0.0, + "velocity": 1.0, "rotation": 54.0 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/OSM to F.path b/src/main/deploy/pathplanner/paths/OSM to F.path index a0ca4648..88f77d8a 100644 --- a/src/main/deploy/pathplanner/paths/OSM to F.path +++ b/src/main/deploy/pathplanner/paths/OSM to F.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.649657534246575, - "y": 2.319584760273973 + "x": 5.676809210526316, + "y": 2.1487664473684207 }, "prevControl": { - "x": 6.433115085082489, - "y": 2.073535689064093 + "x": 6.46026676136223, + "y": 1.9027173761585408 }, "nextControl": null, "isLocked": false, @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 119.99999999999999 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/RSF to E.path b/src/main/deploy/pathplanner/paths/RSF to E.path index b70669ff..7028816e 100644 --- a/src/main/deploy/pathplanner/paths/RSF to E.path +++ b/src/main/deploy/pathplanner/paths/RSF to E.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 1.0, "rotation": 119.99999999999999 }, "reversed": false, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 794d2e2f..13b2edb6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,6 +5,7 @@ package frc.robot; import au.grapplerobotics.CanBridge; +import choreo.auto.AutoFactory; import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.DataLogManager; @@ -42,6 +43,8 @@ public static Robot getInstance() { public final Sensors sensors; public final SuperStructure superStructure; private final PowerDistribution PDH; + public AutoFactory autoFactory; + private String ROUTINE = ""; protected Robot() { // non public for singleton. Protected so test class can subclass @@ -55,6 +58,7 @@ protected Robot() { sensors = new Sensors(); subsystems = new Subsystems(sensors); + autoFactory = subsystems.drivebaseSubsystem.createAutoFactory(); if (SubsystemConstants.DRIVEBASE_ENABLED) { AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem); } @@ -146,9 +150,11 @@ public void autonomousInit() { Shuffleboard.startRecording(); if (SubsystemConstants.DRIVEBASE_ENABLED && AutoLogic.getSelectedAuto() != null) { AutoLogic.getSelectedAuto().schedule(); - } - if (subsystems.climbPivotSubsystem != null) { - subsystems.climbPivotSubsystem.moveCompleteFalse(); + // new Routines().runRoutine2().schedule(); + + if (subsystems.climbPivotSubsystem != null) { + subsystems.climbPivotSubsystem.moveCompleteFalse(); + } } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemSim.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemSim.java new file mode 100644 index 00000000..c184bb7e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemSim.java @@ -0,0 +1,115 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.TalonFXSimState; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; + +/** Simulation logic for the ElevatorSubsystem. */ +public class ElevatorSubsystemSim { + // Simulation PID constants - tuned for smooth motion + private static final double SIM_KP = 50.0; + private static final double SIM_KD = 5.0; + private static final double MOTOR_ROTATIONS_PER_METER = 19.68; + + private final ElevatorSim m_elevatorSim; + private final TalonFXSimState m_motorOneSimState; + private final TalonFXSimState m_motorTwoSimState; + private final StructPublisher m_posePublisher; + public DoubleTopic arm_Pivot = NetworkTableInstance.getDefault().getDoubleTopic("Arm Pivot"); + private double simTargetHeightMeters = 0.0; + private double simPreviousError = 0.0; + final DoubleSubscriber dblSub; + + /** + * Constructor for the elevator simulation. Creates all simulation objects internally. + * + * @param motor1 First elevator motor (leader) + * @param motor2 Second elevator motor (follower) + * @param posePublisher Publisher for visualization pose + */ + public ElevatorSubsystemSim( + TalonFX motor1, TalonFX motor2, StructPublisher posePublisher) { + // Get sim states from motors + this.m_motorOneSimState = motor1.getSimState(); + this.m_motorTwoSimState = motor2.getSimState(); + this.m_posePublisher = posePublisher; + dblSub = arm_Pivot.subscribe(0.0); + // Initialize elevator simulation + // Elevator specs: ~38 rotations = 1.93 meters (38 / 19.68) + this.m_elevatorSim = + new ElevatorSim( + DCMotor.getFalcon500(2), // 2 Falcon 500 motors + 1.0, // Gearing ratio (adjust based on actual mechanism) + 5.0, // Carriage mass in kg (adjust based on actual mass) + 0.02, // Drum radius in meters (adjust for your spool/pulley) + 0.0, // Min height in meters + 1.93, // Max height in meters (~38 rotations / 19.68) + false, // Simulate gravity + 0.0 // Starting height in meters + ); + } + + /** + * Set the target position for the simulated elevator. + * + * @param targetPositionRotations Target position in motor rotations + */ + public void setTargetPosition(double targetPositionRotations) { + simTargetHeightMeters = targetPositionRotations / MOTOR_ROTATIONS_PER_METER; + } + + /** + * Update the simulation. Should be called periodically. + * + * @param currentPositionRotations Current position reading from motor (for visualization) + */ + public void updateSimulation(double currentPositionRotations) { + // Calculate PD control to smoothly move to target position + double currentHeightMeters = m_elevatorSim.getPositionMeters(); + double error = simTargetHeightMeters - currentHeightMeters; + double errorRate = (error - simPreviousError) / 0.02; // derivative + + // PD control output + double pidOutput = (SIM_KP * error) + (SIM_KD * errorRate); + simPreviousError = error; + + // Apply voltage to simulation + m_elevatorSim.setInputVoltage(pidOutput); + m_elevatorSim.update(0.02); // 20ms periodic cycle + + // Get simulated position and convert to motor rotations + double simHeightMeters = m_elevatorSim.getPositionMeters(); + double simPositionRotations = simHeightMeters * MOTOR_ROTATIONS_PER_METER; + + // Update motor sim states with realistic position + m_motorOneSimState.setRawRotorPosition(simPositionRotations); + m_motorTwoSimState.setRawRotorPosition(simPositionRotations); + + // Update visualization + updateVisualization(currentPositionRotations); + } + + /** + * Update the 3D pose visualization for the elevator. + * + * @param currentPositionRotations Current position in motor rotations + */ + private void updateVisualization(double currentPositionRotations) { + double bottomZ = 0.2; + double topZ = 1.55; + double minPos = 0.0; + double maxPos = 37.5; + double targetZ = + (bottomZ + ((currentPositionRotations - minPos) / (maxPos - minPos)) * (topZ - bottomZ)); + double wristPos = dblSub.get(); + // System.out.println("Wrist pose is:" + wristPos); + m_posePublisher.set(new Pose3d(0.2, 0.0, targetZ, new Rotation3d(-90, wristPos, -0))); + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 93df784a..6d9c59a7 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -182,16 +182,16 @@ public static boolean poseInPlace(AlignType type) { private static final Pose2d rRedReefFaceKL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); - private static class AutoAlignCommand extends Command { + public static class AutoAlignCommand extends Command { - protected final PIDController pidX = new PIDController(4, 0, 0); - protected final PIDController pidY = new PIDController(4, 0, 0); - protected final PIDController pidRotate = new PIDController(8, 0, 0); + public static PIDController pidX = new PIDController(4, 0, 0); + public static PIDController pidY = new PIDController(4, 0, 0); + public static PIDController pidRotate = new PIDController(8, 0, 0); - protected final CommandSwerveDrivetrain drive; + public final CommandSwerveDrivetrain drive; protected final Controls controls; - protected Pose2d branchPose; - protected AlignType type; + public Pose2d branchPose; + public AlignType type; private final SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric() // Add a 10% deadband @@ -264,7 +264,7 @@ public static Pose2d getTargetPose(Pose2d pose, AlignType type) { }; } - private static Pose2d getNearestBranch(Pose2d p) { + public static Pose2d getNearestBranch(Pose2d p) { List branchPose2ds = AllianceUtils.isBlue() ? List.of( diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index f65e48a9..ae441884 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -50,7 +50,7 @@ public static enum StartPosition { MISC("Misc", null); final String title; // for shuffleboard display - final Pose2d startPose; // for identifying path's starting positions for filtering + public final Pose2d startPose; // for identifying path's starting positions for filtering StartPosition(String title, Pose2d startPose) { this.title = title; @@ -111,7 +111,8 @@ public static enum StartPosition { new AutoPath("MRSF_G-F_WithWait", "MRSF_G-F_WithWait"), new AutoPath("MRSF_G-H", "MRSF_G-H"), new AutoPath("MLSF_H-K_Cooking", "MLSF_H-K_Cooking"), - new AutoPath("MLSF_H-G", "MLSF_H-G")); + new AutoPath("MLSF_H-G", "MLSF_H-G"), + new AutoPath("CHOREO TEST", "CHOREO TEST")); private static List threePiecePaths = List.of( @@ -166,13 +167,13 @@ public static enum StartPosition { new InstantCommand(() -> controls.vibrateDriveController(0.0))); // shuffleboard - private static ShuffleboardTab tab = Shuffleboard.getTab("Autos"); + public static ShuffleboardTab tab = Shuffleboard.getTab("Autos"); - private static SendableChooser startPositionChooser = + public static SendableChooser startPositionChooser = new SendableChooser(); - private static DynamicSendableChooser availableAutos = + public static DynamicSendableChooser availableAutos = new DynamicSendableChooser(); - private static SendableChooser gameObjects = new SendableChooser(); + public static SendableChooser gameObjects = new SendableChooser(); private static SendableChooser isVision = new SendableChooser(); private static GenericEntry autoDelayEntry; @@ -182,7 +183,7 @@ public static void registerCommands() { // param: String commandName, Command command // Intake - NamedCommands.registerCommand("scoreCommand", scoreCommand()); + NamedCommands.registerCommand("scoreCommand", scoreTestCommand()); NamedCommands.registerCommand("intake", intakeCommand()); NamedCommands.registerCommand("isCollected", isCollected()); NamedCommands.registerCommand("readyIntake", readyIntakeCommand()); @@ -206,11 +207,29 @@ public static Command getAutoCommand(String pathName) throws FileVersionException, IOException, ParseException { // Load the path you want to follow using its name in the GUI PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - // Create a path following command using AutoBuilder. This will also trigger event markers. return AutoBuilder.followPath(path); } + public static Command testAuto() throws FileVersionException, IOException, ParseException { + + return Commands.sequence( + AutoBuilder.resetOdom(getResetPose("OSM to F")) + .andThen(getAutoCommand("OSM to F")) + .andThen(scoreCommand()) + .andThen(Commands.parallel(getAutoCommand("F to RSF")).alongWith(readyIntakeCommand())) + .andThen(isCollected()) + .andThen(Commands.sequence(getAutoCommand("RSF to E")).alongWith(intakeCommand())) + .andThen(scoreCommand()) + .andThen(readyIntakeCommand())); + } + + public static Pose2d getResetPose(String path) + throws FileVersionException, IOException, ParseException { + PathPlannerPath planner = PathPlannerPath.fromPathFile(path); + return planner.getStartingDifferentialPose(); + } + public static void initShuffleBoard() { startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); for (StartPosition startPosition : StartPosition.values()) { @@ -303,6 +322,23 @@ public static Command scoreCommand() { .withName("scoreCommand-noSuperstructure"); } + public static Command scoreTestCommand() { + if (r.superStructure != null) { + return new ConditionalCommand( + // If true: + AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) + .repeatedly() + .withDeadline(r.superStructure.coralLevelOne(() -> AutoAlign.readyToScore())) + .withName("scoreCommand"), + // If false: + Commands.none().withName("scoreCommand-empty"), + // Condition: + () -> ARMSENSOR_ENABLED && r.sensors.armSensor.booleanInClaw()); + } + return AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) + .withName("scoreCommand-noSuperstructure"); + } + public static Command algaeCommand23() { if (r.superStructure != null) { return AlgaeAlign.algaeAlign(s.drivebaseSubsystem, controls) diff --git a/src/main/java/frc/robot/subsystems/auto/Routines.java b/src/main/java/frc/robot/subsystems/auto/Routines.java new file mode 100644 index 00000000..f7c65c3c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/Routines.java @@ -0,0 +1,90 @@ +package frc.robot.subsystems.auto; + +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Robot; + +public class Routines { + + public Routines() {} + + private AutoFactory autoFactory = Robot.getInstance().autoFactory; + + public Command runRoutine() { + AutoRoutine routine = autoFactory.newRoutine("I_K_Far_Right_Blue"); + AutoTrajectory preloadScore = routine.trajectory("finish"); + AutoTrajectory coralStation = routine.trajectory("YSM"); + AutoTrajectory secondScore = routine.trajectory("station"); + AutoTrajectory secondStation = routine.trajectory("K to Station"); + AutoTrajectory thirdScore = routine.trajectory("Station to L"); + AutoTrajectory thirdStation = routine.trajectory("L to Station"); + AutoTrajectory fourthScore = routine.trajectory("Station to A"); + routine + .active() + .onTrue( + Commands.sequence( + preloadScore.resetOdometry(), + preloadScore.cmd(), + Commands.print("preloadScore done"), + AutoLogic.scoreTestCommand(), + Commands.print("Scoring done"), + coralStation.cmd().alongWith(AutoLogic.readyIntakeCommand()), + Commands.print("coralStation and readying intake done"), + AutoLogic.isCollected(), + Commands.print("collection done"), + secondScore.cmd().alongWith(AutoLogic.intakeCommand()), + Commands.print("secondScore and intaking done"), + AutoLogic.scoreTestCommand(), + Commands.print("scoring done!"), + secondStation.cmd().alongWith(AutoLogic.readyIntakeCommand()), + Commands.print("secondStation and readied intake done"), + AutoLogic.isCollected(), + Commands.print("second collection done"), + thirdScore.cmd().alongWith(AutoLogic.intakeCommand()), + Commands.print("third transferred to intake"), + AutoLogic.scoreTestCommand(), + Commands.print("thirdscore done"), + thirdStation.cmd().alongWith(AutoLogic.readyIntakeCommand()), + Commands.print("third statino and readied intake"), + AutoLogic.isCollected(), + Commands.print("third collection done"), + fourthScore.cmd().alongWith(AutoLogic.intakeCommand()), + Commands.print("fourth in intake"), + AutoLogic.scoreTestCommand(), + Commands.print("fourthscore done"), + Commands.print("ROUTINE DONE!"))); + + return routine.cmd(); + } + + public Command runRoutine2() { + AutoRoutine routine = autoFactory.newRoutine("TRY"); + AutoTrajectory preloadScore = routine.trajectory("Low to E"); + AutoTrajectory coralStation = routine.trajectory("E to Station"); + AutoTrajectory secondScore = routine.trajectory("Station to D"); + AutoTrajectory secondStation = routine.trajectory("D to Station"); + routine + .active() + .onTrue( + Commands.sequence( + preloadScore.resetOdometry(), + preloadScore.cmd(), + Commands.print("preloadScore done"), + AutoLogic.scoreTestCommand(), + Commands.print("Scoring done"), + coralStation.cmd().alongWith(AutoLogic.readyIntakeCommand()), + Commands.print("coralStation and readying intake done"), + AutoLogic.isCollected(), + Commands.print("collection done"), + secondScore.cmd().alongWith(AutoLogic.intakeCommand()), + Commands.print("secondScore and intaking done"), + AutoLogic.scoreTestCommand(), + Commands.print("scoring done!"), + secondStation.cmd().alongWith(AutoLogic.readyIntakeCommand()))); + + return routine.cmd(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java index d4427c06..ebdc1b44 100644 --- a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.*; +import choreo.auto.AutoFactory; +import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -9,6 +11,8 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.PIDController; +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.numbers.N1; @@ -20,6 +24,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.subsystems.auto.AutoAlign; import java.util.function.Supplier; /** @@ -45,6 +50,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + /*AutoAlign PID used in constructor for following choreo trajectories */ + private PIDController pidX = AutoAlign.AutoAlignCommand.pidX; + private PIDController pidY = AutoAlign.AutoAlignCommand.pidY; + private PIDController pidRotate = AutoAlign.AutoAlignCommand.pidRotate; /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = @@ -178,6 +187,28 @@ public Command applyRequest(Supplier requestSupplier) { return run(() -> this.setControl(requestSupplier.get())).withName("Drivebase applyRequest()"); } + public void followTrajectory(SwerveSample sample) { + // Get the current pose of the robot + Pose2d pose = this.getState().Pose; + // Pose2d branchPose = AutoAlignCommand.getNearestBranch(pose); + + // Generate the next speeds for the robot + + ChassisSpeeds speeds = + new ChassisSpeeds( + sample.vx + pidX.calculate(pose.getX(), sample.x), + sample.vy + pidY.calculate(pose.getY(), sample.y), + sample.omega + pidRotate.calculate(pose.getRotation().getRadians(), sample.heading)); + + // Apply the generated speeds + this.setControl(new SwerveRequest.ApplyFieldSpeeds().withSpeeds(speeds)); + } + + public SwerveRequest fieldRelative(ChassisSpeeds speeds) { + SwerveRequest req = new SwerveRequest.ApplyFieldSpeeds().withSpeeds(speeds); + return req; + } + /** * Runs the SysId Quasistatic test in the given direction for the routine specified by {@link * #m_sysIdRoutineToApply}. @@ -244,6 +275,15 @@ public ChassisSpeeds returnSpeeds() { return getState().Speeds; } + public AutoFactory createAutoFactory() { + return new AutoFactory( + () -> getState().Pose, this::resetPose, this::followTrajectory, true, this); + } + + public Pose2d getPose() { + return getState().Pose; // For AutoFactory constructor + } + // method for on-demand coasting control public Command coastMotors() { return startEnd(