]> git.taranathan.com Git - FRC2026.git/commitdiff
improve shallow, comment music
authormoo <moogoesmeow123@gmail.com>
Sat, 2 May 2026 16:24:55 +0000 (11:24 -0500)
committerTaran Nathan <moogoesmeow123@gmail.com>
Sun, 3 May 2026 15:43:58 +0000 (10:43 -0500)
src/main/deploy/choreo/shallowSwipe.traj
src/main/java/frc/robot/RobotContainer.java
src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java [deleted file]
src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java [new file with mode: 0644]
src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java

index 48497f4f9fb741a28ce776e9d2a29e54b3b94920..c5260bcfb22fe150389a39e62dde2af31fbb83b1 100644 (file)
@@ -7,8 +7,8 @@
     {"x":6.036516189575195, "y":6.847882270812988, "heading":-1.5313432532484534, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":5.826457977294922, "y":4.701357364654541, "heading":-0.9827935145707228, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":7.098229885101318, "y":4.711697101593018, "heading":1.1270129868473104, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":6.83792781829834, "y":6.324362754821777, "heading":1.4627850621263598, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":5.888542652130127, "y":7.473618507385254, "heading":0.33242572534064313, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.83792781829834, "y":6.324362754821777, "heading":1.4627850621263598, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.888542652130127, "y":7.473618507385254, "heading":0.06807331739317207, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":3.5895698070526123, "y":7.62680196762085, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -23,8 +23,8 @@
     {"x":{"exp":"6.036516189575195 m", "val":6.036516189575195}, "y":{"exp":"6.847882270812988 m", "val":6.847882270812988}, "heading":{"exp":"-1.5313432532484534 rad", "val":-1.5313432532484534}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"5.826457977294922 m", "val":5.826457977294922}, "y":{"exp":"4.701357364654541 m", "val":4.701357364654541}, "heading":{"exp":"-0.9827935145707227 rad", "val":-0.9827935145707228}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"7.098229885101318 m", "val":7.098229885101318}, "y":{"exp":"4.711697101593018 m", "val":4.711697101593018}, "heading":{"exp":"1.1270129868473104 rad", "val":1.1270129868473104}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"6.83792781829834 m", "val":6.83792781829834}, "y":{"exp":"6.324362754821777 m", "val":6.324362754821777}, "heading":{"exp":"1.4627850621263598 rad", "val":1.4627850621263598}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"5.888542652130127 m", "val":5.888542652130127}, "y":{"exp":"7.473618507385254 m", "val":7.473618507385254}, "heading":{"exp":"0.33242572534064313 rad", "val":0.33242572534064313}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.83792781829834 m", "val":6.83792781829834}, "y":{"exp":"6.324362754821777 m", "val":6.324362754821777}, "heading":{"exp":"1.4627850621263598 rad", "val":1.4627850621263598}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.888542652130127 m", "val":5.888542652130127}, "y":{"exp":"7.473618507385254 m", "val":7.473618507385254}, "heading":{"exp":"68.07331739317208 mrad", "val":0.06807331739317207}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"3.5895698070526123 m", "val":3.5895698070526123}, "y":{"exp":"7.62680196762085 m", "val":7.62680196762085}, "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},
    "differentialTrackWidth":0.5588
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.02437,1.62771,2.30382,2.72486,3.24459,4.00317],
+  "waypoints":[0.0,1.02437,1.62837,2.30231,2.72798,3.28679,4.04196],
   "samples":[
-    {"t":0.0, "x":3.58957, "y":7.6268, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.64948, "ay":-1.17972, "alpha":-5.04016, "fx":[135.68651,138.80718,138.77846,133.84435], "fy":[-29.28169,-0.40063,-0.5089,-36.69775]},
-    {"t":0.05391, "x":3.60359, "y":7.62509, "heading":-0.00733, "vx":0.52025, "vy":-0.0636, "omega":-0.27174, "ax":9.66111, "ay":-1.11413, "alpha":-4.85342, "fx":[135.90222,138.79097,138.75963,134.32302], "fy":[-28.1817,-0.18844,0.02604,-34.82592]},
-    {"t":0.10783, "x":3.64568, "y":7.62004, "heading":-0.02903, "vx":1.04112, "vy":-0.12367, "omega":-0.53341, "ax":9.67568, "ay":-1.02774, "alpha":-4.59545, "fx":[136.14732,138.76826,138.73065,134.95586], "fy":[-26.85292,-0.10174,0.8518,-32.169]},
-    {"t":0.16174, "x":3.71588, "y":7.61188, "heading":-0.06447, "vx":1.56278, "vy":-0.17908, "omega":-0.78117, "ax":9.69471, "ay":-0.90603, "alpha":-4.20913, "fx":[136.48559,138.73487,138.68313,135.77724], "fy":[-24.88536,-0.06154,1.8872,-28.31124]},
-    {"t":0.21566, "x":3.81422, "y":7.60091, "heading":-0.1127, "vx":2.08546, "vy":-0.22793, "omega":-1.0081, "ax":9.7201, "ay":-0.71815, "alpha":-3.57385, "fx":[137.02256,138.68074,138.60665,136.81022], "fy":[-21.37332,0.10433,2.97928,-22.42876]},
-    {"t":0.26957, "x":3.94079, "y":7.58757, "heading":-0.17225, "vx":2.60951, "vy":-0.26665, "omega":-1.20078, "ax":9.75144, "ay":-0.38487, "alpha":-2.37685, "fx":[137.88071,138.5747,138.48682,137.95533], "fy":[-13.83278,0.86036,3.778,-12.6274]},
-    {"t":0.32349, "x":4.09565, "y":7.57264, "heading":-0.24044, "vx":3.13525, "vy":-0.2874, "omega":-1.32893, "ax":9.74929, "ay":0.37535, "alpha":0.57699, "fx":[138.08939,138.24151,138.28299,138.16174], "fy":[7.699,3.99044,2.98597,6.60681]},
-    {"t":0.3774, "x":4.27885, "y":7.55769, "heading":-0.31125, "vx":3.66088, "vy":-0.26716, "omega":-1.29782, "ax":9.06926, "ay":1.35386, "alpha":13.477, "fx":[110.8736,135.51967,134.67713,133.14799], "fy":[80.71416,-11.38313,-27.7878,35.21917]},
-    {"t":0.43131, "x":4.48941, "y":7.54525, "heading":-0.36163, "vx":4.14984, "vy":-0.19417, "omega":-0.57122, "ax":6.91459, "ay":3.62301, "alpha":10.16459, "fx":[79.55409,90.10936,114.6442,107.74298], "fy":[84.99294,50.90817,15.2494,54.27056]},
-    {"t":0.48523, "x":4.72319, "y":7.54005, "heading":-0.37766, "vx":4.52264, "vy":0.00116, "omega":-0.0232, "ax":-7.0915, "ay":-4.16434, "alpha":-10.2808, "fx":[-79.94701,-92.60042,-119.37796,-110.15579], "fy":[-92.67785,-62.9472,-20.71928,-59.76971]},
-    {"t":0.53914, "x":4.95672, "y":7.53406, "heading":-0.39385, "vx":4.14031, "vy":-0.22335, "omega":-0.57748, "ax":-8.44543, "ay":-3.48566, "alpha":-11.81234, "fx":[-94.66735,-122.57601,-135.55749,-126.04689], "fy":[-97.04602,-50.57875,2.01996,-52.02877]},
-    {"t":0.59306, "x":5.16767, "y":7.51695, "heading":-0.44215, "vx":3.68498, "vy":-0.41128, "omega":-1.21434, "ax":-8.52255, "ay":-3.72776, "alpha":-11.10674, "fx":[-97.29092,-121.82285,-136.99598,-127.11096], "fy":[-96.60849,-59.59606,-2.7482,-52.40744]},
-    {"t":0.64697, "x":5.35396, "y":7.48936, "heading":-0.52376, "vx":3.22549, "vy":-0.61226, "omega":-1.81315, "ax":-8.41665, "ay":-4.24476, "alpha":-9.89472, "fx":[-98.84694,-115.37332,-136.638,-126.35765], "fy":[-95.88611,-73.6871,-15.85298,-55.24759]},
-    {"t":0.70089, "x":5.51562, "y":7.45018, "heading":-0.6359, "vx":2.77171, "vy":-0.84111, "omega":-2.34662, "ax":-8.10725, "ay":-5.05037, "alpha":-8.01805, "fx":[-98.66279,-105.6984,-132.21602,-123.09604], "fy":[-96.54863,-88.13284,-39.01262,-62.65689]},
-    {"t":0.7548, "x":5.65328, "y":7.39749, "heading":-0.77407, "vx":2.33461, "vy":-1.1134, "omega":-2.7789, "ax":-7.54726, "ay":-6.05633, "alpha":-5.07936, "fx":[-96.74808,-96.65035,-119.21624,-115.30768], "fy":[-98.76182,-98.60663,-69.71557,-76.304]},
-    {"t":0.80871, "x":5.76818, "y":7.32866, "heading":-0.93127, "vx":1.92771, "vy":-1.43992, "omega":-3.05275, "ax":-6.82969, "ay":-6.97006, "alpha":-1.15018, "fx":[-94.64285,-93.61299,-99.13896,-99.84189], "fy":[-100.97053,-101.88708,-96.51146,-95.82665]},
-    {"t":0.86263, "x":5.86218, "y":7.2409, "heading":-1.09753, "vx":1.55949, "vy":-1.81571, "omega":-3.11477, "ax":-6.16812, "ay":-7.549, "alpha":2.92645, "fx":[-91.80002,-96.8042,-84.08022,-77.04188], "fy":[-103.68368,-99.10357,-110.12757,-115.10615]},
-    {"t":0.91654, "x":5.9373, "y":7.13204, "heading":-1.26121, "vx":1.22694, "vy":-2.22271, "omega":-2.95699, "ax":-5.70201, "ay":-7.80774, "alpha":6.28565, "fx":[-86.32707,-102.70225,-78.64153,-55.62765], "fy":[-108.36616,-93.1315,-114.22174,-126.97181]},
-    {"t":0.97046, "x":5.99516, "y":7.00085, "heading":-1.4115, "vx":0.91952, "vy":-2.64366, "omega":-2.6181, "ax":-5.65455, "ay":-7.18254, "alpha":14.66124, "fx":[-90.09798,-127.6957,-83.86681,-18.94714], "fy":[-105.06086,-54.20322,-110.5619,-137.41742]},
-    {"t":1.02437, "x":6.03652, "y":6.84788, "heading":-1.53134, "vx":0.61466, "vy":-3.0309, "omega":-1.82765, "ax":-5.96238, "ay":-3.29592, "alpha":32.2832, "fx":[-108.50865,-138.63734,-94.98568,4.07044], "fy":[57.61594,-4.66803,-101.17066,-138.65304]},
-    {"t":1.06747, "x":6.05747, "y":6.7142, "heading":-1.58013, "vx":0.35771, "vy":-3.17294, "omega":-0.43638, "ax":-6.1444, "ay":-6.06781, "alpha":18.96765, "fx":[-96.87793,-138.1056,-98.91864,-14.47967], "fy":[-96.75011,-12.15498,-97.25211,-137.88166]},
-    {"t":1.11056, "x":6.06718, "y":6.57183, "heading":-1.58132, "vx":0.09291, "vy":-3.43444, "omega":0.38105, "ax":-6.91562, "ay":-6.05132, "alpha":13.94129, "fx":[-108.34827,-134.25183,-102.19645,-47.31226], "fy":[-85.30261,-34.02975,-93.62462,-130.14683]},
-    {"t":1.15366, "x":6.06476, "y":6.4182, "heading":-1.55195, "vx":-0.20513, "vy":-3.69523, "omega":0.98186, "ax":-7.95325, "ay":-5.61053, "alpha":-2.31509, "fx":[-111.8462,-106.35198,-114.12057,-118.62267], "fy":[-81.22624,-88.20193,-77.79685,-70.88681]},
-    {"t":1.19676, "x":6.04853, "y":6.25374, "heading":-1.51179, "vx":-0.54788, "vy":-3.93702, "omega":0.88209, "ax":-8.02791, "ay":-4.92813, "alpha":-10.25252, "fx":[-111.56128,-84.2722,-126.04167,-133.29959], "fy":[-81.26746,-108.86746,-54.05372,-35.23159]},
-    {"t":1.23985, "x":6.01747, "y":6.07949, "heading":-1.48329, "vx":-0.89385, "vy":-4.1494, "omega":0.44025, "ax":-8.26707, "ay":-4.40966, "alpha":-9.49148, "fx":[-114.27604,-93.34129,-128.16868,-132.94878], "fy":[-75.47578,-99.15665,-43.55096,-31.83998]},
-    {"t":1.28295, "x":5.97127, "y":5.89658, "heading":-1.47314, "vx":-1.25013, "vy":-4.33944, "omega":0.03121, "ax":-3.85589, "ay":2.77287, "alpha":2.13409, "fx":[-50.87206,-57.51288,-58.47883,-51.76158], "fy":[44.20259,41.83237,34.46857,36.71548]},
-    {"t":1.32604, "x":5.91381, "y":5.71214, "heading":-1.46981, "vx":-1.4163, "vy":-4.21994, "omega":0.12318, "ax":7.27485, "ay":5.77025, "alpha":11.07008, "fx":[101.85327,65.96964,115.55341,129.1009], "fy":[91.94758,119.57058,70.19116,45.45847]},
-    {"t":1.36914, "x":5.85953, "y":5.53563, "heading":-1.45422, "vx":-1.10278, "vy":-3.97126, "omega":0.60025, "ax":7.39734, "ay":5.78583, "alpha":10.74342, "fx":[102.71995,68.877,118.07084,129.75427], "fy":[92.26739,119.29371,69.75536,46.73462]},
-    {"t":1.41224, "x":5.81888, "y":5.36986, "heading":-1.41837, "vx":-0.78399, "vy":-3.72192, "omega":1.06325, "ax":7.48522, "ay":5.78169, "alpha":10.11723, "fx":[102.86353,72.86921,119.54954,129.12276], "fy":[92.52022,117.35068,68.52786,49.41732]},
-    {"t":1.45533, "x":5.79204, "y":5.21483, "heading":-1.36316, "vx":-0.46141, "vy":-3.47275, "omega":1.49926, "ax":7.5768, "ay":5.76578, "alpha":9.25422, "fx":[102.86143,78.06744,120.77178,127.89656], "fy":[92.72307,114.18815,67.04655,52.95619]},
-    {"t":1.49843, "x":5.77919, "y":5.07053, "heading":-1.28995, "vx":-0.13488, "vy":-3.22427, "omega":1.89808, "ax":7.67881, "ay":5.73471, "alpha":8.16514, "fx":[103.03478,84.38266,121.68362,126.28008], "fy":[92.64776,109.74836,65.80509,56.95122]},
-    {"t":1.54152, "x":5.78051, "y":4.9369, "heading":-1.20057, "vx":0.19605, "vy":-2.97713, "omega":2.24996, "ax":7.79138, "ay":5.68637, "alpha":6.82547, "fx":[103.7114,91.62551,122.07606,124.35082], "fy":[91.96645,103.88475,65.35942,61.20118]},
-    {"t":1.58462, "x":5.79619, "y":4.81388, "heading":-1.09727, "vx":0.53182, "vy":-2.73207, "omega":2.54411, "ax":7.90817, "ay":5.62333, "alpha":5.20494, "fx":[105.26556,99.35188,121.66578,122.10258], "fy":[90.23763,96.61233,66.31727,65.67022]},
-    {"t":1.62771, "x":5.82646, "y":4.70136, "heading":-0.98279, "vx":0.87263, "vy":-2.48973, "omega":2.76843, "ax":7.83663, "ay":5.78489, "alpha":3.66772, "fx":[105.00579,102.84332,118.46236,118.01788], "fy":[90.50565,92.87629,71.89224,72.72354]},
-    {"t":1.66997, "x":5.87033, "y":4.60131, "heading":-0.86253, "vx":1.20378, "vy":-2.24528, "omega":2.92341, "ax":7.53484, "ay":6.19862, "alpha":2.36959, "fx":[102.04649,101.62673,112.10571,111.43919], "fy":[93.70767,94.09819,81.333,82.31721]},
-    {"t":1.71223, "x":5.92792, "y":4.51197, "heading":-0.73689, "vx":1.52218, "vy":-1.98334, "omega":3.02354, "ax":7.15403, "ay":6.64122, "alpha":1.04717, "fx":[98.921,99.19702,103.99724,103.51139], "fy":[96.82481,96.50591,91.31623,91.90403]},
-    {"t":1.75448, "x":5.99863, "y":4.43409, "heading":-0.60819, "vx":1.82449, "vy":-1.70271, "omega":3.06779, "ax":6.66637, "ay":7.11726, "alpha":-0.2113, "fx":[95.07185,94.92555,93.92066,94.05849], "fy":[100.33962,100.48738,101.42591,101.28878]},
-    {"t":1.79674, "x":6.08168, "y":4.36849, "heading":-0.47874, "vx":2.10619, "vy":-1.40195, "omega":3.05886, "ax":6.01205, "ay":7.64503, "alpha":-1.25217, "fx":[89.06751,87.74886,81.52282,82.53811], "fy":[105.27734,106.45274,111.28027,110.45583]},
-    {"t":1.839, "x":6.17605, "y":4.31608, "heading":-0.3506, "vx":2.36023, "vy":-1.0789, "omega":3.00595, "ax":5.02991, "ay":8.27115, "alpha":-1.78057, "fx":[77.29293,75.09985,65.64195,67.15643], "fy":[113.46828,115.08282,120.7018,119.7132]},
-    {"t":1.88125, "x":6.28028, "y":4.27787, "heading":-0.22517, "vx":2.57278, "vy":-0.72939, "omega":2.93071, "ax":3.21207, "ay":9.04618, "alpha":-1.06423, "fx":[49.20149,48.34424,42.01096,42.56489], "fy":[126.8459,127.32328,129.53511,129.20573]},
-    {"t":1.92351, "x":6.39186, "y":4.25513, "heading":-0.10228, "vx":2.70851, "vy":-0.34713, "omega":2.88574, "ax":-0.89216, "ay":9.3654, "alpha":2.62645, "fx":[-18.78798,-22.86175,-5.59849,-3.33614], "fy":[132.66329,131.40115,133.1051,133.83949]},
-    {"t":1.96577, "x":6.50552, "y":4.24882, "heading":0.02201, "vx":2.67081, "vy":0.04862, "omega":2.99672, "ax":-5.26653, "ay":7.66549, "alpha":6.30679, "fx":[-80.09713,-97.45182,-68.63275,-52.42572], "fy":[107.87853,90.95531,112.74854,123.04387]},
-    {"t":2.00803, "x":6.61368, "y":4.25772, "heading":0.15427, "vx":2.44827, "vy":0.37254, "omega":3.26323, "ax":-6.32096, "ay":6.96325, "alpha":6.20695, "fx":[-94.62333,-109.73114,-84.52073,-69.51737], "fy":[97.03923,78.22517,103.72532,115.81963]},
-    {"t":2.05028, "x":6.71149, "y":4.27968, "heading":0.29771, "vx":2.18117, "vy":0.66679, "omega":3.52551, "ax":-5.36176, "ay":8.00369, "alpha":1.78555, "fx":[-79.18426,-82.12098,-72.55333,-70.14808], "fy":[111.54545,109.1838,115.69004,117.38229]},
-    {"t":2.09254, "x":6.79887, "y":4.315, "heading":0.44828, "vx":1.95459, "vy":1.00499, "omega":3.60096, "ax":-4.7644, "ay":8.48008, "alpha":-1.75605, "fx":[-62.59688,-62.40696,-72.07087,-73.06196], "fy":[122.93699,123.11957,117.72863,117.02709]},
-    {"t":2.1348, "x":6.87721, "y":4.36504, "heading":0.59888, "vx":1.75327, "vy":1.36333, "omega":3.52676, "ax":-4.9873, "ay":8.34887, "alpha":-3.59501, "fx":[-59.03252,-62.11758,-80.60223,-81.02303], "fy":[125.08551,123.68517,112.51081,112.09137]},
-    {"t":2.17705, "x":6.94685, "y":4.4301, "heading":0.74469, "vx":1.54252, "vy":1.71613, "omega":3.37485, "ax":-5.31826, "ay":8.11563, "alpha":-5.01041, "fx":[-57.86233,-65.80835,-89.56646,-88.30301], "fy":[125.82084,121.9489,105.72223,106.65676]},
-    {"t":2.21931, "x":7.00728, "y":4.50987, "heading":0.88283, "vx":1.31779, "vy":2.05907, "omega":3.16312, "ax":-5.64403, "ay":7.85513, "alpha":-6.19903, "fx":[-57.34421,-70.72227,-97.76206,-94.18243], "fy":[126.16885,119.27122,98.31482,101.6234]},
-    {"t":2.26157, "x":7.05793, "y":4.60389, "heading":1.01096, "vx":1.07929, "vy":2.391, "omega":2.90117, "ax":-5.93941, "ay":7.58538, "alpha":-7.32583, "fx":[-56.7071,-75.86636,-105.20619,-98.97947], "fy":[126.52867,116.134,90.38628,97.03481]},
-    {"t":2.30382, "x":7.09823, "y":4.7117, "heading":1.12701, "vx":0.82831, "vy":2.71153, "omega":2.59161, "ax":-6.13883, "ay":7.28859, "alpha":-9.5061, "fx":[-50.31336,-79.62795,-113.6409,-104.48336], "fy":[129.18533,113.58312,79.49938,90.98831]},
-    {"t":2.3421, "x":7.12544, "y":4.82082, "heading":1.21925, "vx":0.59334, "vy":2.99051, "omega":2.22775, "ax":-6.24371, "ay":7.19215, "alpha":-9.50081, "fx":[-51.50533,-83.25325,-116.03541,-103.21822], "fy":[128.67387,110.91417,75.88543,92.31441]},
-    {"t":2.38037, "x":7.14357, "y":4.94056, "heading":1.29756, "vx":0.35435, "vy":3.2658, "omega":1.86409, "ax":-6.38781, "ay":7.03852, "alpha":-9.74137, "fx":[-52.82816,-86.95435,-118.9512,-103.44887], "fy":[128.07607,107.98174,71.11787,91.9015]},
-    {"t":2.41865, "x":7.15246, "y":5.07072, "heading":1.36177, "vx":0.10985, "vy":3.53521, "omega":1.49123, "ax":-6.59694, "ay":6.80359, "alpha":-10.10462, "fx":[-55.15206,-91.05675,-122.27542,-105.55573], "fy":[126.99813,104.46059,65.06312,89.23542]},
-    {"t":2.45693, "x":7.15183, "y":5.21102, "heading":1.41145, "vx":-0.14266, "vy":3.79563, "omega":1.10446, "ax":-6.92746, "ay":6.42305, "alpha":-10.42891, "fx":[-60.2186,-96.15499,-125.91781,-110.48902], "fy":[124.50992,99.63925,57.38713,82.6447]},
-    {"t":2.4952, "x":7.1413, "y":5.361, "heading":1.44608, "vx":-0.40781, "vy":4.04148, "omega":0.70528, "ax":-7.51771, "ay":5.70827, "alpha":-10.26623, "fx":[-72.97837,-103.90404,-129.8109,-119.55373], "fy":[117.12801,91.20464,47.23576,68.08483]},
-    {"t":2.53348, "x":7.12018, "y":5.51988, "heading":1.46556, "vx":-0.69557, "vy":4.25997, "omega":0.31232, "ax":-8.74283, "ay":3.74506, "alpha":-8.02871, "fx":[-107.73121,-120.41294,-134.79759,-132.76849], "fy":[84.93565,66.69113,27.35914,33.35564]},
-    {"t":2.57176, "x":7.08715, "y":5.68568, "heading":1.47163, "vx":-1.03021, "vy":4.40332, "omega":0.00501, "ax":-9.30568, "ay":-2.2492, "alpha":-0.14416, "fx":[-131.96051,-132.05161,-131.85264,-131.75858], "fy":[-31.59832,-31.30528,-32.16312,-32.46051]},
-    {"t":2.61003, "x":7.0409, "y":5.85258, "heading":1.47172, "vx":-1.3864, "vy":4.31723, "omega":-0.0005, "ax":-8.77432, "ay":-3.19615, "alpha":0.00127, "fx":[-124.37346,-124.37194,-124.37426,-124.37579], "fy":[-45.30646,-45.30963,-45.30275,-45.29958]},
-    {"t":2.64831, "x":6.98141, "y":6.01548, "heading":1.4717, "vx":-1.72225, "vy":4.19489, "omega":-0.00046, "ax":-7.25109, "ay":-3.31809, "alpha":-0.02784, "fx":[-102.77456,-102.83129,-102.79042,-102.73362], "fy":[-46.99917,-46.94,-47.0672,-47.12643]},
-    {"t":2.68659, "x":6.91017, "y":6.17362, "heading":1.47166, "vx":-1.99979, "vy":4.06789, "omega":-0.00152, "ax":5.86725, "ay":-6.76775, "alpha":-12.03843, "fx":[120.1822,86.09313,39.07915,87.31304], "fy":[-58.12102,-95.44202,-127.54007,-102.62207]},
-    {"t":2.72486, "x":6.83793, "y":6.32436, "heading":1.46279, "vx":-1.77521, "vy":3.80884, "omega":-0.46231, "ax":5.34965, "ay":-7.45665, "alpha":-12.33293, "fx":[119.04477,73.8299,28.38046,82.06482], "fy":[-66.93203,-112.25371,-133.68096,-109.91819]},
-    {"t":2.76484, "x":6.77123, "y":6.47068, "heading":1.43445, "vx":-1.56134, "vy":3.51073, "omega":-0.95537, "ax":4.75775, "ay":-7.82, "alpha":-12.16411, "fx":[113.05374,60.94977,20.68485,75.07149], "fy":[-75.61151,-118.80465,-134.62956,-114.34098]},
-    {"t":2.80482, "x":6.71261, "y":6.60479, "heading":1.38653, "vx":-1.37113, "vy":3.19809, "omega":-1.44169, "ax":3.88365, "ay":-8.24603, "alpha":-11.98232, "fx":[103.34348,43.15183,9.32678,64.37737], "fy":[-87.12146,-125.15123,-135.23811,-120.03113]},
-    {"t":2.8448, "x":6.6609, "y":6.72605, "heading":1.31932, "vx":-1.21586, "vy":2.86842, "omega":-1.92073, "ax":2.61023, "ay":-8.66249, "alpha":-11.78608, "fx":[87.59762,18.51373,-6.67554,48.56177], "fy":[-101.12464,-129.38007,-134.38855,-126.26181]},
-    {"t":2.88478, "x":6.61438, "y":6.83381, "heading":1.23311, "vx":-1.11151, "vy":2.5221, "omega":-2.39193, "ax":0.74844, "ay":-8.89118, "alpha":-11.4819, "fx":[61.09932,-15.03439,-28.66958,25.04066], "fy":[-116.11269,-127.11954,-129.67704,-131.21232]},
-    {"t":2.92476, "x":6.57054, "y":6.92754, "heading":1.1283, "vx":-1.08158, "vy":2.16663, "omega":-2.85097, "ax":-2.21066, "ay":-8.38164, "alpha":-10.38234, "fx":[9.22737,-60.34724,-60.16202,-14.06054], "fy":[-124.77955,-107.42902,-114.19678,-128.82539]},
-    {"t":2.96474, "x":6.52553, "y":7.00746, "heading":1.00602, "vx":-1.16996, "vy":1.83154, "omega":-3.26605, "ax":-7.53188, "ay":-3.3725, "alpha":-3.014, "fx":[-103.98843,-111.16685,-109.64643,-102.24872], "fy":[-49.45084,-36.56785,-46.65266,-58.54619]},
-    {"t":3.00472, "x":6.47273, "y":7.07799, "heading":0.87304, "vx":-1.47108, "vy":1.69671, "omega":-3.38655, "ax":-6.95561, "ay":5.27026, "alpha":6.79934, "fx":[-110.59586,-110.99263,-83.38727,-89.4009], "fy":[63.26814,54.22783,90.71644,90.60611]},
-    {"t":3.0447, "x":6.40836, "y":7.15003, "heading":0.74308, "vx":-1.74917, "vy":1.90741, "omega":-3.11472, "ax":-8.79018, "ay":-0.03077, "alpha":14.4057, "fx":[-135.25278,-118.74601,-127.06011,-117.3358], "fy":[2.05469,-62.272,-6.07001,64.54279]},
-    {"t":3.08468, "x":6.33141, "y":7.22626, "heading":0.63007, "vx":-2.10059, "vy":1.90618, "omega":-2.53879, "ax":-8.55511, "ay":-3.94996, "alpha":9.58169, "fx":[-131.52573,-109.6608,-108.19082,-135.6891], "fy":[-40.07147,-82.80447,-83.32829,-17.75497]},
-    {"t":3.12466, "x":6.24059, "y":7.29932, "heading":0.53623, "vx":-2.44262, "vy":1.74826, "omega":-2.15572, "ax":-8.31506, "ay":-4.70783, "alpha":7.91425, "fx":[-128.85757,-108.14419,-102.70431,-131.74965], "fy":[-49.51009,-85.78501,-91.66945,-39.96486]},
-    {"t":3.16464, "x":6.13629, "y":7.36545, "heading":0.45637, "vx":-2.77505, "vy":1.56005, "omega":-1.83931, "ax":-8.13316, "ay":-5.10843, "alpha":7.25447, "fx":[-127.23121,-107.00098,-98.76818,-128.14225], "fy":[-54.18779,-87.63084,-96.49517,-51.32923]},
-    {"t":3.20462, "x":6.01885, "y":7.42374, "heading":0.38863, "vx":-3.10021, "vy":1.35582, "omega":-1.54928, "ax":-7.95558, "ay":-5.4077, "alpha":7.17293, "fx":[-126.15248,-105.4972,-94.29778,-125.12611], "fy":[-57.00594,-89.67119,-101.16748,-58.76681]},
-    {"t":3.24459, "x":5.88854, "y":7.47362, "heading":0.33243, "vx":-3.41827, "vy":1.13962, "omega":-1.26251, "ax":-7.79682, "ay":-5.60859, "alpha":7.71275, "fx":[-126.08193,-103.89868,-88.77522,-123.31619], "fy":[-57.38975,-91.66709,-106.21534,-62.72953]},
-    {"t":3.29878, "x":5.69188, "y":7.52713, "heading":0.27534, "vx":-3.84073, "vy":0.83572, "omega":-0.8446, "ax":-7.48422, "ay":-5.91527, "alpha":8.58702, "fx":[-125.311,-100.53183,-79.09213,-119.41304], "fy":[-58.34597,-94.97237,-113.17579,-68.89597]},
-    {"t":3.35296, "x":5.47279, "y":7.56373, "heading":0.24218, "vx":-4.24626, "vy":0.51521, "omega":-0.37932, "ax":-5.3159, "ay":-7.8284, "alpha":6.95819, "fx":[-100.48345,-75.55762,-47.56579,-77.79946], "fy":[-91.92732,-113.83005,-127.54242,-110.56304]},
-    {"t":3.40715, "x":5.2349, "y":7.58016, "heading":0.23184, "vx":-4.5343, "vy":0.09103, "omega":-0.0023, "ax":-0.00954, "ay":-3.74745, "alpha":0.03051, "fx":[-0.20514,-0.17836,-0.0654,-0.09212], "fy":[-53.08709,-53.17127,-53.15148,-53.06728]},
-    {"t":3.46133, "x":4.9892, "y":7.57959, "heading":0.23176, "vx":-4.53481, "vy":-0.11202, "omega":-0.00064, "ax":-0.0235, "ay":2.54804, "alpha":0.00221, "fx":[-0.33786,-0.33607,-0.32844,-0.33024], "fy":[36.12033,36.11367,36.11525,36.12191]},
-    {"t":3.51551, "x":4.74346, "y":7.57726, "heading":0.23173, "vx":-4.53609, "vy":0.02605, "omega":-0.00052, "ax":5.75367, "ay":2.70999, "alpha":-8.56006, "fx":[96.4518,84.6488,64.61595,80.51093], "fy":[21.83009,54.70797,59.33876,17.77729]},
-    {"t":3.5697, "x":4.50612, "y":7.58265, "heading":0.21914, "vx":-4.22433, "vy":0.17288, "omega":-0.46434, "ax":9.64497, "ay":-0.10353, "alpha":-5.62609, "fx":[137.46591,136.84932,137.03014,135.51545], "fy":[-13.22069,18.2546,14.52736,-25.43155]},
-    {"t":3.62388, "x":4.29138, "y":7.59187, "heading":0.18572, "vx":-3.70172, "vy":0.16727, "omega":-0.76919, "ax":9.76381, "ay":-0.28402, "alpha":-0.2266, "fx":[138.38689,138.42658,138.41436,138.37089], "fy":[-4.5949,-3.13743,-3.44951,-4.9216]},
-    {"t":3.67807, "x":4.10514, "y":7.60051, "heading":0.14371, "vx":-3.17268, "vy":0.15188, "omega":-0.78147, "ax":9.76704, "ay":-0.37345, "alpha":1.31782, "fx":[138.59436,138.21022,138.35483,138.6226], "fy":[-1.66494,-10.49441,-8.66846,-0.3465]},
-    {"t":3.73225, "x":3.94757, "y":7.60819, "heading":0.1033, "vx":-2.64346, "vy":0.13165, "omega":-0.71006, "ax":9.76348, "ay":-0.42692, "alpha":2.03612, "fx":[138.67883,137.97441,138.23216,138.69464], "fy":[-0.0628,-13.99612,-11.42155,1.27455]},
-    {"t":3.78644, "x":3.81867, "y":7.6147, "heading":0.06781, "vx":-2.11444, "vy":0.10852, "omega":-0.59974, "ax":9.75987, "ay":-0.46364, "alpha":2.44542, "fx":[138.72121,137.80585,138.11589,138.73221], "fy":[0.98464,-15.96462,-13.22286,1.91504]},
-    {"t":3.84062, "x":3.71843, "y":7.6199, "heading":0.03891, "vx":-1.58561, "vy":0.0834, "omega":-0.46723, "ax":9.75685, "ay":-0.49143, "alpha":2.7077, "fx":[138.74516,137.68635,138.01435,138.75795], "fy":[1.70932,-17.20976,-14.52051,2.15714]},
-    {"t":3.8948, "x":3.64684, "y":7.6237, "heading":0.01757, "vx":-1.05694, "vy":0.05677, "omega":-0.32052, "ax":9.75433, "ay":-0.51408, "alpha":2.89009, "fx":[138.76044,137.59504,137.92852,138.77729], "fy":[2.20533,-18.09252,-15.49611,2.23551]},
-    {"t":3.94899, "x":3.60389, "y":7.62602, "heading":0.00444, "vx":-0.52841, "vy":0.02891, "omega":-0.16392, "ax":9.75217, "ay":-0.53361, "alpha":3.02528, "fx":[138.77185,137.51678,137.85833,138.79206], "fy":[2.51823,-18.79949,-16.2372,2.2634]},
-    {"t":4.00317, "x":3.58957, "y":7.6268, "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]}],
+    {"t":0.0, "x":3.58957, "y":7.6268, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.64948, "ay":-1.17974, "alpha":-5.04028, "fx":[135.68641,138.80718,138.77846,133.84413], "fy":[-29.28221,-0.40058,-0.50883,-36.69856]},
+    {"t":0.05391, "x":3.60359, "y":7.62509, "heading":-0.00733, "vx":0.52024, "vy":-0.0636, "omega":-0.27174, "ax":9.66111, "ay":-1.11414, "alpha":-4.8535, "fx":[135.90216,138.79098,138.75964,134.32288], "fy":[-28.18204,-0.1884,0.0261,-34.82645]},
+    {"t":0.10783, "x":3.64568, "y":7.62004, "heading":-0.02903, "vx":1.04111, "vy":-0.12367, "omega":-0.53341, "ax":9.67568, "ay":-1.02774, "alpha":-4.59548, "fx":[136.1473,138.76826,138.73066,134.95583], "fy":[-26.85305,-0.10172,0.85184,-32.16918]},
+    {"t":0.16174, "x":3.71588, "y":7.61188, "heading":-0.06447, "vx":1.56277, "vy":-0.17908, "omega":-0.78118, "ax":9.69471, "ay":-0.90602, "alpha":-4.20909, "fx":[136.48563,138.73488,138.68314,135.7773], "fy":[-24.88521,-0.06156,1.88718,-28.31102]},
+    {"t":0.21566, "x":3.81422, "y":7.60091, "heading":-0.1127, "vx":2.08545, "vy":-0.22793, "omega":-1.00811, "ax":9.7201, "ay":-0.71813, "alpha":-3.57374, "fx":[137.02265,138.68076,138.60668,136.81035], "fy":[-21.37283,0.10426,2.97913,-22.42812]},
+    {"t":0.26957, "x":3.94078, "y":7.58757, "heading":-0.17225, "vx":2.6095, "vy":-0.26665, "omega":-1.20078, "ax":9.75145, "ay":-0.38486, "alpha":-2.3767, "fx":[137.88083,138.57475,138.48688,137.95544], "fy":[-13.83209,0.86016,3.7776,-12.6267]},
+    {"t":0.32348, "x":4.09565, "y":7.57264, "heading":-0.24044, "vx":3.13524, "vy":-0.2874, "omega":-1.32892, "ax":9.74931, "ay":0.37526, "alpha":0.57684, "fx":[138.08964,138.24168,138.28314,138.16196], "fy":[7.69699,3.98942,2.98524,6.60513]},
+    {"t":0.3774, "x":4.27885, "y":7.55769, "heading":-0.31125, "vx":3.66087, "vy":-0.26717, "omega":-1.29782, "ax":9.06933, "ay":1.35398, "alpha":13.47677, "fx":[110.87396,135.52165,134.67827,133.14839], "fy":[80.71545,-11.37992,-27.78628,35.22037]},
+    {"t":0.43131, "x":4.4894, "y":7.54525, "heading":-0.36163, "vx":4.14983, "vy":-0.19417, "omega":-0.57123, "ax":6.91538, "ay":3.63136, "alpha":10.16576, "fx":[79.53346,90.1184,114.68991,107.75377], "fy":[85.10537,51.07773,15.34719,54.36434]},
+    {"t":0.48523, "x":4.72319, "y":7.54006, "heading":-0.37766, "vx":4.52267, "vy":0.00162, "omega":-0.02315, "ax":-7.09546, "ay":-4.1616, "alpha":-10.2868, "fx":[-79.99123,-92.67983,-119.42966,-110.20522], "fy":[-92.68004,-62.89922,-20.64072,-59.73869]},
+    {"t":0.53914, "x":4.95671, "y":7.5341, "heading":-0.39385, "vx":4.14012, "vy":-0.22275, "omega":-0.57776, "ax":-8.44503, "ay":-3.48825, "alpha":-11.81163, "fx":[-94.6525,-122.56505,-135.56506,-126.04246], "fy":[-97.07133,-50.64228,1.98729,-52.05435]},
+    {"t":0.59306, "x":5.16765, "y":7.51702, "heading":-0.44217, "vx":3.68482, "vy":-0.41082, "omega":-1.21457, "ax":-8.52103, "ay":-3.73245, "alpha":-11.10439, "fx":[-97.26158,-121.77975,-136.9983,-127.09466], "fy":[-96.64418,-59.70274,-2.82437,-52.45511]},
+    {"t":0.64697, "x":5.35393, "y":7.48945, "heading":-0.52379, "vx":3.22541, "vy":-0.61205, "omega":-1.81325, "ax":-8.41458, "ay":-4.24982, "alpha":-9.89126, "fx":[-98.81522,-115.31899,-136.62939,-126.3352], "fy":[-95.92295,-73.782,-15.9515,-55.30437]},
+    {"t":0.70088, "x":5.51559, "y":7.45027, "heading":-0.63593, "vx":2.77175, "vy":-0.84118, "omega":-2.34653, "ax":-8.10518, "ay":-5.05436, "alpha":-8.0139, "fx":[-98.63614,-105.65544,-132.19245,-123.0718], "fy":[-96.57878,-88.18957,-39.10067,-62.70826]},
+    {"t":0.7548, "x":5.65325, "y":7.39758, "heading":-0.77409, "vx":2.33477, "vy":-1.11368, "omega":-2.77859, "ax":-7.54607, "ay":-6.05813, "alpha":-5.07629, "fx":[-96.73596,-96.63457,-119.19414,-115.29055], "fy":[-98.77573,-98.62486,-69.75695,-76.3324]},
+    {"t":0.80871, "x":5.76816, "y":7.32873, "heading":-0.93127, "vx":1.92793, "vy":-1.4403, "omega":-3.05228, "ax":-6.83014, "ay":-6.96971, "alpha":-1.15025, "fx":[-94.64912,-93.6196,-99.14571,-99.84827], "fy":[-100.96607,-101.88248,-96.5061,-95.8215]},
+    {"t":0.86263, "x":5.86218, "y":7.24095, "heading":-1.0975, "vx":1.55968, "vy":-1.81606, "omega":-3.11429, "ax":-6.16998, "ay":-7.54759, "alpha":2.92364, "fx":[-91.82378,-96.81909,-84.10759,-77.08157], "fy":[-103.66363,-99.08986,-110.10736,-115.08036]},
+    {"t":0.91654, "x":5.9373, "y":7.13207, "heading":-1.26116, "vx":1.22704, "vy":-2.22298, "omega":-2.95667, "ax":-5.70449, "ay":-7.80611, "alpha":6.28206, "fx":[-86.36753,-102.71765,-78.67105,-55.68317], "fy":[-108.3346,-93.11504,-114.20174,-126.94785]},
+    {"t":0.97045, "x":5.99516, "y":7.00087, "heading":-1.41143, "vx":0.91948, "vy":-2.64384, "omega":-2.61798, "ax":-5.65514, "ay":-7.18807, "alpha":14.61123, "fx":[-90.00929,-127.58651,-83.85356,-19.19177], "fy":[-105.14147,-54.45976,-110.57188,-137.38351]},
+    {"t":1.02437, "x":6.03652, "y":6.84788, "heading":-1.53134, "vx":0.61459, "vy":-3.03138, "omega":-1.83022, "ax":-6.05325, "ay":-3.48345, "alpha":31.07952, "fx":[-113.60073,-138.63317,-94.96806,3.98855], "fy":[47.13195,-4.79704,-101.18746,-138.65573]},
+    {"t":1.06751, "x":6.0574, "y":6.71386, "heading":-1.58138, "vx":0.35344, "vy":-3.18167, "omega":-0.48937, "ax":-6.12696, "ay":-6.0776, "alpha":19.00838, "fx":[-96.01717,-138.12813,-98.95057,-14.29669], "fy":[-97.56771,-11.9038,-97.22055,-137.90186]},
+    {"t":1.11065, "x":6.06694, "y":6.57094, "heading":-1.5848, "vx":0.0891, "vy":-3.44387, "omega":0.3307, "ax":-6.89944, "ay":-6.0441, "alpha":14.13022, "fx":[-108.07603,-134.50429,-102.26011,-46.3511], "fy":[-85.61577,-33.02487,-93.55868,-130.49544]},
+    {"t":1.1538, "x":6.06437, "y":6.41673, "heading":-1.55739, "vx":-0.20856, "vy":-3.70463, "omega":0.94032, "ax":-7.9565, "ay":-5.61867, "alpha":-1.65751, "fx":[-112.1213,-108.25677,-113.69402,-117.05402], "fy":[-80.8226,-85.85777,-78.45204,-73.44103]},
+    {"t":1.19694, "x":6.04796, "y":6.25168, "heading":-1.51836, "vx":-0.55182, "vy":-3.94704, "omega":0.86881, "ax":-8.06462, "ay":-4.87122, "alpha":-10.21041, "fx":[-112.14653,-85.22283,-126.38607,-133.50068], "fy":[-80.44322,-108.11101,-53.21508,-34.42434]},
+    {"t":1.24008, "x":6.01665, "y":6.07686, "heading":-1.49038, "vx":-0.89975, "vy":-4.15719, "omega":0.4283, "ax":-8.36222, "ay":-4.23769, "alpha":-9.32008, "fx":[-115.66903,-96.07808,-129.04101,-133.34161], "fy":[-73.21467,-96.39932,-40.74669,-29.91228]},
+    {"t":1.28322, "x":5.97005, "y":5.89356, "heading":-1.48058, "vx":-1.26052, "vy":-4.34002, "omega":0.02621, "ax":-3.43793, "ay":2.97204, "alpha":2.59145, "fx":[-44.00855,-52.38951,-53.54292,-44.98643], "fy":[47.80231,45.12386,36.51535,39.0701]},
+    {"t":1.32637, "x":5.91247, "y":5.70909, "heading":-1.47703, "vx":-1.40884, "vy":-4.2118, "omega":0.13801, "ax":7.24709, "ay":5.80201, "alpha":11.12545, "fx":[101.72754,65.16,114.88953,129.12625], "fy":[92.12838,120.06363,71.31724,45.45915]},
+    {"t":1.36951, "x":5.85843, "y":5.53278, "heading":-1.46072, "vx":-1.09618, "vy":-3.96148, "omega":0.61799, "ax":7.37216, "ay":5.81419, "alpha":10.7855, "fx":[102.59003,68.16056,117.49293,129.75106], "fy":[92.42756,119.7239,70.739,46.7686]},
+    {"t":1.41265, "x":5.818, "y":5.36728, "heading":-1.42403, "vx":-0.77813, "vy":-3.71064, "omega":1.08331, "ax":7.46032, "ay":5.81091, "alpha":10.14967, "fx":[102.69252,72.18434,119.03726,129.07878], "fy":[92.71918,117.78468,69.42327,49.54575]},
+    {"t":1.4558, "x":5.79138, "y":5.2126, "heading":-1.36784, "vx":-0.45627, "vy":-3.45994, "omega":1.52119, "ax":7.55153, "ay":5.79673, "alpha":9.28032, "fx":[102.63782,77.39967,120.32104,127.80612], "fy":[92.97687,114.64978,67.85947,53.18299]},
+    {"t":1.49894, "x":5.77872, "y":5.06873, "heading":-1.29358, "vx":-0.13047, "vy":-3.20986, "omega":1.92157, "ax":7.65302, "ay":5.76753, "alpha":8.18817, "fx":[102.75608,83.73388,121.28705,126.142], "fy":[92.96158,110.2501,66.53924,57.2628]},
+    {"t":1.54208, "x":5.78021, "y":4.93561, "heading":-1.20306, "vx":0.1997, "vy":-2.96103, "omega":2.27483, "ax":7.76518, "ay":5.72097, "alpha":6.84696, "fx":[103.38059,91.01062,121.72165,124.16552], "fy":[92.34202,104.42846,66.02212,61.58114]},
+    {"t":1.58522, "x":5.79605, "y":4.81319, "heading":-1.09854, "vx":0.53471, "vy":-2.71421, "omega":2.57023, "ax":7.88184, "ay":5.6595, "alpha":5.22373, "fx":[104.89184,98.79504,121.33668,121.86907], "fy":[90.675,97.18534,66.92164,66.10643]},
+    {"t":1.62837, "x":5.82646, "y":4.70136, "heading":-0.98279, "vx":0.87475, "vy":-2.47005, "omega":2.79559, "ax":7.81175, "ay":5.8182, "alpha":3.67775, "fx":[104.63666,102.38805,118.14615,117.74809], "fy":[90.93364,93.3795,72.41214,73.16105]},
+    {"t":1.67049, "x":5.87023, "y":4.60248, "heading":-0.86178, "vx":1.20379, "vy":-2.22498, "omega":2.9505, "ax":7.51488, "ay":6.22309, "alpha":2.36203, "fx":[101.77166,101.32785,111.81532,111.17176], "fy":[94.00774,94.42204,81.73394,82.67975]},
+    {"t":1.71261, "x":5.9276, "y":4.51428, "heading":-0.7354, "vx":1.52033, "vy":-1.96285, "omega":3.04999, "ax":7.1432, "ay":6.65333, "alpha":1.01229, "fx":[98.84438,99.11455,103.7602,103.29344], "fy":[96.90471,96.59362,91.58852,92.1506]},
+    {"t":1.75473, "x":5.99798, "y":4.4375, "heading":-0.60604, "vx":1.82121, "vy":-1.68261, "omega":3.09263, "ax":6.67277, "ay":7.11128, "alpha":-0.2883, "fx":[95.37534,95.16968,93.80243,93.99244], "fy":[100.05226,100.26063,101.53912,101.3506]},
+    {"t":1.79685, "x":6.08061, "y":4.37294, "heading":-0.47603, "vx":2.10227, "vy":-1.38307, "omega":3.08049, "ax":6.0525, "ay":7.61175, "alpha":-1.39643, "fx":[90.09547,88.5604,81.67461,82.84061], "fy":[104.39687,105.78458,111.17277,110.22453]},
+    {"t":1.83897, "x":6.17453, "y":4.32143, "heading":-0.34751, "vx":2.35721, "vy":-1.06246, "omega":3.02167, "ax":5.14163, "ay":8.19819, "alpha":-2.04087, "fx":[79.78238,77.07269,66.4075,68.26309], "fy":[111.71428,113.77517,120.28032,119.05995]},
+    {"t":1.88109, "x":6.27838, "y":4.28395, "heading":-0.22204, "vx":2.57378, "vy":-0.71714, "omega":2.93571, "ax":3.47246, "ay":8.94046, "alpha":-1.52599, "fx":[54.58745,53.0071,44.15201,45.13844], "fy":[124.51749,125.41674,128.77202,128.20945]},
+    {"t":1.92321, "x":6.38987, "y":4.26168, "heading":-0.09974, "vx":2.72005, "vy":-0.34055, "omega":2.87143, "ax":-0.56418, "ay":9.36098, "alpha":2.15265, "fx":[-13.36522,-16.11894,-1.99896,-0.50547], "fy":[132.76292,131.89204,132.75058,133.35312]},
+    {"t":1.96534, "x":6.50394, "y":4.25564, "heading":0.02311, "vx":2.69628, "vy":0.05374, "omega":2.9621, "ax":-5.24969, "ay":7.63909, "alpha":6.29441, "fx":[-79.89655,-97.08275,-68.34198,-52.33134], "fy":[107.55884,90.67554,112.27149,122.62346]},
+    {"t":2.00746, "x":6.61286, "y":4.26468, "heading":0.15347, "vx":2.47516, "vy":0.37551, "omega":3.22723, "ax":-6.389, "ay":6.87658, "alpha":6.3267, "fx":[-95.48644,-110.8178,-85.73637,-70.20943], "fy":[95.98493,76.35548,102.36829,115.18694]},
+    {"t":2.04958, "x":6.71145, "y":4.2866, "heading":0.29501, "vx":2.20605, "vy":0.66516, "omega":3.49372, "ax":-5.76467, "ay":7.66437, "alpha":2.69711, "fx":[-85.82485,-90.8332,-77.13017,-73.06325], "fy":[106.05682,101.39745,112.00386,115.10431]},
+    {"t":2.0917, "x":6.79925, "y":4.32141, "heading":0.44457, "vx":1.96323, "vy":0.98799, "omega":3.60732, "ax":-4.86374, "ay":8.41948, "alpha":-1.49826, "fx":[-64.85639,-64.50936,-72.74287,-73.6608], "fy":[121.66941,121.93454,117.21501,116.55732]},
+    {"t":2.13382, "x":6.87763, "y":4.3705, "heading":0.59518, "vx":1.75837, "vy":1.34263, "omega":3.54422, "ax":-5.04913, "ay":8.31302, "alpha":-3.41712, "fx":[-60.67206,-63.2991,-80.8943,-81.41526], "fy":[124.26179,123.04869,112.26245,111.76766]},
+    {"t":2.17594, "x":6.94722, "y":4.43442, "heading":0.74144, "vx":1.54569, "vy":1.69279, "omega":3.40028, "ax":-5.37242, "ay":8.08328, "alpha":-4.86321, "fx":[-59.35891,-66.71099,-89.80317,-88.73794], "fy":[125.10185,121.43891,105.5004,106.27327]},
+    {"t":2.21806, "x":7.00756, "y":4.5129, "heading":0.88035, "vx":1.3194, "vy":2.03326, "omega":3.19544, "ax":-5.69695, "ay":7.8214, "alpha":-6.06928, "fx":[-58.80323,-71.5126,-98.00098,-94.69479], "fy":[125.48298,118.78772,98.06323,101.13195]},
+    {"t":2.26018, "x":7.05808, "y":4.60548, "heading":1.00956, "vx":1.07944, "vy":2.36271, "omega":2.93979, "ax":-5.99246, "ay":7.55027, "alpha":-7.18405, "fx":[-58.24855,-76.62299,-105.39722,-99.49816], "fy":[125.81799,115.62843,90.15366,96.49321]},
+    {"t":2.30231, "x":7.09823, "y":4.7117, "heading":1.12701, "vx":0.82703, "vy":2.68074, "omega":2.63719, "ax":-6.17905, "ay":7.30296, "alpha":-8.77938, "fx":[-54.05039,-80.47307,-112.4475,-103.37527], "fy":[127.66426,112.97921,81.17067,92.25684]},
+    {"t":2.341, "x":7.12561, "y":4.8209, "heading":1.22249, "vx":0.58791, "vy":2.96335, "omega":2.29745, "ax":-6.27503, "ay":7.19094, "alpha":-9.1241, "fx":[-53.6451,-83.82497,-115.50922,-102.8087], "fy":[127.79269,110.47671,76.67574,92.77443]},
+    {"t":2.3797, "x":7.14366, "y":4.94096, "heading":1.30457, "vx":0.34508, "vy":3.24162, "omega":1.94436, "ax":-6.40826, "ay":7.01996, "alpha":-9.73243, "fx":[-53.22798,-87.37995,-119.19835,-103.53603], "fy":[127.90576,107.63309,70.69388,91.79218]},
+    {"t":2.4184, "x":7.15222, "y":5.07167, "heading":1.37252, "vx":0.09709, "vy":3.51328, "omega":1.56774, "ax":-6.60108, "ay":6.77754, "alpha":-10.35726, "fx":[-54.18469,-91.39336,-122.99868,-105.69813], "fy":[127.40916,104.16326,63.67429,89.03311]},
+    {"t":2.4571, "x":7.15103, "y":5.2127, "heading":1.42544, "vx":-0.15836, "vy":3.77556, "omega":1.16693, "ax":-6.90731, "ay":6.41385, "alpha":-10.75, "fx":[-58.55279,-96.31907,-126.58137,-110.18453], "fy":[125.29782,99.47829,55.89155,82.9917]},
+    {"t":2.4958, "x":7.13973, "y":5.36361, "heading":1.46255, "vx":-0.42566, "vy":4.02376, "omega":0.75093, "ax":-7.46115, "ay":5.75118, "alpha":-10.5514, "fx":[-70.70344,-103.63337,-130.07173,-118.63137], "fy":[118.51222,91.50929,46.48093,69.58408]},
+    {"t":2.53449, "x":7.11767, "y":5.52362, "heading":1.4837, "vx":-0.71439, "vy":4.24632, "omega":0.34261, "ax":-8.63654, "ay":3.91997, "alpha":-8.56153, "fx":[-103.85882,-119.12367,-134.68837,-132.01247], "fy":[89.6066,68.96876,27.79826,35.88466]},
+    {"t":2.57319, "x":7.08356, "y":5.69088, "heading":1.49055, "vx":-1.04861, "vy":4.39801, "omega":0.0113, "ax":-9.28872, "ay":-2.26609, "alpha":-0.29022, "fx":[-131.78149,-131.9584,-131.55523,-131.36647], "fy":[-31.52932,-30.97491,-32.70408,-33.27663]},
+    {"t":2.61189, "x":7.03602, "y":5.85938, "heading":1.49077, "vx":-1.40806, "vy":4.31032, "omega":0.00006, "ax":-8.71661, "ay":-3.23544, "alpha":-0.00207, "fx":[-123.55649,-123.55899,-123.55514,-123.55264], "fy":[-45.85845,-45.8535,-45.86471,-45.86966]},
+    {"t":2.65059, "x":6.97501, "y":6.02376, "heading":1.49077, "vx":-1.74538, "vy":4.18512, "omega":-0.00002, "ax":-2.43204, "ay":-7.72995, "alpha":-8.1517, "fx":[-13.34932,-62.01011,-49.76758,-12.76708], "fy":[-111.32636,-96.76815,-110.59229,-119.59419]},
+    {"t":2.68929, "x":6.90565, "y":6.17993, "heading":1.48467, "vx":-1.83949, "vy":3.88598, "omega":-0.31547, "ax":4.63035, "ay":-7.93785, "alpha":-12.92198, "fx":[115.28001,52.6225,18.21846,76.41556], "fy":[-74.34961,-125.02287,-136.16494,-114.53135]},
+    {"t":2.72798, "x":6.83793, "y":6.32436, "heading":1.46279, "vx":-1.66031, "vy":3.5788, "omega":-0.81552, "ax":4.66365, "ay":-8.00064, "alpha":-12.44351, "fx":[114.24825,55.67733,18.98406,75.51486], "fy":[-76.82954,-124.77152,-136.48942,-115.53831]},
+    {"t":2.7679, "x":6.77537, "y":6.46084, "heading":1.42032, "vx":-1.47416, "vy":3.25946, "omega":-1.31221, "ax":4.19234, "ay":-8.25672, "alpha":-12.2136, "fx":[108.6306,47.38785,12.62814,69.05522], "fy":[-84.14811,-127.71698,-136.99363,-119.28969]},
+    {"t":2.80781, "x":6.71987, "y":6.58436, "heading":1.35821, "vx":-1.30682, "vy":2.92989, "omega":-1.79971, "ax":3.49932, "ay":-8.55374, "alpha":-12.10128, "fx":[100.68216,35.35151,2.71971,59.65474], "fy":[-92.92277,-130.91043,-137.2115,-123.94429]},
+    {"t":2.84773, "x":6.6705, "y":6.69449, "heading":1.27674, "vx":-1.16714, "vy":2.58847, "omega":-2.28274, "ax":2.52432, "ay":-8.84109, "alpha":-12.16025, "fx":[89.54749,18.21581,-11.28087,46.64405], "fy":[-102.81833,-133.31326,-136.23273,-128.91683]},
+    {"t":2.88764, "x":6.62592, "y":6.79077, "heading":1.17594, "vx":-1.06638, "vy":2.23558, "omega":-2.76811, "ax":1.19343, "ay":-9.0125, "alpha":-12.43466, "fx":[73.74897,-5.48102,-29.71382,29.11227], "fy":[-113.12762,-132.41533,-132.38668,-133.07041]},
+    {"t":2.92756, "x":6.58431, "y":6.87282, "heading":1.05554, "vx":-1.01875, "vy":1.87584, "omega":-3.26444, "ax":-0.63715, "ay":-8.80907, "alpha":-12.7967, "fx":[48.59751,-37.16173,-52.45909,4.89751], "fy":[-121.98527,-121.70076,-122.11743,-133.66258]},
+    {"t":2.96747, "x":6.54314, "y":6.94068, "heading":0.91505, "vx":-1.04418, "vy":1.52423, "omega":-3.77522, "ax":-5.61733, "ay":-2.36693, "alpha":-4.54862, "fx":[-72.08231,-83.70755,-86.83588,-75.87152], "fy":[-35.08982,-19.41516,-32.66129,-47.03614]},
+    {"t":3.00739, "x":6.49698, "y":6.99963, "heading":0.76074, "vx":-1.2684, "vy":1.42975, "omega":-3.95678, "ax":0.40805, "ay":9.06528, "alpha":7.70466, "fx":[-28.41286,6.69977,39.46526,5.38395], "fy":[128.13079,128.07569,125.24751,132.53861]},
+    {"t":3.0473, "x":6.44668, "y":7.06392, "heading":0.60894, "vx":-1.25211, "vy":1.79159, "omega":-3.64925, "ax":-8.49659, "ay":2.1304, "alpha":15.46223, "fx":[-132.88587,-128.50742,-121.6132,-98.74207], "fy":[28.96214,-40.85397,40.54897,92.13467]},
+    {"t":3.08722, "x":6.38993, "y":7.13713, "heading":0.4756, "vx":-1.59125, "vy":1.87663, "omega":-3.03208, "ax":-9.2265, "ay":-1.26493, "alpha":11.42472, "fx":[-137.55138,-125.78611,-127.19846,-132.59789], "fy":[-1.6649,-55.30105,-48.83803,34.08348]},
+    {"t":3.12713, "x":6.31907, "y":7.21103, "heading":0.36367, "vx":-1.95953, "vy":1.82614, "omega":-2.57606, "ax":-9.26497, "ay":-1.96656, "alpha":9.58954, "fx":[-137.74677,-126.2903,-124.18039,-137.09762], "fy":[-8.80761,-55.60105,-58.97485,11.88172]},
+    {"t":3.16705, "x":6.23347, "y":7.28235, "heading":0.26849, "vx":-2.32934, "vy":1.74764, "omega":-2.19329, "ax":-9.2433, "ay":-2.34291, "alpha":8.7466, "fx":[-137.71999,-126.47829,-121.93624,-137.95189], "fy":[-12.16823,-55.83107,-64.4923,-0.34914]},
+    {"t":3.20696, "x":6.13313, "y":7.35025, "heading":0.18791, "vx":-2.69828, "vy":1.65412, "omega":-1.84417, "ax":-9.18371, "ay":-2.64984, "alpha":8.52461, "fx":[-137.65258,-126.09776,-119.0839,-137.87325], "fy":[-14.32667,-57.06435,-70.10906,-8.74313]},
+    {"t":3.24688, "x":6.01812, "y":7.41416, "heading":0.12109, "vx":-3.06485, "vy":1.54836, "omega":-1.50391, "ax":-9.08878, "ay":-2.94169, "alpha":8.80262, "fx":[-137.60537,-125.18556,-115.08171,-137.45247], "fy":[-15.65215,-59.28526,-76.7813,-15.07208]},
+    {"t":3.28679, "x":5.88854, "y":7.47362, "heading":0.06807, "vx":-3.42763, "vy":1.43094, "omega":-1.15255, "ax":-8.87918, "ay":-3.40647, "alpha":9.77166, "fx":[-137.45026,-122.65204,-106.83434,-136.50414], "fy":[-17.70839,-64.57722,-88.11259,-22.7456]},
+    {"t":3.34488, "x":5.67445, "y":7.55099, "heading":0.01761, "vx":-3.94342, "vy":1.23306, "omega":-0.58492, "ax":-8.16388, "ay":-4.81475, "alpha":9.77548, "fx":[-134.06693,-113.94956,-88.24519,-126.62257], "fy":[-33.79518,-78.46967,-106.28999,-54.43699]},
+    {"t":3.40297, "x":5.4316, "y":7.6145, "heading":0.00012, "vx":-4.41766, "vy":0.95337, "omega":-0.01706, "ax":-1.73369, "ay":-9.47751, "alpha":0.29542, "fx":[-25.68225,-25.33453,-23.47891,-23.80281], "fy":[-134.12429,-134.22306,-134.55419,-134.46444]},
+    {"t":3.46106, "x":5.17206, "y":7.65389, "heading":-0.00037, "vx":-4.51837, "vy":0.40282, "omega":0.0001, "ax":-0.28191, "ay":-9.0759, "alpha":-0.00082, "fx":[-3.99343,-3.99358,-3.99864,-3.9985], "fy":[-128.64906,-128.64867,-128.64852,-128.64891]},
+    {"t":3.51915, "x":4.90911, "y":7.66198, "heading":-0.00036, "vx":-4.53475, "vy":-0.1244, "omega":0.00005, "ax":0.02615, "ay":-0.26334, "alpha":-0.00068, "fx":[0.37174,0.37174,0.36949,0.36949], "fy":[-3.73388,-3.73164,-3.73164,-3.73388]},
+    {"t":3.57724, "x":4.64573, "y":7.65431, "heading":-0.00036, "vx":-4.53323, "vy":-0.13969, "omega":0.00001, "ax":9.58468, "ay":0.52561, "alpha":0.06705, "fx":[135.84362,135.86705,135.8773,135.85402], "fy":[7.67991,7.24428,7.22144,7.65572]},
+    {"t":3.63533, "x":4.39857, "y":7.64708, "heading":-0.00025, "vx":-3.97646, "vy":-0.10916, "omega":0.00391, "ax":9.75189, "ay":0.34374, "alpha":-0.02654, "fx":[138.23431,138.22816,138.22718,138.23334], "fy":[4.78181,4.95696,4.96327,4.7879]},
+    {"t":3.69342, "x":4.18403, "y":7.64132, "heading":-0.00007, "vx":-3.40997, "vy":-0.08919, "omega":0.00236, "ax":9.7718, "ay":0.30449, "alpha":-0.02349, "fx":[138.51561,138.51078,138.51026,138.51509], "fy":[4.23588,4.39128,4.39621,4.24064]},
+    {"t":3.75151, "x":4.00243, "y":7.63665, "heading":0.00003, "vx":-2.84233, "vy":-0.07151, "omega":0.001, "ax":9.77973, "ay":0.28, "alpha":-0.01525, "fx":[138.62692,138.62403,138.62378,138.62667], "fy":[3.91698,4.01798,4.0209,3.91983]},
+    {"t":3.8096, "x":3.85382, "y":7.63297, "heading":0.00006, "vx":-2.27422, "vy":-0.05524, "omega":0.00011, "ax":9.78408, "ay":0.26088, "alpha":-0.00792, "fx":[138.68773,138.68633,138.68623,138.68763], "fy":[3.67099,3.72343,3.72484,3.67237]},
+    {"t":3.86769, "x":3.73822, "y":7.6302, "heading":0.00006, "vx":-1.70587, "vy":-0.04009, "omega":-0.00035, "ax":9.78687, "ay":0.24447, "alpha":-0.00214, "fx":[138.72672,138.72636,138.72634,138.72669], "fy":[3.45809,3.47229,3.47264,3.45844]},
+    {"t":3.92578, "x":3.65564, "y":7.62828, "heading":0.00003, "vx":-1.13735, "vy":-0.02589, "omega":-0.00047, "ax":9.78884, "ay":0.22968, "alpha":0.00231, "fx":[138.75424,138.7546,138.75462,138.75426], "fy":[3.26349,3.24816,3.2478,3.26313]},
+    {"t":3.98387, "x":3.60609, "y":7.62717, "heading":0.00001, "vx":-0.56872, "vy":-0.01254, "omega":-0.00034, "ax":9.79032, "ay":0.21592, "alpha":0.00578, "fx":[138.77495,138.7758,138.77584,138.775], "fy":[3.08026,3.04193,3.04109,3.07941]},
+    {"t":4.04196, "x":3.58957, "y":7.6268, "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":[]
index 4e36869d76327a51a0a1257fcaf29f9de2d8a0b9..cc40bd99ccb21525990ea48f8a78bc4c5b3e3565 100644 (file)
@@ -24,7 +24,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.commands.DoNothing;
 import frc.robot.commands.LogCommand;
-import frc.robot.commands.auto_comm.ChoreoPathCommand;
+import frc.robot.commands.auto_comm.ChoreoPathCommandBuilder;
 import frc.robot.commands.auto_comm.DynamicAutoBuilder;
 import frc.robot.commands.drive_comm.DefaultDriveCommand;
 import frc.robot.commands.drive_comm.SysIDDriveCommand;
@@ -361,9 +361,9 @@ public class RobotContainer {
     addAuto(leftDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(true));
     addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(false));
 
-    ChoreoPathCommand choreo = new ChoreoPathCommand(intake, spindexer, turret, hood);
+    ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder(intake, spindexer, turret, hood);
 
-    addAuto("testChoreo", ChoreoPathCommand.basicTrajectoryAuto("test.traj", true, autoFactory));
+    addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true, autoFactory));
     addChoreoAuto("choreoLiberalLeft", choreo.leftLiberal(autoFactory));
     addChoreoAuto("choreoLiberalRight", choreo.rightLiberal(autoFactory));
     addChoreoAuto("choreoConservativeLeft", choreo.leftConservative(autoFactory));
diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java
deleted file mode 100644 (file)
index ffd8cb0..0000000
+++ /dev/null
@@ -1,270 +0,0 @@
-package frc.robot.commands.auto_comm;
-
-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 edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.commands.DoNothing;
-import frc.robot.commands.gpm.IntakeMovementCommand;
-import frc.robot.commands.gpm.RunSpindexerWithStop;
-import frc.robot.subsystems.Intake.Intake;
-import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.subsystems.turret.Turret;
-
-public class ChoreoPathCommand {
-
-  private Intake intake;
-  private Spindexer spindexer;
-  private Turret turret;
-  private Hood hood;
-
-  public ChoreoPathCommand(Intake intake, Spindexer spindexer, Turret turret, Hood hood) {
-    this.intake = intake;
-    this.spindexer = spindexer;
-    this.turret = turret;
-
-  }
-
-  public static Command basicTrajectoryAuto(String pathName, boolean resetOdemetry, AutoFactory factory) {
-    Command command = factory.trajectoryCmd(pathName);
-
-    return Commands.sequence(
-        resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
-        command);
-  }
-
-  public AutoRoutine leftLiberal(AutoFactory factory) {
-    AutoRoutine routine = factory.newRoutine("leftLiberal");
-
-    AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0);
-    AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1);
-
-    routine.active().onTrue(
-        Commands.sequence(
-            liberalSwipe.resetOdometry(),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            liberalSwipe.cmd()));
-
-    liberalSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    shallowSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    return routine;
-
-  }
-
-  public AutoRoutine rightLiberal(AutoFactory factory) {
-    AutoRoutine routine = factory.newRoutine("rightLiberal");
-
-    AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0).mirrorY();
-    AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1).mirrorY();
-
-    routine.active().onTrue(
-        Commands.sequence(
-            liberalSwipe.resetOdometry(),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-            }),
-            liberalSwipe.cmd()));
-
-    liberalSwipe.done()
-        .onTrue(Commands.sequence(
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            shallowSwipe.cmd()));
-
-    shallowSwipe.done()
-        .onTrue(Commands.sequence(
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            shallowSwipe.cmd()));
-
-    return routine;
-
-  }
-
-  
-  public AutoRoutine leftConservative(AutoFactory factory) {
-    AutoRoutine routine = factory.newRoutine("leftConservative");
-
-    AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0);
-    AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1);
-
-    routine.active().onTrue(
-        Commands.sequence(
-            liberalSwipe.resetOdometry(),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            liberalSwipe.cmd()));
-
-    liberalSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    shallowSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    return routine;
-
-  }
-
-  public AutoRoutine rightConservative(AutoFactory factory) {
-    AutoRoutine routine = factory.newRoutine("rightConservative");
-
-    AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0).mirrorY();
-    AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1).mirrorY();
-
-    routine.active().onTrue(
-        Commands.sequence(
-            liberalSwipe.resetOdometry(),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            liberalSwipe.cmd()));
-
-    liberalSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    shallowSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    return routine;
-
-  }
-
-  public AutoRoutine leftShallow(AutoFactory factory) {
-    AutoRoutine routine = factory.newRoutine("leftShallow");
-
-    AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe");
-
-    routine.active().onTrue(
-        Commands.sequence(
-            shallowSwipe.resetOdometry(),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    shallowSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    return routine;
-
-  }
-
-  public AutoRoutine rightShallow(AutoFactory factory) {
-    AutoRoutine routine = factory.newRoutine("rightShallow");
-
-    AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe").mirrorY();
-
-    routine.active().onTrue(
-        Commands.sequence(
-            shallowSwipe.resetOdometry(),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    shallowSwipe.done()
-        .onTrue(Commands.sequence(
-            new InstantCommand(() -> {
-              hood.forceHoodDown(false);
-            }),
-            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
-            new InstantCommand(() -> {
-              intake.extend();
-              intake.spinStart();
-              hood.forceHoodDown(true);
-            }),
-            shallowSwipe.cmd()));
-
-    return routine;
-
-  }
-}
diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java
new file mode 100644 (file)
index 0000000..ae39311
--- /dev/null
@@ -0,0 +1,270 @@
+package frc.robot.commands.auto_comm;
+
+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 edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.DoNothing;
+import frc.robot.commands.gpm.IntakeMovementCommand;
+import frc.robot.commands.gpm.RunSpindexerWithStop;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.hood.Hood;
+import frc.robot.subsystems.spindexer.Spindexer;
+import frc.robot.subsystems.turret.Turret;
+
+public class ChoreoPathCommandBuilder {
+
+  private Intake intake;
+  private Spindexer spindexer;
+  private Turret turret;
+  private Hood hood;
+
+  public ChoreoPathCommandBuilder(Intake intake, Spindexer spindexer, Turret turret, Hood hood) {
+    this.intake = intake;
+    this.spindexer = spindexer;
+    this.turret = turret;
+
+  }
+
+  public static Command basicTrajectoryAuto(String pathName, boolean resetOdemetry, AutoFactory factory) {
+    Command command = factory.trajectoryCmd(pathName);
+
+    return Commands.sequence(
+        resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(),
+        command);
+  }
+
+  public AutoRoutine leftLiberal(AutoFactory factory) {
+    AutoRoutine routine = factory.newRoutine("leftLiberal");
+
+    AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0);
+    AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1);
+
+    routine.active().onTrue(
+        Commands.sequence(
+            liberalSwipe.resetOdometry(),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            liberalSwipe.cmd()));
+
+    liberalSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    shallowSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    return routine;
+
+  }
+
+  public AutoRoutine rightLiberal(AutoFactory factory) {
+    AutoRoutine routine = factory.newRoutine("rightLiberal");
+
+    AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0).mirrorY();
+    AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1).mirrorY();
+
+    routine.active().onTrue(
+        Commands.sequence(
+            liberalSwipe.resetOdometry(),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+            }),
+            liberalSwipe.cmd()));
+
+    liberalSwipe.done()
+        .onTrue(Commands.sequence(
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            shallowSwipe.cmd()));
+
+    shallowSwipe.done()
+        .onTrue(Commands.sequence(
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            shallowSwipe.cmd()));
+
+    return routine;
+
+  }
+
+  
+  public AutoRoutine leftConservative(AutoFactory factory) {
+    AutoRoutine routine = factory.newRoutine("leftConservative");
+
+    AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0);
+    AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1);
+
+    routine.active().onTrue(
+        Commands.sequence(
+            liberalSwipe.resetOdometry(),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            liberalSwipe.cmd()));
+
+    liberalSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    shallowSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    return routine;
+
+  }
+
+  public AutoRoutine rightConservative(AutoFactory factory) {
+    AutoRoutine routine = factory.newRoutine("rightConservative");
+
+    AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0).mirrorY();
+    AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1).mirrorY();
+
+    routine.active().onTrue(
+        Commands.sequence(
+            liberalSwipe.resetOdometry(),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            liberalSwipe.cmd()));
+
+    liberalSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    shallowSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    return routine;
+
+  }
+
+  public AutoRoutine leftShallow(AutoFactory factory) {
+    AutoRoutine routine = factory.newRoutine("leftShallow");
+
+    AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe");
+
+    routine.active().onTrue(
+        Commands.sequence(
+            shallowSwipe.resetOdometry(),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    shallowSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    return routine;
+
+  }
+
+  public AutoRoutine rightShallow(AutoFactory factory) {
+    AutoRoutine routine = factory.newRoutine("rightShallow");
+
+    AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe").mirrorY();
+
+    routine.active().onTrue(
+        Commands.sequence(
+            shallowSwipe.resetOdometry(),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    shallowSwipe.done()
+        .onTrue(Commands.sequence(
+            new InstantCommand(() -> {
+              hood.forceHoodDown(false);
+            }),
+            new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)),
+            new InstantCommand(() -> {
+              intake.extend();
+              intake.spinStart();
+              hood.forceHoodDown(true);
+            }),
+            shallowSwipe.cmd()));
+
+    return routine;
+
+  }
+}
index acd45cbedb199415b00f36f568d6d878aa036468..2c45f5747406c55adb192fb549a120aa25519a37 100644 (file)
@@ -193,7 +193,7 @@ public class Drivetrain extends SubsystemBase {
             SmartDashboard.putData("Field", field);
         }
 
-        addMusic();
+        // addMusic();
 
     }