diff --git a/src/main/deploy/choreo/Depot_Neutral_1.traj b/src/main/deploy/choreo/Depot_Neutral_1.traj index 0793c844..97ffab06 100644 --- a/src/main/deploy/choreo/Depot_Neutral_1.traj +++ b/src/main/deploy/choreo/Depot_Neutral_1.traj @@ -3,9 +3,10 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.599, "y":7.33, "heading":3.141592653589793, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.2013142108917236, "y":5.911487102508545, "heading":3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.0, "y":4.029, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.599, "y":7.33, "heading":3.141592653589793, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.159896373748779, "y":7.33, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.691810131072998, "y":7.156898021697998, "heading":-2.788601768492154, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.405144214630127, "y":6.361705780029297, "heading":-0.9075928148041412, "intervals":24, "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}, @@ -14,9 +15,10 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"3.599 m", "val":3.599}, "y":{"exp":"7.33 m", "val":7.33}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.2013142108917236 m", "val":2.2013142108917236}, "y":{"exp":"5.911487102508545 m", "val":5.911487102508545}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2 m", "val":2.0}, "y":{"exp":"4.029 m", "val":4.029}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.599 m", "val":3.599}, "y":{"exp":"7.33 m", "val":7.33}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.159896373748779 m", "val":5.159896373748779}, "y":{"exp":"7.33 m", "val":7.33}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.691810131072998 m", "val":6.691810131072998}, "y":{"exp":"7.156898021697998 m", "val":7.156898021697998}, "heading":{"exp":"-2.7886017684921534 rad", "val":-2.788601768492154}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.405144214630127 m", "val":7.405144214630127}, "y":{"exp":"6.361705780029297 m", "val":6.361705780029297}, "heading":{"exp":"-0.9075928148041412 rad", "val":-0.9075928148041412}, "intervals":24, "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}, @@ -51,40 +53,62 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.59104,1.16228], + "waypoints":[0.0,0.50001,0.8451,1.3373], "samples":[ - {"t":0.0, "x":3.599, "y":7.33, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.2548, "ay":-8.81779, "alpha":0.0, "fx":[-116.2875,-116.2875,-116.2875,-116.2875], "fy":[-99.99207,-99.99207,-99.99207,-99.99207]}, - {"t":0.03694, "x":3.592, "y":7.32398, "heading":3.14159, "vx":-0.37881, "vy":-0.32573, "omega":0.0, "ax":-10.25327, "ay":-8.81659, "alpha":0.0, "fx":[-116.27017,-116.27017,-116.27017,-116.27017], "fy":[-99.97843,-99.97843,-99.97843,-99.97843]}, - {"t":0.07388, "x":3.57101, "y":7.30594, "heading":3.14159, "vx":-0.75757, "vy":-0.65142, "omega":0.0, "ax":-10.25131, "ay":-8.81504, "alpha":0.0, "fx":[-116.24792,-116.24792,-116.24792,-116.24792], "fy":[-99.96093,-99.96093,-99.96093,-99.96093]}, - {"t":0.11082, "x":3.53604, "y":7.27586, "heading":3.14159, "vx":-1.13626, "vy":-0.97704, "omega":0.0, "ax":-10.2487, "ay":-8.81299, "alpha":0.0, "fx":[-116.21832,-116.21832,-116.21832,-116.21832], "fy":[-99.93763,-99.93763,-99.93763,-99.93763]}, - {"t":0.14776, "x":3.48707, "y":7.23375, "heading":3.14159, "vx":-1.51484, "vy":-1.3026, "omega":0.0, "ax":-10.24506, "ay":-8.81012, "alpha":0.0, "fx":[-116.177,-116.177,-116.177,-116.177], "fy":[-99.90511,-99.90511,-99.90511,-99.90511]}, - {"t":0.1847, "x":3.42412, "y":7.17962, "heading":3.14159, "vx":-1.8933, "vy":-1.62804, "omega":0.0, "ax":-10.23961, "ay":-8.80584, "alpha":0.0, "fx":[-116.11527,-116.11527,-116.11527,-116.11527], "fy":[-99.85653,-99.85653,-99.85653,-99.85653]}, - {"t":0.22164, "x":3.3472, "y":7.11348, "heading":3.14159, "vx":-2.27155, "vy":-1.95333, "omega":0.0, "ax":-10.2306, "ay":-8.79875, "alpha":0.0, "fx":[-116.0131,-116.0131,-116.0131,-116.0131], "fy":[-99.77613,-99.77613,-99.77613,-99.77613]}, - {"t":0.25858, "x":3.2563, "y":7.03532, "heading":3.14159, "vx":-2.64947, "vy":-2.27836, "omega":0.0, "ax":-10.21283, "ay":-8.78476, "alpha":0.0, "fx":[-115.81153,-115.81153,-115.81153,-115.81153], "fy":[-99.61749,-99.61749,-99.61749,-99.61749]}, - {"t":0.29552, "x":3.15146, "y":6.94516, "heading":3.14159, "vx":-3.02673, "vy":-2.60287, "omega":0.0, "ax":-10.16148, "ay":-8.74434, "alpha":0.0, "fx":[-115.22922,-115.22922,-115.22922,-115.22922], "fy":[-99.15911,-99.15911,-99.15911,-99.15911]}, - {"t":0.33246, "x":3.03272, "y":6.84304, "heading":3.14159, "vx":-3.4021, "vy":-2.92589, "omega":0.0, "ax":-8.51795, "ay":-7.44158, "alpha":0.0, "fx":[-96.59193,-96.59193,-96.59193,-96.59193], "fy":[-84.38612,-84.38612,-84.38612,-84.38612]}, - {"t":0.3694, "x":2.90124, "y":6.72988, "heading":3.14159, "vx":-3.71675, "vy":-3.20078, "omega":0.0, "ax":0.50672, "ay":-0.59803, "alpha":0.0, "fx":[5.74615,5.74615,5.74615,5.74615], "fy":[-6.78158,-6.78158,-6.78158,-6.78158]}, - {"t":0.40634, "x":2.76429, "y":6.61124, "heading":3.14159, "vx":-3.69803, "vy":-3.22287, "omega":0.0, "ax":3.05391, "ay":-3.38446, "alpha":0.0, "fx":[34.6308,34.6308,34.6308,34.6308], "fy":[-38.37916,-38.37916,-38.37916,-38.37916]}, - {"t":0.44328, "x":2.62976, "y":6.48988, "heading":3.14159, "vx":-3.58522, "vy":-3.34789, "omega":0.0, "ax":7.80742, "ay":-7.69477, "alpha":0.0, "fx":[88.53463,88.53463,88.53463,88.53463], "fy":[-87.25728,-87.25728,-87.25728,-87.25728]}, - {"t":0.48022, "x":2.50265, "y":6.36095, "heading":3.14159, "vx":-3.29682, "vy":-3.63214, "omega":0.0, "ax":9.84181, "ay":-8.10007, "alpha":0.0, "fx":[111.60425,111.60425,111.60425,111.60425], "fy":[-91.85324,-91.85324,-91.85324,-91.85324]}, - {"t":0.51716, "x":2.38758, "y":6.22126, "heading":3.14159, "vx":-2.93326, "vy":-3.93136, "omega":0.0, "ax":10.94995, "ay":-7.34227, "alpha":0.0, "fx":[124.17034,124.17034,124.17034,124.17034], "fy":[-83.25999,-83.25999,-83.25999,-83.25999]}, - {"t":0.5541, "x":2.2867, "y":6.07102, "heading":3.14159, "vx":-2.52877, "vy":-4.20258, "omega":0.0, "ax":11.7666, "ay":-6.28809, "alpha":0.0, "fx":[133.431,133.431,133.431,133.431], "fy":[-71.30569,-71.30569,-71.30569,-71.30569]}, - {"t":0.59104, "x":2.20131, "y":5.91149, "heading":3.14159, "vx":-2.09411, "vy":-4.43486, "omega":0.0, "ax":12.34331, "ay":-5.07531, "alpha":0.0, "fx":[139.97077,139.97077,139.97077,139.97077], "fy":[-57.55302,-57.55302,-57.55302,-57.55302]}, - {"t":0.62912, "x":2.13052, "y":5.73892, "heading":3.14159, "vx":-1.62404, "vy":-4.62814, "omega":0.0, "ax":12.6462, "ay":-3.73161, "alpha":0.0, "fx":[143.40549,143.40549,143.40549,143.40549], "fy":[-42.3157,-42.3157,-42.3157,-42.3157]}, - {"t":0.66721, "x":2.07784, "y":5.55996, "heading":3.14159, "vx":-1.14244, "vy":-4.77025, "omega":0.0, "ax":12.50072, "ay":-2.35302, "alpha":0.0, "fx":[141.75575,141.75575,141.75575,141.75575], "fy":[-26.68275,-26.68275,-26.68275,-26.68275]}, - {"t":0.70529, "x":2.0434, "y":5.37659, "heading":3.14159, "vx":-0.66639, "vy":-4.85986, "omega":0.0, "ax":10.6198, "ay":-1.01225, "alpha":0.0, "fx":[120.42646,120.42646,120.42646,120.42646], "fy":[-11.47877,-11.47877,-11.47877,-11.47877]}, - {"t":0.74337, "x":2.02572, "y":5.19078, "heading":3.14159, "vx":-0.26196, "vy":-4.89841, "omega":0.0, "ax":3.87204, "ay":-0.14693, "alpha":0.0, "fx":[43.90815,43.90815,43.90815,43.90815], "fy":[-1.66615,-1.66615,-1.66615,-1.66615]}, - {"t":0.78145, "x":2.01855, "y":5.00413, "heading":3.14159, "vx":-0.1145, "vy":-4.90401, "omega":0.0, "ax":0.73431, "ay":7.31404, "alpha":0.0, "fx":[8.32697,8.32697,8.32697,8.32697], "fy":[82.93976,82.93976,82.93976,82.93976]}, - {"t":0.81954, "x":2.01472, "y":4.82268, "heading":3.14159, "vx":-0.08654, "vy":-4.62547, "omega":0.0, "ax":0.26224, "ay":13.40327, "alpha":0.0, "fx":[2.97372,2.97372,2.97372,2.97372], "fy":[151.99058,151.99058,151.99058,151.99058]}, - {"t":0.85762, "x":2.01162, "y":4.65625, "heading":3.14159, "vx":-0.07655, "vy":-4.11504, "omega":0.0, "ax":0.25515, "ay":13.47031, "alpha":0.0, "fx":[2.89337,2.89337,2.89337,2.89337], "fy":[152.75078,152.75078,152.75078,152.75078]}, - {"t":0.8957, "x":2.00889, "y":4.5093, "heading":3.14159, "vx":-0.06683, "vy":-3.60206, "omega":0.0, "ax":0.25277, "ay":13.49283, "alpha":0.0, "fx":[2.86633,2.86633,2.86633,2.86633], "fy":[153.00608,153.00608,153.00608,153.00608]}, - {"t":0.93378, "x":2.00653, "y":4.38191, "heading":3.14159, "vx":-0.05721, "vy":-3.08822, "omega":0.0, "ax":0.25157, "ay":13.5041, "alpha":0.0, "fx":[2.85278,2.85278,2.85278,2.85278], "fy":[153.13395,153.13395,153.13395,153.13395]}, - {"t":0.97187, "x":2.00453, "y":4.2741, "heading":3.14159, "vx":-0.04763, "vy":-2.57395, "omega":0.0, "ax":0.25085, "ay":13.51087, "alpha":0.0, "fx":[2.84464,2.84464,2.84464,2.84464], "fy":[153.21071,153.21071,153.21071,153.21071]}, - {"t":1.00995, "x":2.0029, "y":4.18587, "heading":3.14159, "vx":-0.03807, "vy":-2.05942, "omega":0.0, "ax":0.25038, "ay":13.51539, "alpha":0.0, "fx":[2.83922,2.83922,2.83922,2.83922], "fy":[153.26189,153.26189,153.26189,153.26189]}, - {"t":1.04803, "x":2.00163, "y":4.11725, "heading":3.14159, "vx":-0.02854, "vy":-1.54472, "omega":0.0, "ax":0.25003, "ay":13.51861, "alpha":0.0, "fx":[2.83534,2.83534,2.83534,2.83534], "fy":[153.29845,153.29845,153.29845,153.29845]}, - {"t":1.08611, "x":2.00072, "y":4.06822, "heading":3.14159, "vx":-0.01902, "vy":-1.0299, "omega":0.0, "ax":0.24978, "ay":13.52103, "alpha":0.0, "fx":[2.83244,2.83244,2.83244,2.83244], "fy":[153.32587,153.32587,153.32587,153.32587]}, - {"t":1.1242, "x":2.00018, "y":4.03881, "heading":3.14159, "vx":-0.0095, "vy":-0.51499, "omega":0.0, "ax":0.24958, "ay":13.52291, "alpha":0.0, "fx":[2.83018,2.83018,2.83018,2.83018], "fy":[153.34719,153.34719,153.34719,153.34719]}, - {"t":1.16228, "x":2.0, "y":4.029, "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]}], + {"t":0.0, "x":3.599, "y":7.33, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":13.52443, "ay":0.00444, "alpha":-0.00001, "fx":[153.36441,153.36441,153.36441,153.36441], "fy":[0.05036,0.05029,0.05029,0.05036]}, + {"t":0.03572, "x":3.60763, "y":7.33, "heading":3.14159, "vx":0.48303, "vy":0.00016, "omega":0.0, "ax":13.52266, "ay":0.00444, "alpha":-0.00001, "fx":[153.34437,153.34437,153.34437,153.34437], "fy":[0.05035,0.05028,0.05028,0.05035]}, + {"t":0.07143, "x":3.6335, "y":7.33001, "heading":3.14159, "vx":0.96599, "vy":0.00032, "omega":0.0, "ax":13.52045, "ay":0.00444, "alpha":-0.00001, "fx":[153.31929,153.31929,153.31929,153.31929], "fy":[0.05033,0.05026,0.05026,0.05033]}, + {"t":0.10715, "x":3.67662, "y":7.33003, "heading":3.14159, "vx":1.44887, "vy":0.00048, "omega":0.0, "ax":13.5176, "ay":0.00443, "alpha":-0.00001, "fx":[153.28698,153.28698,153.28698,153.28698], "fy":[0.05031,0.05024,0.05024,0.05031]}, + {"t":0.14286, "x":3.73699, "y":7.33005, "heading":3.14159, "vx":1.93165, "vy":0.00063, "omega":0.0, "ax":13.51379, "ay":0.00443, "alpha":-0.00001, "fx":[153.2438,153.2438,153.2438,153.2438], "fy":[0.05028,0.05021,0.05021,0.05028]}, + {"t":0.17858, "x":3.8146, "y":7.33007, "heading":3.14159, "vx":2.4143, "vy":0.00079, "omega":0.0, "ax":13.50844, "ay":0.00443, "alpha":-0.00001, "fx":[153.18314,153.18314,153.18314,153.18314], "fy":[0.05023,0.05017,0.05017,0.05023]}, + {"t":0.21429, "x":3.90944, "y":7.3301, "heading":3.14159, "vx":2.89675, "vy":0.00095, "omega":0.0, "ax":13.50038, "ay":0.00442, "alpha":-0.00001, "fx":[153.09175,153.09175,153.09175,153.09175], "fy":[0.05017,0.05011,0.05011,0.05017]}, + {"t":0.25001, "x":4.02151, "y":7.33014, "heading":3.14159, "vx":3.37892, "vy":0.00111, "omega":0.0, "ax":13.48686, "ay":0.00441, "alpha":0.0, "fx":[152.93839,152.93839,152.93839,152.93839], "fy":[0.05006,0.05001,0.05001,0.05006]}, + {"t":0.28572, "x":4.15079, "y":7.33018, "heading":3.14159, "vx":3.8606, "vy":0.00127, "omega":0.0, "ax":13.45949, "ay":0.00439, "alpha":0.0, "fx":[152.6281,152.6281,152.6281,152.6281], "fy":[0.04985,0.04981,0.04981,0.04985]}, + {"t":0.32144, "x":4.29726, "y":7.33023, "heading":3.14159, "vx":4.34131, "vy":0.00142, "omega":0.0, "ax":13.37528, "ay":0.00434, "alpha":0.0, "fx":[151.67317,151.67317,151.67317,151.67317], "fy":[0.04918,0.04918,0.04918,0.04918]}, + {"t":0.35715, "x":4.46084, "y":7.33028, "heading":3.14159, "vx":4.81901, "vy":0.00158, "omega":0.0, "ax":2.4059, "ay":-0.00155, "alpha":0.00005, "fx":[27.28254,27.28254,27.28227,27.28227], "fy":[-0.0177,-0.01742,-0.01742,-0.0177]}, + {"t":0.39287, "x":4.63448, "y":7.33034, "heading":3.14159, "vx":4.90494, "vy":0.00152, "omega":0.0, "ax":0.00067, "ay":-0.01304, "alpha":0.0, "fx":[0.00759,0.00759,0.00759,0.00759], "fy":[-0.1479,-0.1479,-0.1479,-0.1479]}, + {"t":0.42858, "x":4.80966, "y":7.33038, "heading":3.14159, "vx":4.90496, "vy":0.00106, "omega":0.0, "ax":-0.00006, "ay":-0.0793, "alpha":0.00032, "fx":[0.00019,0.00019,-0.00155,-0.00155], "fy":[-0.90017,-0.89843,-0.89843,-0.90017]}, + {"t":0.4643, "x":4.98484, "y":7.33037, "heading":3.14159, "vx":4.90496, "vy":-0.00178, "omega":0.00001, "ax":-0.20295, "ay":-0.48239, "alpha":0.84255, "fx":[-0.03737,-0.03745,-4.57028,-4.56073], "fy":[-7.73288,-3.21227,-3.20943,-7.72608]}, + {"t":0.50001, "x":5.1599, "y":7.33, "heading":3.14159, "vx":4.89771, "vy":-0.019, "omega":0.0301, "ax":-1.71118, "ay":-0.51364, "alpha":5.90156, "fx":[-3.76556,-3.82976,-35.26525,-34.75706], "fy":[-22.45885,10.52534,9.97875,-21.34355]}, + {"t":0.51918, "x":5.25348, "y":7.32954, "heading":-3.14102, "vx":4.8649, "vy":-0.02885, "omega":0.14325, "ax":-0.33049, "ay":-0.22443, "alpha":1.16319, "fx":[-0.62411,-0.62857,-6.87549,-6.86267], "fy":[-5.67222,0.58074,0.57598,-5.66458]}, + {"t":0.53835, "x":5.34669, "y":7.32895, "heading":-3.13827, "vx":4.85857, "vy":-0.03315, "omega":0.16555, "ax":-0.06501, "ay":-0.11332, "alpha":0.2212, "fx":[-0.14148,-0.14545,-1.3331,-1.32898], "fy":[-1.87682,-0.68931,-0.6932,-1.88062]}, + {"t":0.55753, "x":5.43982, "y":7.32829, "heading":-3.1351, "vx":4.85732, "vy":-0.03533, "omega":0.16979, "ax":-0.01812, "ay":-0.1274, "alpha":0.0523, "fx":[-0.06419,-0.06602,-0.34683,-0.345], "fy":[-1.58414,-1.30338,-1.3052,-1.58596]}, + {"t":0.5767, "x":5.53294, "y":7.32759, "heading":-3.13184, "vx":4.85697, "vy":-0.03777, "omega":0.17079, "ax":-0.05334, "ay":-0.27394, "alpha":0.16773, "fx":[-0.15003,-0.15885,-1.05975,-1.05072], "fy":[-3.55216,-2.65206,-2.66071,-3.56077]}, + {"t":0.59587, "x":5.62605, "y":7.32682, "heading":-3.12857, "vx":4.85595, "vy":-0.04302, "omega":0.17401, "ax":-0.29117, "ay":-0.66272, "alpha":0.9748, "fx":[-0.64498,-0.71566,-5.96719,-5.87925], "fy":[-10.10004,-4.87314,-4.93417,-10.15307]}, + {"t":0.61504, "x":5.71909, "y":7.32587, "heading":-3.12523, "vx":4.85037, "vy":-0.05573, "omega":0.19269, "ax":-1.35356, "ay":-1.56239, "alpha":4.53912, "fx":[-2.86361,-3.3967,-28.24326,-26.89276], "fy":[-30.12144,-5.45771,-5.68324,-29.60648]}, + {"t":0.63421, "x":5.81183, "y":7.32451, "heading":-3.12154, "vx":4.82442, "vy":-0.08568, "omega":0.27972, "ax":-3.78468, "ay":-2.98193, "alpha":12.24587, "fx":[-8.93509,-13.48744,-80.30685,-68.94075], "fy":[-73.56083,1.05273,-0.64208,-62.10782]}, + {"t":0.65338, "x":5.90363, "y":7.32232, "heading":-3.11617, "vx":4.75186, "vy":-0.14285, "omega":0.51449, "ax":-5.54805, "ay":-4.38199, "alpha":16.70124, "fx":[-12.30752,-31.35009,-116.44736,-91.55023], "fy":[-111.82337,0.13451,-2.54791,-84.52685]}, + {"t":0.67256, "x":5.99371, "y":7.31878, "heading":-3.10631, "vx":4.6455, "vy":-0.22686, "omega":0.83468, "ax":-6.27679, "ay":-5.89563, "alpha":16.87509, "fx":[-10.05354,-46.67336,-131.55728,-96.42624], "fy":[-130.75769,-26.47767,-11.99151,-98.19429]}, + {"t":0.69173, "x":6.08162, "y":7.31335, "heading":-3.09031, "vx":4.52516, "vy":-0.33989, "omega":1.15821, "ax":-6.56534, "ay":-7.46612, "alpha":15.09125, "fx":[-8.06819,-56.33317,-137.70664,-95.69077], "fy":[-139.6297,-68.11194,-23.80769,-107.10834]}, + {"t":0.7109, "x":6.16717, "y":7.30546, "heading":-3.0681, "vx":4.39929, "vy":-0.48303, "omega":1.44753, "ax":-6.81627, "ay":-8.56716, "alpha":13.16899, "fx":[-9.51025,-66.19571,-139.51647,-93.95821], "fy":[-144.01132,-95.87912,-35.93036,-112.77897]}, + {"t":0.73007, "x":6.25026, "y":7.29462, "heading":-3.04035, "vx":4.26861, "vy":-0.64727, "omega":1.70001, "ax":-7.1049, "ay":-9.18646, "alpha":11.63168, "fx":[-13.7868,-77.06311,-138.9774,-92.4454], "fy":[-146.20216,-107.06864,-47.04142,-116.3787]}, + {"t":0.74924, "x":6.33079, "y":7.28052, "heading":-3.00776, "vx":4.1324, "vy":-0.8234, "omega":1.92301, "ax":-7.38743, "ay":-9.53167, "alpha":10.40403, "fx":[-19.82001,-86.7013,-137.21939,-91.34743], "fy":[-147.08216,-109.94928,-56.61872,-118.69929]}, + {"t":0.76842, "x":6.40866, "y":7.26299, "heading":-2.97089, "vx":3.99077, "vy":-1.00613, "omega":2.12247, "ax":-7.63289, "ay":-9.73523, "alpha":9.39372, "fx":[-26.61314,-94.13901,-134.91168,-90.55814], "fy":[-147.06095,-109.61418,-64.64245,-120.26493]}, + {"t":0.78759, "x":6.48376, "y":7.24191, "heading":-2.9302, "vx":3.84443, "vy":-1.19278, "omega":2.30256, "ax":-7.8285, "ay":-9.86707, "alpha":8.54799, "fx":[-33.30496,-99.47023,-132.42845,-89.89113], "fy":[-146.44292,-108.3782,-71.30601,-121.43552]}, + {"t":0.80676, "x":6.55603, "y":7.21723, "heading":-2.88606, "vx":3.69435, "vy":-1.38194, "omega":2.46644, "ax":-7.97172, "ay":-9.96419, "alpha":7.84057, "fx":[-39.3019,-103.154,-129.95419,-89.1811], "fy":[-145.50954,-107.1431,-76.86805,-122.44732]}, + {"t":0.82593, "x":6.62539, "y":7.1889, "heading":-2.83877, "vx":3.54151, "vy":-1.57297, "omega":2.61676, "ax":-8.04971, "ay":-10.05408, "alpha":7.28616, "fx":[-43.87356,-105.50441,-127.57857,-88.17202], "fy":[-144.63048,-106.31623,-81.55133,-123.54756]}, + {"t":0.8451, "x":6.69181, "y":7.1569, "heading":-2.7886, "vx":3.38719, "vy":-1.76573, "omega":2.75645, "ax":-8.03225, "ay":-10.24004, "alpha":6.42094, "fx":[-49.57223,-104.86124,-123.29772,-86.60568], "fy":[-143.11074,-108.06542,-88.40351,-124.90071]}, + {"t":0.86854, "x":6.76899, "y":7.1127, "heading":-2.724, "vx":3.19893, "vy":-2.00573, "omega":2.90694, "ax":-8.37964, "ay":-9.71308, "alpha":7.5915, "fx":[-47.82111,-116.73211,-128.14181,-87.39893], "fy":[-142.92892,-93.41427,-80.39615,-123.83835]}, + {"t":0.89198, "x":6.84167, "y":7.06302, "heading":-2.65586, "vx":3.00253, "vy":-2.23339, "omega":3.08487, "ax":-8.74283, "ay":-8.82272, "alpha":9.95461, "fx":[-42.33929,-132.27058,-134.07165,-87.88647], "fy":[-143.26162,-65.56638,-68.65744,-122.70655]}, + {"t":0.91542, "x":6.90964, "y":7.00825, "heading":-2.58356, "vx":2.79761, "vy":-2.44017, "omega":3.31818, "ax":-9.04633, "ay":-6.95787, "alpha":14.8112, "fx":[-34.196,-144.64721,-141.26169,-90.22985], "fy":[-142.17611,-4.66172,-49.23609,-119.5299]}, + {"t":0.93885, "x":6.97272, "y":6.94915, "heading":-2.50579, "vx":2.58559, "vy":-2.60325, "omega":3.66533, "ax":-8.38723, "ay":-3.38657, "alpha":22.11522, "fx":[-19.11586,-112.69397,-147.50508,-101.12336], "fy":[-126.74995,88.798,-9.21064,-106.4498]}, + {"t":0.96229, "x":7.03102, "y":6.8872, "heading":-2.41988, "vx":2.38901, "vy":-2.68263, "omega":4.18366, "ax":-7.23979, "ay":6.86451, "alpha":18.55433, "fx":[3.77717,-63.58456,-131.62751,-136.95661], "fy":[133.68868,130.83272,62.77052,-15.92281]}, + {"t":0.98573, "x":7.08503, "y":6.82622, "heading":-2.32183, "vx":2.21932, "vy":-2.52174, "omega":4.61854, "ax":-8.6404, "ay":6.07148, "alpha":16.5162, "fx":[-32.52112,-82.95083,-138.45544,-137.99437], "fy":[131.18356,120.18293,47.88642,-23.85523]}, + {"t":1.00917, "x":7.13467, "y":6.76878, "heading":-2.21358, "vx":2.01681, "vy":-2.37943, "omega":5.00564, "ax":-9.55331, "ay":5.01925, "alpha":15.8662, "fx":[-55.12112,-98.20116,-143.69158,-136.3172], "fy":[125.05772,108.92072,31.65818,-37.96719]}, + {"t":1.03261, "x":7.17931, "y":6.71439, "heading":-2.09626, "vx":1.7929, "vy":-2.26179, "omega":5.37751, "ax":-10.0258, "ay":4.05678, "alpha":16.1171, "fx":[-66.27352,-109.36768,-146.76202,-132.35924], "fy":[121.38729,98.62232,16.30163,-52.29858]}, + {"t":1.05604, "x":7.21858, "y":6.66249, "heading":-1.97022, "vx":1.55792, "vy":-2.16671, "omega":5.75526, "ax":-11.15569, "ay":4.8927, "alpha":9.37579, "fx":[-98.34932,-118.94718,-145.22178,-143.49532], "fy":[104.71204,87.66709,27.13836,2.41156]}, + {"t":1.07948, "x":7.25203, "y":6.61305, "heading":-1.83533, "vx":1.29645, "vy":-2.05204, "omega":5.97501, "ax":-8.6391, "ay":8.42772, "alpha":-12.0038, "fx":[-143.2165,-131.69099,-22.70492,-94.25073], "fy":[48.91628,65.31314,149.09454,118.95104]}, + {"t":1.10292, "x":7.28005, "y":6.56727, "heading":-1.69529, "vx":1.09397, "vy":-1.85451, "omega":5.69367, "ax":-7.46273, "ay":8.99819, "alpha":-13.43156, "fx":[-148.97987,-98.33221,3.51064,-94.70234], "fy":[31.35666,104.93621,152.18353,119.67467]}, + {"t":1.12636, "x":7.30364, "y":6.52628, "heading":-1.56184, "vx":0.91906, "vy":-1.64361, "omega":5.37886, "ax":-4.36074, "ay":9.35327, "alpha":-19.50944, "fx":[-151.50624,49.13191,4.72045,-100.14612], "fy":[18.02327,138.04325,152.63932,115.55123]}, + {"t":1.14979, "x":7.32398, "y":6.49032, "heading":-1.43577, "vx":0.81685, "vy":-1.42439, "omega":4.9216, "ax":-3.65151, "ay":8.5429, "alpha":-23.90981, "fx":[-152.60358,93.23608,0.01653,-106.27861], "fy":[6.4023,118.00357,152.9486,110.14505]}, + {"t":1.17323, "x":7.34212, "y":6.45929, "heading":-1.32042, "vx":0.73127, "vy":-1.22416, "omega":4.3612, "ax":-3.78125, "ay":8.1128, "alpha":-25.45397, "fx":[-152.78141,99.5818,-6.2992,-112.0157], "fy":[-3.88592,114.48486,152.95498,104.43628]}, + {"t":1.19667, "x":7.35822, "y":6.43282, "heading":-1.2182, "vx":0.64264, "vy":-1.03401, "omega":3.76462, "ax":-4.04376, "ay":7.8217, "alpha":-26.16787, "fx":[-152.34516,98.61621,-12.70074,-116.99215], "fy":[-12.88141,116.10186,152.64408,98.92161]}, + {"t":1.22011, "x":7.37217, "y":6.41074, "heading":-1.12997, "vx":0.54787, "vy":-0.85069, "omega":3.1513, "ax":-4.30922, "ay":7.58863, "alpha":-26.55312, "fx":[-151.52956,95.76805,-18.56635,-121.13499], "fy":[-20.63281,118.87272,152.10226,93.87213]}, + {"t":1.24355, "x":7.38383, "y":6.39288, "heading":-1.05611, "vx":0.44687, "vy":-0.67283, "omega":2.52895, "ax":-4.54078, "ay":7.39195, "alpha":-26.78139, "fx":[-150.52115,92.63065,-23.6082,-124.46783], "fy":[-27.18818,121.57494,151.44533,89.461]}, + {"t":1.26698, "x":7.39306, "y":6.37914, "heading":-0.99683, "vx":0.34044, "vy":-0.49958, "omega":1.90125, "ax":-4.72564, "ay":7.22682, "alpha":-26.9309, "fx":[-149.47049,89.84941,-27.68483,-127.04559], "fy":[-32.58812,123.79849,150.78643,85.80636]}, + {"t":1.29042, "x":7.39974, "y":6.36942, "heading":-0.95227, "vx":0.22968, "vy":-0.3302, "omega":1.27005, "ax":-4.85925, "ay":7.09365, "alpha":-27.04382, "fx":[-148.50109,87.74104,-30.72493,-128.92709], "fy":[-36.85619,125.40533,150.22222,82.99122]}, + {"t":1.31386, "x":7.40379, "y":6.36363, "heading":-0.9225, "vx":0.11579, "vy":-0.16394, "omega":0.6362, "ax":-4.94035, "ay":6.99447, "alpha":-27.14395, "fx":[-147.71624,86.47881,-32.69123,-130.1617], "fy":[-39.98982,126.35163,149.82719,81.07482]}, + {"t":1.3373, "x":7.40514, "y":6.36171, "heading":-0.90759, "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/Depot_ToDepot.traj b/src/main/deploy/choreo/Depot_ToDepot.traj index b0faeba1..c1a351fe 100644 --- a/src/main/deploy/choreo/Depot_ToDepot.traj +++ b/src/main/deploy/choreo/Depot_ToDepot.traj @@ -3,8 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":2.599, "y":7.33, "heading":3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.3641765117645264, "y":5.963484287261963, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":2.599, "y":7.33, "heading":3.141592653589793, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2307121753692627, "y":5.964109897613525, "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}, @@ -13,12 +13,13 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"2.599 m", "val":2.599}, "y":{"exp":"7.33 m", "val":7.33}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"1.3641765117645264 m", "val":1.3641765117645264}, "y":{"exp":"5.963484287261963 m", "val":5.963484287261963}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"2.599 m", "val":2.599}, "y":{"exp":"7.33 m", "val":7.33}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2307121753692627 m", "val":1.2307121753692627}, "y":{"exp":"5.964109897613525 m", "val":5.964109897613525}, "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}], + {"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":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.4 m / s", "val":0.4}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -49,24 +50,25 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.73983], + "waypoints":[0.0,0.75837], "samples":[ - {"t":0.0, "x":2.599, "y":7.33, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.0702, "ay":-10.03752, "alpha":0.0, "fx":[-102.85431,-102.85431,-102.85431,-102.85431], "fy":[-113.82358,-113.82358,-113.82358,-113.82358]}, - {"t":0.04932, "x":2.58797, "y":7.31779, "heading":3.14159, "vx":-0.44736, "vy":-0.49507, "omega":0.0, "ax":-9.06889, "ay":-10.03607, "alpha":0.0, "fx":[-102.83943,-102.83943,-102.83943,-102.83943], "fy":[-113.80711,-113.80711,-113.80711,-113.80711]}, - {"t":0.09864, "x":2.55487, "y":7.28117, "heading":3.14159, "vx":-0.89466, "vy":-0.99007, "omega":0.0, "ax":-9.06705, "ay":-10.03404, "alpha":0.0, "fx":[-102.81858,-102.81858,-102.81858,-102.81858], "fy":[-113.78404,-113.78404,-113.78404,-113.78404]}, - {"t":0.14797, "x":2.49972, "y":7.22013, "heading":3.14159, "vx":-1.34186, "vy":-1.48497, "omega":0.0, "ax":-9.06429, "ay":-10.03098, "alpha":0.0, "fx":[-102.78728,-102.78728,-102.78728,-102.78728], "fy":[-113.7494,-113.7494,-113.7494,-113.7494]}, - {"t":0.19729, "x":2.42251, "y":7.13469, "heading":3.14159, "vx":-1.78893, "vy":-1.97972, "omega":0.0, "ax":-9.05968, "ay":-10.02588, "alpha":0.0, "fx":[-102.73502,-102.73502,-102.73502,-102.73502], "fy":[-113.69156,-113.69156,-113.69156,-113.69156]}, - {"t":0.24661, "x":2.32325, "y":7.02485, "heading":3.14159, "vx":-2.23578, "vy":-2.47422, "omega":0.0, "ax":-9.05044, "ay":-10.01565, "alpha":0.0, "fx":[-102.63022,-102.63022,-102.63022,-102.63022], "fy":[-113.57559,-113.57559,-113.57559,-113.57559]}, - {"t":0.29593, "x":2.20197, "y":6.89063, "heading":3.14159, "vx":-2.68216, "vy":-2.96821, "omega":0.0, "ax":-9.02258, "ay":-9.98482, "alpha":0.0, "fx":[-102.31433,-102.31433,-102.31433,-102.31433], "fy":[-113.22601,-113.22601,-113.22601,-113.22601]}, - {"t":0.34526, "x":2.05871, "y":6.73209, "heading":3.14159, "vx":-3.12718, "vy":-3.46069, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}, - {"t":0.39458, "x":1.90447, "y":6.5614, "heading":3.14159, "vx":-3.12718, "vy":-3.46069, "omega":0.0, "ax":9.02258, "ay":9.98482, "alpha":0.0, "fx":[102.31433,102.31433,102.31433,102.31433], "fy":[113.22601,113.22601,113.22601,113.22601]}, - {"t":0.4439, "x":1.7612, "y":6.40285, "heading":3.14159, "vx":-2.68216, "vy":-2.96821, "omega":0.0, "ax":9.05044, "ay":10.01565, "alpha":0.0, "fx":[102.63022,102.63022,102.63022,102.63022], "fy":[113.57559,113.57559,113.57559,113.57559]}, - {"t":0.49322, "x":1.63992, "y":6.26864, "heading":3.14159, "vx":-2.23578, "vy":-2.47422, "omega":0.0, "ax":9.05968, "ay":10.02588, "alpha":0.0, "fx":[102.73502,102.73502,102.73502,102.73502], "fy":[113.69156,113.69156,113.69156,113.69156]}, - {"t":0.54254, "x":1.54067, "y":6.1588, "heading":3.14159, "vx":-1.78893, "vy":-1.97972, "omega":0.0, "ax":9.06429, "ay":10.03098, "alpha":0.0, "fx":[102.78728,102.78728,102.78728,102.78728], "fy":[113.7494,113.7494,113.7494,113.7494]}, - {"t":0.59187, "x":1.46346, "y":6.07336, "heading":3.14159, "vx":-1.34186, "vy":-1.48497, "omega":0.0, "ax":9.06705, "ay":10.03404, "alpha":0.0, "fx":[102.81858,102.81858,102.81858,102.81858], "fy":[113.78404,113.78404,113.78404,113.78404]}, - {"t":0.64119, "x":1.4083, "y":6.01232, "heading":3.14159, "vx":-0.89466, "vy":-0.99007, "omega":0.0, "ax":9.06889, "ay":10.03607, "alpha":0.0, "fx":[102.83943,102.83943,102.83943,102.83943], "fy":[113.80711,113.80711,113.80711,113.80711]}, - {"t":0.69051, "x":1.37521, "y":5.97569, "heading":3.14159, "vx":-0.44736, "vy":-0.49507, "omega":0.0, "ax":9.0702, "ay":10.03752, "alpha":0.0, "fx":[102.85431,102.85431,102.85431,102.85431], "fy":[113.82358,113.82358,113.82358,113.82358]}, - {"t":0.73983, "x":1.36418, "y":5.96348, "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]}], + {"t":0.0, "x":2.599, "y":7.33, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.57413, "ay":-9.55736, "alpha":0.0, "fx":[-108.56884,-108.56884,-108.56884,-108.56884], "fy":[-108.37859,-108.37859,-108.37859,-108.37859]}, + {"t":0.0474, "x":2.58825, "y":7.31926, "heading":3.14159, "vx":-0.4538, "vy":-0.453, "omega":0.0, "ax":-9.5727, "ay":-9.55592, "alpha":0.0, "fx":[-108.55254,-108.55254,-108.55254,-108.55254], "fy":[-108.36231,-108.36231,-108.36231,-108.36231]}, + {"t":0.0948, "x":2.55598, "y":7.28706, "heading":3.14159, "vx":-0.90753, "vy":-0.90594, "omega":0.0, "ax":-9.57068, "ay":-9.55391, "alpha":0.0, "fx":[-108.52973,-108.52973,-108.52973,-108.52973], "fy":[-108.33954,-108.33954,-108.33954,-108.33954]}, + {"t":0.14219, "x":2.50222, "y":7.23339, "heading":3.14159, "vx":-1.36116, "vy":-1.35877, "omega":0.0, "ax":-9.56767, "ay":-9.5509, "alpha":0.0, "fx":[-108.49553,-108.49553,-108.49553,-108.49553], "fy":[-108.30541,-108.30541,-108.30541,-108.30541]}, + {"t":0.18959, "x":2.42695, "y":7.15826, "heading":3.14159, "vx":-1.81465, "vy":-1.81147, "omega":0.0, "ax":-9.56265, "ay":-9.54589, "alpha":0.0, "fx":[-108.43862,-108.43862,-108.43862,-108.43862], "fy":[-108.2486,-108.2486,-108.2486,-108.2486]}, + {"t":0.23699, "x":2.3302, "y":7.06167, "heading":3.14159, "vx":-2.2679, "vy":-2.26393, "omega":0.0, "ax":-9.55264, "ay":-9.5359, "alpha":0.0, "fx":[-108.32513,-108.32513,-108.32513,-108.32513], "fy":[-108.1353,-108.1353,-108.1353,-108.1353]}, + {"t":0.28439, "x":2.21198, "y":6.94365, "heading":3.14159, "vx":-2.72068, "vy":-2.71591, "omega":0.0, "ax":-9.52289, "ay":-9.5062, "alpha":0.0, "fx":[-107.98774,-107.98774,-107.98774,-107.98774], "fy":[-107.7985,-107.7985,-107.7985,-107.7985]}, + {"t":0.33179, "x":2.07232, "y":6.80425, "heading":3.14159, "vx":-3.17205, "vy":-3.16649, "omega":0.0, "ax":-6.33629, "ay":-6.32519, "alpha":0.0, "fx":[-71.85235,-71.85235,-71.85235,-71.85235], "fy":[-71.72644,-71.72644,-71.72644,-71.72644]}, + {"t":0.37919, "x":1.91486, "y":6.64705, "heading":3.14159, "vx":-3.47238, "vy":-3.46629, "omega":0.0, "ax":6.33629, "ay":6.32519, "alpha":0.0, "fx":[71.85235,71.85235,71.85235,71.85235], "fy":[71.72644,71.72644,71.72644,71.72644]}, + {"t":0.42658, "x":1.75739, "y":6.48986, "heading":3.14159, "vx":-3.17205, "vy":-3.16649, "omega":0.0, "ax":9.52289, "ay":9.5062, "alpha":0.0, "fx":[107.98774,107.98774,107.98774,107.98774], "fy":[107.7985,107.7985,107.7985,107.7985]}, + {"t":0.47398, "x":1.61774, "y":6.35046, "heading":3.14159, "vx":-2.72068, "vy":-2.71591, "omega":0.0, "ax":9.55264, "ay":9.5359, "alpha":0.0, "fx":[108.32513,108.32513,108.32513,108.32513], "fy":[108.1353,108.1353,108.1353,108.1353]}, + {"t":0.52138, "x":1.49951, "y":6.23244, "heading":3.14159, "vx":-2.2679, "vy":-2.26393, "omega":0.0, "ax":9.56265, "ay":9.54589, "alpha":0.0, "fx":[108.43862,108.43862,108.43862,108.43862], "fy":[108.2486,108.2486,108.2486,108.2486]}, + {"t":0.56878, "x":1.40276, "y":6.13585, "heading":3.14159, "vx":-1.81465, "vy":-1.81147, "omega":0.0, "ax":9.56767, "ay":9.5509, "alpha":0.0, "fx":[108.49553,108.49553,108.49553,108.49553], "fy":[108.30541,108.30541,108.30541,108.30541]}, + {"t":0.61618, "x":1.32749, "y":6.06072, "heading":3.14159, "vx":-1.36116, "vy":-1.35877, "omega":0.0, "ax":9.57068, "ay":9.55391, "alpha":0.0, "fx":[108.52973,108.52973,108.52973,108.52973], "fy":[108.33954,108.33954,108.33954,108.33954]}, + {"t":0.66358, "x":1.27373, "y":6.00705, "heading":3.14159, "vx":-0.90753, "vy":-0.90594, "omega":0.0, "ax":9.5727, "ay":9.55592, "alpha":0.0, "fx":[108.55254,108.55254,108.55254,108.55254], "fy":[108.36231,108.36231,108.36231,108.36231]}, + {"t":0.71097, "x":1.24147, "y":5.97485, "heading":3.14159, "vx":-0.4538, "vy":-0.453, "omega":0.0, "ax":9.57413, "ay":9.55736, "alpha":0.0, "fx":[108.56884,108.56884,108.56884,108.56884], "fy":[108.37859,108.37859,108.37859,108.37859]}, + {"t":0.75837, "x":1.23071, "y":5.96411, "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/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4b05d7bb..eee21fca 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,10 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.autochooser.FieldLocation; +import frc.robot.commands.auto.ClearAutonomousCommand; import frc.robot.constants.Constants; +import frc.robot.utils.BlinkinPattern; +import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.utils.diag.Diagnostics; import frc.robot.utils.logging.TimeoutLogger; import frc.robot.utils.logging.commands.CommandLogger; @@ -89,6 +92,11 @@ public Robot() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. robotContainer = new RobotContainer(); + if(Constants.DEBUG){ + SmartDashboard.putData( + "Auto/Clear Autonomous Command", + new ClearAutonomousCommand(this::clearAutonomousCommand, robotContainer.getShootingState())); + } } public static RobotMode getMode() { @@ -129,7 +137,7 @@ public void robotPeriodic() { if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { allianceColor = DriverStation.getAlliance(); } - + SmartDashboard.putBoolean("Hub Active?", hubActive()); if (Constants.DEBUG) { SmartDashboard.putNumber("driverXbox.getLeftY()", driverXbox.getLeftY()); SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); @@ -139,7 +147,6 @@ public void robotPeriodic() { SmartDashboard.putNumber("Robot Y", robotContainer.getDriveBase().getPose().getY()); // Puts data on the elastic dashboard SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); - SmartDashboard.putBoolean("Hub Active?", hubActive()); if (Constants.currentMode == Constants.Mode.SIM) { Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get()); Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationPose().get()); @@ -159,6 +166,7 @@ public void disabledInit() { @Override public void disabledPeriodic() { + robotContainer.getLightStrip().setPattern(BlinkinPattern.RAINBOW_LAVA_PALETTE); } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @@ -201,6 +209,13 @@ public void teleopInit() { } } + private void clearAutonomousCommand() { + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + autonomousCommand = null; + } + /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { @@ -319,4 +334,4 @@ public FieldLocation location() { public Pose2d getStartingLocation() { return location().getLocation(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 967bf250..f6be4b07 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -102,6 +103,9 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); + + private final PowerDistribution powerDistribution = new PowerDistribution(); + private final ClimberSubsystem climberSubsystem; private final AnglerSubsystem anglerSubsystem; private final IntakeSubsystem intakeSubsystem; @@ -155,7 +159,8 @@ public RobotContainer() { apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase,intakeDeployer, this) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); - } + powerDistribution.setSwitchableChannel(true); + } case REPLAY -> { anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createMockIo()); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo()); @@ -568,4 +573,7 @@ public ShootingState getShootingState() { public IntakeDeployerSubsystem getDeployer(){ return intakeDeployer; } + public LightStripSubsystem getLightStrip() { + return lightStripSubsystem; + } } diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index d2d1e746..ce806027 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -8,9 +8,9 @@ public enum AutoAction { DO_NOTHING("Do Nothing"), SHOOT("Shoot"), - SHOOT_PICKUP("Shoot and Pickup"), - DISTURBANCE("Disturbance"), - NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"), + //SHOOT_PICKUP("Shoot and Pickup"), + //DISTURBANCE("Disturbance"), + //NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"), 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 27993c4e..eb5267f1 100644 --- a/src/main/java/frc/robot/autochooser/AutoChooser.java +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -13,11 +13,8 @@ import frc.robot.commands.auto.neutral.OutpostNeutral; import frc.robot.commands.auto.disturbance.DepotDisturbance; import frc.robot.commands.auto.disturbance.OutpostDisturbance; -import frc.robot.commands.auto.shoot.DepotShoot; -import frc.robot.commands.auto.shoot.MidShoot; -import frc.robot.commands.auto.shoot.OutpostShoot; +import frc.robot.commands.auto.shoot.Shoot; import frc.robot.commands.auto.shootpickup.DepotShootPickup; -import frc.robot.commands.auto.shootpickup.OutpostShootPickup; import frc.robot.commands.auto.DoNothing; import frc.robot.constants.enums.ShootingState; import frc.robot.subsystems.AnglerSubsystem; @@ -114,18 +111,18 @@ private void populateCommandMap() { //shoot commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.DEPOT_SIDE), - new DepotShoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler)); + new Shoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.MID), - new MidShoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler)); + new Shoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); commandMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE), - new OutpostShoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler)); + new Shoot(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); + /* //shoot-pickup commandMap.put(new AutoEvent(AutoAction.SHOOT_PICKUP, FieldLocation.DEPOT_SIDE), new DepotShootPickup(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, controller, intake)); - commandMap.put(new AutoEvent(AutoAction.SHOOT_PICKUP, FieldLocation.OUTPOST_SIDE), - new OutpostShootPickup(drivetrain, auto, shooter, shootstate, hopper, feeder, turret, angler, intake, controller)); + //disturbance commandMap.put(new AutoEvent(AutoAction.DISTURBANCE, FieldLocation.DEPOT_SIDE), new DepotDisturbance(drivetrain, auto, shootstate, turret, angler, controller)); @@ -137,6 +134,7 @@ private void populateCommandMap() { new DepotNeutral(drivetrain, auto, shootstate, turret, angler, controller)); commandMap.put(new AutoEvent(AutoAction.NEUTRAL_ZONE, FieldLocation.OUTPOST_SIDE), new OutpostNeutral(drivetrain, auto, shootstate, turret, angler, controller)); + */ } @@ -157,11 +155,10 @@ private void populateDescriptionMap() { "shoot from the middle"); descriptionMap.put(new AutoEvent(AutoAction.SHOOT, FieldLocation.OUTPOST_SIDE), "shoot from the outpost"); - + /* descriptionMap.put(new AutoEvent(AutoAction.SHOOT_PICKUP, FieldLocation.DEPOT_SIDE), "shoot and pickup from the depot"); - descriptionMap.put(new AutoEvent(AutoAction.SHOOT_PICKUP, FieldLocation.OUTPOST_SIDE), - "shoot and pickup from the outpost"); + descriptionMap.put(new AutoEvent(AutoAction.DISTURBANCE, FieldLocation.DEPOT_SIDE), "run the depot disturbance route"); @@ -172,6 +169,7 @@ private void populateDescriptionMap() { "shoot and run depot neutral zone cycle"); descriptionMap.put(new AutoEvent(AutoAction.NEUTRAL_ZONE, FieldLocation.OUTPOST_SIDE), "shoot and run outpost neutral zone cycle"); + */ } public AutoEvent getSelectedEvent() { diff --git a/src/main/java/frc/robot/commands/auto/AutoReset.java b/src/main/java/frc/robot/commands/auto/AutoReset.java index ef9d38e5..95d9fdbe 100644 --- a/src/main/java/frc/robot/commands/auto/AutoReset.java +++ b/src/main/java/frc/robot/commands/auto/AutoReset.java @@ -1,11 +1,8 @@ package frc.robot.commands.auto; import frc.robot.commands.angler.RunAnglerToReverseLimit; -import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.RunTurretToRevLimit; -import frc.robot.commands.turret.SetTurretAngle; import frc.robot.constants.enums.ShootingState; -import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; diff --git a/src/main/java/frc/robot/commands/auto/AutoShoot.java b/src/main/java/frc/robot/commands/auto/AutoShoot.java index b8896a9b..b80f8880 100644 --- a/src/main/java/frc/robot/commands/auto/AutoShoot.java +++ b/src/main/java/frc/robot/commands/auto/AutoShoot.java @@ -8,9 +8,7 @@ import frc.robot.utils.logging.commands.LoggableWaitCommand; public class AutoShoot extends LoggableRaceCommandGroup{ - public AutoShoot(HopperSubsystem hopperSubsystem, FeederSubsystem feederSubsystem, double time){ - super( new SpinHopper(hopperSubsystem), new SpinFeeder(feederSubsystem), diff --git a/src/main/java/frc/robot/commands/auto/ClearAutonomousCommand.java b/src/main/java/frc/robot/commands/auto/ClearAutonomousCommand.java new file mode 100644 index 00000000..df759c14 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/ClearAutonomousCommand.java @@ -0,0 +1,18 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.commands.shooter.SetShootingState; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; + +public class ClearAutonomousCommand extends InstantCommand { + public ClearAutonomousCommand(Runnable clearAutonomousCommand, ShootingState shootstate) { + super(clearAutonomousCommand); + new SetShootingState(shootstate, ShootState.STOPPED); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/auto/shoot/MidShoot.java b/src/main/java/frc/robot/commands/auto/shoot/MidShoot.java deleted file mode 100644 index ef551dad..00000000 --- a/src/main/java/frc/robot/commands/auto/shoot/MidShoot.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.robot.commands.auto.shoot; - -import choreo.auto.AutoFactory; -import frc.robot.commands.auto.AutoReset; -import frc.robot.commands.auto.AutoShoot; -import frc.robot.commands.drive.DriveSwerve; -import frc.robot.commands.shooter.SetShootingState; -import frc.robot.commands.turret.RunTurretToRevLimit; -import frc.robot.constants.enums.DriveDirection; -import frc.robot.constants.enums.ShootingState; -import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.AnglerSubsystem; -import frc.robot.subsystems.FeederSubsystem; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.TurretSubsystem; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -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, TurretSubsystem turret, AnglerSubsystem angler) { - super( - new AutoReset(shootstate, turret, angler), - new LoggableParallelCommandGroup( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), - new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 2, 0.5) - ), - new AutoShoot(hopper, feeder, 5), - new SetShootingState(shootstate, ShootState.STOPPED) - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/shoot/OutpostShoot.java b/src/main/java/frc/robot/commands/auto/shoot/OutpostShoot.java deleted file mode 100644 index 1028cd7f..00000000 --- a/src/main/java/frc/robot/commands/auto/shoot/OutpostShoot.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.robot.commands.auto.shoot; - -import choreo.auto.AutoFactory; -import frc.robot.commands.auto.AutoReset; -import frc.robot.commands.auto.AutoShoot; -import frc.robot.commands.drive.DriveSwerve; -import frc.robot.commands.shooter.SetShootingState; -import frc.robot.commands.turret.RunTurretToRevLimit; -import frc.robot.constants.enums.DriveDirection; -import frc.robot.constants.enums.ShootingState; -import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.AnglerSubsystem; -import frc.robot.subsystems.FeederSubsystem; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.TurretSubsystem; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; -import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; - -public class OutpostShoot extends LoggableSequentialCommandGroup{ - public OutpostShoot( - SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, - HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler) { - super( - new AutoReset(shootstate, turret, angler), - new LoggableParallelCommandGroup( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), - new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 2, 0.5) - ), - new AutoShoot(hopper, feeder, 5), - new SetShootingState(shootstate, ShootState.STOPPED) - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/shoot/DepotShoot.java b/src/main/java/frc/robot/commands/auto/shoot/Shoot.java similarity index 58% rename from src/main/java/frc/robot/commands/auto/shoot/DepotShoot.java rename to src/main/java/frc/robot/commands/auto/shoot/Shoot.java index 3119529e..28a46dda 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/DepotShoot.java +++ b/src/main/java/frc/robot/commands/auto/shoot/Shoot.java @@ -4,31 +4,38 @@ import frc.robot.commands.auto.AutoReset; import frc.robot.commands.auto.AutoShoot; import frc.robot.commands.drive.DriveSwerve; +import frc.robot.commands.intakeDeployment.ToggleDeployment; import frc.robot.commands.shooter.SetShootingState; import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.constants.enums.DriveDirection; import frc.robot.constants.enums.ShootingState; import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.AnglerSubsystem; +import frc.robot.subsystems.ControllerSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeDeployerSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.TurretSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.utils.logging.commands.LoggableWaitCommand; -public class DepotShoot extends LoggableSequentialCommandGroup{ - public DepotShoot( +public class Shoot extends LoggableSequentialCommandGroup{ + public Shoot( SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, - HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler) { + HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, + ControllerSubsystem controller, IntakeDeployerSubsystem intakeDeployer) { super( new AutoReset(shootstate, turret, angler), - new LoggableParallelCommandGroup( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), - new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 2, 0.5) - ), - new AutoShoot(hopper, feeder, 5), + new LoggableWaitCommand(2), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB), + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 3, 0.5), + new ToggleDeployment(intakeDeployer, controller), //initial fuel falls in + new LoggableWaitCommand(4), + new ToggleDeployment(intakeDeployer, controller), + new LoggableWaitCommand(2), new SetShootingState(shootstate, ShootState.STOPPED) ); } diff --git a/src/main/java/frc/robot/commands/auto/shootpickup/DepotShootPickup.java b/src/main/java/frc/robot/commands/auto/shootpickup/DepotShootPickup.java index df34945d..6dde485c 100644 --- a/src/main/java/frc/robot/commands/auto/shootpickup/DepotShootPickup.java +++ b/src/main/java/frc/robot/commands/auto/shootpickup/DepotShootPickup.java @@ -15,6 +15,7 @@ import frc.robot.utils.logging.commands.LoggableCommandWrapper; import frc.robot.utils.logging.commands.LoggableParallelCommandGroup; import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup; +import frc.robot.utils.logging.commands.LoggableWaitCommand; public class DepotShootPickup extends LoggableSequentialCommandGroup { public DepotShootPickup( @@ -22,24 +23,28 @@ public DepotShootPickup( HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, ControllerSubsystem controller, IntakeDeployerSubsystem intake) { super( + new AutoReset(shootstate, turret, angler), + new LoggableWaitCommand(2), + new LoggableParallelCommandGroup( + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 2, 0.5), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB) + ), + new ToggleDeployment(intake, controller), //initial fuel falls in + new LoggableWaitCommand(4), + new ToggleDeployment(intake, controller), + new LoggableWaitCommand(2), + new SetShootingState(shootstate, ShootState.STOPPED), //shoot new AutoReset(shootstate, turret, angler), new LoggableParallelCommandGroup( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), - new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 2, 0.5) + LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_ToDepot")), + LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_ToDepot").withTimeout(2)), + new SetShootingState(shootstate, ShootState.SHOOTING_HUB) ), - new AutoShoot(hopper, feeder, 5), - - LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_ToDepot")), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_ToDepot").withTimeout(1)), //0.7 s - - //pickup and shoot - LoggableCommandWrapper.wrap(auto.resetOdometry("Depot_Pickup")), - new LoggableParallelCommandGroup( - LoggableCommandWrapper.wrap(auto.trajectoryCmd("Depot_Pickup").withTimeout(1)), //0.7 s - new AutoShoot(hopper, feeder, 5), - new ToggleDeployment(intake, controller) - ) + new ToggleDeployment(intake, controller), + new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 5, 0.2), + new LoggableWaitCommand(3), + new ToggleDeployment(intake, controller) ); } } diff --git a/src/main/java/frc/robot/commands/auto/shootpickup/OutpostShootPickup.java b/src/main/java/frc/robot/commands/auto/shootpickup/OutpostShootPickup.java deleted file mode 100644 index d5289dc5..00000000 --- a/src/main/java/frc/robot/commands/auto/shootpickup/OutpostShootPickup.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.commands.auto.shootpickup; - -import choreo.auto.AutoFactory; -import frc.robot.commands.auto.AutoReset; -import frc.robot.commands.auto.AutoShoot; -import frc.robot.commands.drive.DriveSwerve; -import frc.robot.commands.intakeDeployment.ToggleDeployment; -import frc.robot.commands.shooter.SetShootingState; -import frc.robot.commands.turret.RunTurretToRevLimit; -import frc.robot.constants.enums.DriveDirection; -import frc.robot.constants.enums.ShootingState; -import frc.robot.constants.enums.ShootingState.ShootState; -import frc.robot.subsystems.*; -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; -import frc.robot.utils.logging.commands.LoggableWaitCommand; - -public class OutpostShootPickup extends LoggableSequentialCommandGroup{ - public OutpostShootPickup( - SwerveSubsystem drivetrain, AutoFactory auto, ShooterSubsystem shooter, ShootingState shootstate, - HopperSubsystem hopper, FeederSubsystem feeder, TurretSubsystem turret, AnglerSubsystem angler, - IntakeDeployerSubsystem intake, ControllerSubsystem controller) { - super( - new AutoReset(shootstate, turret, angler), - new LoggableParallelCommandGroup( - new SetShootingState(shootstate, ShootState.SHOOTING_HUB), - new DriveSwerve(drivetrain, DriveDirection.BACKWARD, 2, 0.5) - ), - new AutoShoot(hopper, feeder, 5), - - LoggableCommandWrapper.wrap(auto.resetOdometry("Outpost_ToOutpost")), - LoggableCommandWrapper.wrap(auto.trajectoryCmd("Outpost_ToDepot").withTimeout(1.9)), - - new LoggableWaitCommand(2), - - LoggableCommandWrapper.wrap(auto.resetOdometry("Outpost_Pickup")), - new LoggableParallelCommandGroup( - LoggableCommandWrapper.wrap(auto.trajectoryCmd("Outpost_Pickup").withTimeout(1.9)), - new ToggleDeployment(intake, controller), - new AutoShoot(hopper, feeder, 5) - ) - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/lightStrip/SetLed.java b/src/main/java/frc/robot/commands/lightStrip/SetLed.java index 4f39c8d9..4158e718 100644 --- a/src/main/java/frc/robot/commands/lightStrip/SetLed.java +++ b/src/main/java/frc/robot/commands/lightStrip/SetLed.java @@ -11,12 +11,10 @@ public class SetLed extends LoggableCommand{ private final LightStripSubsystem lightStrip; - private final SwerveSubsystem drivebase; private ShootingState shootingState; - private double x; - private double y; + private SwerveSubsystem drivebase; - public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState) { + public SetLed(LightStripSubsystem lightStrip, ShootingState shootingState, SwerveSubsystem drivebase) { this.lightStrip = lightStrip; this.drivebase = drivebase; @@ -32,9 +30,8 @@ public void initialize() { @Override public void execute() { - - x = drivebase.getPose().getX(); - y = drivebase.getPose().getY(); + double x = drivebase.getPose().getX(); + double y = drivebase.getPose().getY(); // If statement checks if the robot is near the trench if (x > Trench.RED_BOTTOM_LOWER.getX() && x < Trench.RED_BOTTOM_HIGHER.getX() && y > Trench.RED_BOTTOM_LOWER.getY() && y < Trench.RED_BOTTOM_HIGHER.getY() || @@ -47,16 +44,15 @@ public void execute() { } } else { - switch (shootingState.getShootState()) { case STOPPED: - lightStrip.setPattern(BlinkinPattern.WHITE); + lightStrip.setPattern(BlinkinPattern.BLACK); break; case FIXED: - lightStrip.setPattern(BlinkinPattern.COLOR_WAVES_OCEAN_PALETTE); + lightStrip.setPattern(BlinkinPattern.AQUA); break; case FIXED_2: - lightStrip.setPattern(BlinkinPattern.RED_ORANGE); + lightStrip.setPattern(BlinkinPattern.AQUA); break; case SHOOTING_HUB: lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE); @@ -67,7 +63,6 @@ public void execute() { } } - } @Override diff --git a/src/main/java/frc/robot/constants/ConstantsReal2026.java b/src/main/java/frc/robot/constants/ConstantsReal2026.java index 78ab0267..f63ba37b 100644 --- a/src/main/java/frc/robot/constants/ConstantsReal2026.java +++ b/src/main/java/frc/robot/constants/ConstantsReal2026.java @@ -22,7 +22,7 @@ public class ConstantsReal2026 extends GameConstants { public static final double INITIAL_ROBOT_HEIGHT = 0; public static final int INTAKE_DIGITAL_INPUT_CHANNEL = 0; - public static final int LIGHT_STRIP_CHANNEL = 1; // Example ID, may change later + public static final int LIGHT_STRIP_CHANNEL = 0; public static final double GYRO_DIAGS_ANGLE = 30; diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 8d488b33..394e0803 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -100,12 +100,12 @@ public enum Mode { public static final double ANGLER_FF = 0.0; public static final double ANGLER_HOME_ROTATIONS = 0.0; public static final double ANGLER_ENCODER_LOW = 0; //Lowest encoder position of Angler - public static final double ANGLER_ENCODER_HIGH = 250; //Highest encoder position of Angler - public static final double ANGLER_ANGLE_LOW = 17; //Lowest angle position of Angler - public static final double ANGLER_ANGLE_HIGH = 45; //Highest angle position of Angler + public static final double ANGLER_ENCODER_HIGH = 272; //Highest encoder position of Angler + public static final double ANGLER_ANGLE_LOW = 16; //Lowest angle position of Angler + public static final double ANGLER_ANGLE_HIGH = 37; //Highest angle position of Angler public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState - public static final double ANGLER_LIMIT_SPEED = 0.2; + public static final double ANGLER_LIMIT_SPEED = 0.3; // turret (pan angle) PID @@ -113,7 +113,7 @@ public enum Mode { public static final double TURRET_SHORT_RANGE_I = 0.000000; public static final double TURRET_SHORT_RANGE_D = 0.001; public static final double TURRET_SHORT_RANGE_FF = 0.0; - public static final double TURRET_LONG_RANGE_P = 1.2; + public static final double TURRET_LONG_RANGE_P = 1; public static final double TURRET_LONG_RANGE_I = 0.000000; public static final double TURRET_LONG_RANGE_D = 0.0; public static final double TURRET_LONG_RANGE_FF = 0.0; @@ -122,9 +122,9 @@ public enum Mode { public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward public static final double TURRET_MIN_ANGLE = -92; public static final double TURRET_MAX_ANGLE = 92; - public static final double TURRET_LIMIT_SPEED = 0.1; + public static final double TURRET_LIMIT_SPEED = 0.2; public static final double TURRET_OUT_OF_RANGE_FLOP_RPM = -1500.0; - public static final double TURRET_PID_DISTANCE_THRESHOLD = 5; + public static final double TURRET_PID_DISTANCE_THRESHOLD = 10; //Minimum target encoder distance needed to use the longer pid slot diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 6158ba3c..757d4bf4 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -28,7 +28,7 @@ public class ControllerSubsystem extends SubsystemBase { private static final double STOP_DELAY_SECONDS = 0.5; - private static final double SHOOT_DELAY_SECONDS = 0.5; + private static final double SHOOT_DELAY_SECONDS = 1.5; // Placeholder target poses until real field target values are finalized private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, @@ -59,9 +59,9 @@ public class ControllerSubsystem extends SubsystemBase { 14.0); private static final PoseControlProfile RED_HUB_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 32.0, 230.0, 14.0); - private static final PoseControlProfile RED_SHUTTLE_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 45.0, 90.0, + private static final PoseControlProfile RED_SHUTTLE_PROFILE = new PoseControlProfile(RED_HUB_TARGET_POSE, 37.0, 90.0, -14.0); - private static final PoseControlProfile BLUE_SHUTTLE_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 45.0, 90.0, + private static final PoseControlProfile BLUE_SHUTTLE_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 37.0, 90.0, -14.0); private final SwerveSubsystem drivebase; @@ -205,6 +205,7 @@ private void updateStoppedTargets() { } private void useShotTargets(ShotTargets shotTargets) { + double shooterVelocityRpm = shotTargets.shooterVelocityRpm; if (isTurretTargetOutOfRange(shotTargets.turretAngleDegrees) && shooterVelocityRpm != 0.0) { shooterVelocityRpm = Constants.TURRET_OUT_OF_RANGE_FLOP_RPM; @@ -225,15 +226,15 @@ private void useShotTargets(ShotTargets shotTargets) { } else { activeTargets = new ShotTargets( - activeTargets.anglerAngleDegrees, + shotTargets.anglerAngleDegrees, shooterVelocityRpm, // Shooter starts half a second before everything else - activeTargets.turretAngleDegrees, - activeTargets.distanceMeters, + shotTargets.turretAngleDegrees, + shotTargets.distanceMeters, false, false, activeTargets.intakeDeploy); - } + } } @@ -301,7 +302,7 @@ private double calculateShooterVelocity(ShootState state, double computedDistanc if (state == ShootState.SHOOTING_HUB) { return (8.46 * distance * distance - 237 * distance - - 1380); + - 1380) * 0.9; }else if(state == ShootState.SHUTTLING){ return (((-distance*distance) - 5 * distance) - 2800); } diff --git a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java index 880cce68..de7bb97f 100644 --- a/src/main/java/frc/robot/subsystems/LightStripSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LightStripSubsystem.java @@ -16,7 +16,7 @@ public class LightStripSubsystem extends SubsystemBase{ public LightStripSubsystem(SwerveSubsystem drivebase, ShootingState shootingState) { this.io = new Spark(Constants.LIGHT_STRIP_CHANNEL); - setDefaultCommand(new SetLed(this, drivebase, shootingState)); + setDefaultCommand(new SetLed(this, shootingState, drivebase)); } public void setPattern(BlinkinPattern pattern) { diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 8c3cf7cc..2f30b0e8 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.spark.ClosedLoopSlot; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; @@ -46,8 +48,10 @@ public void setPosition(double targetEncoderPosition) { if (Math.abs(targetEncoderPosition - io.getEncoderPosition()) >= Constants.TURRET_PID_DISTANCE_THRESHOLD) { io.setPidPosition(targetEncoderPosition, ClosedLoopSlot.kSlot1); // longer encoder distance pid + Logger.recordOutput(LOGGING_NAME+"/Using long PID", 1); } else { io.setPidPosition(targetEncoderPosition, ClosedLoopSlot.kSlot0); // shorter encoder distance pid + Logger.recordOutput(LOGGING_NAME+"/Using long PID", 0); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ef7becba..de6f5584 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -50,7 +50,6 @@ import java.util.function.Supplier; import static edu.wpi.first.units.Units.Meter; - public class SwerveSubsystem extends SubsystemBase { /** * Swerve drive object.