From: moo Date: Fri, 1 May 2026 22:00:28 +0000 (-0500) Subject: add shallow swipe X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=53e34659f30eef8c735ac3f7ec2baf301882af66;p=FRC2026.git add shallow swipe --- diff --git a/src/main/deploy/choreo/shallowSwipe.traj b/src/main/deploy/choreo/shallowSwipe.traj new file mode 100644 index 0000000..48497f4 --- /dev/null +++ b/src/main/deploy/choreo/shallowSwipe.traj @@ -0,0 +1,157 @@ +{ + "name":"shallowSwipe", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.5895698070526123, "y":7.62680196762085, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"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":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}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":4.744918951764703, "y":6.406979292631149, "r":0.6145389142105349}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.5895698070526123 m", "val":3.5895698070526123}, "y":{"exp":"7.62680196762085 m", "val":7.62680196762085}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"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":"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}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.744918951764703 m", "val":4.744918951764703}, "y":{"exp":"6.406979292631149 m", "val":6.406979292631149}, "r":{"exp":"0.6145389142105349 m", "val":0.6145389142105349}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.52705, + "y":0.52705 + }, + "backLeft":{ + "x":-0.52705, + "y":0.52705 + }, + "mass":56.69904625, + "inertia":7.0, + "gearing":7.03125, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":7.0, + "cof":1.0, + "bumper":{ + "front":0.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.02437,1.62771,2.30382,2.72486,3.24459,4.00317], + "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]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java index 44ca505..3a0c3d9 100644 --- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java +++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommand.java @@ -16,17 +16,17 @@ 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; - - } + 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); @@ -55,7 +55,7 @@ public class ChoreoPathCommand { liberalSwipe.done() .onTrue(Commands.sequence( new InstantCommand(() -> { - hood.forceHoodDown(false); + hood.forceHoodDown(false); }), new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), new InstantCommand(() -> { @@ -68,7 +68,7 @@ public class ChoreoPathCommand { shallowSwipe.done() .onTrue(Commands.sequence( new InstantCommand(() -> { - hood.forceHoodDown(false); + hood.forceHoodDown(false); }), new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), new InstantCommand(() -> { @@ -83,7 +83,7 @@ public class ChoreoPathCommand { } public AutoRoutine rightLiberal(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("leftLiberal"); + AutoRoutine routine = factory.newRoutine("rightLiberal"); AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0).mirrorY(); AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1).mirrorY(); @@ -110,4 +110,68 @@ public class ChoreoPathCommand { 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; + + } }