diff --git a/src/main/deploy/choreo/DSideCornerToDSideTrench.traj b/src/main/deploy/choreo/DSideCornerToDSideTrench.traj new file mode 100644 index 0000000..36ace88 --- /dev/null +++ b/src/main/deploy/choreo/DSideCornerToDSideTrench.traj @@ -0,0 +1,125 @@ +{ + "name":"DSideCornerToDSideTrench", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":0.4003106951713562, "y":6.92300271987915, "heading":-1.5707963267948966, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3140870332717896, "y":6.975209712982178, "heading":-1.5707963267948966, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.676270008087158, "y":7.483976364135742, "heading":1.5707963267948966, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.12706184387207, "y":7.658281326293945, "heading":1.5707963267948966, "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":false}, + {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0001}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"d_side_corner.x", "val":0.4003106951713562}, "y":{"exp":"d_side_corner.y", "val":6.92300271987915}, "heading":{"exp":"d_side_corner.heading", "val":-1.5707963267948966}, "intervals":13, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3140870332717896 m", "val":1.3140870332717896}, "y":{"exp":"6.975209712982178 m", "val":6.975209712982178}, "heading":{"exp":"-90 deg", "val":-1.5707963267948966}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.676270008087158 m", "val":2.676270008087158}, "y":{"exp":"7.483976364135742 m", "val":7.483976364135742}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"d_side_trench.x", "val":4.12706184387207}, "y":{"exp":"d_side_trench.y", "val":7.658281326293945}, "heading":{"exp":"d_side_trench.heading", "val":1.5707963267948966}, "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":false}, + {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"1e-4 rad / s", "val":0.0001}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2794, + "y":0.2794 + }, + "backLeft":{ + "x":-0.2794, + "y":0.2794 + }, + "mass":68.0388555, + "inertia":6.0, + "gearing":6.5, + "radius":0.0508, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.3937, + "side":0.3937, + "back":0.3937 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.47605,1.02564,1.61481], + "samples":[ + {"t":0.0, "x":0.40031, "y":6.923, "heading":-1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.72648, "ay":-0.20644, "alpha":-7.3663, "fx":[144.94839,150.06604,151.14993,147.57503], "fy":[-49.96358,-31.80735,26.17067,41.5542]}, + {"t":0.03662, "x":0.40616, "y":6.92286, "heading":-1.5708, "vx":0.31956, "vy":-0.00756, "omega":-0.26975, "ax":8.77944, "ay":-0.12124, "alpha":-6.63668, "fx":[147.00956,150.68471,151.28326,148.36569], "fy":[-43.45384,-28.63413,25.27791,38.5613]}, + {"t":0.07324, "x":0.42375, "y":6.92251, "heading":-1.58067, "vx":0.64105, "vy":-0.012, "omega":-0.51278, "ax":8.83543, "ay":-0.01368, "alpha":-5.75965, "fx":[149.15638,151.34375,151.49081,149.16179], "fy":[-35.28149,-24.76668,23.8533,35.26404]}, + {"t":0.10986, "x":0.45315, "y":6.92206, "heading":-1.59945, "vx":0.9646, "vy":-0.0125, "omega":-0.72369, "ax":8.89204, "ay":0.12634, "alpha":-4.68441, "fx":[151.16992,152.07367,151.75279,150.00779], "fy":[-25.12095,-19.54108,21.9091,31.34897]}, + {"t":0.14648, "x":0.49443, "y":6.92168, "heading":-1.62595, "vx":1.29022, "vy":-0.00787, "omega":-0.89523, "ax":8.94318, "ay":0.31335, "alpha":-3.33528, "fx":[152.6842,152.82258,152.04607,150.93101], "fy":[-12.67363,-11.80799,19.42906,26.37282]}, + {"t":0.1831, "x":0.54768, "y":6.92161, "heading":-1.65874, "vx":1.61771, "vy":0.0036, "omega":-1.01736, "ax":8.97453, "ay":0.57041, "alpha":-1.57596, "fx":[153.15607,153.20968,152.34381,151.90717], "fy":[2.21557,0.57521,16.32185,19.6972]}, + {"t":0.21971, "x":0.61293, "y":6.92212, "heading":-1.69599, "vx":1.94635, "vy":0.02449, "omega":-1.07507, "ax":8.9482, "ay":0.93876, "alpha":0.89919, "fx":[151.89773,151.54151,152.60903,152.77699], "fy":[19.41166,21.84283,12.26299,10.35482]}, + {"t":0.25633, "x":0.69021, "y":6.92365, "heading":-1.73536, "vx":2.27402, "vy":0.05887, "omega":-1.04215, "ax":8.74203, "ay":1.4821, "alpha":4.83379, "fx":[148.20063,140.83276,152.74052,153.02396], "fy":[38.39977,59.66854,5.97413,-3.20229]}, + {"t":0.29295, "x":0.77934, "y":6.9268, "heading":-1.77352, "vx":2.59415, "vy":0.11314, "omega":-0.86514, "ax":7.98626, "ay":2.05465, "alpha":11.53829, "fx":[141.47904,99.18352,151.57752,151.13591], "fy":[58.40445,116.21896,-11.26897,-23.5586]}, + {"t":0.32957, "x":0.87969, "y":6.93232, "heading":-1.8052, "vx":2.8866, "vy":0.18838, "omega":-0.44262, "ax":2.57407, "ay":1.26841, "alpha":35.62909, "fx":[131.2834,19.68967,-118.90699,143.07087], "fy":[78.63544,151.543,-90.12653,-53.75078]}, + {"t":0.36619, "x":0.98712, "y":6.94007, "heading":-1.82141, "vx":2.98086, "vy":0.23483, "omega":0.86209, "ax":0.6751, "ay":1.74599, "alpha":38.60053, "fx":[116.78357,-43.09472,-148.79213,121.03612], "fy":[98.86875,146.79056,-33.75663,-93.10774]}, + {"t":0.40281, "x":1.09673, "y":6.94984, "heading":-1.78984, "vx":3.00558, "vy":0.29876, "omega":2.27561, "ax":-0.72207, "ay":1.37493, "alpha":39.06367, "fx":[94.09921,-80.00989,-149.64464,86.42662], "fy":[120.66408,130.55928,-31.85013,-125.82469]}, + {"t":0.43943, "x":1.20631, "y":6.9617, "heading":-1.70651, "vx":2.97914, "vy":0.34911, "omega":3.70608, "ax":-1.95727, "ay":1.08595, "alpha":37.84516, "fx":[61.20363,-103.68975,-148.3957,57.71132], "fy":[140.28711,112.80504,-37.88401,-141.32131]}, + {"t":0.47605, "x":1.31409, "y":6.97521, "heading":-1.5708, "vx":2.90746, "vy":0.38888, "omega":5.09194, "ax":-3.57513, "ay":0.89096, "alpha":33.81019, "fx":[6.22353,-120.38869,-148.12869,19.04644], "fy":[152.28031,94.16587,-37.00031,-148.82615]}, + {"t":0.49378, "x":1.36507, "y":6.98224, "heading":-1.48052, "vx":2.84408, "vy":0.40467, "omega":5.69135, "ax":-5.59831, "ay":0.99802, "alpha":25.97688, "fx":[-31.70447,-128.85915,-148.67428,-71.66496], "fy":[148.89826,82.00122,-34.1063,-128.88877]}, + {"t":0.51151, "x":1.41461, "y":6.98958, "heading":-1.37962, "vx":2.74483, "vy":0.42237, "omega":6.15188, "ax":-7.46808, "ay":2.32511, "alpha":14.02593, "fx":[-73.68084,-136.4465,-150.10663,-147.88596], "fy":[133.036,68.33503,-26.08878,-17.08444]}, + {"t":0.52923, "x":1.4621, "y":6.99743, "heading":-1.27056, "vx":2.61243, "vy":0.46359, "omega":6.40054, "ax":-8.03185, "ay":2.48364, "alpha":9.54498, "fx":[-104.50493,-142.0789,-151.10924,-148.78501], "fy":[110.2979,55.22056,-17.76865,21.2343]}, + {"t":0.54696, "x":1.50715, "y":7.00604, "heading":-1.15709, "vx":2.47004, "vy":0.50762, "omega":6.56976, "ax":-8.0441, "ay":2.18716, "alpha":9.92553, "fx":[-106.10395,-145.02086,-148.8415,-147.34471], "fy":[108.48734,46.42663,-30.25991,24.15775]}, + {"t":0.56469, "x":1.54968, "y":7.01538, "heading":-1.04061, "vx":2.32743, "vy":0.54639, "omega":6.74573, "ax":-7.93912, "ay":1.93091, "alpha":10.92718, "fx":[-104.95615,-147.45334,-144.07447,-143.68478], "fy":[109.23969,37.14451,-46.8961,31.88877]}, + {"t":0.58242, "x":1.58969, "y":7.02537, "heading":-0.92102, "vx":2.18668, "vy":0.58063, "omega":6.93945, "ax":-7.79257, "ay":1.77593, "alpha":11.92374, "fx":[-107.2304,-149.40777,-137.91758,-135.64163], "fy":[106.48687,26.51805,-61.39564,49.22289]}, + {"t":0.60015, "x":1.62724, "y":7.03594, "heading":-0.79799, "vx":2.04853, "vy":0.61211, "omega":7.15085, "ax":-7.64159, "ay":1.61019, "alpha":12.95871, "fx":[-112.80383,-150.48052,-131.70734,-124.93356], "fy":[99.68321,15.03114,-71.97538,66.81639]}, + {"t":0.61788, "x":1.66235, "y":7.04705, "heading":-0.67122, "vx":1.91305, "vy":0.64066, "omega":7.38059, "ax":-7.4766, "ay":1.37918, "alpha":14.01535, "fx":[-118.71154,-150.26895,-125.44657,-114.27222], "fy":[90.88594,3.67339,-79.49303,78.7712]}, + {"t":0.63561, "x":1.69509, "y":7.05862, "heading":-0.54037, "vx":1.7805, "vy":0.66511, "omega":7.62906, "ax":-7.23892, "ay":1.18783, "alpha":14.90938, "fx":[-122.48565,-148.19094,-119.55726,-102.2937], "fy":[81.8326,-5.57665,-81.8922,86.45482]}, + {"t":0.65333, "x":1.72552, "y":7.0706, "heading":-0.40512, "vx":1.65216, "vy":0.68617, "omega":7.89338, "ax":-6.65219, "ay":1.65384, "alpha":14.27021, "fx":[-116.07971,-140.54149,-116.72589,-79.26044], "fy":[77.62647,-1.00053,-57.12673,93.02613]}, + {"t":0.67106, "x":1.75377, "y":7.08303, "heading":-0.26518, "vx":1.53423, "vy":0.71549, "omega":8.14637, "ax":4.09324, "ay":4.7561, "alpha":-11.75815, "fx":[115.24811,97.72469,35.90536,29.62094], "fy":[28.58951,86.49038,121.24784,87.27162]}, + {"t":0.68879, "x":1.78161, "y":7.09646, "heading":-0.12075, "vx":1.6068, "vy":0.79981, "omega":7.93792, "ax":6.86463, "ay":3.05823, "alpha":-12.22932, "fx":[144.58759,126.21518,69.43089,126.82792], "fy":[-6.44097,74.33857,124.9133,15.26768]}, + {"t":0.70652, "x":1.81118, "y":7.11112, "heading":0.01997, "vx":1.7285, "vy":0.85403, "omega":7.72111, "ax":7.34798, "ay":3.3918, "alpha":-10.12825, "fx":[148.06223,125.9807,84.80125,141.10364], "fy":[11.00427,79.91435,119.88131,19.97423]}, + {"t":0.72425, "x":1.84297, "y":7.12679, "heading":0.15686, "vx":1.85877, "vy":0.91416, "omega":7.54155, "ax":7.47917, "ay":3.92304, "alpha":-7.75562, "fx":[146.6579,123.22569,96.30393,142.68653], "fy":[31.17803,86.03848,113.33207,36.37051]}, + {"t":0.74198, "x":1.8771, "y":7.14362, "heading":0.29056, "vx":1.99136, "vy":0.98371, "omega":7.40405, "ax":7.43555, "ay":4.36404, "alpha":-6.22992, "fx":[143.26172,119.73994,101.75894,141.14561], "fy":[46.8336,91.83994,109.91429,48.33655]}, + {"t":0.75971, "x":1.91358, "y":7.16174, "heading":0.42182, "vx":2.12319, "vy":1.06108, "omega":7.2936, "ax":5.9292, "ay":2.95554, "alpha":-21.83169, "fx":[150.7381,104.0987,18.70312,129.87578], "fy":[14.87018,110.10106,147.87996,-71.75947]}, + {"t":0.77744, "x":1.95215, "y":7.18102, "heading":0.55113, "vx":2.2283, "vy":1.11347, "omega":6.90656, "ax":4.77586, "ay":2.311, "alpha":-28.02434, "fx":[151.82123,98.91724,-29.17112,103.37641], "fy":[5.96323,115.13407,145.31671,-109.17624]}, + {"t":0.79516, "x":1.9924, "y":7.20112, "heading":0.67357, "vx":2.31297, "vy":1.15444, "omega":6.40972, "ax":5.20663, "ay":2.44053, "alpha":-25.64783, "fx":[151.9671,100.96431,-11.00397,112.3259], "fy":[8.45476,113.52013,145.3895,-101.31343]}, + {"t":0.81289, "x":2.03423, "y":7.22197, "heading":0.78721, "vx":2.40528, "vy":1.19771, "omega":5.95502, "ax":5.53938, "ay":2.4952, "alpha":-23.37134, "fx":[151.89277,99.64214,8.49622,116.86206], "fy":[12.3511,114.83515,139.44202,-96.85752]}, + {"t":0.83062, "x":2.07774, "y":7.2436, "heading":0.89278, "vx":2.50348, "vy":1.24195, "omega":5.54068, "ax":6.01734, "ay":1.44396, "alpha":-20.92566, "fx":[151.56639,95.38749,42.94523,119.51357], "fy":[17.20156,118.51492,56.63684,-94.10771]}, + {"t":0.84835, "x":2.12307, "y":7.26584, "heading":0.99101, "vx":2.61016, "vy":1.26755, "omega":5.1697, "ax":6.89277, "ay":-0.65844, "alpha":-18.73911, "fx":[150.90171,94.95479,97.30673,125.81268], "fy":[22.96421,118.94268,-100.82433,-85.88203]}, + {"t":0.86608, "x":2.17043, "y":7.28821, "heading":1.08266, "vx":2.73236, "vy":1.25587, "omega":4.83748, "ax":7.21256, "ay":-0.38205, "alpha":-17.68135, "fx":[150.0419,96.72277,111.97081,131.99905], "fy":[28.43986,117.5718,-95.67199,-76.3338]}, + {"t":0.88381, "x":2.22, "y":7.31042, "heading":1.16843, "vx":2.86023, "vy":1.2491, "omega":4.52401, "ax":7.40239, "ay":-0.13256, "alpha":-17.03469, "fx":[149.08541,98.83863,118.95051,136.77543], "fy":[33.36599,115.85274,-90.57252,-67.66524]}, + {"t":0.90154, "x":2.27187, "y":7.33254, "heading":1.24863, "vx":2.99147, "vy":1.24675, "omega":4.22201, "ax":6.6625, "ay":-0.46884, "alpha":-21.58986, "fx":[147.85169,87.35798,82.24163,135.85729], "fy":[38.73074,124.77092,-125.65589,-69.74482]}, + {"t":0.91926, "x":2.32596, "y":7.35457, "heading":1.32348, "vx":3.10958, "vy":1.23844, "omega":3.83925, "ax":4.79424, "ay":-0.6274, "alpha":-30.27287, "fx":[145.84648,53.87693,-3.79023,130.2612], "fy":[45.94582,142.50679,-151.19717,-79.94314]}, + {"t":0.93699, "x":2.38184, "y":7.37643, "heading":1.39155, "vx":3.19458, "vy":1.22732, "omega":3.30255, "ax":3.63918, "ay":-0.4028, "alpha":-34.35547, "fx":[143.24885,17.57104,-39.84464,126.6301], "fy":[53.66142,151.41175,-146.72845,-85.75094]}, + {"t":0.95472, "x":2.43904, "y":7.39812, "heading":1.4501, "vx":3.25909, "vy":1.22018, "omega":2.69348, "ax":2.86747, "ay":-0.27557, "alpha":-36.51385, "fx":[140.28393,-15.75408,-53.91271,124.48249], "fy":[61.1113,151.71659,-142.60873,-88.96834]}, + {"t":0.97245, "x":2.49727, "y":7.41971, "heading":1.49785, "vx":3.30993, "vy":1.21529, "omega":2.04614, "ax":2.3067, "ay":-0.24646, "alpha":-37.78827, "fx":[137.21676,-42.83006,-60.57779,123.13609], "fy":[67.80782,146.51374,-140.16941,-90.92108]}, + {"t":0.99018, "x":2.55632, "y":7.44122, "heading":1.53412, "vx":3.35083, "vy":1.21092, "omega":1.3762, "ax":1.8925, "ay":-0.26983, "alpha":-38.56818, "fx":[134.33074,-63.00556,-64.62989,122.06792], "fy":[73.43133,139.15364,-138.51662,-92.42746]}, + {"t":1.00791, "x":2.61602, "y":7.46264, "heading":1.55852, "vx":3.38438, "vy":1.20614, "omega":0.69244, "ax":1.58361, "ay":-0.31305, "alpha":-39.05756, "fx":[131.89976,-77.11007,-67.94683,120.90438], "fy":[77.77355,131.97059,-137.03725,-94.00625]}, + {"t":1.02564, "x":2.67627, "y":7.48398, "heading":1.5708, "vx":3.41245, "vy":1.20059, "omega":0.0, "ax":8.51198, "ay":-2.8388, "alpha":-0.00012, "fx":[144.78663,144.78649,144.7861,144.78624], "fy":[-48.28635,-48.28674,-48.28794,-48.28754]}, + {"t":1.06029, "x":2.79965, "y":7.52388, "heading":1.5708, "vx":3.70745, "vy":1.1022, "omega":0.0, "ax":7.92373, "ay":-4.14863, "alpha":0.0, "fx":[134.78044,134.78044,134.78044,134.78044], "fy":[-70.56701,-70.56701,-70.56701,-70.56701]}, + {"t":1.09495, "x":2.9329, "y":7.55959, "heading":1.5708, "vx":3.98207, "vy":0.95842, "omega":0.0, "ax":5.155, "ay":-7.22425, "alpha":0.0, "fx":[87.68506,87.68506,87.68506,87.68506], "fy":[-122.88238,-122.88238,-122.88238,-122.88238]}, + {"t":1.12961, "x":3.074, "y":7.58847, "heading":1.5708, "vx":4.16072, "vy":0.70805, "omega":0.0, "ax":-4.60532, "ay":-7.56798, "alpha":0.0, "fx":[-78.33521,-78.33521,-78.33521,-78.33521], "fy":[-128.72923,-128.72923,-128.72923,-128.72923]}, + {"t":1.16426, "x":3.21543, "y":7.60846, "heading":1.5708, "vx":4.00112, "vy":0.44577, "omega":0.0, "ax":-8.05356, "ay":-3.87342, "alpha":0.0, "fx":[-136.98871,-136.98871,-136.98871,-136.98871], "fy":[-65.88579,-65.88579,-65.88579,-65.88579]}, + {"t":1.19892, "x":3.34926, "y":7.62158, "heading":1.5708, "vx":3.722, "vy":0.31153, "omega":0.0, "ax":-8.6624, "ay":-2.32729, "alpha":0.0, "fx":[-147.34494,-147.34494,-147.34494,-147.34494], "fy":[-39.58651,-39.58651,-39.58651,-39.58651]}, + {"t":1.23358, "x":3.47305, "y":7.63098, "heading":1.5708, "vx":3.42179, "vy":0.23087, "omega":0.0, "ax":-8.84386, "ay":-1.58851, "alpha":0.0, "fx":[-150.43154,-150.43154,-150.43154,-150.43154], "fy":[-27.02013,-27.02013,-27.02013,-27.02013]}, + {"t":1.26824, "x":3.58633, "y":7.63803, "heading":1.5708, "vx":3.11529, "vy":0.17582, "omega":0.0, "ax":-8.91866, "ay":-1.16527, "alpha":0.0, "fx":[-151.70385,-151.70385,-151.70385,-151.70385], "fy":[-19.82085,-19.82085,-19.82085,-19.82085]}, + {"t":1.30289, "x":3.68894, "y":7.64342, "heading":1.5708, "vx":2.80619, "vy":0.13543, "omega":0.0, "ax":-8.95591, "ay":-0.89282, "alpha":0.0, "fx":[-152.33752,-152.33752,-152.33752,-152.33752], "fy":[-15.18667,-15.18667,-15.18667,-15.18667]}, + {"t":1.33755, "x":3.78082, "y":7.64758, "heading":1.5708, "vx":2.49581, "vy":0.10449, "omega":0.0, "ax":-8.97687, "ay":-0.70329, "alpha":0.0, "fx":[-152.69395,-152.69395,-152.69395,-152.69395], "fy":[-11.96283,-11.96283,-11.96283,-11.96283]}, + {"t":1.37221, "x":3.86192, "y":7.65078, "heading":1.5708, "vx":2.18469, "vy":0.08011, "omega":0.0, "ax":-8.98969, "ay":-0.56401, "alpha":0.0, "fx":[-152.91207,-152.91207,-152.91207,-152.91207], "fy":[-9.59371,-9.59371,-9.59371,-9.59371]}, + {"t":1.40686, "x":3.93224, "y":7.65322, "heading":1.5708, "vx":1.87314, "vy":0.06057, "omega":0.0, "ax":-8.99804, "ay":-0.45742, "alpha":0.0, "fx":[-153.05406,-153.05406,-153.05406,-153.05406], "fy":[-7.78053,-7.78053,-7.78053,-7.78053]}, + {"t":1.44152, "x":3.99175, "y":7.65504, "heading":1.5708, "vx":1.56129, "vy":0.04471, "omega":0.0, "ax":-9.00373, "ay":-0.37324, "alpha":0.0, "fx":[-153.15094,-153.15094,-153.15094,-153.15094], "fy":[-6.34878,-6.34878,-6.34878,-6.34878]}, + {"t":1.47618, "x":4.04046, "y":7.65637, "heading":1.5708, "vx":1.24925, "vy":0.03178, "omega":0.0, "ax":-9.00777, "ay":-0.30511, "alpha":0.0, "fx":[-153.21953,-153.21953,-153.21953,-153.21953], "fy":[-5.18989,-5.18989,-5.18989,-5.18989]}, + {"t":1.51084, "x":4.07834, "y":7.65728, "heading":1.5708, "vx":0.93707, "vy":0.0212, "omega":0.0, "ax":-9.01071, "ay":-0.24885, "alpha":0.0, "fx":[-153.26955,-153.26955,-153.26955,-153.26955], "fy":[-4.23282,-4.23282,-4.23282,-4.23282]}, + {"t":1.54549, "x":4.10541, "y":7.65787, "heading":1.5708, "vx":0.62478, "vy":0.01258, "omega":0.0, "ax":-9.0129, "ay":-0.2016, "alpha":0.0, "fx":[-153.30693,-153.30693,-153.30693,-153.30693], "fy":[-3.42919,-3.42919,-3.42919,-3.42919]}, + {"t":1.58015, "x":4.12165, "y":7.65818, "heading":1.5708, "vx":0.31242, "vy":0.00559, "omega":0.0, "ax":-9.01458, "ay":-0.16137, "alpha":0.00001, "fx":[-153.33543,-153.33543,-153.33543,-153.33543], "fy":[-2.74494,-2.74494,-2.74486,-2.74486]}, + {"t":1.61481, "x":4.12706, "y":7.65828, "heading":1.5708, "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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b174d21..b91f266 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -257,6 +257,14 @@ public void configAutonomous() { ConstAuto.DEPOT_SHOOTING_TIMEOUT)); autoStartingPoses.put(DSideNeutralWithDepot, ChoreoTraj.DSideTrenchToNeutral); + Command SecondDSideNeutralWithDepot = Commands.sequence( + IntakeAndShootOnTheFly(ChoreoTraj.DSideTrenchToNeutral, + ChoreoTraj.SecondDSideNeutralToAlliance, + ChoreoTraj.DSideAllianceToDepot, + ConstAuto.NEUTRAL_TO_ALLIANCE_TRAVELING_SHOOTING_TIMEOUT, + ConstAuto.DEPOT_SHOOTING_TIMEOUT)); + autoStartingPoses.put(SecondDSideNeutralWithDepot, ChoreoTraj.DSideTrenchToNeutral); + Command DSideDoubleNeutralWithDepot = Commands.sequence( DSideNeutral.asProxy(), IntakeAndShootOnTheFly(ChoreoTraj.DSideTrenchToNeutral, @@ -266,6 +274,17 @@ public void configAutonomous() { ConstAuto.DEPOT_SHOOTING_TIMEOUT)); autoStartingPoses.put(DSideDoubleNeutralWithDepot, ChoreoTraj.DSideTrenchToNeutral); + Command DSideNeutralWithDepotThenNeutral = Commands.sequence( + SecondDSideNeutralWithDepot.asProxy(), + ShootingOnMove(ChoreoTraj.DSideCornerToDSideTrench), + IntakeAndShootOnTheFly( + ChoreoTraj.DSideTrenchToNeutral, + ChoreoTraj.SecondDSideNeutralToAlliance, + ChoreoTraj.DSideAllianceToTrench, + ConstAuto.NEUTRAL_TO_ALLIANCE_TRAVELING_SHOOTING_TIMEOUT, + ConstAuto.DEPOT_SHOOTING_TIMEOUT)); + autoStartingPoses.put(DSideNeutralWithDepotThenNeutral, ChoreoTraj.DSideTrenchToNeutral); + Command OSideNeutral = Commands.sequence( IntakeAndShootOnTheFly(ChoreoTraj.OSideTrenchToNeutral, ChoreoTraj.FirstOSideNeutralToAlliance, @@ -301,8 +320,7 @@ public void configAutonomous() { autoStartingPoses.put(OSideDoubleNeutralWithOutpost, ChoreoTraj.OSideTrenchToNeutral); Command OSidePreloadAndBackup = Commands.sequence( - ShootingOnMove(ChoreoTraj.OSidePreloadAndBackup, - ConstAuto.OUTPOST_SHOOTING_TIMEOUT)); + ShootingOnMove(ChoreoTraj.OSidePreloadAndBackup)); autoStartingPoses.put(OSidePreloadAndBackup, ChoreoTraj.OSidePreloadAndBackup); // Example: Add autonomous routines to the chooser @@ -313,7 +331,9 @@ public void configAutonomous() { autoChooser.addOption("DSideNeutral", DSideNeutral); autoChooser.addOption("DSideDoubleNeutral", DSideDoubleNeutral); autoChooser.addOption("DSideNeutralWithDepot", DSideNeutralWithDepot); + autoChooser.addOption("SecondDSideNeutralWithDepot", SecondDSideNeutralWithDepot); autoChooser.addOption("DSideDoubleNeutralWithDepot", DSideDoubleNeutralWithDepot); + autoChooser.addOption("DSideNeutralWithDepotThenNeutral", DSideNeutralWithDepotThenNeutral); autoChooser.addOption("OSideNeutral", OSideNeutral); autoChooser.addOption("OSideDoubleNeutral", OSideDoubleNeutral); autoChooser.addOption("OSideNeutralWithOutpost", OSideNeutralWithOutpost); @@ -340,21 +360,21 @@ Command ShootOnly(Command prepPreset, Time shootingTime, Time preppingTime) { Commands.runOnce(() -> stateMachineInstance.setRobotState(RobotState.NONE)).asProxy(), prepPreset.asProxy().withTimeout(preppingTime), TRY_SHOOTING_ON_PRESET.asProxy().withTimeout(shootingTime), - TRY_NONE.asProxy()); + TRY_NONE.asProxy().withTimeout(0.001)); } Command IntakeOnly(ChoreoTraj intakingPath, Time intakingTime) { return Commands.sequence( Commands.runOnce(() -> stateMachineInstance.setRobotState(RobotState.NONE)).asProxy(), runPath(intakingPath).deadlineFor(TRY_INTAKING.asProxy()), - TRY_NONE.asProxy()); + TRY_NONE.asProxy().withTimeout(0.001)); } - Command ShootingOnMove(ChoreoTraj shootingPath, Time shootingTime) { + Command ShootingOnMove(ChoreoTraj shootingPath) { return Commands.sequence( Commands.runOnce(() -> stateMachineInstance.setRobotState(RobotState.NONE)).asProxy(), - runPath(shootingPath).deadlineFor(TRY_SHOOTING_ON_FLY.asProxy()), - TRY_NONE.asProxy()); + runPath(shootingPath).asProxy().deadlineFor(TRY_SHOOTING_ON_FLY.asProxy()), + TRY_NONE.asProxy().withTimeout(0.001)); } Command IntakeAndShootOnTheFly(ChoreoTraj intakingPath, ChoreoTraj travelingPath, ChoreoTraj shootingPath, diff --git a/src/main/java/frc/robot/constants/ChoreoTraj.java b/src/main/java/frc/robot/constants/ChoreoTraj.java index f7f0700..ef29915 100644 --- a/src/main/java/frc/robot/constants/ChoreoTraj.java +++ b/src/main/java/frc/robot/constants/ChoreoTraj.java @@ -1,4 +1,4 @@ -// spotless:off + package frc.robot.constants; import edu.wpi.first.math.geometry.Pose2d; @@ -6,10 +6,10 @@ import java.util.Map; import java.util.OptionalInt; +// If these imports cause errors because you're not using ChoreoLib, +// turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; -// If the 2 imports above cause errors because you're not using ChoreoLib, -// turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. /** * A class containing the name, start pose, end pose, and total time of every Choreo trajectory. @@ -24,220 +24,228 @@ public record ChoreoTraj( Pose2d initialPoseBlue, Pose2d endPoseBlue ) { - public static final ChoreoTraj xPreviewPreloadDepotOutpost = new ChoreoTraj( - "xPreviewPreloadDepotOutpost", - OptionalInt.empty(), - 2.9149, - new Pose2d(3.64442, 5.95939, Rotation2d.fromRadians(0)), - new Pose2d(0.74877, 0.82106, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj xPreviewFirstDSideTrenchNeutral = new ChoreoTraj( - "xPreviewFirstDSideTrenchNeutral", - OptionalInt.empty(), - 5.67607, - new Pose2d(4.12706, 7.65828, Rotation2d.fromRadians(1.5708)), - new Pose2d(3.3141, 5.37187, Rotation2d.fromRadians(0.5236)) - ); - public static final ChoreoTraj FirstDSideNeutralToAlliance = new ChoreoTraj( - "FirstDSideNeutralToAlliance", - OptionalInt.empty(), - 2.42112, - new Pose2d(7.84374, 4.53587, Rotation2d.fromRadians(1.5708)), - new Pose2d(3.3141, 5.37187, Rotation2d.fromRadians(0.5236)) - ); - public static final ChoreoTraj OSideAllianceToOutpost = new ChoreoTraj( - "OSideAllianceToOutpost", - OptionalInt.empty(), - 1.57579, - new Pose2d(3.3141, 2.69813, Rotation2d.fromRadians(-0.5236)), - new Pose2d(0.39741, 0.66858, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj SecondOSideNeutralToAlliance = new ChoreoTraj( - "SecondOSideNeutralToAlliance", - OptionalInt.empty(), - 3.35108, - new Pose2d(7.84374, 3.53413, Rotation2d.fromRadians(-1.5708)), - new Pose2d(3.3141, 2.69813, Rotation2d.fromRadians(-0.5236)) - ); - public static final ChoreoTraj xPreviewSecondOSideTrenchNeutral = new ChoreoTraj( - "xPreviewSecondOSideTrenchNeutral", - OptionalInt.empty(), - 6.5858, - new Pose2d(4.12706, 0.41172, Rotation2d.fromRadians(-1.5708)), - new Pose2d(3.3141, 2.69813, Rotation2d.fromRadians(-0.5236)) - ); - public static final ChoreoTraj DSideAllianceToTrench = new ChoreoTraj( - "DSideAllianceToTrench", - OptionalInt.empty(), - 1.41441, - new Pose2d(3.3141, 5.37187, Rotation2d.fromRadians(0.5236)), - new Pose2d(4.12706, 7.65828, Rotation2d.fromRadians(1.5708)) - ); - public static final ChoreoTraj DSideBumpToDepot = new ChoreoTraj( - "DSideBumpToDepot", - OptionalInt.empty(), - 1.14827, - new Pose2d(3.64442, 5.95939, Rotation2d.fromRadians(0)), - new Pose2d(0.68343, 5.97907, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj xPreviewOSidePreloadAndBackup = new ChoreoTraj( - "xPreviewOSidePreloadAndBackup", - OptionalInt.empty(), - 1.33437, - new Pose2d(3.58253, 4.02943, Rotation2d.fromRadians(0)), - new Pose2d(0.4406, 1.92714, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj xPreviewPreloadDepot = new ChoreoTraj( - "xPreviewPreloadDepot", - OptionalInt.empty(), - 1.93922, - new Pose2d(3.74442, 5.95939, Rotation2d.fromRadians(0)), - new Pose2d(0.67815, 7.4473, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj DepotToOutpost = new ChoreoTraj( - "DepotToOutpost", - OptionalInt.empty(), - 1.76382, - new Pose2d(0.78688, 5.95277, Rotation2d.fromRadians(0)), - new Pose2d(0.74877, 0.82106, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj xPreview4414DSideTrenchNeutral = new ChoreoTraj( - "xPreview4414DSideTrenchNeutral", - OptionalInt.empty(), - 8.52061, - new Pose2d(4.12706, 7.65828, Rotation2d.fromRadians(1.5708)), - new Pose2d(0.40031, 6.923, Rotation2d.fromRadians(-1.5708)) - ); - public static final ChoreoTraj SecondDSideNeutralToAlliance = new ChoreoTraj( - "SecondDSideNeutralToAlliance", - OptionalInt.empty(), - 3.31265, - new Pose2d(7.84374, 4.53587, Rotation2d.fromRadians(1.5708)), - new Pose2d(3.3141, 5.37187, Rotation2d.fromRadians(0.5236)) - ); - public static final ChoreoTraj OSidePreloadAndBackup = new ChoreoTraj( - "OSidePreloadAndBackup", - OptionalInt.empty(), - 1.33437, - new Pose2d(3.58253, 4.02943, Rotation2d.fromRadians(0)), - new Pose2d(0.4406, 1.92714, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj OSideTrenchToNeutral = new ChoreoTraj( - "OSideTrenchToNeutral", - OptionalInt.empty(), - 3.39378, - new Pose2d(4.12706, 0.41172, Rotation2d.fromRadians(-1.5708)), - new Pose2d(7.84374, 3.53413, Rotation2d.fromRadians(-1.5708)) - ); - public static final ChoreoTraj OSideAllianceToTrench = new ChoreoTraj( - "OSideAllianceToTrench", - OptionalInt.empty(), - 1.41441, - new Pose2d(3.3141, 2.69813, Rotation2d.fromRadians(-0.5236)), - new Pose2d(4.12706, 0.41172, Rotation2d.fromRadians(-1.5708)) - ); - public static final ChoreoTraj xPreviewSecondDSideTrenchNeutral = new ChoreoTraj( - "xPreviewSecondDSideTrenchNeutral", - OptionalInt.empty(), - 6.52944, - new Pose2d(4.12706, 7.65828, Rotation2d.fromRadians(1.5708)), - new Pose2d(3.3141, 5.37187, Rotation2d.fromRadians(0.5236)) - ); public static final ChoreoTraj AutoPIDTuning = new ChoreoTraj( - "AutoPIDTuning", - OptionalInt.empty(), - 1.15674, - new Pose2d(3.57341, 6.05182, Rotation2d.fromRadians(0)), - new Pose2d(3.57341, 3.05182, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj FirstOSideNeutralToAlliance = new ChoreoTraj( - "FirstOSideNeutralToAlliance", - OptionalInt.empty(), - 2.42112, - new Pose2d(7.84374, 3.53413, Rotation2d.fromRadians(-1.5708)), - new Pose2d(3.3141, 2.69813, Rotation2d.fromRadians(-0.5236)) - ); - public static final ChoreoTraj xPreviewFirstOSideTrenchNeutral = new ChoreoTraj( - "xPreviewFirstOSideTrenchNeutral", - OptionalInt.empty(), - 5.67607, - new Pose2d(4.12706, 0.41172, Rotation2d.fromRadians(-1.5708)), - new Pose2d(3.3141, 2.69813, Rotation2d.fromRadians(-0.5236)) - ); - public static final ChoreoTraj DSideAllianceToDepot = new ChoreoTraj( - "DSideAllianceToDepot", - OptionalInt.empty(), - 2.50806, - new Pose2d(3.3141, 5.37187, Rotation2d.fromRadians(0.5236)), - new Pose2d(0.40031, 6.923, Rotation2d.fromRadians(-1.5708)) - ); - public static final ChoreoTraj DSideTrenchToNeutral = new ChoreoTraj( - "DSideTrenchToNeutral", - OptionalInt.empty(), - 3.37498, - new Pose2d(4.12706, 7.65828, Rotation2d.fromRadians(1.5708)), - new Pose2d(7.84374, 4.53587, Rotation2d.fromRadians(1.5708)) - ); - public static final ChoreoTraj PreloadAndBackupTower = new ChoreoTraj( - "PreloadAndBackupTower", - OptionalInt.empty(), - 0.95141, - new Pose2d(3.58253, 4.02943, Rotation2d.fromRadians(0)), - new Pose2d(1.54855, 4.02943, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj DepotToCorner = new ChoreoTraj( - "DepotToCorner", - OptionalInt.empty(), - 0.8504, - new Pose2d(0.68343, 5.97907, Rotation2d.fromRadians(0)), - new Pose2d(0.59227, 7.60044, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj xPreviewOSideTrenchNeutralOutpost = new ChoreoTraj( - "xPreviewOSideTrenchNeutralOutpost", - OptionalInt.empty(), - 8.00154, - new Pose2d(4.12706, 0.41172, Rotation2d.fromRadians(-1.5708)), - new Pose2d(0.39741, 0.66858, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj xPreviewPreloadAndBackup = new ChoreoTraj( - "xPreviewPreloadAndBackup", - OptionalInt.empty(), - 0.95141, - new Pose2d(3.58253, 4.02943, Rotation2d.fromRadians(0)), - new Pose2d(1.54855, 4.02943, Rotation2d.fromRadians(0)) - ); + "AutoPIDTuning", + OptionalInt.empty(), + 1.15674, + new Pose2d(3.573, 6.052, Rotation2d.fromRadians(0)), + new Pose2d(3.573, 3.052, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj DepotToCorner = new ChoreoTraj( + "DepotToCorner", + OptionalInt.empty(), + 0.8504, + new Pose2d(0.683, 5.979, Rotation2d.fromRadians(0)), + new Pose2d(0.592, 7.6, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj DepotToOutpost = new ChoreoTraj( + "DepotToOutpost", + OptionalInt.empty(), + 1.76382, + new Pose2d(0.787, 5.953, Rotation2d.fromRadians(0)), + new Pose2d(0.749, 0.821, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj DSideAllianceToDepot = new ChoreoTraj( + "DSideAllianceToDepot", + OptionalInt.empty(), + 2.50806, + new Pose2d(3.314, 5.372, Rotation2d.fromRadians(0.524)), + new Pose2d(0.4, 6.923, Rotation2d.fromRadians(-1.571)) + ); + public static final ChoreoTraj DSideAllianceToTrench = new ChoreoTraj( + "DSideAllianceToTrench", + OptionalInt.empty(), + 1.41441, + new Pose2d(3.314, 5.372, Rotation2d.fromRadians(0.524)), + new Pose2d(4.127, 7.658, Rotation2d.fromRadians(1.571)) + ); + public static final ChoreoTraj DSideBumpToDepot = new ChoreoTraj( + "DSideBumpToDepot", + OptionalInt.empty(), + 1.14827, + new Pose2d(3.644, 5.959, Rotation2d.fromRadians(0)), + new Pose2d(0.683, 5.979, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj DSideTrenchToNeutral = new ChoreoTraj( + "DSideTrenchToNeutral", + OptionalInt.empty(), + 3.37498, + new Pose2d(4.127, 7.658, Rotation2d.fromRadians(1.571)), + new Pose2d(7.844, 4.536, Rotation2d.fromRadians(1.571)) + ); + public static final ChoreoTraj FirstDSideNeutralToAlliance = new ChoreoTraj( + "FirstDSideNeutralToAlliance", + OptionalInt.empty(), + 2.42112, + new Pose2d(7.844, 4.536, Rotation2d.fromRadians(1.571)), + new Pose2d(3.314, 5.372, Rotation2d.fromRadians(0.524)) + ); + public static final ChoreoTraj FirstOSideNeutralToAlliance = new ChoreoTraj( + "FirstOSideNeutralToAlliance", + OptionalInt.empty(), + 2.42112, + new Pose2d(7.844, 3.534, Rotation2d.fromRadians(-1.571)), + new Pose2d(3.314, 2.698, Rotation2d.fromRadians(-0.524)) + ); + public static final ChoreoTraj OSideAllianceToOutpost = new ChoreoTraj( + "OSideAllianceToOutpost", + OptionalInt.empty(), + 1.57579, + new Pose2d(3.314, 2.698, Rotation2d.fromRadians(-0.524)), + new Pose2d(0.397, 0.669, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj OSideAllianceToTrench = new ChoreoTraj( + "OSideAllianceToTrench", + OptionalInt.empty(), + 1.41441, + new Pose2d(3.314, 2.698, Rotation2d.fromRadians(-0.524)), + new Pose2d(4.127, 0.412, Rotation2d.fromRadians(-1.571)) + ); + public static final ChoreoTraj OSidePreloadAndBackup = new ChoreoTraj( + "OSidePreloadAndBackup", + OptionalInt.empty(), + 1.33437, + new Pose2d(3.583, 4.029, Rotation2d.fromRadians(0)), + new Pose2d(0.441, 1.927, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj OSideTrenchToNeutral = new ChoreoTraj( + "OSideTrenchToNeutral", + OptionalInt.empty(), + 3.39378, + new Pose2d(4.127, 0.412, Rotation2d.fromRadians(-1.571)), + new Pose2d(7.844, 3.534, Rotation2d.fromRadians(-1.571)) + ); + public static final ChoreoTraj PreloadAndBackupTower = new ChoreoTraj( + "PreloadAndBackupTower", + OptionalInt.empty(), + 0.95141, + new Pose2d(3.583, 4.029, Rotation2d.fromRadians(0)), + new Pose2d(1.549, 4.029, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj SecondDSideNeutralToAlliance = new ChoreoTraj( + "SecondDSideNeutralToAlliance", + OptionalInt.empty(), + 3.31265, + new Pose2d(7.844, 4.536, Rotation2d.fromRadians(1.571)), + new Pose2d(3.314, 5.372, Rotation2d.fromRadians(0.524)) + ); + public static final ChoreoTraj SecondOSideNeutralToAlliance = new ChoreoTraj( + "SecondOSideNeutralToAlliance", + OptionalInt.empty(), + 3.35108, + new Pose2d(7.844, 3.534, Rotation2d.fromRadians(-1.571)), + new Pose2d(3.314, 2.698, Rotation2d.fromRadians(-0.524)) + ); + public static final ChoreoTraj xPreview4414DSideTrenchNeutral = new ChoreoTraj( + "xPreview4414DSideTrenchNeutral", + OptionalInt.empty(), + 8.52061, + new Pose2d(4.127, 7.658, Rotation2d.fromRadians(1.571)), + new Pose2d(0.4, 6.923, Rotation2d.fromRadians(-1.571)) + ); + public static final ChoreoTraj xPreviewFirstDSideTrenchNeutral = new ChoreoTraj( + "xPreviewFirstDSideTrenchNeutral", + OptionalInt.empty(), + 5.67607, + new Pose2d(4.127, 7.658, Rotation2d.fromRadians(1.571)), + new Pose2d(3.314, 5.372, Rotation2d.fromRadians(0.524)) + ); + public static final ChoreoTraj xPreviewFirstOSideTrenchNeutral = new ChoreoTraj( + "xPreviewFirstOSideTrenchNeutral", + OptionalInt.empty(), + 5.67607, + new Pose2d(4.127, 0.412, Rotation2d.fromRadians(-1.571)), + new Pose2d(3.314, 2.698, Rotation2d.fromRadians(-0.524)) + ); + public static final ChoreoTraj xPreviewOSidePreloadAndBackup = new ChoreoTraj( + "xPreviewOSidePreloadAndBackup", + OptionalInt.empty(), + 1.33437, + new Pose2d(3.583, 4.029, Rotation2d.fromRadians(0)), + new Pose2d(0.441, 1.927, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj xPreviewOSideTrenchNeutralOutpost = new ChoreoTraj( + "xPreviewOSideTrenchNeutralOutpost", + OptionalInt.empty(), + 8.00154, + new Pose2d(4.127, 0.412, Rotation2d.fromRadians(-1.571)), + new Pose2d(0.397, 0.669, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj xPreviewPreloadAndBackup = new ChoreoTraj( + "xPreviewPreloadAndBackup", + OptionalInt.empty(), + 0.95141, + new Pose2d(3.583, 4.029, Rotation2d.fromRadians(0)), + new Pose2d(1.549, 4.029, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj xPreviewPreloadDepot = new ChoreoTraj( + "xPreviewPreloadDepot", + OptionalInt.empty(), + 1.93922, + new Pose2d(3.744, 5.959, Rotation2d.fromRadians(0)), + new Pose2d(0.678, 7.447, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj xPreviewPreloadDepotOutpost = new ChoreoTraj( + "xPreviewPreloadDepotOutpost", + OptionalInt.empty(), + 2.9149, + new Pose2d(3.644, 5.959, Rotation2d.fromRadians(0)), + new Pose2d(0.749, 0.821, Rotation2d.fromRadians(0)) + ); + public static final ChoreoTraj xPreviewSecondDSideTrenchNeutral = new ChoreoTraj( + "xPreviewSecondDSideTrenchNeutral", + OptionalInt.empty(), + 6.52944, + new Pose2d(4.127, 7.658, Rotation2d.fromRadians(1.571)), + new Pose2d(3.314, 5.372, Rotation2d.fromRadians(0.524)) + ); + public static final ChoreoTraj xPreviewSecondOSideTrenchNeutral = new ChoreoTraj( + "xPreviewSecondOSideTrenchNeutral", + OptionalInt.empty(), + 6.5858, + new Pose2d(4.127, 0.412, Rotation2d.fromRadians(-1.571)), + new Pose2d(3.314, 2.698, Rotation2d.fromRadians(-0.524)) + ); + public static final ChoreoTraj DSideCornerToDSideTrench = new ChoreoTraj( + "DSideCornerToDSideTrench", + OptionalInt.empty(), + 1.61481, + new Pose2d(0.4, 6.923, Rotation2d.fromRadians(-1.571)), + new Pose2d(4.127, 7.658, Rotation2d.fromRadians(1.571)) + ); /** * A map between trajectory names and their corresponding data. * This allows for trajectory data to be looked up with strings during runtime. */ public static final Map ALL_TRAJECTORIES = Map.ofEntries( - Map.entry("xPreviewPreloadDepotOutpost", xPreviewPreloadDepotOutpost), - Map.entry("xPreviewFirstDSideTrenchNeutral", xPreviewFirstDSideTrenchNeutral), - Map.entry("FirstDSideNeutralToAlliance", FirstDSideNeutralToAlliance), - Map.entry("OSideAllianceToOutpost", OSideAllianceToOutpost), - Map.entry("SecondOSideNeutralToAlliance", SecondOSideNeutralToAlliance), - Map.entry("xPreviewSecondOSideTrenchNeutral", xPreviewSecondOSideTrenchNeutral), - Map.entry("DSideAllianceToTrench", DSideAllianceToTrench), - Map.entry("DSideBumpToDepot", DSideBumpToDepot), - Map.entry("xPreviewOSidePreloadAndBackup", xPreviewOSidePreloadAndBackup), - Map.entry("xPreviewPreloadDepot", xPreviewPreloadDepot), - Map.entry("DepotToOutpost", DepotToOutpost), - Map.entry("xPreview4414DSideTrenchNeutral", xPreview4414DSideTrenchNeutral), - Map.entry("SecondDSideNeutralToAlliance", SecondDSideNeutralToAlliance), - Map.entry("OSidePreloadAndBackup", OSidePreloadAndBackup), - Map.entry("OSideTrenchToNeutral", OSideTrenchToNeutral), - Map.entry("OSideAllianceToTrench", OSideAllianceToTrench), - Map.entry("xPreviewSecondDSideTrenchNeutral", xPreviewSecondDSideTrenchNeutral), - Map.entry("AutoPIDTuning", AutoPIDTuning), - Map.entry("FirstOSideNeutralToAlliance", FirstOSideNeutralToAlliance), - Map.entry("xPreviewFirstOSideTrenchNeutral", xPreviewFirstOSideTrenchNeutral), - Map.entry("DSideAllianceToDepot", DSideAllianceToDepot), - Map.entry("DSideTrenchToNeutral", DSideTrenchToNeutral), - Map.entry("PreloadAndBackupTower", PreloadAndBackupTower), - Map.entry("DepotToCorner", DepotToCorner), - Map.entry("xPreviewOSideTrenchNeutralOutpost", xPreviewOSideTrenchNeutralOutpost), - Map.entry("xPreviewPreloadAndBackup", xPreviewPreloadAndBackup) + Map.entry("AutoPIDTuning", AutoPIDTuning), + Map.entry("DepotToCorner", DepotToCorner), + Map.entry("DepotToOutpost", DepotToOutpost), + Map.entry("DSideAllianceToDepot", DSideAllianceToDepot), + Map.entry("DSideAllianceToTrench", DSideAllianceToTrench), + Map.entry("DSideBumpToDepot", DSideBumpToDepot), + Map.entry("DSideTrenchToNeutral", DSideTrenchToNeutral), + Map.entry("FirstDSideNeutralToAlliance", FirstDSideNeutralToAlliance), + Map.entry("FirstOSideNeutralToAlliance", FirstOSideNeutralToAlliance), + Map.entry("OSideAllianceToOutpost", OSideAllianceToOutpost), + Map.entry("OSideAllianceToTrench", OSideAllianceToTrench), + Map.entry("OSidePreloadAndBackup", OSidePreloadAndBackup), + Map.entry("OSideTrenchToNeutral", OSideTrenchToNeutral), + Map.entry("PreloadAndBackupTower", PreloadAndBackupTower), + Map.entry("SecondDSideNeutralToAlliance", SecondDSideNeutralToAlliance), + Map.entry("SecondOSideNeutralToAlliance", SecondOSideNeutralToAlliance), + Map.entry("xPreview4414DSideTrenchNeutral", xPreview4414DSideTrenchNeutral), + Map.entry("xPreviewFirstDSideTrenchNeutral", xPreviewFirstDSideTrenchNeutral), + Map.entry("xPreviewFirstOSideTrenchNeutral", xPreviewFirstOSideTrenchNeutral), + Map.entry("xPreviewOSidePreloadAndBackup", xPreviewOSidePreloadAndBackup), + Map.entry("xPreviewOSideTrenchNeutralOutpost", xPreviewOSideTrenchNeutralOutpost), + Map.entry("xPreviewPreloadAndBackup", xPreviewPreloadAndBackup), + Map.entry("xPreviewPreloadDepot", xPreviewPreloadDepot), + Map.entry("xPreviewPreloadDepotOutpost", xPreviewPreloadDepotOutpost), + Map.entry("xPreviewSecondDSideTrenchNeutral", xPreviewSecondDSideTrenchNeutral), + Map.entry("xPreviewSecondOSideTrenchNeutral", xPreviewSecondOSideTrenchNeutral), + Map.entry("DSideCornerToDSideTrench", DSideCornerToDSideTrench) ); /** @@ -264,4 +272,3 @@ public AutoTrajectory asAutoTraj(AutoRoutine routine) { return routine.trajectory(this.name); } } -// spotless:on diff --git a/src/main/java/frc/robot/constants/ChoreoVars.java b/src/main/java/frc/robot/constants/ChoreoVars.java index 9a68e0d..ee2da39 100644 --- a/src/main/java/frc/robot/constants/ChoreoVars.java +++ b/src/main/java/frc/robot/constants/ChoreoVars.java @@ -1,5 +1,5 @@ -// spotless:off package frc.robot.constants; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.Units; @@ -12,31 +12,34 @@ */ public final class ChoreoVars { public static final LinearVelocity intaking_velocity = Units.MetersPerSecond.of(1.524); - public static final Angle over_bump_heading = Units.Radians.of(0.5235988); + public static final Angle over_bump_heading = Units.Radians.of(0.524); public static final class Poses { - public static final Pose2d against_hub = new Pose2d(3.5825274, 4.0294271, Rotation2d.fromRadians(0)); - public static final Pose2d against_hub_neutral = new Pose2d(5.9098597, 4.062469, Rotation2d.fromRadians(0)); - public static final Pose2d against_outpost = new Pose2d(0.3974082, 0.6685812, Rotation2d.fromRadians(0)); - public static final Pose2d against_tower_inline_hub = new Pose2d(1.5485528, 4.0294271, Rotation2d.fromRadians(0)); - public static final Pose2d aligned_oside_wall = new Pose2d(1.6718733, 0.6572021, Rotation2d.fromRadians(0)); - public static final Pose2d between_tower_depot = new Pose2d(0.410383, 5.0589038, Rotation2d.fromRadians(-1.5707963)); - public static final Pose2d between_tower_outpost = new Pose2d(0.4405997, 1.9271417, Rotation2d.fromRadians(0)); - public static final Pose2d d_side_alliance_bump = new Pose2d(3.3140977, 5.3718681, Rotation2d.fromRadians(0.5235988)); - public static final Pose2d d_side_bump = new Pose2d(3.6444179, 5.9593887, Rotation2d.fromRadians(0)); - public static final Pose2d d_side_corner = new Pose2d(0.4003107, 6.9230027, Rotation2d.fromRadians(-1.5707963)); - public static final Pose2d d_side_neutral_bump = new Pose2d(5.859498, 5.3718681, Rotation2d.fromRadians(0.5235988)); - public static final Pose2d d_side_neutral_middle = new Pose2d(7.8437409, 4.5358677, Rotation2d.fromRadians(1.5707963)); - public static final Pose2d d_side_neutral_wall = new Pose2d(7.8437409, 7.6582813, Rotation2d.fromRadians(1.5707963)); - public static final Pose2d d_side_trench = new Pose2d(4.1270618, 7.6582813, Rotation2d.fromRadians(1.5707963)); - public static final Pose2d depot = new Pose2d(0.6834254, 5.9790735, Rotation2d.fromRadians(0)); - public static final Pose2d o_side_alliance_bump = new Pose2d(3.3140977, 2.6981319, Rotation2d.fromRadians(-0.5235988)); - public static final Pose2d o_side_neutral_bump = new Pose2d(5.859498, 2.6981319, Rotation2d.fromRadians(-0.5235988)); - public static final Pose2d o_side_neutral_middle = new Pose2d(7.8437409, 3.5341323, Rotation2d.fromRadians(-1.5707963)); - public static final Pose2d o_side_neutral_wall = new Pose2d(7.8437409, 0.4117187, Rotation2d.fromRadians(-1.5707963)); - public static final Pose2d o_side_trench = new Pose2d(4.1270618, 0.4117187, Rotation2d.fromRadians(-1.5707963)); - public static final Pose2d pid_end = new Pose2d(3.57341, 3.051815, Rotation2d.fromRadians(0)); - public static final Pose2d pid_start = new Pose2d(3.57341, 6.051815, Rotation2d.fromRadians(0)); + public static final Pose2d against_hub = new Pose2d(3.583, 4.029, Rotation2d.kZero); + public static final Pose2d against_hub_neutral = new Pose2d(5.91, 4.062, Rotation2d.kZero); + public static final Pose2d against_outpost = new Pose2d(0.397, 0.669, Rotation2d.kZero); + public static final Pose2d against_tower_inline_hub = new Pose2d(1.549, 4.029, Rotation2d.kZero); + public static final Pose2d aligned_oside_wall = new Pose2d(1.672, 0.657, Rotation2d.kZero); + public static final Pose2d between_tower_depot = new Pose2d(0.41, 5.059, Rotation2d.fromRadians(-1.571)); + public static final Pose2d between_tower_outpost = new Pose2d(0.441, 1.927, Rotation2d.kZero); + public static final Pose2d d_side_alliance_bump = new Pose2d(3.314, 5.372, Rotation2d.fromRadians(0.524)); + public static final Pose2d d_side_bump = new Pose2d(3.644, 5.959, Rotation2d.kZero); + public static final Pose2d d_side_corner = new Pose2d(0.4, 6.923, Rotation2d.fromRadians(-1.571)); + public static final Pose2d d_side_neutral_bump = new Pose2d(5.859, 5.372, Rotation2d.fromRadians(0.524)); + public static final Pose2d d_side_neutral_middle = new Pose2d(7.844, 4.536, Rotation2d.fromRadians(1.571)); + public static final Pose2d d_side_neutral_wall = new Pose2d(7.844, 7.658, Rotation2d.fromRadians(1.571)); + public static final Pose2d d_side_trench = new Pose2d(4.127, 7.658, Rotation2d.fromRadians(1.571)); + public static final Pose2d depot = new Pose2d(0.683, 5.979, Rotation2d.kZero); + public static final Pose2d o_side_alliance_bump = new Pose2d(3.314, 2.698, Rotation2d.fromRadians(-0.524)); + public static final Pose2d o_side_neutral_bump = new Pose2d(5.859, 2.698, Rotation2d.fromRadians(-0.524)); + public static final Pose2d o_side_neutral_middle = new Pose2d(7.844, 3.534, Rotation2d.fromRadians(-1.571)); + public static final Pose2d o_side_neutral_wall = new Pose2d(7.844, 0.412, Rotation2d.fromRadians(-1.571)); + public static final Pose2d o_side_trench = new Pose2d(4.127, 0.412, Rotation2d.fromRadians(-1.571)); + public static final Pose2d pid_end = new Pose2d(3.573, 3.052, Rotation2d.kZero); + public static final Pose2d pid_start = new Pose2d(3.573, 6.052, Rotation2d.kZero); + + private Poses() {} } -} -// spotless:on + + private ChoreoVars() {} +} \ No newline at end of file