diff --git a/src/main/deploy/choreo/Madtown_G_Right.traj b/src/main/deploy/choreo/Madtown_G_Right.traj new file mode 100644 index 0000000..19e0ec7 --- /dev/null +++ b/src/main/deploy/choreo/Madtown_G_Right.traj @@ -0,0 +1,149 @@ +{ + "name":"Madtown_G_Right", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":6.102848052978516, "y":3.7678885459899902, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.606588363647461, "y":2.028738498687744, "heading":2.325434996055092, "intervals":61, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.8133447170257568, "y":0.8635300397872925, "heading":0.9332802447333852, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.540279984474182, "y":0.7697955369949341, "heading":0.9332802447333852, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":0, "to":0, "data":{"type":"MaxVelocity", "props":{"max":1.75}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":2.6}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"6.102848052978516 m", "val":6.102848052978516}, "y":{"exp":"3.7678885459899902 m", "val":3.7678885459899902}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.606588363647461 m", "val":6.606588363647461}, "y":{"exp":"2.028738498687744 m", "val":2.028738498687744}, "heading":{"exp":"2.325434996055092 rad", "val":2.325434996055092}, "intervals":61, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.8133447170257568 m", "val":1.8133447170257568}, "y":{"exp":"0.8635300397872925 m", "val":0.8635300397872925}, "heading":{"exp":"HPRight.heading", "val":0.9332802447333852}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"HPRight.x", "val":1.540279984474182}, "y":{"exp":"HPRight.y", "val":0.7697955369949341}, "heading":{"exp":"HPRight.heading", "val":0.9332802447333852}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":0, "to":0, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"reefSpeed", "val":1.75}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"movingSpeed", "val":2.6}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"coralSpeed", "val":1.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.91931,3.00581,3.38563], + "samples":[ + {"t":0.0, "x":6.10285, "y":3.76789, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.41922, "ay":-4.39462, "alpha":-0.07982, "fx":[54.22508,53.82425,54.34061,54.74064], "fy":[-69.81262,-70.12341,-69.72503,-69.41012]}, + {"t":0.02627, "x":6.10403, "y":3.76637, "heading":3.14159, "vx":0.08981, "vy":-0.11543, "omega":-0.0021, "ax":3.38272, "ay":-4.42224, "alpha":-0.08235, "fx":[53.63944,53.2278,53.76723,54.17817], "fy":[-70.25506,-70.56884,-70.15976,-69.84153]}, + {"t":0.05253, "x":6.10755, "y":3.76182, "heading":3.14154, "vx":0.17866, "vy":-0.23158, "omega":-0.00426, "ax":3.34222, "ay":-4.45234, "alpha":-0.08514, "fx":[52.98931,52.56604,53.13127,53.55398], "fy":[-70.73737,-71.05399,-70.63346,-70.31199]}, + {"t":0.0788, "x":6.1134, "y":3.7542, "heading":3.14143, "vx":0.26645, "vy":-0.34853, "omega":-0.0065, "ax":3.29703, "ay":-4.48526, "alpha":-0.08821, "fx":[52.26368,51.82787,52.42213,52.85755], "fy":[-71.26493,-71.58416,-71.15132,-70.82681]}, + {"t":0.10506, "x":6.12153, "y":3.74349, "heading":3.14126, "vx":0.35305, "vy":-0.46634, "omega":-0.00881, "ax":3.24632, "ay":-4.52139, "alpha":-0.09163, "fx":[51.44892,50.99961,51.62674,52.07588], "fy":[-71.84406,-72.16551,-71.71949,-71.39224]}, + {"t":0.13133, "x":6.13193, "y":3.72969, "heading":3.14102, "vx":0.43832, "vy":-0.5851, "omega":-0.01122, "ax":3.18905, "ay":-4.56118, "alpha":-0.09544, "fx":[50.528,50.0642,50.72881,51.19274], "fy":[-72.48218,-72.80531,-72.34518,-72.01565]}, + {"t":0.1576, "x":6.14454, "y":3.71274, "heading":3.14073, "vx":0.52208, "vy":-0.7049, "omega":-0.01373, "ax":3.12387, "ay":-4.60519, "alpha":-0.09973, "fx":[49.47942,49.00011,49.70777,50.18762], "fy":[-73.18806,-73.51205,-73.03689,-72.70579]}, + {"t":0.18386, "x":6.15933, "y":3.69264, "heading":3.14037, "vx":0.60413, "vy":-0.82586, "omega":-0.01635, "ax":3.04911, "ay":-4.65404, "alpha":-0.10457, "fx":[48.27563,47.77985,48.53739,49.03426], "fy":[-73.972,-74.2957,-73.80464,-73.473]}, + {"t":0.21013, "x":6.17625, "y":3.66934, "heading":3.13994, "vx":0.68422, "vy":-0.94811, "omega":-0.01909, "ax":2.96256, "ay":-4.7085, "alpha":-0.11008, "fx":[46.88091,46.36784,47.18368,47.69858], "fy":[-74.84611,-75.16786,-74.66019,-74.3295]}, + {"t":0.23639, "x":6.19525, "y":3.64282, "heading":3.13944, "vx":0.76203, "vy":-1.07178, "omega":-0.02198, "ax":2.86133, "ay":-4.76943, "alpha":-0.1164, "fx":[45.24813,44.71731,45.602,46.13567], "fy":[-75.82439,-76.14185,-75.61722,-75.28962]}, + {"t":0.26266, "x":6.21625, "y":3.61302, "heading":3.13886, "vx":0.83719, "vy":-1.19706, "omega":-0.02504, "ax":2.74158, "ay":-4.83783, "alpha":-0.12372, "fx":[43.3142,42.76588,43.73271,44.28533], "fy":[-76.92267,-77.23256,-76.6913,-76.36986]}, + {"t":0.28893, "x":6.23918, "y":3.57991, "heading":3.1382, "vx":0.9092, "vy":-1.32413, "omega":-0.02829, "ax":2.59802, "ay":-4.91476, "alpha":-0.13227, "fx":[40.99309,40.42882,41.49475,42.06537], "fy":[-78.15797,-78.45565,-77.89938,-77.58847]}, + {"t":0.31519, "x":6.26396, "y":3.54343, "heading":3.13746, "vx":0.97744, "vy":-1.45322, "omega":-0.03177, "ax":2.42338, "ay":-5.00125, "alpha":-0.14236, "fx":[38.1653,37.58907,38.77592,39.36147], "fy":[-79.54648,-79.82547,-79.25817,-78.96397]}, + {"t":0.34146, "x":6.29047, "y":3.50354, "heading":3.13662, "vx":1.04109, "vy":-1.58458, "omega":-0.0355, "ax":2.20735, "ay":-5.09802, "alpha":-0.15438, "fx":[34.66175,34.08203,35.41797,36.0114], "fy":[-81.0985,-81.34994,-80.77986,-80.51103]}, + {"t":0.36773, "x":6.31858, "y":3.46016, "heading":3.13569, "vx":1.09907, "vy":-1.71849, "omega":-0.03956, "ax":1.93513, "ay":-5.20477, "alpha":-0.16885, "fx":[30.23924,29.67266,31.1938,31.78061], "fy":[-82.80666,-83.01883,-82.46206,-82.23031]}, + {"t":0.39399, "x":6.34811, "y":3.41323, "heading":3.13465, "vx":1.1499, "vy":-1.8552, "omega":-0.04399, "ax":1.58521, "ay":-5.31858, "alpha":-0.18642, "fx":[24.54416,24.02234,25.77351,26.32531], "fy":[-84.61878,-84.77767,-84.26454,-84.08461]}, + {"t":0.42026, "x":6.37886, "y":3.36266, "heading":3.1335, "vx":1.19154, "vy":-1.99489, "omega":-0.04889, "ax":1.12642, "ay":-5.43043, "alpha":-0.20778, "fx":[17.06504,16.6464,18.67824,19.14109], "fy":[-86.3771,-86.47051,-86.05693,-85.94352]}, + {"t":0.44652, "x":6.41055, "y":3.30839, "heading":3.13221, "vx":1.22112, "vy":-2.13753, "omega":-0.05435, "ax":0.51531, "ay":-5.51741, "alpha":-0.23345, "fx":[7.09418,6.88198,9.23599,9.51142], "fy":[-87.68758,-87.71963,-87.50459,-87.46002]}, + {"t":0.47279, "x":6.4428, "y":3.25034, "heading":3.13079, "vx":1.23466, "vy":-2.28245, "omega":-0.06048, "ax":-0.30092, "ay":-5.52783, "alpha":-0.26297, "fx":[-6.20819,-6.05032,-3.38767,-3.46323], "fy":[-87.6699,-87.7005,-87.84284,-87.82028]}, + {"t":0.49906, "x":6.47513, "y":3.18848, "heading":3.1292, "vx":1.22676, "vy":-2.42765, "omega":-0.06739, "ax":-1.35993, "ay":-5.36046, "alpha":-0.29302, "fx":[-23.37174,-22.65291,-19.84972,-20.48548], "fy":[-84.63331,-84.85323,-85.54673,-85.37159]}, + {"t":0.52532, "x":6.50688, "y":3.12287, "heading":3.12743, "vx":1.19103, "vy":-2.56844, "omega":-0.07508, "ax":-2.61349, "ay":-4.86844, "alpha":-0.31507, "fx":[-43.4224,-42.10162,-39.57853,-40.86159], "fy":[-76.23216,-76.99844,-78.31084,-77.61884]}, + {"t":0.55159, "x":6.53726, "y":3.05373, "heading":3.12546, "vx":1.12239, "vy":-2.69632, "omega":-0.08336, "ax":-3.84775, "ay":-3.9635, "alpha":-0.31748, "fx":[-62.73267,-61.16121,-59.42126,-61.02847], "fy":[-61.30324,-62.90101,-64.51994,-62.9697]}, + {"t":0.57785, "x":6.56542, "y":2.98154, "heading":3.12327, "vx":1.02132, "vy":-2.80042, "omega":-0.0917, "ax":-4.77755, "ay":-2.77789, "alpha":-0.29627, "fx":[-76.88775,-75.62107,-74.77443,-76.10504], "fy":[-42.29823,-44.54984,-45.91344,-43.64282]}, + {"t":0.60412, "x":6.59059, "y":2.90702, "heading":3.12086, "vx":0.89584, "vy":-2.87339, "omega":-0.09948, "ax":-5.29685, "ay":-1.59425, "alpha":-0.26055, "fx":[-84.57492,-83.86602,-83.58713,-84.33746], "fy":[-23.70823,-26.12669,-26.93983,-24.46504]}, + {"t":0.63039, "x":6.6123, "y":2.831, "heading":3.11824, "vx":0.75671, "vy":-2.91526, "omega":-0.10632, "ax":-5.5047, "ay":-0.60033, "alpha":-0.22243, "fx":[-87.5343,-87.29682,-87.24099,-87.49227], "fy":[-8.26569,-10.49757,-10.82371,-8.53567]}, + {"t":0.65665, "x":6.63027, "y":2.75422, "heading":3.11545, "vx":0.61212, "vy":-2.93103, "omega":-0.11217, "ax":-5.54001, "ay":0.16917, "alpha":-0.18847, "fx":[-87.92857,-87.98713,-87.97553,-87.91547], "fy":[3.64295,1.72033,1.70714,3.67244]}, + {"t":0.68292, "x":6.64444, "y":2.67729, "heading":3.11251, "vx":0.46661, "vy":-2.92659, "omega":-0.11712, "ax":-5.49594, "ay":0.75094, "alpha":-0.16032, "fx":[-87.1592,-87.37751,-87.34848,-87.12286], "fy":[12.64325,11.02671,11.18541,12.83149]}, + {"t":0.70918, "x":6.6548, "y":2.60068, "heading":3.10943, "vx":0.32225, "vy":-2.90686, "omega":-0.12133, "ax":-5.42096, "ay":1.19264, "alpha":-0.1375, "fx":[-85.94563,-86.24033,-86.18217,-85.87892], "fy":[19.48478,18.13018,18.37322,19.74772]}, + {"t":0.73545, "x":6.66139, "y":2.52474, "heading":3.10624, "vx":0.17986, "vy":-2.87554, "omega":-0.12494, "ax":-5.3377, "ay":1.53321, "alpha":-0.11902, "fx":[-84.62121,-84.94569,-84.86267,-84.5302], "fy":[24.76912,23.62782,23.90586,25.06057]}, + {"t":0.76172, "x":6.66428, "y":2.44974, "heading":3.10296, "vx":0.03966, "vy":-2.83527, "omega":-0.12807, "ax":-5.25601, "ay":1.8008, "alpha":-0.10392, "fx":[-83.3303,-83.65951,-83.55918,-83.22307], "fy":[28.92844,27.95853,28.2449,29.22396]}, + {"t":0.78798, "x":6.66351, "y":2.37589, "heading":3.0996, "vx":-0.09839, "vy":-2.78797, "omega":-0.1308, "ax":-5.1798, "ay":2.01501, "alpha":-0.09146, "fx":[-82.12954,-82.45043,-82.33962,-82.01296], "fy":[32.26387,31.43193,31.71262,32.55091]}, + {"t":0.81425, "x":6.65914, "y":2.30336, "heading":3.09616, "vx":-0.23445, "vy":-2.73504, "omega":-0.1332, "ax":-5.1103, "ay":2.18952, "alpha":-0.08104, "fx":[-81.03587,-81.34195,-81.2259,-80.91508], "fy":[34.98516,34.26519,34.53316,35.25761]}, + {"t":0.84052, "x":6.65121, "y":2.23227, "heading":3.09266, "vx":-0.36867, "vy":-2.67753, "omega":-0.13533, "ax":-5.04753, "ay":2.33393, "alpha":-0.07224, "fx":[-80.04881,-80.33718,-80.21969,-79.92743], "fy":[37.24012,36.61198,36.86399,37.49533]}, + {"t":0.86678, "x":6.63979, "y":2.16275, "heading":3.08911, "vx":-0.50125, "vy":-2.61623, "omega":-0.13722, "ax":-4.99106, "ay":2.45511, "alpha":-0.06472, "fx":[-79.16081,-79.43053,-79.31421,-79.04131], "fy":[39.1346,38.58265,38.81753,39.37182]}, + {"t":0.89305, "x":6.6249, "y":2.09488, "heading":3.0855, "vx":-0.63235, "vy":-2.55174, "omega":-0.13892, "ax":-4.94026, "ay":2.55806, "alpha":-0.05822, "fx":[-78.36196,-78.61314,-78.49975,-78.24597], "fy":[40.7458,40.25776,40.47548,40.96524]}, + {"t":0.91931, "x":6.60659, "y":2.02874, "heading":3.08186, "vx":-0.76211, "vy":-2.48455, "omega":-0.14045, "ax":-5.19375, "ay":2.0023, "alpha":-0.05773, "fx":[-82.38599,-82.58962,-82.52446,-82.31857], "fy":[31.96889,31.43816,31.60579,32.13919]}, + {"t":0.95468, "x":6.57639, "y":1.94213, "heading":3.07689, "vx":-0.94578, "vy":-2.41374, "omega":-0.14249, "ax":-5.09319, "ay":2.24256, "alpha":-0.06196, "fx":[-80.78241,-81.0236,-80.93511,-80.69112], "fy":[35.77721,35.22644,35.42609,35.97952]}, + {"t":0.99004, "x":6.53976, "y":1.85817, "heading":3.07185, "vx":-1.1259, "vy":-2.33443, "omega":-0.14469, "ax":-4.94482, "ay":2.54954, "alpha":-0.06711, "fx":[-78.42114,-78.71145,-78.58592,-78.29214], "fy":[40.63735,40.07071,40.31296,40.88204]}, + {"t":1.02541, "x":6.49685, "y":1.77721, "heading":3.06673, "vx":-1.30077, "vy":-2.24427, "omega":-0.14706, "ax":-4.73978, "ay":2.90912, "alpha":-0.07342, "fx":[-75.164,-75.51462,-75.33304,-74.97818], "fy":[46.32435,45.74872,46.0435,46.62098]}, + {"t":1.06077, "x":6.44788, "y":1.69966, "heading":3.06153, "vx":-1.46839, "vy":-2.14139, "omega":-0.14966, "ax":-4.48483, "ay":3.28414, "alpha":-0.08121, "fx":[-71.12156,-71.54075,-71.28097,-70.85679], "fy":[52.2499,51.67204,52.02586,52.60453]}, + {"t":1.09614, "x":6.39315, "y":1.62598, "heading":3.05624, "vx":-1.62699, "vy":-2.02525, "omega":-0.15253, "ax":-4.20669, "ay":3.62839, "alpha":-0.09095, "fx":[-66.71882,-67.2122,-66.8526,-66.35362], "fy":[57.68496,57.1061,57.52211,58.10025]}, + {"t":1.1315, "x":6.33298, "y":1.55663, "heading":3.05085, "vx":-1.77576, "vy":-1.89693, "omega":-0.15574, "ax":-3.91193, "ay":3.93803, "alpha":-0.1034, "fx":[-62.06185,-62.63745,-62.15053,-61.56891], "fy":[62.5688,61.98807,62.47081,63.04866]}, + {"t":1.16686, "x":6.26774, "y":1.49201, "heading":3.04534, "vx":-1.9141, "vy":-1.75767, "omega":-0.1594, "ax":-3.59732, "ay":4.21921, "alpha":-0.11976, "fx":[-57.10366,-57.77386,-57.11955,-56.44337], "fy":[66.99804,66.4145,66.97123,67.54866]}, + {"t":1.20223, "x":6.1978, "y":1.43249, "heading":3.0397, "vx":-2.04132, "vy":-1.60846, "omega":-0.16364, "ax":-3.26275, "ay":4.47208, "alpha":-0.14198, "fx":[-51.84867,-52.63183,-51.75079,-50.96272], "fy":[70.97404,70.38542,71.02665,71.60419]}, + {"t":1.23759, "x":6.12357, "y":1.3784, "heading":3.03391, "vx":-2.15671, "vy":-1.45031, "omega":-0.16866, "ax":-2.90956, "ay":4.69416, "alpha":-0.17341, "fx":[-46.32952,-47.25383,-46.05399,-45.1281], "fy":[74.45698,73.85794,74.5992,75.17912]}, + {"t":1.27296, "x":6.04548, "y":1.33005, "heading":3.02795, "vx":-2.2596, "vy":-1.2843, "omega":-0.17479, "ax":-2.53947, "ay":4.88117, "alpha":-0.2203, "fx":[-40.59419,-41.70567,-40.03411,-38.92978], "fy":[77.37833,76.75708,77.62239,78.21066]}, + {"t":1.30832, "x":5.96398, "y":1.28768, "heading":3.02177, "vx":-2.34941, "vy":-1.11168, "omega":-0.18258, "ax":-2.15426, "ay":5.02525, "alpha":-0.29547, "fx":[-34.71297,-36.09274,-33.67281,-32.32305], "fy":[79.61548,78.94477,79.97332,80.58485]}, + {"t":1.34369, "x":5.87955, "y":1.25151, "heading":3.01531, "vx":-2.42559, "vy":-0.93396, "omega":-0.19303, "ax":-1.75552, "ay":5.10929, "alpha":-0.42851, "fx":[-28.80652,-30.61349,-26.88579,-25.17473], "fy":[80.90905,80.11682,81.37612,82.0531]}, + {"t":1.37905, "x":5.79267, "y":1.22168, "heading":3.00848, "vx":-2.48767, "vy":-0.75328, "omega":-0.20818, "ax":-1.34421, "ay":5.087, "alpha":-0.70021, "fx":[-23.13399,-25.72793,-19.38539,-17.11407], "fy":[80.59715,79.44238,81.05079,81.94938]}, + {"t":1.41441, "x":5.70385, "y":1.19822, "heading":3.00112, "vx":-2.53521, "vy":-0.57338, "omega":-0.23295, "ax":-0.91958, "ay":4.80142, "alpha":-1.38359, "fx":[-18.37663,-22.73168,-10.15962,-7.12814], "fy":[76.67517,74.02123,76.10605,78.10191]}, + {"t":1.44978, "x":5.61362, "y":1.18094, "heading":2.99288, "vx":-2.56773, "vy":-0.40358, "omega":-0.28188, "ax":-0.48241, "ay":3.66814, "alpha":-3.49106, "fx":[-16.08825,-24.51832,3.69467,6.27733], "fy":[63.47991,52.42157,53.42308,63.61295]}, + {"t":1.48514, "x":5.52251, "y":1.16896, "heading":2.98292, "vx":-2.58479, "vy":-0.27386, "omega":-0.40533, "ax":-0.14327, "ay":1.49828, "alpha":-6.47739, "fx":[-15.48779,-24.66959,14.25122,16.80791], "fy":[40.9988,11.23371,5.51956,37.3931]}, + {"t":1.52051, "x":5.43101, "y":1.16022, "heading":2.96858, "vx":-2.58986, "vy":-0.22087, "omega":-0.6344, "ax":-0.03416, "ay":0.41432, "alpha":-6.4321, "fx":[-13.75758,-20.40485,13.53048,18.4629], "fy":[25.59531,-6.73327,-12.82325,20.27144]}, + {"t":1.55587, "x":5.3394, "y":1.15267, "heading":2.94615, "vx":-2.59107, "vy":-0.20622, "omega":-0.86187, "ax":-0.00844, "ay":0.10705, "alpha":-5.64341, "fx":[-11.58004,-17.43933,11.47856,17.00479], "fy":[18.86608,-9.77986,-15.55144,13.26326]}, + {"t":1.59124, "x":5.24777, "y":1.14544, "heading":2.91567, "vx":-2.59137, "vy":-0.20243, "omega":-1.06145, "ax":-0.0021, "ay":0.02693, "alpha":-4.86032, "fx":[-9.48816,-15.1709,9.45161,15.07423], "fy":[15.54052,-9.03321,-14.70313,9.90579]}, + {"t":1.6266, "x":5.15612, "y":1.1383, "heading":2.87813, "vx":-2.59144, "vy":-0.20148, "omega":-1.23333, "ax":-0.00052, "ay":0.00666, "alpha":-4.1452, "fx":[-7.59228,-13.20199,7.58103,13.18043], "fy":[13.29513,-7.47912,-13.08721,7.69416]}, + {"t":1.66197, "x":5.06448, "y":1.13118, "heading":2.83451, "vx":-2.59146, "vy":-0.20125, "omega":-1.37992, "ax":-0.00013, "ay":0.00164, "alpha":-3.50465, "fx":[-5.92351,-11.4243,5.92033,11.41942], "fy":[11.44758,-5.89549,-11.39614,5.94835]}, + {"t":1.69733, "x":4.97283, "y":1.12406, "heading":2.78571, "vx":-2.59146, "vy":-0.20119, "omega":-1.50386, "ax":-0.00003, "ay":0.00042, "alpha":-2.93886, "fx":[-4.49317,-9.80934,4.49231,9.8082], "fy":[9.8153,-4.48607,-9.80224,4.4994]}, + {"t":1.73269, "x":4.88119, "y":1.11695, "heading":2.73253, "vx":-2.59146, "vy":-0.20117, "omega":-1.60779, "ax":-0.00001, "ay":0.00011, "alpha":-2.44556, "fx":[-3.29955,-8.34964,3.29933,8.34937], "fy":[8.35121,-3.29771,-8.3478,3.30118]}, + {"t":1.76806, "x":4.78954, "y":1.10983, "heading":2.67567, "vx":-2.59146, "vy":-0.20117, "omega":-1.69428, "ax":0.0, "ay":0.00002, "alpha":-2.02023, "fx":[-2.32922,-7.04112,2.3292,7.0411], "fy":[7.04137,-2.32894,-7.04085,2.32947]}, + {"t":1.80342, "x":4.6979, "y":1.10272, "heading":2.61575, "vx":-2.59146, "vy":-0.20117, "omega":-1.76572, "ax":0.0, "ay":-0.00003, "alpha":-1.65717, "fx":[-1.56131,-5.87976,1.5614,5.87985], "fy":[5.87937,-1.5618,-5.88024,1.56092]}, + {"t":1.83879, "x":4.60625, "y":1.0956, "heading":2.55331, "vx":-2.59146, "vy":-0.20117, "omega":-1.82433, "ax":0.00001, "ay":-0.00006, "alpha":-1.34932, "fx":[-0.96996,-4.85744,0.97012,4.85761], "fy":[4.85656,-0.97101,-4.85848,0.96908]}, + {"t":1.87415, "x":4.51461, "y":1.08849, "heading":2.48879, "vx":-2.59146, "vy":-0.20117, "omega":-1.87204, "ax":0.00001, "ay":-0.00009, "alpha":-1.08902, "fx":[-0.52843,-3.96264,0.52865,3.96286], "fy":[3.96137,-0.52992,-3.96412,0.52716]}, + {"t":1.90952, "x":4.42296, "y":1.08137, "heading":2.42259, "vx":-2.59146, "vy":-0.20118, "omega":-1.91056, "ax":0.00001, "ay":-0.0001, "alpha":-0.86801, "fx":[-0.21124,-3.17938,0.21151,3.17965], "fy":[3.17787,-0.21303,-3.18116,0.20973]}, + {"t":1.94488, "x":4.33131, "y":1.07426, "heading":2.35502, "vx":-2.59146, "vy":-0.20118, "omega":-1.94125, "ax":0.00001, "ay":-0.00011, "alpha":-0.67776, "fx":[0.00306,-2.48795,-0.00279,2.48823], "fy":[2.48637,0.0012,-2.48981,-0.00465]}, + {"t":1.98025, "x":4.23967, "y":1.06714, "heading":2.28637, "vx":-2.59146, "vy":-0.20118, "omega":-1.96522, "ax":0.00001, "ay":-0.0001, "alpha":-0.51001, "fx":[0.13077,-1.8676,-0.13053,1.86784], "fy":[1.86615,0.12908,-1.86929,-0.13222]}, + {"t":2.01561, "x":4.14802, "y":1.06003, "heading":2.21687, "vx":-2.59146, "vy":-0.20119, "omega":-1.98326, "ax":0.00001, "ay":-0.00008, "alpha":-0.35643, "fx":[0.18181,-1.29569,-0.18162,1.29587], "fy":[1.29458,0.18051,-1.29698,-0.18292]}, + {"t":2.05097, "x":4.05638, "y":1.05291, "heading":2.14674, "vx":-2.59146, "vy":-0.20119, "omega":-1.99586, "ax":0.0, "ay":-0.00004, "alpha":-0.20975, "fx":[0.16018,-0.75312,-0.16008,0.75322], "fy":[0.7525,0.15946,-0.75383,-0.16079]}, + {"t":2.08634, "x":3.96473, "y":1.0458, "heading":2.07616, "vx":-2.59146, "vy":-0.20119, "omega":-2.00328, "ax":0.0, "ay":0.0, "alpha":-0.06284, "fx":[0.06376,-0.22169,-0.06376,0.22169], "fy":[0.22167,0.06374,-0.22171,-0.06378]}, + {"t":2.1217, "x":3.87309, "y":1.03868, "heading":2.00531, "vx":-2.59146, "vy":-0.20119, "omega":-2.0055, "ax":0.0, "ay":0.00004, "alpha":0.09001, "fx":[-0.11362,0.31027,0.11352,-0.31037], "fy":[-0.30969,-0.11294,0.31095,0.1142]}, + {"t":2.15707, "x":3.78144, "y":1.03157, "heading":1.93439, "vx":-2.59146, "vy":-0.20119, "omega":-2.00232, "ax":-0.00001, "ay":0.00008, "alpha":0.25432, "fx":[-0.38234,0.8517,0.38215,-0.8519], "fy":[-0.8506,-0.38104,0.853,0.38344]}, + {"t":2.19243, "x":3.6898, "y":1.02446, "heading":1.86358, "vx":-2.59146, "vy":-0.20119, "omega":-1.99333, "ax":-0.00001, "ay":0.0001, "alpha":0.4344, "fx":[-0.75432,1.40496,0.75406,-1.40522], "fy":[-1.40348,-0.75258,1.40669,0.75579]}, + {"t":2.2278, "x":3.59815, "y":1.01734, "heading":1.79308, "vx":-2.59146, "vy":-0.20118, "omega":-1.97796, "ax":-0.00001, "ay":0.00011, "alpha":0.63464, "fx":[-1.24385,1.96994,1.24356,-1.97022], "fy":[-1.96829,-1.24192,1.97186,1.24549]}, + {"t":2.26316, "x":3.50651, "y":1.01023, "heading":1.72313, "vx":-2.59146, "vy":-0.20118, "omega":-1.95552, "ax":-0.00001, "ay":0.00011, "alpha":0.859, "fx":[-1.86577,2.54223,1.86549,-2.54251], "fy":[-2.54065,-1.86391,2.54409,1.86735]}, + {"t":2.29852, "x":3.41486, "y":1.00311, "heading":1.65398, "vx":-2.59146, "vy":-0.20118, "omega":-1.92514, "ax":-0.00001, "ay":0.00009, "alpha":1.1121, "fx":[-2.63712,3.11659,2.63688,-3.11682], "fy":[-3.11526,-2.63556,3.11814,2.63844]}, + {"t":2.33389, "x":3.32321, "y":0.996, "heading":1.5859, "vx":-2.59146, "vy":-0.20117, "omega":-1.88581, "ax":-0.00001, "ay":0.00006, "alpha":1.39901, "fx":[-3.57644,3.68593,3.57627,-3.68609], "fy":[-3.68502,-3.57536,3.68701,3.57735]}, + {"t":2.36925, "x":3.23157, "y":0.98888, "heading":1.51921, "vx":-2.59146, "vy":-0.20117, "omega":-1.83634, "ax":0.0, "ay":0.00003, "alpha":1.7259, "fx":[-4.70526,4.2431,4.70516,-4.24318], "fy":[-4.24268,-4.70476,4.24359,4.70566]}, + {"t":2.40462, "x":3.13992, "y":0.98177, "heading":1.45426, "vx":-2.59146, "vy":-0.20117, "omega":-1.7753, "ax":0.0, "ay":-0.00001, "alpha":2.0997, "fx":[-6.0472,4.77977,6.04721,-4.77976], "fy":[-4.77995,-6.04739,4.77959,6.04703]}, + {"t":2.43998, "x":3.04828, "y":0.97465, "heading":1.39148, "vx":-2.59146, "vy":-0.20117, "omega":-1.70105, "ax":0.00001, "ay":-0.00008, "alpha":2.52804, "fx":[-7.62747,5.28678,7.62767,-5.28663], "fy":[-5.28795,-7.62881,5.28545,7.62633]}, + {"t":2.47535, "x":2.95663, "y":0.96754, "heading":1.33133, "vx":-2.59146, "vy":-0.20117, "omega":-1.61165, "ax":0.00002, "ay":-0.00028, "alpha":3.01849, "fx":[-9.46998,5.75367,9.47076,-5.75312], "fy":[-5.75784,-9.47474,5.74895,9.466]}, + {"t":2.51071, "x":2.86499, "y":0.96043, "heading":1.27433, "vx":-2.59146, "vy":-0.20118, "omega":-1.5049, "ax":0.00008, "ay":-0.00105, "alpha":3.57772, "fx":[-11.59359,6.16982,11.59674,-6.16783], "fy":[-6.18579,-11.61167,6.15186,11.57866]}, + {"t":2.54608, "x":2.77334, "y":0.95331, "heading":1.22111, "vx":-2.59146, "vy":-0.20122, "omega":-1.37837, "ax":0.00033, "ay":-0.00422, "alpha":4.21042, "fx":[-14.00593,6.52734,14.01924,-6.51987], "fy":[-6.59204,-14.07814,6.45515,13.947]}, + {"t":2.58144, "x":2.6817, "y":0.94619, "heading":1.17237, "vx":-2.59145, "vy":-0.20137, "omega":-1.22948, "ax":0.00132, "ay":-0.017, "alpha":4.91816, "fx":[-16.69178,6.82739,16.74821,-6.79985], "fy":[-7.09209,-16.98099,6.53496,16.45841]}, + {"t":2.6168, "x":2.59005, "y":0.93906, "heading":1.12889, "vx":-2.5914, "vy":-0.20197, "omega":-1.05555, "ax":0.00529, "ay":-0.06749, "alpha":5.69739, "fx":[-19.57803,7.09424,19.81462,-6.99486], "fy":[-8.16359,-20.71329,5.92231,18.66874]}, + {"t":2.65217, "x":2.49841, "y":0.93187, "heading":1.09156, "vx":-2.59121, "vy":-0.20436, "omega":-0.85406, "ax":0.02121, "ay":-0.26137, "alpha":6.51918, "fx":[-22.37187,7.40776,23.35368,-7.04298], "fy":[-11.61114,-26.66478,2.79082,18.88715]}, + {"t":2.68753, "x":2.40679, "y":0.92448, "heading":1.06135, "vx":-2.59046, "vy":-0.2136, "omega":-0.62352, "ax":0.94687, "ay":-0.88333, "alpha":6.74856, "fx":[-9.48644,20.95572,38.46111,10.19847], "fy":[-23.63298,-36.20631,-7.47234,11.21762]}, + {"t":2.7229, "x":2.31577, "y":0.91638, "heading":1.0393, "vx":-2.55698, "vy":-0.24484, "omega":-0.38486, "ax":5.4785, "ay":0.38683, "alpha":0.14565, "fx":[86.97603,87.04819,86.98051,86.8964], "fy":[5.94643,5.11044,6.332,7.17616]}, + {"t":2.75826, "x":2.22877, "y":0.90796, "heading":1.02569, "vx":-2.36324, "vy":-0.23116, "omega":-0.37971, "ax":5.51954, "ay":0.409, "alpha":0.08248, "fx":[87.63059,87.67028,87.62484,87.58116], "fy":[6.39299,5.90267,6.59215,7.08484]}, + {"t":2.79363, "x":2.14865, "y":0.90004, "heading":1.01227, "vx":-2.16804, "vy":-0.21669, "omega":-0.37679, "ax":5.53313, "ay":0.41645, "alpha":0.06172, "fx":[87.84593,87.87571,87.84017,87.80812], "fy":[6.54293,6.16725,6.67921,7.0561]}, + {"t":2.82899, "x":2.07544, "y":0.89264, "heading":0.99894, "vx":-1.97236, "vy":-0.20197, "omega":-0.37461, "ax":5.5399, "ay":0.42018, "alpha":0.05145, "fx":[87.95292,87.97801,87.94791,87.92124], "fy":[6.61884,6.29923,6.72217,7.04252]}, + {"t":2.86436, "x":2.00915, "y":0.88576, "heading":0.98569, "vx":-1.77645, "vy":-0.18711, "omega":-0.37279, "ax":5.54396, "ay":0.42243, "alpha":0.04533, "fx":[88.01686,88.03924,88.01259,87.98896], "fy":[6.66516,6.37817,6.74732,7.03483]}, + {"t":2.89972, "x":1.94979, "y":0.87941, "heading":0.97251, "vx":-1.58039, "vy":-0.17217, "omega":-0.37118, "ax":5.54666, "ay":0.42393, "alpha":0.04129, "fx":[88.05934,88.08001,88.05574,88.03403], "fy":[6.6967,6.43063,6.76352,7.02997]}, + {"t":2.93508, "x":1.89737, "y":0.87358, "heading":0.95938, "vx":-1.38424, "vy":-0.15718, "omega":-0.36972, "ax":5.54859, "ay":0.425, "alpha":0.03842, "fx":[88.08959,88.1091,88.0866,88.06619], "fy":[6.71981,6.46798,6.77457,7.02667]}, + {"t":2.97045, "x":1.85189, "y":0.86829, "heading":0.94631, "vx":-1.18801, "vy":-0.14215, "omega":-0.36837, "ax":5.55003, "ay":0.42581, "alpha":0.03629, "fx":[88.11221,88.13091,88.10978,88.09028], "fy":[6.73764,6.49592,6.78239,7.02431]}, + {"t":3.00581, "x":1.81334, "y":0.86353, "heading":0.93328, "vx":-0.99174, "vy":-0.12709, "omega":-0.36708, "ax":1.07295, "ay":-4.77353, "alpha":1.91457, "fx":[4.68093,16.65904,28.99716,17.79824], "fy":[-78.10215,-77.81413,-72.90614,-74.31046]}, + {"t":3.04379, "x":1.77645, "y":0.85526, "heading":0.91934, "vx":-0.95099, "vy":-0.30839, "omega":-0.29436, "ax":0.29664, "ay":-0.86349, "alpha":8.67236, "fx":[-27.47413,8.68385,36.56091,1.06656], "fy":[-20.15919,-43.02062,-10.8102,19.15605]}, + {"t":3.08178, "x":1.74054, "y":0.84292, "heading":0.90816, "vx":-0.93972, "vy":-0.34119, "omega":0.03503, "ax":0.01749, "ay":-0.04804, "alpha":5.34481, "fx":[-19.20103,2.68357,19.74821,-2.11987], "fy":[-3.20578,-20.19525,1.60129,18.74932]}, + {"t":3.11976, "x":1.70486, "y":0.82993, "heading":0.90949, "vx":-0.93906, "vy":-0.34302, "omega":0.23803, "ax":0.00081, "ay":-0.00223, "alpha":0.69159, "fx":[-2.50643,0.32715,2.53224,-0.30132], "fy":[-0.34961,-2.55465,0.27887,2.48402]}, + {"t":3.15774, "x":1.6692, "y":0.8169, "heading":0.91853, "vx":-0.93903, "vy":-0.3431, "omega":0.2643, "ax":0.00017, "ay":-0.00015, "alpha":-4.10838, "fx":[14.95128,-1.9992,-14.94599,2.00472], "fy":[1.99953,14.94638,-2.00439,-14.95089]}, + {"t":3.19572, "x":1.63353, "y":0.80387, "heading":0.92857, "vx":-0.93902, "vy":-0.34311, "omega":0.10826, "ax":3.8228, "ay":1.39694, "alpha":-2.66255, "fx":[65.97666,56.61283,55.36808,64.80116], "fy":[21.23051,35.72754,24.15885,7.59275]}, + {"t":3.2337, "x":1.60062, "y":0.79184, "heading":0.93268, "vx":-0.79382, "vy":-0.29005, "omega":0.00713, "ax":5.21315, "ay":1.90477, "alpha":-0.06766, "fx":[82.79931,82.59637,82.72654,82.92787], "fy":[30.14752,30.69395,30.33322,29.78388]}, + {"t":3.27168, "x":1.57423, "y":0.7822, "heading":0.93295, "vx":-0.59582, "vy":-0.2177, "omega":0.00456, "ax":5.22553, "ay":1.90931, "alpha":-0.04607, "fx":[82.98332,82.84559,82.93519,83.07216], "fy":[30.24842,30.62182,30.37553,30.00078]}, + {"t":3.30967, "x":1.55537, "y":0.77531, "heading":0.93312, "vx":-0.39735, "vy":-0.14518, "omega":0.00281, "ax":5.2297, "ay":1.91084, "alpha":-0.0388, "fx":[83.04551,82.92966,83.00539,83.12071], "fy":[30.28255,30.59737,30.38974,30.07396]}, + {"t":3.34765, "x":1.54405, "y":0.77117, "heading":0.93323, "vx":-0.19871, "vy":-0.07261, "omega":0.00133, "ax":5.2318, "ay":1.9116, "alpha":-0.03515, "fx":[83.07677,82.97188,83.04062,83.14506], "fy":[30.29971,30.58507,30.39687,30.11072]}, + {"t":3.38563, "x":1.54028, "y":0.7698, "heading":0.93328, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/reefscape.chor b/src/main/deploy/choreo/reefscape.chor index 10d204d..c949ba6 100644 --- a/src/main/deploy/choreo/reefscape.chor +++ b/src/main/deploy/choreo/reefscape.chor @@ -1,619 +1 @@ -{ - "name":"reefscape", - "version":1, - "type":"Swerve", - "variables":{ - "expressions":{ - "GYVal":{ - "dimension":"Number", - "var":{ - "exp":"3.8580894470214844", - "val":3.858089447021485 - } - }, - "HYVal":{ - "dimension":"Number", - "var":{ - "exp":"4.19", - "val":4.19 - } - }, - "PushingDistance":{ - "dimension":"Number", - "var":{ - "exp":"7.260873794555664", - "val":7.260873794555664 - } - }, - "StartLineXVal":{ - "dimension":"Number", - "var":{ - "exp":"7.169135570526123", - "val":7.169135570526123 - } - }, - "coralSpeed":{ - "dimension":"LinVel", - "var":{ - "exp":"1 m / s", - "val":1.0 - } - }, - "degOffset":{ - "dimension":"Number", - "var":{ - "exp":"90", - "val":90.0 - } - }, - "maxAccel":{ - "dimension":"LinAcc", - "var":{ - "exp":"4 m / s ^ 2", - "val":4.0 - } - }, - "movingSpeed":{ - "dimension":"LinVel", - "var":{ - "exp":"2.6 m / s", - "val":2.6 - } - }, - "offset":{ - "dimension":"Number", - "var":{ - "exp":"0.0826", - "val":0.0826 - } - }, - "reefSpeed":{ - "dimension":"LinVel", - "var":{ - "exp":"1.75 m / s", - "val":1.75 - } - }, - "startSpeed":{ - "dimension":"LinVel", - "var":{ - "exp":"3 m / s", - "val":3.0 - } - } - }, - "poses":{ - "A":{ - "x":{ - "exp":"3.1920857429504395 m + (offset * cos(0 deg + degOffset deg)) m", - "val":3.1920857429504395 - }, - "y":{ - "exp":"HYVal m + (offset * sin(0 deg + degOffset deg)) m", - "val":4.272600000000001 - }, - "heading":{ - "exp":"0 deg", - "val":0.0 - } - }, - "A_Short":{ - "x":{ - "exp":"2.9455809593200684 m", - "val":2.9455809593200684 - }, - "y":{ - "exp":"4.269176959991455 m", - "val":4.269176959991455 - }, - "heading":{ - "exp":"A.heading", - "val":0.0 - } - }, - "B":{ - "x":{ - "exp":"3.1920857429504395 m + (offset * cos(0 deg + degOffset deg)) m", - "val":3.1920857429504395 - }, - "y":{ - "exp":"GYVal m + (offset * sin(0 deg + degOffset deg)) m", - "val":3.9406894470214846 - }, - "heading":{ - "exp":"0 deg", - "val":0.0 - } - }, - "B_Short":{ - "x":{ - "exp":"2.936753749847412 m", - "val":2.936753749847412 - }, - "y":{ - "exp":"B.y", - "val":3.9406894470214846 - }, - "heading":{ - "exp":"B.heading", - "val":0.0 - } - }, - "C":{ - "x":{ - "exp":"3.6992833614349365 m + (offset * cos(60 deg + degOffset deg)) m", - "val":3.627749663082342 - }, - "y":{ - "exp":"2.987058401107788 m + (offset * sin(60 deg + degOffset deg)) m", - "val":3.028358401107788 - }, - "heading":{ - "exp":"60 deg", - "val":1.0471975511965976 - } - }, - "C_Short":{ - "x":{ - "exp":"3.5017032623291016 m", - "val":3.5017032623291016 - }, - "y":{ - "exp":"2.812666416168213 m", - "val":2.812666416168213 - }, - "heading":{ - "exp":"C.heading", - "val":1.0471975511965976 - } - }, - "D":{ - "x":{ - "exp":"3.9885246753692627 m + (offset * cos(60 deg + degOffset deg)) m", - "val":3.916990977016668 - }, - "y":{ - "exp":"2.828442096710205 m + (offset * sin(60 deg + degOffset deg)) m", - "val":2.869742096710205 - }, - "heading":{ - "exp":"60 deg", - "val":1.0471975511965976 - } - }, - "D_Short":{ - "x":{ - "exp":"3.7841780185699463 m", - "val":3.7841780185699463 - }, - "y":{ - "exp":"2.644947052001953 m", - "val":2.644947052001953 - }, - "heading":{ - "exp":"D.heading", - "val":1.0471975511965976 - } - }, - "E":{ - "x":{ - "exp":"4.991955280303955 m + (offset * cos(120 deg + degOffset deg)) m", - "val":4.92042158195136 - }, - "y":{ - "exp":"2.8231382369995117 m + (offset * sin(120 deg + degOffset deg)) m", - "val":2.7818382369995116 - }, - "heading":{ - "exp":"120 deg", - "val":2.0943951023931953 - } - }, - "E_Short":{ - "x":{ - "exp":"5.064141750335693 m", - "val":5.064141750335693 - }, - "y":{ - "exp":"2.5390188694000244 m", - "val":2.5390188694000244 - }, - "heading":{ - "exp":"E.heading", - "val":2.0943951023931953 - } - }, - "F":{ - "x":{ - "exp":"5.276031017303467 m + (offset * cos(120 deg + degOffset deg)) m", - "val":5.204497318950872 - }, - "y":{ - "exp":"2.9924910068511963 m + (offset * sin(120 deg + degOffset deg)) m", - "val":2.951191006851196 - }, - "heading":{ - "exp":"120 deg", - "val":2.0943951023931953 - } - }, - "F_Short":{ - "x":{ - "exp":"5.337789058685303 m", - "val":5.337789058685303 - }, - "y":{ - "exp":"2.724392890930176 m", - "val":2.724392890930176 - }, - "heading":{ - "exp":"E.heading", - "val":2.0943951023931953 - } - }, - "G":{ - "x":{ - "exp":"5.772265434265137 m + (offset * cos(180 deg + degOffset deg)) m", - "val":5.772265434265137 - }, - "y":{ - "exp":"GYVal m + (offset * sin(180 deg + degOffset deg)) m", - "val":3.775489447021484 - }, - "heading":{ - "exp":"180 deg", - "val":3.141592653589793 - } - }, - "G_Short":{ - "x":{ - "exp":"6.052803993225098 m", - "val":6.052803993225098 - }, - "y":{ - "exp":"G.y", - "val":3.775489447021484 - }, - "heading":{ - "exp":"G.heading", - "val":3.141592653589793 - } - }, - "H":{ - "x":{ - "exp":"5.772265434265137 m + (offset * cos(180 deg + degOffset deg)) m", - "val":5.772265434265137 - }, - "y":{ - "exp":"HYVal m + (offset * sin(180 deg + degOffset deg)) m", - "val":4.1074 - }, - "heading":{ - "exp":"180 deg", - "val":3.141592653589793 - } - }, - "HPLeft":{ - "x":{ - "exp":"1.3222596645355225 m", - "val":1.3222596645355225 - }, - "y":{ - "exp":"7.231224536895752 m", - "val":7.231224536895752 - }, - "heading":{ - "exp":"-0.9332802447333852 rad", - "val":-0.9332802447333852 - } - }, - "HPRight":{ - "x":{ - "exp":"1.5402799844741821 m", - "val":1.540279984474182 - }, - "y":{ - "exp":"0.7697955369949341 m", - "val":0.7697955369949341 - }, - "heading":{ - "exp":"0.9332802447333852 rad", - "val":0.9332802447333852 - } - }, - "H_Short":{ - "x":{ - "exp":"6.035149097442627 m", - "val":6.035149097442627 - }, - "y":{ - "exp":"H.y", - "val":4.1074 - }, - "heading":{ - "exp":"H.heading", - "val":3.141592653589793 - } - }, - "I":{ - "x":{ - "exp":"5.276031017303467 m + (offset * cos(-120 deg + degOffset deg)) m", - "val":5.347564715656062 - }, - "y":{ - "exp":"5.062966823577881 m + (offset * sin(-120 deg + degOffset deg)) m", - "val":5.021666823577881 - }, - "heading":{ - "exp":"-120 deg", - "val":-2.0943951023931953 - } - }, - "I_Short":{ - "x":{ - "exp":"5.479026794433594 m", - "val":5.479026794433594 - }, - "y":{ - "exp":"5.240184307098389 m", - "val":5.240184307098389 - }, - "heading":{ - "exp":"I.heading", - "val":-2.0943951023931953 - } - }, - "J":{ - "x":{ - "exp":"4.99741792678833 m + (offset * cos(-120 deg + degOffset deg)) m", - "val":5.068951625140925 - }, - "y":{ - "exp":"5.226856708526611 m + (offset * sin(-120 deg + degOffset deg)) m", - "val":5.185556708526612 - }, - "heading":{ - "exp":"-120 deg", - "val":-2.0943951023931953 - } - }, - "J_Short":{ - "x":{ - "exp":"5.205379486083984 m", - "val":5.205379486083984 - }, - "y":{ - "exp":"5.425558090209961 m", - "val":5.425558090209961 - }, - "heading":{ - "exp":"J.heading", - "val":-2.0943951023931953 - } - }, - "K":{ - "x":{ - "exp":"3.9867637157440186 m + (offset * cos(-60 deg + degOffset deg)) m", - "val":4.0582974140966135 - }, - "y":{ - "exp":"5.226856708526611 m + (offset * sin(-60 deg + degOffset deg)) m", - "val":5.268156708526611 - }, - "heading":{ - "exp":"-60 deg", - "val":-1.0471975511965976 - } - }, - "K_Short":{ - "x":{ - "exp":"3.9342429637908936 m", - "val":3.9342429637908936 - }, - "y":{ - "exp":"5.487349510192871 m", - "val":5.487349510192871 - }, - "heading":{ - "exp":"K.heading", - "val":-1.0471975511965976 - } - }, - "L":{ - "x":{ - "exp":"3.708150863647461 m + (offset * cos(-60 deg + degOffset deg)) m", - "val":3.779684562000055 - }, - "y":{ - "exp":"5.052041053771973 m + (offset * sin(-60 deg + degOffset deg)) m", - "val":5.093341053771972 - }, - "heading":{ - "exp":"-60 deg", - "val":-1.0471975511965976 - } - }, - "L_Short":{ - "x":{ - "exp":"3.6429407596588135 m", - "val":3.6429407596588135 - }, - "y":{ - "exp":"5.328457832336426 m", - "val":5.328457832336426 - }, - "heading":{ - "exp":"L.heading", - "val":-1.0471975511965976 - } - }, - "StartLeft":{ - "x":{ - "exp":"StartLineXVal m", - "val":7.169135570526123 - }, - "y":{ - "exp":"6.1655120849609375 m", - "val":6.1655120849609375 - }, - "heading":{ - "exp":"270 deg", - "val":4.71238898038469 - } - }, - "StartMidG":{ - "x":{ - "exp":"StartLineXVal m", - "val":7.169135570526123 - }, - "y":{ - "exp":"GYVal m", - "val":3.858089447021485 - }, - "heading":{ - "exp":"180 deg", - "val":3.141592653589793 - } - }, - "StartMidH":{ - "x":{ - "exp":"StartLineXVal m", - "val":7.169135570526123 - }, - "y":{ - "exp":"HYVal m", - "val":4.19 - }, - "heading":{ - "exp":"180 deg", - "val":3.141592653589793 - } - }, - "StartRight":{ - "x":{ - "exp":"7.024972438812256 m", - "val":7.024972438812256 - }, - "y":{ - "exp":"1.6989885568618774 m", - "val":1.6989885568618774 - }, - "heading":{ - "exp":"E.heading", - "val":2.0943951023931953 - } - }, - "StartStraightRight":{ - "x":{ - "exp":"StartLineXVal m", - "val":7.169135570526123 - }, - "y":{ - "exp":"1.8764067888259888 m", - "val":1.8764067888259888 - }, - "heading":{ - "exp":"180 deg", - "val":3.141592653589793 - } - }, - "StartSuperLeft":{ - "x":{ - "exp":"StartLineXVal m", - "val":7.169135570526123 - }, - "y":{ - "exp":"6.917787075042725 m", - "val":6.917787075042725 - }, - "heading":{ - "exp":"-90 deg", - "val":-1.5707963267948966 - } - }, - "StartSuperRight":{ - "x":{ - "exp":"StartLineXVal m", - "val":7.169135570526123 - }, - "y":{ - "exp":"1.1446685791015625 m", - "val":1.1446685791015625 - }, - "heading":{ - "exp":"90 deg", - "val":1.5707963267948966 - } - } - } - }, - "config":{ - "frontLeft":{ - "x":{ - "exp":"11.375 in", - "val":0.288925 - }, - "y":{ - "exp":"11.375 in", - "val":0.288925 - } - }, - "backLeft":{ - "x":{ - "exp":"-11.375 in", - "val":-0.288925 - }, - "y":{ - "exp":"11.375 in", - "val":0.288925 - } - }, - "mass":{ - "exp":"140 lbs", - "val":63.502931800000006 - }, - "inertia":{ - "exp":"6 kg m ^ 2", - "val":6.0 - }, - "gearing":{ - "exp":"6.12", - "val":6.12 - }, - "radius":{ - "exp":"1.905 in", - "val":0.048387 - }, - "vmax":{ - "exp":"5000 RPM", - "val":523.5987755982989 - }, - "tmax":{ - "exp":"0.7 N * m", - "val":0.7 - }, - "cof":{ - "exp":"1.4", - "val":1.4 - }, - "bumper":{ - "front":{ - "exp":"17.25 in", - "val":0.43815 - }, - "side":{ - "exp":"17.25 in", - "val":0.43815 - }, - "back":{ - "exp":"17.25 in", - "val":0.43815 - } - }, - "differentialTrackWidth":{ - "exp":"22 in", - "val":0.5588 - } - }, - "generationFeatures":[] -} +{"name":"reefscape","version":1,"type":"Swerve","variables":{"expressions":{"GYVal":{"dimension":"Number","var":{"exp":"3.8580894470214844","val":3.858089447021485}},"HYVal":{"dimension":"Number","var":{"exp":"4.19","val":4.19}},"PushingDistance":{"dimension":"Number","var":{"exp":"7.260873794555664","val":7.260873794555664}},"StartLineXVal":{"dimension":"Number","var":{"exp":"7.169135570526123","val":7.169135570526123}},"coralSpeed":{"dimension":"LinVel","var":{"exp":"1 m / s","val":1.0}},"degOffset":{"dimension":"Number","var":{"exp":"90","val":90.0}},"maxAccel":{"dimension":"LinAcc","var":{"exp":"4 m / s ^ 2","val":4.0}},"movingSpeed":{"dimension":"LinVel","var":{"exp":"2.6 m / s","val":2.6}},"offset":{"dimension":"Number","var":{"exp":"0.0826","val":0.0826}},"reefSpeed":{"dimension":"LinVel","var":{"exp":"1.75 m / s","val":1.75}},"startSpeed":{"dimension":"LinVel","var":{"exp":"3 m / s","val":3.0}}},"poses":{"A":{"x":{"exp":"3.1920857429504395 m + (offset * cos(0 deg + degOffset deg)) m","val":3.1920857429504395},"y":{"exp":"HYVal m + (offset * sin(0 deg + degOffset deg)) m","val":4.272600000000001},"heading":{"exp":"0 deg","val":0.0}},"A_Short":{"x":{"exp":"2.9455809593200684 m","val":2.9455809593200684},"y":{"exp":"4.269176959991455 m","val":4.269176959991455},"heading":{"exp":"A.heading","val":0.0}},"B":{"x":{"exp":"3.1920857429504395 m + (offset * cos(0 deg + degOffset deg)) m","val":3.1920857429504395},"y":{"exp":"GYVal m + (offset * sin(0 deg + degOffset deg)) m","val":3.9406894470214846},"heading":{"exp":"0 deg","val":0.0}},"B_Short":{"x":{"exp":"2.936753749847412 m","val":2.936753749847412},"y":{"exp":"B.y","val":3.9406894470214846},"heading":{"exp":"B.heading","val":0.0}},"C":{"x":{"exp":"3.6992833614349365 m + (offset * cos(60 deg + degOffset deg)) m","val":3.627749663082342},"y":{"exp":"2.987058401107788 m + (offset * sin(60 deg + degOffset deg)) m","val":3.028358401107788},"heading":{"exp":"60 deg","val":1.0471975511965976}},"C_Short":{"x":{"exp":"3.5017032623291016 m","val":3.5017032623291016},"y":{"exp":"2.812666416168213 m","val":2.812666416168213},"heading":{"exp":"C.heading","val":1.0471975511965976}},"D":{"x":{"exp":"3.9885246753692627 m + (offset * cos(60 deg + degOffset deg)) m","val":3.916990977016668},"y":{"exp":"2.828442096710205 m + (offset * sin(60 deg + degOffset deg)) m","val":2.869742096710205},"heading":{"exp":"60 deg","val":1.0471975511965976}},"D_Short":{"x":{"exp":"3.7841780185699463 m","val":3.7841780185699463},"y":{"exp":"2.644947052001953 m","val":2.644947052001953},"heading":{"exp":"D.heading","val":1.0471975511965976}},"E":{"x":{"exp":"4.991955280303955 m + (offset * cos(120 deg + degOffset deg)) m","val":4.92042158195136},"y":{"exp":"2.8231382369995117 m + (offset * sin(120 deg + degOffset deg)) m","val":2.7818382369995116},"heading":{"exp":"120 deg","val":2.0943951023931953}},"E_Short":{"x":{"exp":"5.064141750335693 m","val":5.064141750335693},"y":{"exp":"2.5390188694000244 m","val":2.5390188694000244},"heading":{"exp":"E.heading","val":2.0943951023931953}},"F":{"x":{"exp":"5.276031017303467 m + (offset * cos(120 deg + degOffset deg)) m","val":5.204497318950872},"y":{"exp":"2.9924910068511963 m + (offset * sin(120 deg + degOffset deg)) m","val":2.951191006851196},"heading":{"exp":"120 deg","val":2.0943951023931953}},"F_Short":{"x":{"exp":"5.337789058685303 m","val":5.337789058685303},"y":{"exp":"2.724392890930176 m","val":2.724392890930176},"heading":{"exp":"E.heading","val":2.0943951023931953}},"G":{"x":{"exp":"5.772265434265137 m + (offset * cos(180 deg + degOffset deg)) m","val":5.772265434265137},"y":{"exp":"GYVal m + (offset * sin(180 deg + degOffset deg)) m","val":3.775489447021484},"heading":{"exp":"180 deg","val":3.141592653589793}},"G_Short":{"x":{"exp":"6.052803993225098 m","val":6.052803993225098},"y":{"exp":"G.y","val":3.775489447021484},"heading":{"exp":"G.heading","val":3.141592653589793}},"H":{"x":{"exp":"5.772265434265137 m + (offset * cos(180 deg + degOffset deg)) m","val":5.772265434265137},"y":{"exp":"HYVal m + (offset * sin(180 deg + degOffset deg)) m","val":4.1074},"heading":{"exp":"180 deg","val":3.141592653589793}},"HPLeft":{"x":{"exp":"1.3222596645355225 m","val":1.3222596645355225},"y":{"exp":"7.231224536895752 m","val":7.231224536895752},"heading":{"exp":"-0.9332802447333852 rad","val":-0.9332802447333852}},"HPRight":{"x":{"exp":"1.5402799844741821 m","val":1.540279984474182},"y":{"exp":"0.7697955369949341 m","val":0.7697955369949341},"heading":{"exp":"0.9332802447333852 rad","val":0.9332802447333852}},"H_Short":{"x":{"exp":"6.035149097442627 m","val":6.035149097442627},"y":{"exp":"H.y","val":4.1074},"heading":{"exp":"H.heading","val":3.141592653589793}},"I":{"x":{"exp":"5.276031017303467 m + (offset * cos(-120 deg + degOffset deg)) m","val":5.347564715656062},"y":{"exp":"5.062966823577881 m + (offset * sin(-120 deg + degOffset deg)) m","val":5.021666823577881},"heading":{"exp":"-120 deg","val":-2.0943951023931953}},"I_Short":{"x":{"exp":"5.479026794433594 m","val":5.479026794433594},"y":{"exp":"5.240184307098389 m","val":5.240184307098389},"heading":{"exp":"I.heading","val":-2.0943951023931953}},"J":{"x":{"exp":"4.99741792678833 m + (offset * cos(-120 deg + degOffset deg)) m","val":5.068951625140925},"y":{"exp":"5.226856708526611 m + (offset * sin(-120 deg + degOffset deg)) m","val":5.185556708526612},"heading":{"exp":"-120 deg","val":-2.0943951023931953}},"J_Short":{"x":{"exp":"5.205379486083984 m","val":5.205379486083984},"y":{"exp":"5.425558090209961 m","val":5.425558090209961},"heading":{"exp":"J.heading","val":-2.0943951023931953}},"K":{"x":{"exp":"3.9867637157440186 m + (offset * cos(-60 deg + degOffset deg)) m","val":4.0582974140966135},"y":{"exp":"5.226856708526611 m + (offset * sin(-60 deg + degOffset deg)) m","val":5.268156708526611},"heading":{"exp":"-60 deg","val":-1.0471975511965976}},"K_Short":{"x":{"exp":"3.9342429637908936 m","val":3.9342429637908936},"y":{"exp":"5.487349510192871 m","val":5.487349510192871},"heading":{"exp":"K.heading","val":-1.0471975511965976}},"L":{"x":{"exp":"3.708150863647461 m + (offset * cos(-60 deg + degOffset deg)) m","val":3.779684562000055},"y":{"exp":"5.052041053771973 m + (offset * sin(-60 deg + degOffset deg)) m","val":5.093341053771972},"heading":{"exp":"-60 deg","val":-1.0471975511965976}},"L_Short":{"x":{"exp":"3.6429407596588135 m","val":3.6429407596588135},"y":{"exp":"5.328457832336426 m","val":5.328457832336426},"heading":{"exp":"L.heading","val":-1.0471975511965976}},"StartLeft":{"x":{"exp":"StartLineXVal m","val":7.169135570526123},"y":{"exp":"6.1655120849609375 m","val":6.1655120849609375},"heading":{"exp":"270 deg","val":4.71238898038469}},"StartMidG":{"x":{"exp":"StartLineXVal m","val":7.169135570526123},"y":{"exp":"GYVal m","val":3.858089447021485},"heading":{"exp":"180 deg","val":3.141592653589793}},"StartMidH":{"x":{"exp":"StartLineXVal m","val":7.169135570526123},"y":{"exp":"HYVal m","val":4.19},"heading":{"exp":"180 deg","val":3.141592653589793}},"StartRight":{"x":{"exp":"7.024972438812256 m","val":7.024972438812256},"y":{"exp":"1.6989885568618774 m","val":1.6989885568618774},"heading":{"exp":"E.heading","val":2.0943951023931953}},"StartStraightRight":{"x":{"exp":"StartLineXVal m","val":7.169135570526123},"y":{"exp":"1.8764067888259888 m","val":1.8764067888259888},"heading":{"exp":"180 deg","val":3.141592653589793}},"StartSuperLeft":{"x":{"exp":"StartLineXVal m","val":7.169135570526123},"y":{"exp":"6.917787075042725 m","val":6.917787075042725},"heading":{"exp":"-90 deg","val":-1.5707963267948966}},"StartSuperRight":{"x":{"exp":"StartLineXVal m","val":7.169135570526123},"y":{"exp":"1.1446685791015625 m","val":1.1446685791015625},"heading":{"exp":"90 deg","val":1.5707963267948966}}}},"config":{"frontLeft":{"x":{"exp":"11.375 in","val":0.288925},"y":{"exp":"11.375 in","val":0.288925}},"backLeft":{"x":{"exp":"-11.375 in","val":-0.288925},"y":{"exp":"11.375 in","val":0.288925}},"mass":{"exp":"140 lbs","val":63.502931800000006},"inertia":{"exp":"6 kg m ^ 2","val":6.0},"gearing":{"exp":"6.12","val":6.12},"radius":{"exp":"1.905 in","val":0.048387},"vmax":{"exp":"5000 RPM","val":523.5987755982989},"tmax":{"exp":"0.7 N * m","val":0.7},"cof":{"exp":"1.4","val":1.4},"bumper":{"front":{"exp":"17.25 in","val":0.43815},"side":{"exp":"17.25 in","val":0.43815},"back":{"exp":"17.25 in","val":0.43815}},"differentialTrackWidth":{"exp":"22 in","val":0.5588}},"generationFeatures":[]} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 57f1a3c..3ac2e67 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -498,10 +498,22 @@ public void disabledPeriodic() { List.of(HPStation.HP_RIGHT) )); - Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Left 3 piece auton generated")); + Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Mid Auton generated")); WaltAutonBuilder.nte_midOnePiece.setBoolean(false); } + if(WaltAutonBuilder.nte_midTown.getBoolean(false)) { + waltAutonFactory = Optional.of(autonFactoryFactory( + StartingLocs.MID_G, + List.of(REEF_G, REEF_D, REEF_E), + List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4), + List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT) + )); + + Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Midtown auton generated")); + WaltAutonBuilder.nte_midTown.setBoolean(false); + } + // fail-case (no auton selected) - do nothing (its no longer that now) if (readyToMakeAuton && waltAutonFactory.isEmpty()) { diff --git a/src/main/java/frc/robot/autons/TrajsAndLocs.java b/src/main/java/frc/robot/autons/TrajsAndLocs.java index 3e19ede..a17a2d9 100644 --- a/src/main/java/frc/robot/autons/TrajsAndLocs.java +++ b/src/main/java/frc/robot/autons/TrajsAndLocs.java @@ -155,7 +155,6 @@ public static class Trajectories { public static HashMap, String> StartToReefTrajs = new HashMap<>(); public static HashMap, String> StartToReefShortTrajs = new HashMap<>(); public static HashMap, String> ReefToHPTrajs = new HashMap<>(); - public static HashMap, String> ReefToHPShortTrajs = new HashMap<>(); public static HashMap, String> HPToReefTrajs = new HashMap<>(); public static HashMap, String> HPToReefShortTrajs = new HashMap<>(); public static HashMap PushingTrajs = new HashMap<>(); @@ -189,11 +188,6 @@ public static class Trajectories { ReefToHPTrajs.put(new Pair(ReefLocs.REEF_J, HPStation.HP_LEFT), "J_Left"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_K, HPStation.HP_LEFT), "K_Left"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_L, HPStation.HP_LEFT), "L_Left"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_A, HPStation.HP_LEFT), "A_Left_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_B, HPStation.HP_LEFT), "B_Left_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_J, HPStation.HP_LEFT), "J_Left_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_K, HPStation.HP_LEFT), "K_Left_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_L, HPStation.HP_LEFT), "L_Left_short"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_A, HPStation.HP_RIGHT), "A_Right"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_B, HPStation.HP_RIGHT), "B_Right"); @@ -201,12 +195,7 @@ public static class Trajectories { ReefToHPTrajs.put(new Pair(ReefLocs.REEF_D, HPStation.HP_RIGHT), "D_Right"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_E, HPStation.HP_RIGHT), "E_Right"); ReefToHPTrajs.put(new Pair(ReefLocs.REEF_F, HPStation.HP_RIGHT), "F_Right"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_A, HPStation.HP_RIGHT), "A_Right_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_B, HPStation.HP_RIGHT), "B_Right_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_C, HPStation.HP_RIGHT), "C_Right_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_D, HPStation.HP_RIGHT), "D_Right_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_E, HPStation.HP_RIGHT), "E_Right_short"); - ReefToHPShortTrajs.put(new Pair(ReefLocs.REEF_F, HPStation.HP_RIGHT), "F_Right_short"); + ReefToHPTrajs.put(new Pair(ReefLocs.REEF_G, HPStation.HP_RIGHT), "Madtown_G_Right"); HPToReefTrajs.put(new Pair(HPStation.HP_LEFT, ReefLocs.REEF_A), "Left_A"); HPToReefTrajs.put(new Pair(HPStation.HP_LEFT, ReefLocs.REEF_B), "Left_B"); diff --git a/src/main/java/frc/robot/autons/WaltAutonBuilder.java b/src/main/java/frc/robot/autons/WaltAutonBuilder.java index 5ddd75b..2ffda1e 100644 --- a/src/main/java/frc/robot/autons/WaltAutonBuilder.java +++ b/src/main/java/frc/robot/autons/WaltAutonBuilder.java @@ -26,6 +26,7 @@ public class WaltAutonBuilder { public static GenericEntry nte_rightThreePiece; public static GenericEntry nte_leftThreePiece; public static GenericEntry nte_midOnePiece; + public static GenericEntry nte_midTown; // public static GenericEntry nte_midGOnly; // ---- Initial @@ -101,7 +102,11 @@ public class WaltAutonBuilder { .add("Mid 1 Piece", false) .withWidget(BuiltInWidgets.kToggleSwitch) .getEntry(); - + + nte_midTown = Shuffleboard.getTab("AutonChooser") + .add("Midtown", false) + .withWidget(BuiltInWidgets.kToggleSwitch) + .getEntry(); } // adds the options diff --git a/src/main/java/frc/robot/autons/WaltAutonFactory.java b/src/main/java/frc/robot/autons/WaltAutonFactory.java index c1816c2..a0c378d 100644 --- a/src/main/java/frc/robot/autons/WaltAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltAutonFactory.java @@ -182,13 +182,16 @@ private ArrayList>> trajMaker() { ArrayList>> trajsList = new ArrayList<>(); try { for (int i = 0; i < m_scoreLocs.size(); i++) { - String rToH = ReefToHPTrajs.get(new Pair(m_scoreLocs.get(i), m_hpStations.get(i))); + var curScoreLog = m_scoreLocs.get(i); + var curHpStation = m_hpStations.get(i); + String rToH = ReefToHPTrajs.get(new Pair(curScoreLog, curHpStation)); + var rToHTraj = m_routine.trajectory(rToH); trajsList.add( - new Pair>(m_routine.trajectory(rToH), Optional.empty())); + new Pair>(rToHTraj, Optional.empty())); System.out.println("Adding RtoH Path: " + rToH); if (i < m_scoreLocs.size() - 1) { var reefLoc = m_scoreLocs.get(i + 1); - String hToR = HPToReefShortTrajs.get(new Pair(m_hpStations.get(i), reefLoc)); + String hToR = HPToReefShortTrajs.get(new Pair(curHpStation, reefLoc)); trajsList.add( new Pair>(m_routine.trajectory(hToR), Optional.of(reefLoc))); System.out.println("Adding HtoR Path: " + hToR + " with score: " + reefLoc); @@ -265,6 +268,10 @@ public AutoRoutine midAuton() { firstCmd ); + m_routine.active().debounce(0.25).onTrue( + m_funnel.ejectFlap().withTimeout(0.25) + ); + firstScoreTraj.done() .onTrue( Commands.sequence(