diff --git a/elastic_layouts/elastic-4048-2026-dev-v1.0.json b/elastic_layouts/elastic-4048-2026-dev-v1.0.json index 903fd438..b61f834f 100644 --- a/elastic_layouts/elastic-4048-2026-dev-v1.0.json +++ b/elastic_layouts/elastic-4048-2026-dev-v1.0.json @@ -62,18 +62,6 @@ } } ] - }, - { - "title": "camera goes here", - "x": 768.0, - "y": 0.0, - "width": 512.0, - "height": 384.0, - "type": "List Layout", - "properties": { - "label_position": "TOP" - }, - "children": [] } ], "containers": [ @@ -117,16 +105,56 @@ }, { "title": "Auto Chooser", + "x": 1024.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Action Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "DriverCam", "x": 768.0, - "y": 384.0, + "y": 0.0, "width": 512.0, - "height": 256.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/DriverCam", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Location Chooser", + "x": 768.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, "type": "ComboBox Chooser", "properties": { - "topic": "/SmartDashboard/Action Chooser", + "topic": "/SmartDashboard/Location Chooser", "period": 0.06, "sort_options": false } + }, + { + "title": "Selected Action", + "x": 768.0, + "y": 384.0, + "width": 512.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Selected Action", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } } ] } @@ -661,6 +689,19 @@ "show_type": false, "maximize_button_space": true } + }, + { + "title": "Location Chooser", + "x": 512.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Location Chooser", + "period": 0.06, + "sort_options": false + } } ] } diff --git a/src/main/deploy/choreo/LeftToTower.traj b/src/main/deploy/choreo/LeftToTower.traj new file mode 100644 index 00000000..c5e696a8 --- /dev/null +++ b/src/main/deploy/choreo/LeftToTower.traj @@ -0,0 +1,130 @@ +{ + "name":"LeftToTower", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.733068466186523, "y":7.429088115692139, "heading":0.0, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.268970489501953, "y":5.231799125671387, "heading":-3.141592653589793, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.8350459933280945, "y":4.654308795928955, "heading":-3.141592653589793, "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}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.9086382389068604 m", "val":3.908638238906861}, "y":{"exp":"7.501871109008789 m", "val":7.501871109008789}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.268970489501953 m", "val":2.268970489501953}, "y":{"exp":"5.231799125671387 m", "val":5.231799125671387}, "heading":{"exp":"-180 deg", "val":-3.141592653589793}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"0.8350459933280945 m", "val":0.8350459933280945}, "y":{"exp":"4.654308795928955 m", "val":4.654308795928955}, "heading":{"exp":"-180 deg", "val":-3.141592653589793}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2286, + "y":0.2286 + }, + "backLeft":{ + "x":-0.2286, + "y":0.2286 + }, + "mass":52.16312255, + "inertia":3.0, + "gearing":6.5, + "radius":0.1016, + "vmax":628.3185307179587, + "tmax":1.2, + "cof":1.5, + "bumper":{ + "front":0.6858, + "side":0.6858, + "back":0.6858 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.03333,1.77656], + "samples":[ + {"t":0.0, "x":3.73307, "y":7.42909, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-1.76606, "ay":-5.14588, "alpha":9.15613, "fx":[-59.70005,-36.68432,1.30046,2.96059], "fy":[-48.00468,-67.32441,-76.64508,-76.45106]}, + {"t":0.02199, "x":3.73264, "y":7.42784, "heading":0.0, "vx":-0.03883, "vy":-0.11314, "omega":0.2013, "ax":-1.78871, "ay":-5.13947, "alpha":9.13253, "fx":[-59.88458,-36.87673,1.0539,2.40252], "fy":[-47.76724,-67.21588,-76.64534,-76.46222]}, + {"t":0.04397, "x":3.73136, "y":7.42411, "heading":0.00443, "vx":-0.07815, "vy":-0.22613, "omega":0.40209, "ax":-1.81329, "ay":-5.13447, "alpha":9.08028, "fx":[-60.04832,-36.97684,0.87515,1.56326], "fy":[-47.55444,-67.15734,-76.64368,-76.47434]}, + {"t":0.06596, "x":3.7292, "y":7.4179, "heading":0.01327, "vx":-0.11802, "vy":-0.33902, "omega":0.60173, "ax":-1.84019, "ay":-5.13077, "alpha":8.99856, "fx":[-60.18911,-36.98615,0.75976,0.42522], "fy":[-47.36961,-67.14851,-76.64055,-76.47815]}, + {"t":0.08794, "x":3.72616, "y":7.40921, "heading":0.0265, "vx":-0.15848, "vy":-0.45182, "omega":0.79957, "ax":-1.86998, "ay":-5.12818, "alpha":8.88678, "fx":[-60.30388,-36.9066,0.70153,-1.03487], "fy":[-47.21718,-67.18835,-76.63627,-76.45996]}, + {"t":0.10993, "x":3.72222, "y":7.39804, "heading":0.04407, "vx":-0.19959, "vy":-0.56457, "omega":0.99495, "ax":-1.90329, "ay":-5.12639, "alpha":8.74498, "fx":[-60.38892,-36.74067,0.69245,-2.84459], "fy":[-47.10236,-67.27508,-76.63096,-76.40021]}, + {"t":0.13191, "x":3.71738, "y":7.38438, "heading":0.06595, "vx":-0.24144, "vy":-0.67727, "omega":1.18721, "ax":-1.94089, "ay":-5.12495, "alpha":8.57444, "fx":[-60.44027,-36.49151,0.72251,-5.03353], "fy":[-47.03064,-67.40612,-76.62466,-76.272]}, + {"t":0.1539, "x":3.7116, "y":7.36826, "heading":0.09205, "vx":-0.28411, "vy":-0.78995, "omega":1.37573, "ax":-1.98353, "ay":-5.1232, "alpha":8.37837, "fx":[-60.45402,-36.16313,0.7795,-7.62966], "fy":[-47.00728,-67.57811,-76.61737,-76.03949]}, + {"t":0.17589, "x":3.70487, "y":7.34965, "heading":0.1223, "vx":-0.32772, "vy":-0.90259, "omega":1.55993, "ax":-2.03194, "ay":-5.12028, "alpha":8.16267, "fx":[-60.42668,-35.76052,0.84892,-10.65401], "fy":[-47.0368,-67.7869,-76.6091,-75.65704]}, + {"t":0.19787, "x":3.69718, "y":7.32857, "heading":0.15659, "vx":-0.37239, "vy":-1.01516, "omega":1.73939, "ax":-2.08663, "ay":-5.1151, "alpha":7.93659, "fx":[-60.35544,-35.28976,0.9137,-14.11369], "fy":[-47.12249,-68.02763,-76.59987,-75.06958]}, + {"t":0.21986, "x":3.68848, "y":7.30501, "heading":0.19484, "vx":-0.41827, "vy":-1.12762, "omega":1.91389, "ax":-2.14781, "ay":-5.10641, "alpha":7.71297, "fx":[-60.23847,-34.75818,0.95398,-17.99377], "fy":[-47.26609,-68.29473,-76.58983,-74.21545]}, + {"t":0.24184, "x":3.67877, "y":7.27899, "heading":0.23691, "vx":-0.46549, "vy":-1.23989, "omega":2.08346, "ax":-2.21521, "ay":-5.0929, "alpha":7.50778, "fx":[-60.075,-34.17441,0.9467,-22.2493], "fy":[-47.46758,-68.58212,-76.57911,-73.03263]}, + {"t":0.26383, "x":3.668, "y":7.2505, "heading":0.28272, "vx":-0.51419, "vy":-1.35186, "omega":2.24853, "ax":-2.28799, "ay":-5.07341, "alpha":7.33856, "fx":[-59.86536,-33.54845,0.86508,-26.80018], "fy":[-47.72518,-68.88324,-76.5678,-71.46858]}, + {"t":0.28581, "x":3.65614, "y":7.21955, "heading":0.33216, "vx":-0.5645, "vy":-1.4634, "omega":2.40987, "ax":-2.36482, "ay":-5.04715, "alpha":7.2217, "fx":[-59.61095,-32.89173,0.67769,-31.53155], "fy":[-48.03549,-69.19122,-76.5557,-69.49253]}, + {"t":0.3078, "x":3.64316, "y":7.18616, "heading":0.38514, "vx":-0.61649, "vy":-1.57437, "omega":2.56864, "ax":-2.44399, "ay":-5.01392, "alpha":7.16889, "fx":[-59.31406,-32.2173,0.34712,-36.30212], "fy":[-48.39381,-69.4989,-76.5419,-67.10726]}, + {"t":0.32979, "x":3.62902, "y":7.15033, "heading":0.44161, "vx":-0.67022, "vy":-1.6846, "omega":2.72626, "ax":-2.52381, "ay":-4.97427, "alpha":7.18354, "fx":[-58.97777,-31.54019,-0.172,-40.95991], "fy":[-48.79441,-69.79882,-76.52409,-64.35605]}, + {"t":0.35177, "x":3.61367, "y":7.11209, "heading":0.50155, "vx":-0.72571, "vy":-1.79396, "omega":2.88419, "ax":-2.60302, "ay":-4.92939, "alpha":7.25818, "fx":[-58.60589,-30.87808,-0.93578,-45.36195], "fy":[-49.23069,-70.08301,-76.49729,-61.32156]}, + {"t":0.37376, "x":3.59709, "y":7.07146, "heading":0.56496, "vx":-0.78294, "vy":-1.90234, "omega":3.04377, "ax":-2.68125, "ay":-4.88095, "alpha":7.37368, "fx":[-58.20312,-30.25242,-2.01464,-49.39243], "fy":[-49.69516,-70.34259,-76.45177,-58.11605]}, + {"t":0.39574, "x":3.57922, "y":7.02845, "heading":0.63188, "vx":-0.84189, "vy":-2.00965, "omega":3.20588, "ax":-2.75937, "ay":-4.83064, "alpha":7.50041, "fx":[-57.77556,-29.69025,-3.49757,-52.9741], "fy":[-50.17897,-70.56711,-76.36945,-54.86562]}, + {"t":0.41773, "x":3.56005, "y":6.9831, "heading":0.70236, "vx":-0.90255, "vy":-2.11586, "omega":3.37079, "ax":-2.83966, "ay":-4.77973, "alpha":7.60092, "fx":[-57.3316,-29.22673,-5.49639,-56.07101], "fy":[-50.67107,-70.74349,-76.21787,-51.69329]}, + {"t":0.43971, "x":3.53952, "y":6.93543, "heading":0.77647, "vx":-0.96499, "vy":-2.22094, "omega":3.5379, "ax":-2.9259, "ay":-4.72856, "alpha":7.63363, "fx":[-56.88339,-28.90866,-8.14837,-58.6838], "fy":[-51.15683,-70.85438,-75.94046,-48.70485]}, + {"t":0.4617, "x":3.5176, "y":6.88546, "heading":0.85426, "vx":-1.02931, "vy":-2.3249, "omega":3.70573, "ax":-3.02325, "ay":-4.67597, "alpha":7.55761, "fx":[-56.44893,-28.79926,-11.61326,-60.84086], "fy":[-51.61601,-70.87569,-75.44179,-45.97967]}, + {"t":0.48369, "x":3.49423, "y":6.83321, "heading":0.93573, "vx":-1.09578, "vy":-2.42771, "omega":3.87189, "ax":-3.13796, "ay":-4.61872, "alpha":7.33885, "fx":[-56.05501,-28.98441,-16.05738,-62.58885], "fy":[-52.02005,-70.77244,-74.56821,-43.56622]}, + {"t":0.50567, "x":3.46938, "y":6.77872, "heading":1.02086, "vx":-1.16477, "vy":-2.52925, "omega":4.03324, "ax":-3.27663, "ay":-4.55092, "alpha":6.9593, "fx":[-55.74103,-29.58064,-21.61266,-63.98469], "fy":[-52.32814,-70.49176,-73.08954,-41.481]}, + {"t":0.52766, "x":3.44298, "y":6.72201, "heading":1.10953, "vx":-1.23681, "vy":-2.62931, "omega":4.18624, "ax":-3.44494, "ay":-4.46373, "alpha":6.42784, "fx":[-55.56418,-30.74497,-28.2991,-65.09034], "fy":[-52.48154,-69.95064,-70.70068,-39.70933]}, + {"t":0.54964, "x":3.41496, "y":6.66313, "heading":1.20157, "vx":-1.31255, "vy":-2.72745, "omega":4.32756, "ax":-3.64587, "ay":-4.34586, "alpha":5.78746, "fx":[-55.6061,-32.6854,-35.91798,-65.97024], "fy":[-52.39458,-69.01416,-67.0783,-38.20645]}, + {"t":0.57163, "x":3.38522, "y":6.60211, "heading":1.29671, "vx":-1.39271, "vy":-2.82299, "omega":4.45481, "ax":-3.87853, "ay":-4.18526, "alpha":5.10452, "fx":[-55.98176,-35.66702,-43.97627,-66.69114], "fy":[-51.9392,-67.45726,-62.02198,-36.89792]}, + {"t":0.59361, "x":3.35366, "y":6.53904, "heading":1.39465, "vx":-1.47798, "vy":-2.91501, "omega":4.56703, "ax":-4.13938, "ay":-3.97083, "alpha":4.42763, "fx":[-56.85084,-39.9969,-51.75139,-67.32411], "fy":[-50.91667,-64.90214,-55.63512,-35.67703]}, + {"t":0.6156, "x":3.32017, "y":6.47399, "heading":1.49506, "vx":-1.56899, "vy":-3.00231, "omega":4.66438, "ax":-4.42572, "ay":-3.69086, "alpha":3.73239, "fx":[-58.42956,-45.94232,-58.53883,-67.94869], "fy":[-49.00342,-60.7355,-48.39215,-34.39575]}, + {"t":0.63759, "x":3.2846, "y":6.40709, "heading":1.59761, "vx":-1.66629, "vy":-3.08346, "omega":4.74644, "ax":-4.7365, "ay":-3.32686, "alpha":2.90787, "fx":[-60.98947,-53.493,-63.92836,-68.65955], "fy":[-45.64728,-54.07537,-40.97494,-32.84203]}, + {"t":0.65957, "x":3.24682, "y":6.33849, "heading":1.70197, "vx":-1.77043, "vy":-3.1566, "omega":4.81037, "ax":-5.0643, "ay":-2.84856, "alpha":1.82995, "fx":[-64.78637,-61.91878,-67.89048,-69.57408], "fy":[-39.88231,-44.02983,-33.98705,-30.69055]}, + {"t":0.68156, "x":3.20668, "y":6.2684, "heading":1.80773, "vx":-1.88177, "vy":-3.21923, "omega":4.8506, "ax":-5.38153, "ay":-2.22039, "alpha":0.46321, "fx":[-69.73365,-69.49698,-70.65455,-70.83235], "fy":[-30.09806,-30.56041,-27.76485,-27.39939]}, + {"t":0.70354, "x":3.164, "y":6.19709, "heading":1.91437, "vx":-2.00008, "vy":-3.26805, "omega":4.86078, "ax":-5.63387, "ay":-1.41879, "alpha":-1.17646, "fx":[-74.463,-74.33406,-72.53313,-72.55022], "fy":[-14.27913,-15.38072,-22.3588,-21.98985]}, + {"t":0.72553, "x":3.11867, "y":6.1249, "heading":2.02124, "vx":-2.12395, "vy":-3.29924, "omega":4.83492, "ax":-5.74448, "ay":-0.45594, "alpha":-3.05565, "fx":[-75.35698,-75.93187,-73.80743,-74.55394], "fy":[7.61281,-1.19557,-17.60431,-12.59601]}, + {"t":0.74751, "x":3.07058, "y":6.05225, "heading":2.12754, "vx":-2.25025, "vy":-3.30926, "omega":4.76774, "ax":-5.65058, "ay":0.5991, "alpha":-4.82858, "fx":[-69.53438,-75.30492,-74.68328,-75.2294], "fy":[30.1124,10.23284,-13.19044,4.09613]}, + {"t":0.7695, "x":3.01974, "y":5.97964, "heading":2.23236, "vx":-2.37448, "vy":-3.29609, "omega":4.66158, "ax":-5.31782, "ay":1.67443, "alpha":-6.52038, "fx":[-59.83591,-73.73013,-75.28078,-68.54722], "fy":[46.68598,18.66786,-8.67556,30.66509]}, + {"t":0.79149, "x":2.96625, "y":5.90757, "heading":2.33485, "vx":-2.49139, "vy":-3.25928, "omega":4.51822, "ax":-4.72337, "ay":2.59976, "alpha":-9.03587, "fx":[-50.79915,-72.0004,-75.61072,-47.97559], "fy":[56.56638,24.67282,-3.45549,57.82776]}, + {"t":0.81347, "x":2.91034, "y":5.83655, "heading":2.43418, "vx":-2.59524, "vy":-3.20212, "omega":4.31956, "ax":-4.10513, "ay":3.17852, "alpha":-11.38699, "fx":[-44.09139,-70.44403,-75.48886,-24.11204], "fy":[62.07749,28.93561,3.29273,71.49596]}, + {"t":0.83546, "x":2.85229, "y":5.76691, "heading":2.52915, "vx":-2.6855, "vy":-3.13224, "omega":4.06921, "ax":-3.65623, "ay":3.55209, "alpha":-12.24342, "fx":[-39.52398,-69.136,-74.29916,-7.76116], "fy":[65.18516,32.0211,12.72122,75.36086]}, + {"t":0.85744, "x":2.79236, "y":5.69891, "heading":2.61862, "vx":-2.76588, "vy":-3.05414, "omega":3.80003, "ax":-3.31824, "ay":3.90222, "alpha":-11.73279, "fx":[-36.51023,-68.04842,-70.40779,1.87651], "fy":[66.99799,34.33563,26.26215,75.95646]}, + {"t":0.87943, "x":2.73075, "y":5.6327, "heading":2.70216, "vx":-2.83883, "vy":-2.96835, "omega":3.54208, "ax":-2.96095, "ay":4.30271, "alpha":-10.47151, "fx":[-34.55668,-67.12507,-60.29702,7.52642], "fy":[68.08609,36.1555,44.44258,75.7584]}, + {"t":0.90141, "x":2.66762, "y":5.56848, "heading":2.78004, "vx":-2.90393, "vy":-2.87375, "omega":3.31185, "ax":-2.47409, "ay":4.69509, "alpha":-9.50758, "fx":[-33.32291,-66.303,-40.30864,10.87811], "fy":[68.74516,37.67784,63.02927,75.45829]}, + {"t":0.9234, "x":2.60318, "y":5.50643, "heading":2.85285, "vx":-2.95833, "vy":-2.77053, "omega":3.10282, "ax":-1.91815, "ay":4.92575, "alpha":-9.77916, "fx":[-32.58118,-65.52035,-14.79659,12.84132], "fy":[69.13706,39.04949,73.52774,75.22833]}, + {"t":0.94539, "x":2.53767, "y":5.44671, "heading":2.92107, "vx":-3.0005, "vy":-2.66223, "omega":2.88782, "ax":-1.46972, "ay":4.98221, "alpha":-10.86244, "fx":[-32.17123,-64.72292,6.29544,13.93367], "fy":[69.35897,40.37653,75.0588,75.09352]}, + {"t":0.96737, "x":2.47135, "y":5.38939, "heading":2.98456, "vx":-3.03281, "vy":-2.55269, "omega":2.649, "ax":-1.17442, "ay":4.96796, "alpha":-11.93927, "fx":[-31.96776,-63.86416,20.08916,14.48144], "fy":[69.47789,41.73519,72.89489,75.03618]}, + {"t":0.98936, "x":2.40439, "y":5.33446, "heading":3.0428, "vx":-3.05863, "vy":-2.44347, "omega":2.38651, "ax":-0.98705, "ay":4.94565, "alpha":-12.71725, "fx":[-31.87193,-62.90834,28.58829,14.70422], "fy":[69.54281,43.17252,70.23573,75.02957]}, + {"t":1.01134, "x":2.3369, "y":5.28194, "heading":3.09527, "vx":-3.08033, "vy":-2.33473, "omega":2.10691, "ax":-0.86251, "ay":4.93355, "alpha":-13.19889, "fx":[-31.81176,-61.83498,33.90443,14.75118], "fy":[69.58821,44.70451,68.00679,75.05001]}, + {"t":1.03333, "x":2.26897, "y":5.2318, "heading":-3.14159, "vx":-3.0993, "vy":-2.22627, "omega":1.81672, "ax":-0.61842, "ay":5.09915, "alpha":-12.31969, "fx":[-28.74796,-55.24981,36.14089,15.59811], "fy":[70.98947,52.79825,67.22619,74.97387]}, + {"t":1.06564, "x":2.1685, "y":5.16252, "heading":-3.08289, "vx":-3.11928, "vy":-2.06149, "omega":1.41862, "ax":-0.06094, "ay":5.19931, "alpha":-11.77091, "fx":[-23.35682,-44.19332,44.50749,19.86379], "fy":[72.92336,62.25712,62.07639,73.95544]}, + {"t":1.09796, "x":2.06767, "y":5.09862, "heading":-3.03704, "vx":-3.12125, "vy":-1.89348, "omega":1.03825, "ax":0.62669, "ay":5.3, "alpha":-10.26462, "fx":[-16.11932,-26.23587,50.31112,24.73427], "fy":[74.83861,71.6261,57.53088,72.469]}, + {"t":1.13027, "x":1.96713, "y":5.0402, "heading":-3.00349, "vx":-3.101, "vy":-1.72221, "omega":0.70655, "ax":1.44637, "ay":5.29738, "alpha":-8.38117, "fx":[-6.9098,-2.38814,54.63976,30.10549], "fy":[76.22553,76.21313,53.48435,70.40471]}, + {"t":1.16259, "x":1.86768, "y":4.98731, "heading":-2.98066, "vx":-3.05426, "vy":-1.55103, "omega":0.43572, "ax":2.27965, "ay":5.12362, "alpha":-6.8287, "fx":[4.07944,21.03022,58.02515,35.77894], "fy":[76.41563,73.32057,49.83173,67.69634]}, + {"t":1.1949, "x":1.77017, "y":4.93987, "heading":-2.96658, "vx":-2.98059, "vy":-1.38546, "omega":0.21506, "ax":3.01009, "ay":4.8217, "alpha":-5.72648, "fx":[16.19712,38.55982,60.7468,41.51173], "fy":[74.78254,65.8808,46.51117,64.34022]}, + {"t":1.22722, "x":1.67543, "y":4.89762, "heading":-2.95963, "vx":-2.88332, "vy":-1.22965, "omega":0.03001, "ax":3.6114, "ay":4.46181, "alpha":-4.80738, "fx":[28.39347,49.95208,62.9703,47.06585], "fy":[71.05284,57.80496,43.48623,60.39777]}, + {"t":1.25953, "x":1.58414, "y":4.86021, "heading":-2.95866, "vx":-2.76662, "vy":-1.08547, "omega":-0.12534, "ax":4.09766, "ay":4.08362, "alpha":-3.91572, "fx":[39.56328,57.13983,64.80422,52.2392], "fy":[65.50184,50.79193,40.73334,55.98736]}, + {"t":1.29184, "x":1.49688, "y":4.82727, "heading":-2.96271, "vx":-2.63421, "vy":-0.95351, "omega":-0.25187, "ax":4.4852, "ay":3.70801, "alpha":-3.04709, "fx":[48.9635,61.78911,66.32461,56.88463], "fy":[58.8237,45.094,38.23655,51.26704]}, + {"t":1.32416, "x":1.4141, "y":4.79839, "heading":-2.97085, "vx":-2.48927, "vy":-0.83369, "omega":-0.35034, "ax":4.78907, "ay":3.34885, "alpha":-2.23918, "fx":[56.37716,64.92693,67.58808,60.92043], "fy":[51.78471,40.51063,35.98342,46.40796]}, + {"t":1.35647, "x":1.33616, "y":4.7732, "heading":-2.98217, "vx":-2.33452, "vy":-0.72547, "omega":-0.4227, "ax":5.02436, "ay":3.01498, "alpha":-1.51612, "fx":[61.979,67.13992,68.63931,64.32784], "fy":[44.95756,36.78411,33.96146,41.56776]}, + {"t":1.38879, "x":1.26334, "y":4.75133, "heading":-2.99583, "vx":-2.17216, "vy":-0.62805, "omega":-0.47169, "ax":5.20534, "ay":2.7105, "alpha":-0.88206, "fx":[66.11074,68.76403,69.51497,67.13708], "fy":[38.65776,33.70145,32.15625,36.87266]}, + {"t":1.4211, "x":1.19587, "y":4.73245, "heading":-3.01107, "vx":-2.00395, "vy":-0.54046, "omega":-0.50019, "ax":5.34431, "ay":2.43601, "alpha":-0.32982, "fx":[69.12406,69.99728,70.24575,69.40879], "fy":[33.00321,31.10471,30.55115,32.4109]}, + {"t":1.45342, "x":1.1339, "y":4.71626, "heading":-3.02724, "vx":-1.83125, "vy":-0.46174, "omega":-0.51085, "ax":5.45115, "ay":2.19005, "alpha":0.15158, "fx":[71.31345,70.96068,70.85737,71.21728], "fy":[27.99666,28.88054,29.12785,28.23495]}, + {"t":1.48573, "x":1.07757, "y":4.70248, "heading":-3.04375, "vx":-1.6551, "vy":-0.39097, "omega":-0.50595, "ax":5.53349, "ay":1.97017, "alpha":0.57311, "fx":[72.90351,71.73093,71.37133,72.63849], "fy":[23.58699,26.948,27.86724,24.36814]}, + {"t":1.51805, "x":1.02698, "y":4.69087, "heading":-3.0601, "vx":-1.47629, "vy":-0.3273, "omega":-0.48743, "ax":5.59715, "ay":1.7736, "alpha":0.94434, "fx":[74.05826,72.35838,71.8055,73.74281], "fy":[19.70481,25.24909,26.75017,20.81256]}, + {"t":1.55036, "x":0.9822, "y":4.68122, "heading":-3.07585, "vx":-1.29542, "vy":-0.26999, "omega":-0.45692, "ax":5.64649, "ay":1.59761, "alpha":1.27324, "fx":[74.89535,72.87709,72.17466,74.59161], "fy":[16.28009,23.74189,25.75817,17.5561]}, + {"t":1.58267, "x":0.94328, "y":4.67333, "heading":-3.09061, "vx":-1.11296, "vy":-0.21837, "omega":-0.41577, "ax":5.6848, "ay":1.43967, "alpha":1.56631, "fx":[75.49895,73.31081,72.49095,75.23638], "fy":[13.24954,22.39595,24.87381,14.57813]}, + {"t":1.61499, "x":0.91029, "y":4.66703, "heading":-3.10405, "vx":-0.92926, "vy":-0.17184, "omega":-0.36516, "ax":5.71457, "ay":1.29751, "alpha":1.82891, "fx":[75.92954,73.67652,72.76439,75.71914], "fy":[10.55889,21.18894,24.08096,11.8535]}, + {"t":1.6473, "x":0.88324, "y":4.66215, "heading":-3.11585, "vx":-0.7446, "vy":-0.12992, "omega":-0.30606, "ax":5.73766, "ay":1.16917, "alpha":2.06543, "fx":[76.23106,73.98667,73.00318,76.07352], "fy":[8.16286,20.1044,23.36492,9.35533]}, + {"t":1.67962, "x":0.86218, "y":4.65856, "heading":-3.12574, "vx":-0.55919, "vy":-0.09213, "omega":-0.23931, "ax":5.75553, "ay":1.05292, "alpha":2.27951, "fx":[76.43581,74.2506,73.2141,76.32605], "fy":[6.02419,19.13017,22.71234,7.05682]}, + {"t":1.71193, "x":0.84711, "y":4.65614, "heading":-3.13347, "vx":-0.3732, "vy":-0.05811, "omega":-0.16565, "ax":5.76928, "ay":0.94728, "alpha":2.47421, "fx":[76.56787,74.47547,73.40274,76.49741], "fy":[4.11244,18.25722,22.11115,4.93227]}, + {"t":1.74425, "x":0.83806, "y":4.65475, "heading":-3.13882, "vx":-0.18677, "vy":-0.0275, "omega":-0.0857, "ax":5.77975, "ay":0.85098, "alpha":2.65209, "fx":[76.64547,74.66683,73.57379,76.60359], "fy":[2.40284,17.4788,21.55037,2.95782]}, + {"t":1.77656, "x":0.83505, "y":4.65431, "heading":-3.14159, "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/MidToTower.traj b/src/main/deploy/choreo/MidToTower.traj new file mode 100644 index 00000000..98c31997 --- /dev/null +++ b/src/main/deploy/choreo/MidToTower.traj @@ -0,0 +1,91 @@ +{ + "name":"MidToTower", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.609598159790039, "y":4.007639408111572, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.1108198165893555, "y":2.857714891433716, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.636425733566284 m", "val":3.636425733566284}, "y":{"exp":"4.022153377532959 m", "val":4.022153377532959}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.1108198165893555 m", "val":2.1108198165893555}, "y":{"exp":"2.857714891433716 m", "val":2.857714891433716}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "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.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.70219,1.129], + "samples":[ + {"t":0.0, "x":3.6096, "y":4.00764, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.7722, "ay":-6.92692, "alpha":0.0, "fx":[-98.18353,-98.18353,-98.18353,-98.18353], "fy":[-117.82496,-117.82496,-117.82496,-117.82496]}, + {"t":0.03696, "x":3.60566, "y":4.00291, "heading":0.0, "vx":-0.21333, "vy":-0.256, "omega":0.0, "ax":-5.83207, "ay":-6.8755, "alpha":0.0, "fx":[-99.20183,-99.20183,-99.20183,-99.20183], "fy":[-116.95025,-116.95025,-116.95025,-116.95025]}, + {"t":0.07392, "x":3.59379, "y":3.98875, "heading":0.0, "vx":-0.42887, "vy":-0.5101, "omega":0.0, "ax":-5.90213, "ay":-6.81415, "alpha":0.0, "fx":[-100.39357,-100.39357,-100.39357,-100.39357], "fy":[-115.90667,-115.90667,-115.90667,-115.90667]}, + {"t":0.11087, "x":3.57391, "y":3.96525, "heading":0.0, "vx":-0.64699, "vy":-0.76194, "omega":0.0, "ax":-5.98519, "ay":-6.73973, "alpha":0.0, "fx":[-101.80644,-101.80644,-101.80644,-101.80644], "fy":[-114.64086,-114.64086,-114.64086,-114.64086]}, + {"t":0.14783, "x":3.54591, "y":3.93248, "heading":0.0, "vx":-0.86819, "vy":-1.01102, "omega":0.0, "ax":-6.08517, "ay":-6.64766, "alpha":0.0, "fx":[-103.50701,-103.50701,-103.50701,-103.50701], "fy":[-113.07477,-113.07477,-113.07477,-113.07477]}, + {"t":0.18479, "x":3.50967, "y":3.89058, "heading":0.0, "vx":-1.09309, "vy":-1.2567, "omega":0.0, "ax":-6.20767, "ay":-6.53096, "alpha":0.0, "fx":[-105.59073,-105.59073,-105.59073,-105.59073], "fy":[-111.08972,-111.08972,-111.08972,-111.08972]}, + {"t":0.22175, "x":3.46503, "y":3.83967, "heading":0.0, "vx":-1.32251, "vy":-1.49807, "omega":0.0, "ax":-6.361, "ay":-6.37852, "alpha":0.0, "fx":[-108.19872,-108.19872,-108.19872,-108.19872], "fy":[-108.49683,-108.49683,-108.49683,-108.49683]}, + {"t":0.2587, "x":3.41181, "y":3.77995, "heading":0.0, "vx":-1.55759, "vy":-1.73381, "omega":0.0, "ax":-6.55783, "ay":-6.17164, "alpha":0.0, "fx":[-111.54682,-111.54682,-111.54682,-111.54682], "fy":[-104.97779,-104.97779,-104.97779,-104.97779]}, + {"t":0.29566, "x":3.34977, "y":3.71166, "heading":0.0, "vx":-1.79996, "vy":-1.9619, "omega":0.0, "ax":-6.81824, "ay":-5.87646, "alpha":0.0, "fx":[-115.97624,-115.97624,-115.97624,-115.97624], "fy":[-99.9569,-99.9569,-99.9569,-99.9569]}, + {"t":0.33262, "x":3.27859, "y":3.63514, "heading":0.0, "vx":-2.05194, "vy":-2.17908, "omega":0.0, "ax":-7.17457, "ay":-5.42601, "alpha":0.0, "fx":[-122.03743,-122.03743,-122.03743,-122.03743], "fy":[-92.29494,-92.29494,-92.29494,-92.29494]}, + {"t":0.36958, "x":3.19785, "y":3.5509, "heading":0.0, "vx":-2.3171, "vy":-2.37961, "omega":0.0, "ax":-7.67647, "ay":-4.67197, "alpha":0.0, "fx":[-130.57459,-130.57459,-130.57459,-130.57459], "fy":[-79.46887,-79.46887,-79.46887,-79.46887]}, + {"t":0.40653, "x":3.10698, "y":3.45977, "heading":0.0, "vx":-2.6008, "vy":-2.55227, "omega":0.0, "ax":-8.36556, "ay":-3.24198, "alpha":0.0, "fx":[-142.29585,-142.29585,-142.29585,-142.29585], "fy":[-55.14515,-55.14515,-55.14515,-55.14515]}, + {"t":0.44349, "x":3.00514, "y":3.36323, "heading":0.0, "vx":-2.90997, "vy":-2.67209, "omega":0.0, "ax":-8.94655, "ay":-0.18463, "alpha":0.0, "fx":[-152.17829,-152.17829,-152.17829,-152.17829], "fy":[-3.14051,-3.14051,-3.14051,-3.14051]}, + {"t":0.48045, "x":2.89149, "y":3.26435, "heading":0.0, "vx":-3.24061, "vy":-2.67891, "omega":0.0, "ax":-7.2967, "ay":5.15067, "alpha":0.0, "fx":[-124.1147,-124.1147,-124.1147,-124.1147], "fy":[87.6114,87.6114,87.6114,87.6114]}, + {"t":0.51741, "x":2.76674, "y":3.16886, "heading":0.0, "vx":-3.51028, "vy":-2.48856, "omega":0.0, "ax":-2.98654, "ay":8.43748, "alpha":0.0, "fx":[-50.80027,-50.80027,-50.80027,-50.80027], "fy":[143.51918,143.51918,143.51918,143.51918]}, + {"t":0.55436, "x":2.63497, "y":3.08265, "heading":0.0, "vx":-3.62066, "vy":-2.17673, "omega":0.0, "ax":-0.08484, "ay":8.97274, "alpha":0.0, "fx":[-1.44316,-1.44316,-1.44316,-1.44316], "fy":[152.62372,152.62372,152.62372,152.62372]}, + {"t":0.59132, "x":2.5011, "y":3.00833, "heading":0.0, "vx":-3.62379, "vy":-1.84512, "omega":0.0, "ax":1.42977, "ay":8.87273, "alpha":0.0, "fx":[24.31997,24.31997,24.31997,24.31997], "fy":[150.92266,150.92266,150.92266,150.92266]}, + {"t":0.62828, "x":2.36815, "y":2.9462, "heading":0.0, "vx":-3.57095, "vy":-1.5172, "omega":0.0, "ax":2.2881, "ay":8.69997, "alpha":0.0, "fx":[38.91995,38.91995,38.91995,38.91995], "fy":[147.98401,147.98401,147.98401,147.98401]}, + {"t":0.66524, "x":2.23774, "y":2.89607, "heading":0.0, "vx":-3.48639, "vy":-1.19567, "omega":0.0, "ax":2.82631, "ay":8.54628, "alpha":0.0, "fx":[48.07472,48.07472,48.07472,48.07472], "fy":[145.36983,145.36983,145.36983,145.36983]}, + {"t":0.70219, "x":2.11082, "y":2.85771, "heading":0.0, "vx":-3.38194, "vy":-0.87982, "omega":0.0, "ax":3.86823, "ay":8.12858, "alpha":0.0, "fx":[65.79755,65.79755,65.79755,65.79755], "fy":[138.2648,138.2648,138.2648,138.2648]}, + {"t":0.73776, "x":1.99298, "y":2.83156, "heading":0.0, "vx":-3.24435, "vy":-0.59071, "omega":0.0, "ax":5.51487, "ay":7.11424, "alpha":0.0, "fx":[93.80639,93.80639,93.80639,93.80639], "fy":[121.01114,121.01114,121.01114,121.01114]}, + {"t":0.77333, "x":1.88108, "y":2.81505, "heading":0.0, "vx":-3.04821, "vy":-0.33768, "omega":0.0, "ax":6.9095, "ay":5.77021, "alpha":0.0, "fx":[117.52858,117.52858,117.52858,117.52858], "fy":[98.14955,98.14955,98.14955,98.14955]}, + {"t":0.8089, "x":1.77703, "y":2.80669, "heading":0.0, "vx":-2.80246, "vy":-0.13245, "omega":0.0, "ax":7.90863, "ay":4.30321, "alpha":0.0, "fx":[134.52346,134.52346,134.52346,134.52346], "fy":[73.19638,73.19638,73.19638,73.19638]}, + {"t":0.84446, "x":1.68236, "y":2.8047, "heading":0.0, "vx":-2.52117, "vy":0.0206, "omega":0.0, "ax":8.52107, "ay":2.91409, "alpha":0.0, "fx":[144.94092,144.94092,144.94092,144.94092], "fy":[49.5678,49.5678,49.5678,49.5678]}, + {"t":0.88003, "x":1.59808, "y":2.80728, "heading":0.0, "vx":-2.2181, "vy":0.12425, "omega":0.0, "ax":8.84379, "ay":1.71057, "alpha":0.0, "fx":[150.4303,150.4303,150.4303,150.4303], "fy":[29.09637,29.09637,29.09637,29.09637]}, + {"t":0.9156, "x":1.52478, "y":2.81278, "heading":0.0, "vx":-1.90355, "vy":0.18509, "omega":0.0, "ax":8.98125, "ay":0.71515, "alpha":0.0, "fx":[152.76843,152.76843,152.76843,152.76843], "fy":[12.16457,12.16457,12.16457,12.16457]}, + {"t":0.95116, "x":1.46276, "y":2.81982, "heading":0.0, "vx":-1.58411, "vy":0.21053, "omega":0.0, "ax":9.01094, "ay":-0.09297, "alpha":0.0, "fx":[153.27352,153.27352,153.27352,153.27352], "fy":[-1.58145,-1.58145,-1.58145,-1.58145]}, + {"t":0.98673, "x":1.41211, "y":2.82725, "heading":0.0, "vx":-1.26362, "vy":0.20722, "omega":0.0, "ax":8.98189, "ay":-0.74727, "alpha":0.0, "fx":[152.77944,152.77944,152.77944,152.77944], "fy":[-12.71093,-12.71093,-12.71093,-12.71093]}, + {"t":1.0223, "x":1.37285, "y":2.83414, "heading":0.0, "vx":-0.94416, "vy":0.18064, "omega":0.0, "ax":8.92288, "ay":-1.27996, "alpha":0.0, "fx":[151.77559,151.77559,151.77559,151.77559], "fy":[-21.77168,-21.77168,-21.77168,-21.77168]}, + {"t":1.05787, "x":1.34491, "y":2.83976, "heading":0.0, "vx":-0.6268, "vy":0.13512, "omega":0.0, "ax":8.85017, "ay":-1.71768, "alpha":0.0, "fx":[150.53878,150.53878,150.53878,150.53878], "fy":[-29.21721,-29.21721,-29.21721,-29.21721]}, + {"t":1.09343, "x":1.32822, "y":2.84348, "heading":0.0, "vx":-0.31202, "vy":0.07402, "omega":0.0, "ax":8.77276, "ay":-2.08125, "alpha":0.0, "fx":[149.22206,149.22206,149.22206,149.22206], "fy":[-35.40146,-35.40146,-35.40146,-35.40146]}, + {"t":1.129, "x":1.32267, "y":2.84479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj new file mode 100644 index 00000000..65062d4d --- /dev/null +++ b/src/main/deploy/choreo/NewPath.traj @@ -0,0 +1,29 @@ +{ + "name":"NewPath", + "version":3, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.9904961585998535 m", "val":3.9904961585998535}, "y":{"exp":"8.07487678527832 m", "val":8.07487678527832}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":null, + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/RightToTower.traj b/src/main/deploy/choreo/RightToTower.traj new file mode 100644 index 00000000..f56d2bc0 --- /dev/null +++ b/src/main/deploy/choreo/RightToTower.traj @@ -0,0 +1,95 @@ +{ + "name":"RightToTower", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.6271181106567383, "y":0.7985455989837646, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.5972683429718018, "y":2.493582248687744, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"3.6271181106567383 m", "val":3.6271181106567383}, "y":{"exp":"0.7985455989837646 m", "val":0.7985455989837646}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.5972683429718018 m", "val":2.5972683429718018}, "y":{"exp":"2.493582248687744 m", "val":2.493582248687744}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + "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.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.71185,1.27531], + "samples":[ + {"t":0.0, "x":3.62712, "y":0.79855, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.27304, "ay":8.72562, "alpha":0.0, "fx":[-38.66378,-38.66378,-38.66378,-38.66378], "fy":[148.42029,148.42029,148.42029,148.42029]}, + {"t":0.03747, "x":3.62552, "y":0.80467, "heading":0.0, "vx":-0.08516, "vy":0.32691, "omega":0.0, "ax":-2.40365, "ay":8.68971, "alpha":0.0, "fx":[-40.88537,-40.88537,-40.88537,-40.88537], "fy":[147.80956,147.80956,147.80956,147.80956]}, + {"t":0.07493, "x":3.62065, "y":0.82302, "heading":0.0, "vx":-0.17522, "vy":0.65248, "omega":0.0, "ax":-2.55637, "ay":8.64504, "alpha":0.0, "fx":[-43.48315,-43.48315,-43.48315,-43.48315], "fy":[147.04962,147.04962,147.04962,147.04962]}, + {"t":0.1124, "x":3.61229, "y":0.85353, "heading":0.0, "vx":-0.27099, "vy":0.97638, "omega":0.0, "ax":-2.73715, "ay":8.58834, "alpha":0.0, "fx":[-46.5581,-46.5581,-46.5581,-46.5581], "fy":[146.08527,146.08527,146.08527,146.08527]}, + {"t":0.14986, "x":3.60021, "y":0.89614, "heading":0.0, "vx":-0.37354, "vy":1.29815, "omega":0.0, "ax":-2.95414, "ay":8.51472, "alpha":0.0, "fx":[-50.24911,-50.24911,-50.24911,-50.24911], "fy":[144.83301,144.83301,144.83301,144.83301]}, + {"t":0.18733, "x":3.58414, "y":0.95075, "heading":0.0, "vx":-0.48422, "vy":1.61716, "omega":0.0, "ax":-3.21882, "ay":8.41648, "alpha":0.0, "fx":[-54.75128,-54.75128,-54.75128,-54.75128], "fy":[143.16189,143.16189,143.16189,143.16189]}, + {"t":0.2248, "x":3.56374, "y":1.01725, "heading":0.0, "vx":-0.60482, "vy":1.93249, "omega":0.0, "ax":-3.54765, "ay":8.28102, "alpha":0.0, "fx":[-60.34447,-60.34447,-60.34447,-60.34447], "fy":[140.85786,140.85786,140.85786,140.85786]}, + {"t":0.26226, "x":3.53859, "y":1.09546, "heading":0.0, "vx":-0.73774, "vy":2.24275, "omega":0.0, "ax":-3.96463, "ay":8.08679, "alpha":0.0, "fx":[-67.43714,-67.43714,-67.43714,-67.43714], "fy":[137.55394,137.55394,137.55394,137.55394]}, + {"t":0.29973, "x":3.50817, "y":1.18516, "heading":0.0, "vx":-0.88627, "vy":2.54573, "omega":0.0, "ax":-4.505, "ay":7.79477, "alpha":0.0, "fx":[-76.6288,-76.6288,-76.6288,-76.6288], "fy":[132.58677,132.58677,132.58677,132.58677]}, + {"t":0.33719, "x":3.4718, "y":1.28601, "heading":0.0, "vx":-1.05506, "vy":2.83777, "omega":0.0, "ax":-5.2188, "ay":7.33052, "alpha":0.0, "fx":[-88.77023,-88.77023,-88.77023,-88.77023], "fy":[124.6901,124.6901,124.6901,124.6901]}, + {"t":0.37466, "x":3.42861, "y":1.39748, "heading":0.0, "vx":-1.25059, "vy":3.11241, "omega":0.0, "ax":-6.16556, "ay":6.54596, "alpha":0.0, "fx":[-104.87435,-104.87435,-104.87435,-104.87435], "fy":[111.34488,111.34488,111.34488,111.34488]}, + {"t":0.41213, "x":3.37743, "y":1.51868, "heading":0.0, "vx":-1.48158, "vy":3.35766, "omega":0.0, "ax":-7.35998, "ay":5.15308, "alpha":0.0, "fx":[-125.19116,-125.19116,-125.19116,-125.19116], "fy":[87.65239,87.65239,87.65239,87.65239]}, + {"t":0.44959, "x":3.31676, "y":1.6481, "heading":0.0, "vx":-1.75733, "vy":3.55073, "omega":0.0, "ax":-8.55256, "ay":2.72575, "alpha":0.0, "fx":[-145.4766,-145.4766,-145.4766,-145.4766], "fy":[46.36422,46.36422,46.36422,46.36422]}, + {"t":0.48706, "x":3.24491, "y":1.78304, "heading":0.0, "vx":-2.07776, "vy":3.65285, "omega":0.0, "ax":-8.94127, "ay":-0.74959, "alpha":0.0, "fx":[-152.08847,-152.08847,-152.08847,-152.08847], "fy":[-12.75026,-12.75026,-12.75026,-12.75026]}, + {"t":0.52452, "x":3.16079, "y":1.91937, "heading":0.0, "vx":-2.41276, "vy":3.62476, "omega":0.0, "ax":-8.00128, "ay":-4.07008, "alpha":0.0, "fx":[-136.09955,-136.09955,-136.09955,-136.09955], "fy":[-69.23097,-69.23097,-69.23097,-69.23097]}, + {"t":0.56199, "x":3.06478, "y":2.05232, "heading":0.0, "vx":-2.71253, "vy":3.47228, "omega":0.0, "ax":-6.48977, "ay":-6.21431, "alpha":0.0, "fx":[-110.38918,-110.38918,-110.38918,-110.38918], "fy":[-105.70368,-105.70368,-105.70368,-105.70368]}, + {"t":0.59946, "x":2.9586, "y":2.17805, "heading":0.0, "vx":-2.95568, "vy":3.23945, "omega":0.0, "ax":-5.14654, "ay":-7.37465, "alpha":0.0, "fx":[-87.54117,-87.54117,-87.54117,-87.54117], "fy":[-125.44074,-125.44074,-125.44074,-125.44074]}, + {"t":0.63692, "x":2.84425, "y":2.29424, "heading":0.0, "vx":-3.1485, "vy":2.96315, "omega":0.0, "ax":-4.12694, "ay":-7.99668, "alpha":0.0, "fx":[-70.19803,-70.19803,-70.19803,-70.19803], "fy":[-136.02132,-136.02132,-136.02132,-136.02132]}, + {"t":0.67439, "x":2.72339, "y":2.39965, "heading":0.0, "vx":-3.30312, "vy":2.66355, "omega":0.0, "ax":-3.37292, "ay":-8.34753, "alpha":0.0, "fx":[-57.3724,-57.3724,-57.3724,-57.3724], "fy":[-141.98909,-141.98909,-141.98909,-141.98909]}, + {"t":0.71185, "x":2.59727, "y":2.49358, "heading":0.0, "vx":-3.42949, "vy":2.3508, "omega":0.0, "ax":-2.47926, "ay":-8.65406, "alpha":0.0, "fx":[-42.17154,-42.17154,-42.17154,-42.17154], "fy":[-147.20311,-147.20311,-147.20311,-147.20311]}, + {"t":0.74707, "x":2.47496, "y":2.571, "heading":0.0, "vx":-3.5168, "vy":2.04604, "omega":0.0, "ax":-1.03423, "ay":-8.93974, "alpha":0.0, "fx":[-17.59196,-17.59196,-17.59196,-17.59196], "fy":[-152.06235,-152.06235,-152.06235,-152.06235]}, + {"t":0.78229, "x":2.35047, "y":2.63751, "heading":0.0, "vx":-3.55322, "vy":1.73122, "omega":0.0, "ax":0.75877, "ay":-8.96486, "alpha":0.0, "fx":[12.9065,12.9065,12.9065,12.9065], "fy":[-152.48969,-152.48969,-152.48969,-152.48969]}, + {"t":0.8175, "x":2.22581, "y":2.69292, "heading":0.0, "vx":-3.5265, "vy":1.41551, "omega":0.0, "ax":2.76311, "ay":-8.5607, "alpha":0.0, "fx":[46.99977,46.99977,46.99977,46.99977], "fy":[-145.61505,-145.61505,-145.61505,-145.61505]}, + {"t":0.85272, "x":2.10333, "y":2.73746, "heading":0.0, "vx":-3.42919, "vy":1.11404, "omega":0.0, "ax":4.68681, "ay":-7.67853, "alpha":0.0, "fx":[79.72138,79.72138,79.72138,79.72138], "fy":[-130.6096,-130.6096,-130.6096,-130.6096]}, + {"t":0.88793, "x":1.98548, "y":2.77193, "heading":0.0, "vx":-3.26414, "vy":0.84363, "omega":0.0, "ax":6.2465, "ay":-6.47613, "alpha":0.0, "fx":[106.25121,106.25121,106.25121,106.25121], "fy":[-110.1571,-110.1571,-110.1571,-110.1571]}, + {"t":0.92315, "x":1.8744, "y":2.79762, "heading":0.0, "vx":-3.04416, "vy":0.61557, "omega":0.0, "ax":7.34361, "ay":-5.20369, "alpha":0.0, "fx":[124.91278,124.91278,124.91278,124.91278], "fy":[-88.51327,-88.51327,-88.51327,-88.51327]}, + {"t":0.95837, "x":1.77175, "y":2.81607, "heading":0.0, "vx":-2.78555, "vy":0.43231, "omega":0.0, "ax":8.04627, "ay":-4.03927, "alpha":0.0, "fx":[136.86476,136.86476,136.86476,136.86476], "fy":[-68.70679,-68.70679,-68.70679,-68.70679]}, + {"t":0.99358, "x":1.67864, "y":2.82879, "heading":0.0, "vx":-2.50219, "vy":0.29006, "omega":0.0, "ax":8.4735, "ay":-3.05046, "alpha":0.0, "fx":[144.13189,144.13189,144.13189,144.13189], "fy":[-51.88741,-51.88741,-51.88741,-51.88741]}, + {"t":1.0288, "x":1.59578, "y":2.83712, "heading":0.0, "vx":-2.20379, "vy":0.18264, "omega":0.0, "ax":8.7259, "ay":-2.23731, "alpha":0.0, "fx":[148.4251,148.4251,148.4251,148.4251], "fy":[-38.05598,-38.05598,-38.05598,-38.05598]}, + {"t":1.06401, "x":1.52358, "y":2.84216, "heading":0.0, "vx":-1.8965, "vy":0.10385, "omega":0.0, "ax":8.87146, "ay":-1.57454, "alpha":0.0, "fx":[150.90105,150.90105,150.90105,150.90105], "fy":[-26.7825,-26.7825,-26.7825,-26.7825]}, + {"t":1.09923, "x":1.4623, "y":2.84484, "heading":0.0, "vx":-1.58408, "vy":0.0484, "omega":0.0, "ax":8.95236, "ay":-1.03288, "alpha":0.0, "fx":[152.27715,152.27715,152.27715,152.27715], "fy":[-17.56891,-17.56891,-17.56891,-17.56891]}, + {"t":1.13445, "x":1.41206, "y":2.84591, "heading":0.0, "vx":-1.26881, "vy":0.01203, "omega":0.0, "ax":8.99403, "ay":-0.5866, "alpha":0.0, "fx":[152.98585,152.98585,152.98585,152.98585], "fy":[-9.9779,-9.9779,-9.9779,-9.9779]}, + {"t":1.16966, "x":1.37296, "y":2.84597, "heading":0.0, "vx":-0.95208, "vy":-0.00863, "omega":0.0, "ax":9.01175, "ay":-0.21516, "alpha":0.0, "fx":[153.28721,153.28721,153.28721,153.28721], "fy":[-3.65982,-3.65982,-3.65982,-3.65982]}, + {"t":1.20488, "x":1.34502, "y":2.84553, "heading":0.0, "vx":-0.63472, "vy":-0.01621, "omega":0.0, "ax":9.01479, "ay":0.09731, "alpha":0.0, "fx":[153.33905,153.33905,153.33905,153.33905], "fy":[1.65514,1.65514,1.65514,1.65514]}, + {"t":1.24009, "x":1.32826, "y":2.84502, "heading":0.0, "vx":-0.31726, "vy":-0.01278, "omega":0.0, "ax":9.00888, "ay":0.36291, "alpha":0.0, "fx":[153.23843,153.23843,153.23843,153.23843], "fy":[6.17291,6.17291,6.17291,6.17291]}, + {"t":1.27531, "x":1.32267, "y":2.84479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/TowerToClimb.traj b/src/main/deploy/choreo/TowerToClimb.traj new file mode 100644 index 00000000..c6b7695b --- /dev/null +++ b/src/main/deploy/choreo/TowerToClimb.traj @@ -0,0 +1,120 @@ +{ + "name":"TowerToClimb", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":1.3226691484451294, "y":2.844794273376465, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.7245495319366455, "y":3.234717607498169, "heading":0.7853981633974483, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.4321849346160889, "y":3.5758097171783447, "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":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"1.3226691484451294 m", "val":1.3226691484451294}, "y":{"exp":"2.844794273376465 m", "val":2.844794273376465}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.7245495319366455 m", "val":1.7245495319366455}, "y":{"exp":"3.234717607498169 m", "val":3.234717607498169}, "heading":{"exp":"45 deg", "val":0.7853981633974483}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.4321849346160889 m", "val":1.4321849346160889}, "y":{"exp":"3.5758097171783447 m", "val":3.5758097171783447}, "heading":{"exp":"90 deg", "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":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "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.4064, + "side":0.4064, + "back":0.4064 + }, + "differentialTrackWidth":0.5588 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.1565,2.09057], + "samples":[ + {"t":0.0, "x":1.32267, "y":2.84479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":6.56194, "ay":6.17797, "alpha":0.00535, "fx":[111.58893,111.61752,111.64452,111.61594], "fy":[105.11504,105.08459,105.056,105.08646]}, + {"t":0.03731, "x":1.32724, "y":2.84909, "heading":0.0, "vx":0.2448, "vy":0.23048, "omega":0.0002, "ax":3.19367, "ay":3.00679, "alpha":1.85827, "fx":[48.27034,50.54209,60.44809,58.03287], "fy":[57.42498,47.20668,44.96406,54.98285]}, + {"t":0.07461, "x":1.33859, "y":2.85978, "heading":0.00001, "vx":0.36395, "vy":0.34265, "omega":0.06953, "ax":0.00001, "ay":0.0, "alpha":2.66386, "fx":[-7.15062,-7.15052,7.1508,7.1507], "fy":[7.15069,-7.15063,-7.15052,7.1508]}, + {"t":0.11192, "x":1.35217, "y":2.87257, "heading":0.0026, "vx":0.36395, "vy":0.34265, "omega":0.1689, "ax":0.0, "ay":0.0, "alpha":2.36366, "fx":[-6.36131,-6.3283,6.36131,6.3283], "fy":[6.3283,-6.36131,-6.3283,6.36131]}, + {"t":0.14923, "x":1.36575, "y":2.88535, "heading":0.0089, "vx":0.36395, "vy":0.34265, "omega":0.25708, "ax":0.0, "ay":0.0, "alpha":2.09801, "fx":[-5.68164,-5.58137,5.68164,5.58137], "fy":[5.58137,-5.68164,-5.58137,5.68164]}, + {"t":0.18653, "x":1.37932, "y":2.89813, "heading":0.01849, "vx":0.36395, "vy":0.34265, "omega":0.33535, "ax":0.0, "ay":0.0, "alpha":1.86325, "fx":[-5.09319,-4.90821,5.09319,4.90821], "fy":[4.90821,-5.09319,-4.90821,5.09319]}, + {"t":0.22384, "x":1.3929, "y":2.91092, "heading":0.031, "vx":0.36395, "vy":0.34265, "omega":0.40486, "ax":0.0, "ay":0.0, "alpha":1.6561, "fx":[-4.58117,-4.30556,4.58117,4.30556], "fy":[4.30556,-4.58117,-4.30556,4.58117]}, + {"t":0.26114, "x":1.40648, "y":2.9237, "heading":0.04611, "vx":0.36395, "vy":0.34265, "omega":0.46665, "ax":0.0, "ay":0.0, "alpha":1.47361, "fx":[-4.13377,-3.76912,4.13377,3.76912], "fy":[3.76912,-4.13377,-3.76912,4.13377]}, + {"t":0.29845, "x":1.42006, "y":2.93648, "heading":0.06352, "vx":0.36395, "vy":0.34265, "omega":0.52162, "ax":0.0, "ay":0.0, "alpha":1.31317, "fx":[-3.7416,-3.29411,3.7416,3.29411], "fy":[3.29411,-3.7416,-3.29411,3.7416]}, + {"t":0.33576, "x":1.43363, "y":2.94927, "heading":0.08298, "vx":0.36395, "vy":0.34265, "omega":0.57061, "ax":0.0, "ay":0.0, "alpha":1.17243, "fx":[-3.39721,-2.87552,3.39721,2.87552], "fy":[2.87552,-3.39721,-2.87552,3.39721]}, + {"t":0.37306, "x":1.44721, "y":2.96205, "heading":0.10426, "vx":0.36395, "vy":0.34265, "omega":0.61435, "ax":0.0, "ay":0.0, "alpha":1.04935, "fx":[-3.09467,-2.50835,3.09467,2.50835], "fy":[2.50835,-3.09467,-2.50835,3.09467]}, + {"t":0.41037, "x":1.46079, "y":2.97483, "heading":0.12718, "vx":0.36395, "vy":0.34265, "omega":0.6535, "ax":0.0, "ay":0.0, "alpha":0.94212, "fx":[-2.82929,-2.18774,2.82929,2.18774], "fy":[2.18774,-2.82929,-2.18774,2.82929]}, + {"t":0.44768, "x":1.47437, "y":2.98761, "heading":0.15156, "vx":0.36395, "vy":0.34265, "omega":0.68865, "ax":0.0, "ay":0.0, "alpha":0.84913, "fx":[-2.59735,-1.90907,2.59735,1.90907], "fy":[1.90907,-2.59735,-1.90907,2.59735]}, + {"t":0.48498, "x":1.48794, "y":3.0004, "heading":0.17725, "vx":0.36395, "vy":0.34265, "omega":0.72032, "ax":0.0, "ay":0.0, "alpha":0.76901, "fx":[-2.39593,-1.66795,2.39593,1.66795], "fy":[1.66795,-2.39593,-1.66795,2.39593]}, + {"t":0.52229, "x":1.50152, "y":3.01318, "heading":0.20413, "vx":0.36395, "vy":0.34265, "omega":0.74901, "ax":0.0, "ay":0.0, "alpha":0.70057, "fx":[-2.22273,-1.4603,2.22273,1.4603], "fy":[1.4603,-2.22273,-1.4603,2.22273]}, + {"t":0.5596, "x":1.5151, "y":3.02596, "heading":0.23207, "vx":0.36395, "vy":0.34265, "omega":0.77515, "ax":0.0, "ay":0.0, "alpha":0.64278, "fx":[-2.07601,-1.28234,2.07601,1.28234], "fy":[1.28234,-2.07601,-1.28234,2.07601]}, + {"t":0.5969, "x":1.52868, "y":3.03875, "heading":0.26099, "vx":0.36395, "vy":0.34265, "omega":0.79913, "ax":0.0, "ay":0.0, "alpha":0.59477, "fx":[-1.95446,-1.13052,1.95446,1.13052], "fy":[1.13052,-1.95446,-1.13052,1.95446]}, + {"t":0.63421, "x":1.54225, "y":3.05153, "heading":0.2908, "vx":0.36395, "vy":0.34265, "omega":0.82132, "ax":0.0, "ay":0.0, "alpha":0.55582, "fx":[-1.85715,-1.00158,1.85715,1.00158], "fy":[1.00158,-1.85715,-1.00158,1.85715]}, + {"t":0.67152, "x":1.55583, "y":3.06431, "heading":0.32144, "vx":0.36395, "vy":0.34265, "omega":0.84205, "ax":0.0, "ay":0.0, "alpha":0.52535, "fx":[-1.78353,-0.89245,1.78353,0.89245], "fy":[0.89245,-1.78353,-0.89245,1.78353]}, + {"t":0.70882, "x":1.56941, "y":3.0771, "heading":0.35286, "vx":0.36395, "vy":0.34265, "omega":0.86165, "ax":0.0, "ay":0.0, "alpha":0.5029, "fx":[-1.7333,-0.80027,1.7333,0.80027], "fy":[0.80027,-1.7333,-0.80027,1.7333]}, + {"t":0.74613, "x":1.58299, "y":3.08988, "heading":0.385, "vx":0.36395, "vy":0.34265, "omega":0.88041, "ax":0.0, "ay":0.0, "alpha":0.48814, "fx":[-1.7065,-0.7223,1.7065,0.7223], "fy":[0.7223,-1.7065,-0.7223,1.7065]}, + {"t":0.78343, "x":1.59656, "y":3.10266, "heading":0.41785, "vx":0.36395, "vy":0.34265, "omega":0.89862, "ax":0.0, "ay":0.0, "alpha":0.48083, "fx":[-1.70342,-0.6559,1.70342,0.6559], "fy":[0.6559,-1.70342,-0.6559,1.70342]}, + {"t":0.82074, "x":1.61014, "y":3.11545, "heading":0.45137, "vx":0.36395, "vy":0.34265, "omega":0.91656, "ax":0.0, "ay":0.0, "alpha":0.48088, "fx":[-1.72464,-0.5985,1.72464,0.5985], "fy":[0.5985,-1.72464,-0.5985,1.72464]}, + {"t":0.85805, "x":1.62372, "y":3.12823, "heading":0.48556, "vx":0.36395, "vy":0.34265, "omega":0.9345, "ax":0.0, "ay":0.0, "alpha":0.4883, "fx":[-1.77098,-0.54751,1.77098,0.54751], "fy":[0.54751,-1.77098,-0.54751,1.77098]}, + {"t":0.89535, "x":1.6373, "y":3.14101, "heading":0.52043, "vx":0.36395, "vy":0.34265, "omega":0.95272, "ax":0.0, "ay":0.0, "alpha":0.50319, "fx":[-1.84355,-0.50025,1.84355,0.50025], "fy":[0.50025,-1.84355,-0.50025,1.84355]}, + {"t":0.93266, "x":1.65087, "y":3.15379, "heading":0.55597, "vx":0.36395, "vy":0.34265, "omega":0.97149, "ax":0.0, "ay":0.0, "alpha":0.5258, "fx":[-1.94374,-0.45394,1.94374,0.45394], "fy":[0.45394,-1.94374,-0.45394,1.94374]}, + {"t":0.96997, "x":1.66445, "y":3.16658, "heading":0.59221, "vx":0.36395, "vy":0.34265, "omega":0.99111, "ax":0.0, "ay":0.0, "alpha":0.55647, "fx":[-2.07318,-0.40557,2.07318,0.40557], "fy":[0.40557,-2.07318,-0.40557,2.07318]}, + {"t":1.00727, "x":1.67803, "y":3.17936, "heading":0.62919, "vx":0.36395, "vy":0.34265, "omega":1.01187, "ax":-0.00002, "ay":0.00002, "alpha":0.59568, "fx":[-2.23415,-0.35216,2.23346,0.35147], "fy":[0.35218,-2.23344,-0.35145,2.23417]}, + {"t":1.04458, "x":1.69161, "y":3.19214, "heading":0.66694, "vx":0.36395, "vy":0.34265, "omega":1.03409, "ax":-0.00436, "ay":0.00463, "alpha":0.64404, "fx":[-2.50196,-0.36317,2.35362,0.21474], "fy":[0.36773,-2.34904,-0.21018,2.50654]}, + {"t":1.08189, "x":1.70518, "y":3.20493, "heading":0.70551, "vx":0.36378, "vy":0.34282, "omega":1.05812, "ax":-0.96642, "ay":0.92764, "alpha":0.67204, "fx":[-18.96955,-16.70303,-13.89692,-16.18427], "fy":[15.93042,13.22532,15.63484,18.32501]}, + {"t":1.11919, "x":1.71808, "y":3.21837, "heading":0.74499, "vx":0.32773, "vy":0.37743, "omega":1.08319, "ax":-8.27377, "ay":3.26411, "alpha":0.0114, "fx":[-140.74582,-140.76357,-140.72301,-140.70524], "fy":[55.49592,55.44649,55.54726,55.59661]}, + {"t":1.1565, "x":1.72455, "y":3.23472, "heading":0.7854, "vx":0.01907, "vy":0.4992, "omega":1.08361, "ax":-8.58, "ay":-2.41753, "alpha":0.00742, "fx":[-145.94812,-145.92904,-145.93864,-145.95772], "fy":[-41.10711,-41.17304,-41.13579,-41.06984]}, + {"t":1.19109, "x":1.72007, "y":3.25054, "heading":0.82289, "vx":-0.27776, "vy":0.41557, "omega":1.08387, "ax":-1.56821, "ay":-1.21133, "alpha":0.42326, "fx":[-28.25512,-26.54358,-25.08855,-26.8123], "fy":[-20.59277,-22.23015,-20.62062,-18.97418]}, + {"t":1.22569, "x":1.70953, "y":3.26419, "heading":0.86038, "vx":-0.33201, "vy":0.37366, "omega":1.09851, "ax":-0.00899, "ay":-0.00799, "alpha":0.34733, "fx":[-1.46774,-0.05416,1.16192,-0.25171], "fy":[-0.23477,-1.45081,-0.03722,1.17886]}, + {"t":1.26028, "x":1.69804, "y":3.27711, "heading":0.89839, "vx":-0.33233, "vy":0.37339, "omega":1.11053, "ax":-0.00005, "ay":-0.00005, "alpha":0.23314, "fx":[-0.88031,0.0989,0.87854,-0.10067], "fy":[-0.10058,-0.88021,0.099,0.87863]}, + {"t":1.29488, "x":1.68654, "y":3.29003, "heading":0.93681, "vx":-0.33233, "vy":0.37338, "omega":1.11859, "ax":0.0, "ay":0.0, "alpha":0.12204, "fx":[-0.458,0.06987,0.45799,-0.06988], "fy":[-0.06988,-0.458,0.06987,0.45799]}, + {"t":1.32948, "x":1.67504, "y":3.30295, "heading":0.9755, "vx":-0.33233, "vy":0.37338, "omega":1.12282, "ax":0.0, "ay":0.0, "alpha":0.01255, "fx":[-0.04679,0.009,0.04679,-0.009], "fy":[-0.009,-0.04679,0.009,0.04679]}, + {"t":1.36407, "x":1.66354, "y":3.31587, "heading":1.01435, "vx":-0.33233, "vy":0.37338, "omega":1.12325, "ax":0.0, "ay":0.0, "alpha":-0.09677, "fx":[0.35778,-0.08337,-0.35778,0.08337], "fy":[0.08337,0.35778,-0.08337,-0.35778]}, + {"t":1.39867, "x":1.65205, "y":3.32878, "heading":1.05321, "vx":-0.33233, "vy":0.37338, "omega":1.1199, "ax":0.0, "ay":0.0, "alpha":-0.20737, "fx":[0.75917,-0.20832,-0.75917,0.20832], "fy":[0.20832,0.75917,-0.20832,-0.75917]}, + {"t":1.43326, "x":1.64055, "y":3.3417, "heading":1.09195, "vx":-0.33233, "vy":0.37338, "omega":1.11273, "ax":0.0, "ay":0.0, "alpha":-0.32072, "fx":[1.16076,-0.36741,-1.16076,0.36741], "fy":[0.36741,1.16076,-0.36741,-1.16076]}, + {"t":1.46786, "x":1.62905, "y":3.35462, "heading":1.13044, "vx":-0.33233, "vy":0.37338, "omega":1.10163, "ax":0.0, "ay":0.0, "alpha":-0.43831, "fx":[1.56583,-0.5628,-1.56583,0.5628], "fy":[0.5628,1.56583,-0.5628,-1.56583]}, + {"t":1.50245, "x":1.61756, "y":3.36754, "heading":1.16856, "vx":-0.33233, "vy":0.37338, "omega":1.08647, "ax":0.0, "ay":0.0, "alpha":-0.56168, "fx":[1.97764,-0.79714,-1.97764,0.79714], "fy":[0.79714,1.97764,-0.79714,-1.97764]}, + {"t":1.53705, "x":1.60606, "y":3.38045, "heading":1.20614, "vx":-0.33233, "vy":0.37338, "omega":1.06704, "ax":0.0, "ay":0.0, "alpha":-0.69246, "fx":[2.39945,-1.07367,-2.39945,1.07367], "fy":[1.07367,2.39945,-1.07367,-2.39945]}, + {"t":1.57164, "x":1.59456, "y":3.39337, "heading":1.24306, "vx":-0.33233, "vy":0.37338, "omega":1.04308, "ax":0.0, "ay":0.0, "alpha":-0.83235, "fx":[2.83459,-1.39614,-2.83459,1.39614], "fy":[1.39614,2.83459,-1.39614,-2.83459]}, + {"t":1.60624, "x":1.58307, "y":3.40629, "heading":1.27914, "vx":-0.33233, "vy":0.37338, "omega":1.01429, "ax":0.0, "ay":0.0, "alpha":-0.98317, "fx":[3.28653,-1.76884,-3.28653,1.76884], "fy":[1.76884,3.28653,-1.76884,-3.28653]}, + {"t":1.64083, "x":1.57157, "y":3.41921, "heading":1.31423, "vx":-0.33233, "vy":0.37338, "omega":0.98027, "ax":0.0, "ay":0.0, "alpha":-1.14686, "fx":[3.75897,-2.19656,-3.75897,2.19656], "fy":[2.19656,3.75897,-2.19656,-3.75897]}, + {"t":1.67543, "x":1.56007, "y":3.43212, "heading":1.34815, "vx":-0.33233, "vy":0.37338, "omega":0.9406, "ax":0.0, "ay":0.0, "alpha":-1.32551, "fx":[4.25596,-2.68459,-4.25596,2.68459], "fy":[2.68459,4.25596,-2.68459,-4.25596]}, + {"t":1.71002, "x":1.54858, "y":3.44504, "heading":1.38069, "vx":-0.33233, "vy":0.37338, "omega":0.89474, "ax":0.0, "ay":0.0, "alpha":-1.52139, "fx":[4.78206,-3.2386,-4.78206,3.2386], "fy":[3.2386,4.78206,-3.2386,-4.78206]}, + {"t":1.74462, "x":1.53708, "y":3.45796, "heading":1.41164, "vx":-0.33233, "vy":0.37338, "omega":0.84211, "ax":0.0, "ay":0.0, "alpha":-1.73695, "fx":[5.34254,-3.86465,-5.34254,3.86465], "fy":[3.86465,5.34254,-3.86465,-5.34254]}, + {"t":1.77921, "x":1.52558, "y":3.47087, "heading":1.44077, "vx":-0.33233, "vy":0.37338, "omega":0.78202, "ax":0.0, "ay":0.0, "alpha":-1.97483, "fx":[5.94366,-4.569,-5.94366,4.569], "fy":[4.569,5.94366,-4.569,-5.94366]}, + {"t":1.81381, "x":1.51408, "y":3.48379, "heading":1.46783, "vx":-0.33233, "vy":0.37338, "omega":0.7137, "ax":0.0, "ay":0.0, "alpha":-2.23792, "fx":[6.59296,-5.358,-6.59296,5.358], "fy":[5.358,6.59296,-5.358,-6.59296]}, + {"t":1.8484, "x":1.50259, "y":3.49671, "heading":1.49252, "vx":-0.33233, "vy":0.37338, "omega":0.63628, "ax":0.0, "ay":0.0, "alpha":-2.52934, "fx":[7.29973,-6.23784,-7.29973,6.23784], "fy":[6.23784,7.29973,-6.23784,-7.29973]}, + {"t":1.883, "x":1.49109, "y":3.50963, "heading":1.51453, "vx":-0.33233, "vy":0.37338, "omega":0.54878, "ax":0.0, "ay":0.0, "alpha":-2.85247, "fx":[8.07545,-7.21423,-8.07545,7.21423], "fy":[7.21423,8.07545,-7.21423,-8.07545]}, + {"t":1.91759, "x":1.47959, "y":3.52254, "heading":1.53351, "vx":-0.33233, "vy":0.37338, "omega":0.45009, "ax":0.0, "ay":0.0, "alpha":-3.21093, "fx":[8.93445,-8.29192,-8.93445,8.29192], "fy":[8.29192,8.93445,-8.29192,-8.93445]}, + {"t":1.95219, "x":1.4681, "y":3.53546, "heading":1.54909, "vx":-0.33233, "vy":0.37338, "omega":0.33901, "ax":0.0, "ay":0.0, "alpha":-3.6086, "fx":[9.89466,-9.47407,-9.89466,9.47407], "fy":[9.47407,9.89466,-9.47407,-9.89466]}, + {"t":1.98678, "x":1.4566, "y":3.54838, "heading":1.56081, "vx":-0.33233, "vy":0.37338, "omega":0.21417, "ax":0.00001, "ay":-0.00001, "alpha":-4.04959, "fx":[10.97855,-10.76118,-10.97822,10.76151], "fy":[10.76116,10.9782,-10.76153,-10.97857]}, + {"t":2.02138, "x":1.4451, "y":3.5613, "heading":1.56822, "vx":-0.33233, "vy":0.37338, "omega":0.07407, "ax":3.61567, "ay":-4.06237, "alpha":-2.1318, "fx":[69.5779,57.56627,53.51959,65.34257], "fy":[-61.56365,-65.66334,-76.62474,-72.54711]}, + {"t":2.05597, "x":1.43577, "y":3.57178, "heading":1.57079, "vx":-0.20724, "vy":0.23285, "omega":0.00032, "ax":5.99048, "ay":-6.73056, "alpha":-0.00936, "fx":[101.9492,101.89315,101.84341,101.89948], "fy":[-114.43784,-114.48758,-114.53201,-114.4823]}, + {"t":2.09057, "x":1.43218, "y":3.57581, "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/deploy/choreo/path.chor b/src/main/deploy/choreo/path.chor index fdb671e9..789265a8 100644 --- a/src/main/deploy/choreo/path.chor +++ b/src/main/deploy/choreo/path.chor @@ -9,39 +9,39 @@ "config":{ "frontLeft":{ "x":{ - "exp":"11 in", - "val":0.2794 + "exp":"9 in", + "val":0.2286 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"9 in", + "val":0.2286 } }, "backLeft":{ "x":{ - "exp":"-11 in", - "val":-0.2794 + "exp":"-9 in", + "val":-0.2286 }, "y":{ - "exp":"11 in", - "val":0.2794 + "exp":"9 in", + "val":0.2286 } }, "mass":{ - "exp":"150 lbs", - "val":68.0388555 + "exp":"115 lbs", + "val":52.16312255 }, "inertia":{ - "exp":"6 kg m ^ 2", - "val":6.0 + "exp":"3 kg m ^ 2", + "val":3.0 }, "gearing":{ "exp":"6.5", "val":6.5 }, "radius":{ - "exp":"2 in", - "val":0.0508 + "exp":"4 in", + "val":0.1016 }, "vmax":{ "exp":"6000 RPM", @@ -57,16 +57,16 @@ }, "bumper":{ "front":{ - "exp":"16 in", - "val":0.4064 + "exp":"27 in", + "val":0.6858 }, "side":{ - "exp":"16 in", - "val":0.4064 + "exp":"27 in", + "val":0.6858 }, "back":{ - "exp":"16 in", - "val":0.4064 + "exp":"27 in", + "val":0.6858 } }, "differentialTrackWidth":{ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..447c680e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -132,7 +132,6 @@ public void robotPeriodic() { SmartDashboard.putBoolean("Hub Active?", hubActive()); SmartDashboard.putString("Selected Action", robotContainer.getAutoChooser().getCommandDescription()); - SmartDashboard.putString("Starting Location", location().toString()); // Gets the alliance color. if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59209e01..35f5f29e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,10 +75,6 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; -import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; - /** * This class is where the bulk of the robot should be declared. Since * Command-based is a @@ -90,7 +86,7 @@ */ public class RobotContainer { // Instantiate the autochooser. - private final AutoChooser autoChooser = new AutoChooser(); + private final AutoChooser autoChooser; // The robot's subsystems and commands are defined here... // private final TiltSubsystem tiltSubsystem; private final CommandXboxController controller = @@ -196,9 +192,10 @@ public RobotContainer() { } } + setUpAutoFactory(); + autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem, feederSubsystem, hopperSubsystem); configureBindings(); putShuffleboardCommands(); - setUpAutoFactory(); } /** @@ -504,16 +501,15 @@ public void putShuffleboardCommands() { } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - return autoChooser.getCommand(); - // return straightRoutine.cmd(straightTrajectory.done()); - - // return new ExampleAuto(drivebase, autoFactory); + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return autoChooser.getCommand(); + // return straightRoutine.cmd(straightTrajectory.done()); + // return new ExampleAuto(drivebase, autoFactory); } public ClimberSubsystem getClimberSubsystem() { diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index cbb8da92..1afc7305 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -7,10 +7,8 @@ public enum AutoAction { DO_NOTHING("Do Nothing"), - TWO_PIECE_HIGH("2 Piece L4"), - TWO_PIECE_LOW("2 Piece L2"), - ONE_PIECE("1 Piece"), - CROSS_THE_LINE("Cross The Line"), + SHOOT("Shoot"), + SHOOT_AND_CLIMB("Shoot and Climb"), INVALID("INVALID"); private final String name; private static final HashMap nameMap = diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java index 877b9c62..319fc62f 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -2,10 +2,29 @@ import java.util.HashMap; import java.util.Map; +import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import frc.robot.utils.logging.commands.LoggableCommand; +import choreo.auto.AutoFactory; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.auto.BlueLeftShootClimb; +import frc.robot.commands.auto.BlueMidShootClimb; +import frc.robot.commands.auto.BlueRightShootClimb; +import frc.robot.commands.auto.LeftShoot; +import frc.robot.commands.auto.RedLeftShootClimb; +import frc.robot.commands.auto.MidShoot; +import frc.robot.commands.auto.RedMidShootClimb; +import frc.robot.commands.auto.RightShoot; +import frc.robot.commands.auto.RedRightShootClimb; +import frc.robot.constants.enums.ShootingState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.DoNothingCommand; public class AutoChooser { @@ -14,11 +33,26 @@ public class AutoChooser { /** Drop-down chooser for the action. */ private LoggedDashboardChooser actionChooser; /** Structure for mapping possible choices to commands. */ - private final Map commandMap = new HashMap<>(); + private final Map commandMap = new HashMap<>(); - private final AutoCommand DEFAULT_COMMAND = AutoCommand.Invalid; + private final SwerveSubsystem drivetrain; + private final ShootingState shootstate; + private final ShooterSubsystem shooter; + private final AutoFactory auto; + private final ClimberSubsystem climber; + private final FeederSubsystem feeder; + private final HopperSubsystem hopper; + + public AutoChooser(SwerveSubsystem drivetrain, ShootingState shootstate, AutoFactory auto, + ShooterSubsystem shooter, ClimberSubsystem climber, FeederSubsystem feeder, HopperSubsystem hopper) { + this.drivetrain = drivetrain; + this.auto = auto; + this.shootstate = shootstate; + this.shooter = shooter; + this.climber = climber; + this.hopper = hopper; + this.feeder = feeder; - public AutoChooser() { this.locationChooser = new LoggedDashboardChooser<>( "Location Chooser" ); @@ -56,26 +90,62 @@ private void populateChoosers() { private void populateMap() { // Currently, we have some example mappings. commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.LEFT), - AutoCommand.DoNothing); + new DoNothingCommand()); commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.RIGHT), - AutoCommand.DoSomething); + new DoNothingCommand()); + commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MIDDLE), + new DoNothingCommand()); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.LEFT), + new LeftShoot(drivetrain, auto, shooter, shootstate, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.RIGHT), + new RightShoot(drivetrain, auto, shooter, shootstate, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MIDDLE), + new MidShoot(drivetrain, auto, shooter, shootstate, hopper, feeder)); + + Optional alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Blue) { + //Blue ShootClimbAutos + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), + new BlueLeftShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), + new BlueRightShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), + new BlueMidShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); + } + + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + //Red ShootClimb Autos + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.LEFT), + new RedLeftShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.RIGHT), + new RedRightShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); + commandMap.put(new AutoEvent(AutoAction.SHOOT_AND_CLIMB, FieldLocation.MIDDLE), + new RedMidShootClimb(drivetrain, auto, shooter, shootstate, climber, hopper, feeder)); + } } - private AutoCommand get() { + public AutoEvent getSelectedEvent() { AutoAction chosenAction = actionChooser.get(); FieldLocation chosenLocation = locationChooser.get(); - AutoEvent event = new AutoEvent(chosenAction, chosenLocation); + return new AutoEvent(chosenAction, chosenLocation); + } - return commandMap.getOrDefault(event, DEFAULT_COMMAND); + public Command getSelectedCommand() { + AutoEvent event = getSelectedEvent(); + return commandMap.getOrDefault(event, new DoNothingCommand()); } - public LoggableCommand getCommand() { - return get().getCommand(); + public Command getCommand() { + return getSelectedCommand(); } - /** @return A human-readable description of the selected command. */ public String getCommandDescription() { - return get().getDescription(); + AutoEvent event = getSelectedEvent(); + Command command = commandMap.get(event); + if (command == null) { + return "No auto mapped for " + event.getAction() + " at " + event.getLocation(); + } + return event.getAction() + " at " + event.getLocation() + " -> " + command.getName(); } public FieldLocation getLocation() { diff --git a/src/main/java/frc/robot/autochooser/AutoCommand.java b/src/main/java/frc/robot/autochooser/AutoCommand.java deleted file mode 100644 index 6d31ab44..00000000 --- a/src/main/java/frc/robot/autochooser/AutoCommand.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.autochooser; - -import frc.robot.utils.logging.commands.LoggableCommand; -import frc.robot.utils.logging.commands.DoNothingCommand; -import frc.robot.utils.logging.commands.DoSomethingCommand; - -/** An enum to associate commands with human-readable descriptions. */ -public enum AutoCommand { - - // Add commands here. Importantly, you should give each command - // a readable description so that the drive team can tell what - // the robot will actually do. This will be used to give the - // drive team visual feedback on the elastic dashboard when - // selecting an autonoumous command. - Invalid( - "The selection is invalid. (The robot won't do anything.)", - new DoNothingCommand() - ), - DoNothing("The robot won't do anything.", new DoNothingCommand()), - DoSomething( - "Something will be printed to the terminal.", - new DoSomethingCommand(""" - SUCCESSFULLY - - DID - - SOMETHING - """) - ); - - private String description; - private LoggableCommand command; - - AutoCommand(String description, LoggableCommand command) { - this.description = description; - this.command = command; - } - - public String getDescription() { - return description; - } - - public LoggableCommand getCommand() { - return command; - } - -} diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java index 43e21ef1..d25180a8 100644 --- a/src/main/java/frc/robot/autochooser/FieldLocation.java +++ b/src/main/java/frc/robot/autochooser/FieldLocation.java @@ -12,15 +12,15 @@ import java.util.stream.Collectors; public enum FieldLocation { - ZERO(0, 0, 0, "Zero"), - INVALID(-1, -1, -1, "INVALID"), - LEFT(7.150, 7.000, 180, "NON Processor Side"), - MIDDLE(7.150, 4.500, 180, "Middle"), - RIGHT(7.150, 2.000, 180, "Processor Side"); - - private static final double RED_X_POS = 2.3876; // meters - public static final double HEIGHT_OF_FIELD = 8.05; - public static final double LENGTH_OF_FIELD = 17.548225; + ZERO(0, 0, 0, "zero"), + INVALID(-1, -1, -1, "invalid"), + LEFT(3.622, 7.662, 0, "depot side"), + MIDDLE(3.622, 4.024, 0, "middle"), + RIGHT(3.622, 0.407, 0, "outpost side"); + + private static final double RED_X_POS = 9.338; // meters + public static final double HEIGHT_OF_FIELD = 8.043; + public static final double LENGTH_OF_FIELD = 16.411; private final double yPose; private final double xPose; private final double angle; diff --git a/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java new file mode 100644 index 00000000..0746b90a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/BlueLeftShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class BlueLeftShootClimb extends LoggableSequentialCommandGroup{ + public BlueLeftShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("BlueLeftClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("BlueLeftClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java new file mode 100644 index 00000000..4e5434d9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/BlueMidShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class BlueMidShootClimb extends LoggableSequentialCommandGroup{ + public BlueMidShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("MidToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("BlueMidClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("BlueMidClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java new file mode 100644 index 00000000..ae20ea91 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/BlueRightShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class BlueRightShootClimb extends LoggableSequentialCommandGroup{ + public BlueRightShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("BlueRightClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("BlueRightClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/DoNothing.java b/src/main/java/frc/robot/commands/auto/DoNothing.java new file mode 100644 index 00000000..71d7c0ff --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/DoNothing.java @@ -0,0 +1,11 @@ +package frc.robot.commands.auto; + +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class DoNothing extends LoggableSequentialCommandGroup{ + public DoNothing() { + super( + new DoNothing() + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/ExampleAuto.java b/src/main/java/frc/robot/commands/auto/ExampleAuto.java index bece64d4..01a28ab7 100644 --- a/src/main/java/frc/robot/commands/auto/ExampleAuto.java +++ b/src/main/java/frc/robot/commands/auto/ExampleAuto.java @@ -8,7 +8,7 @@ import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; public class ExampleAuto extends LoggableSequentialCommandGroup{ - public ExampleAuto(SwerveSubsystem subsystem, AutoFactory auto) { + public ExampleAuto(SwerveSubsystem drivetrain, AutoFactory auto) { super( LoggableCommandWrapper.wrap(auto.resetOdometry("ExamplePathOne")), new LoggableParallelCommandGroup( diff --git a/src/main/java/frc/robot/commands/auto/LeftShoot.java b/src/main/java/frc/robot/commands/auto/LeftShoot.java new file mode 100644 index 00000000..2de078e1 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/LeftShoot.java @@ -0,0 +1,34 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class LeftShoot extends LoggableSequentialCommandGroup{ + public LeftShoot(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ) + ); + } +} + diff --git a/src/main/java/frc/robot/commands/auto/MidShoot.java b/src/main/java/frc/robot/commands/auto/MidShoot.java new file mode 100644 index 00000000..10f53244 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/MidShoot.java @@ -0,0 +1,34 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class MidShoot extends LoggableSequentialCommandGroup{ + public MidShoot(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ) + ); + } +} + diff --git a/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java new file mode 100644 index 00000000..cb783e07 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/RedLeftShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class RedLeftShootClimb extends LoggableSequentialCommandGroup{ + public RedLeftShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("LeftToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("LeftToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("RedLeftClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RedLeftClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java new file mode 100644 index 00000000..efc4dc69 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/RedMidShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class RedMidShootClimb extends LoggableSequentialCommandGroup{ + public RedMidShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("MidToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("MidToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("RedMidClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RedMidClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java new file mode 100644 index 00000000..39299fc9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/RedRightShootClimb.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.climber.ClimberDown; +import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class RedRightShootClimb extends LoggableSequentialCommandGroup{ + public RedRightShootClimb(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, ClimberSubsystem climber, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ), + LoggableCommandWrapper.wrap(auto.resetOdometry("RedRightClimb")), + new ClimberUp(climber), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RedRightClimb")/*.withTimeout(n)*/), + new ClimberDown(climber) + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/RightShoot.java b/src/main/java/frc/robot/commands/auto/RightShoot.java new file mode 100644 index 00000000..613335fa --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/RightShoot.java @@ -0,0 +1,33 @@ +package frc.robot.commands.auto; + +import choreo.auto.AutoFactory; +import frc.robot.commands.feeder.SpinFeeder; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; +import frc.robot.commands.hopper.SpinHopper; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommandWrapper; +import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; +import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; + +public class RightShoot extends LoggableSequentialCommandGroup{ + public RightShoot(SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, + ShootingState shootstate, HopperSubsystem hopper, FeederSubsystem feeder) { + super( + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + LoggableCommandWrapper.wrap(auto.resetOdometry("RightToTower")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("RightToTower")/*.withTimeout(n)*/), + new LoggableParallelCommandGroup( + new SpinShooter(shooter, Constants.SHOOTER_SPEED), + new SpinHopper(hopper), + new SpinFeeder(feeder) + ) + ); + } +}