diff --git a/src/main/deploy/choreo/OtoCR.traj b/src/main/deploy/choreo/OtoCR.traj index 7d97a05..b3c6340 100644 --- a/src/main/deploy/choreo/OtoCR.traj +++ b/src/main/deploy/choreo/OtoCR.traj @@ -3,7 +3,7 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":0.4355199635028839, "y":0.5460934042930603, "heading":1.5707963267948966, "intervals":69, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":0.7321799635887146, "y":0.7198631167411804, "heading":1.5707963267948966, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.918229579925537, "y":2.544593811035156, "heading":-1.5737992594811685, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -14,7 +14,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"O.x", "val":0.4355199635028839}, "y":{"exp":"O.y", "val":0.5460934042930603}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":69, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"O.x", "val":0.7321799635887146}, "y":{"exp":"O.y", "val":0.7198631167411804}, "heading":{"exp":"O.heading", "val":1.5707963267948966}, "intervals":63, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.918229579925537 m", "val":1.918229579925537}, "y":{"exp":"2.5445938110351562 m", "val":2.544593811035156}, "heading":{"exp":"CR.heading", "val":-1.5737992594811685}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -51,78 +51,72 @@ "differentialTrackWidth":0.5427218 }, "sampleType":"Swerve", - "waypoints":[0.0,2.62479], + "waypoints":[0.0,2.31226], "samples":[ - {"t":0.0, "x":0.43552, "y":0.54609, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.48298, "ay":6.04247, "alpha":0.00748, "fx":[74.10727,74.19831,74.1309,74.03985], "fy":[99.91144,99.84398,99.89421,99.96158]}, - {"t":0.03804, "x":0.43876, "y":0.55047, "heading":1.5708, "vx":0.17053, "vy":0.22986, "omega":0.00028, "ax":4.48025, "ay":6.03879, "alpha":0.01044, "fx":[74.05746,74.18442,74.09049,73.96351], "fy":[99.85392,99.7599,99.83004,99.92389]}, - {"t":0.07608, "x":0.44849, "y":0.56358, "heading":1.57081, "vx":0.34097, "vy":0.45958, "omega":0.00068, "ax":4.4721, "ay":6.02779, "alpha":0.01929, "fx":[73.90843,74.14274,73.96989,73.73553], "fy":[99.68184,99.50867,99.63855,99.81113]}, - {"t":0.11412, "x":0.4647, "y":0.58542, "heading":1.57083, "vx":0.51109, "vy":0.68888, "omega":0.00142, "ax":2.22369, "ay":2.99735, "alpha":2.81362, "fx":[24.77598,54.67068,47.33322,20.2811], "fy":[40.1773,34.31597,58.72416,65.00891]}, - {"t":0.15216, "x":0.48575, "y":0.6138, "heading":1.57089, "vx":0.59568, "vy":0.8029, "omega":0.10845, "ax":0.00005, "ay":0.0, "alpha":4.22282, "fx":[-19.91862,19.92402,19.9204,-19.92222], "fy":[-19.9231,-19.9194,19.92314,19.91962]}, - {"t":0.1902, "x":0.50841, "y":0.64434, "heading":1.57501, "vx":0.59568, "vy":0.8029, "omega":0.26908, "ax":0.0, "ay":0.0, "alpha":3.89866, "fx":[-18.31433,18.46944,18.31436,-18.46941], "fy":[-18.46944,-18.31435,18.46941,18.31433]}, - {"t":0.22824, "x":0.53107, "y":0.67488, "heading":1.58525, "vx":0.59568, "vy":0.8029, "omega":0.41739, "ax":0.0, "ay":0.0, "alpha":3.59311, "fx":[-16.70387,17.1938,16.70387,-17.1938], "fy":[-17.1938,-16.70387,17.1938,16.70387]}, - {"t":0.26628, "x":0.55373, "y":0.70542, "heading":1.60113, "vx":0.59568, "vy":0.8029, "omega":0.55407, "ax":0.0, "ay":0.0, "alpha":3.30606, "fx":[-15.11632,16.06225,15.11632,-16.06225], "fy":[-16.06225,-15.11632,16.06225,15.11632]}, - {"t":0.30432, "x":0.57639, "y":0.73597, "heading":1.6222, "vx":0.59568, "vy":0.8029, "omega":0.67984, "ax":0.0, "ay":0.0, "alpha":3.03722, "fx":[-13.57301,15.04551,13.57301,-15.04551], "fy":[-15.04551,-13.57301,15.04551,13.57301]}, - {"t":0.34236, "x":0.59905, "y":0.76651, "heading":1.64806, "vx":0.59568, "vy":0.8029, "omega":0.79538, "ax":0.0, "ay":0.0, "alpha":2.78611, "fx":[-12.08977,14.11892,12.08977,-14.11892], "fy":[-14.11892,-12.08977,14.11892,12.08977]}, - {"t":0.3804, "x":0.62171, "y":0.79705, "heading":1.67832, "vx":0.59568, "vy":0.8029, "omega":0.90136, "ax":0.0, "ay":0.0, "alpha":2.55213, "fx":[-10.67813,13.26229,10.67813,-13.26229], "fy":[-13.26229,-10.67813,13.26229,10.67813]}, - {"t":0.41844, "x":0.64437, "y":0.82759, "heading":1.71261, "vx":0.59568, "vy":0.8029, "omega":0.99844, "ax":0.0, "ay":0.0, "alpha":2.33456, "fx":[-9.3462,12.45943,9.3462,-12.45943], "fy":[-12.45943,-9.3462,12.45943,9.3462]}, - {"t":0.45649, "x":0.66703, "y":0.85814, "heading":1.75059, "vx":0.59568, "vy":0.8029, "omega":1.08725, "ax":0.0, "ay":0.0, "alpha":2.13262, "fx":[-8.09942,11.69769,8.09942,-11.69769], "fy":[-11.69769,-8.09942,11.69769,8.09942]}, - {"t":0.49453, "x":0.68969, "y":0.88868, "heading":1.79195, "vx":0.59568, "vy":0.8029, "omega":1.16838, "ax":0.0, "ay":0.0, "alpha":1.94547, "fx":[-6.94109,10.96751,6.94109,-10.96751], "fy":[-10.96751,-6.94109,10.96751,6.94109]}, - {"t":0.53257, "x":0.71235, "y":0.91922, "heading":1.8364, "vx":0.59568, "vy":0.8029, "omega":1.24239, "ax":0.0, "ay":0.0, "alpha":1.77223, "fx":[-5.87284,10.26192,5.87284,-10.26192], "fy":[-10.26192,-5.87284,10.26192,5.87284]}, - {"t":0.57061, "x":0.73501, "y":0.94976, "heading":1.88366, "vx":0.59568, "vy":0.8029, "omega":1.3098, "ax":0.0, "ay":0.0, "alpha":1.612, "fx":[-4.89493,9.57607,4.89493,-9.57607], "fy":[-9.57607,-4.89493,9.57607,4.89493]}, - {"t":0.60865, "x":0.75767, "y":0.98031, "heading":1.93348, "vx":0.59568, "vy":0.8029, "omega":1.37112, "ax":0.0, "ay":0.0, "alpha":1.46389, "fx":[-4.00655,8.90682,4.00655,-8.90682], "fy":[-8.90682,-4.00655,8.90682,4.00655]}, - {"t":0.64669, "x":0.78033, "y":1.01085, "heading":1.98564, "vx":0.59568, "vy":0.8029, "omega":1.42681, "ax":0.0, "ay":0.0, "alpha":1.32701, "fx":[-3.20606,8.2524,3.20606,-8.25239], "fy":[-8.2524,-3.20606,8.25239,3.20606]}, - {"t":0.68473, "x":0.80299, "y":1.04139, "heading":2.03992, "vx":0.59568, "vy":0.8029, "omega":1.47729, "ax":0.0, "ay":0.0, "alpha":1.20051, "fx":[-2.49114,7.61203,2.49114,-7.61203], "fy":[-7.61203,-2.49114,7.61203,2.49114]}, - {"t":0.72277, "x":0.82565, "y":1.07193, "heading":2.09611, "vx":0.59568, "vy":0.8029, "omega":1.52296, "ax":0.0, "ay":0.0, "alpha":1.08353, "fx":[-1.85897,6.98575,1.85897,-6.98575], "fy":[-6.98575,-1.85897,6.98575,1.85897]}, - {"t":0.76081, "x":0.84831, "y":1.10248, "heading":2.15405, "vx":0.59568, "vy":0.8029, "omega":1.56418, "ax":0.0, "ay":0.0, "alpha":0.97527, "fx":[-1.30635,6.37411,1.30635,-6.37411], "fy":[-6.37411,-1.30635,6.37411,1.30635]}, - {"t":0.79885, "x":0.87097, "y":1.13302, "heading":2.21355, "vx":0.59568, "vy":0.8029, "omega":1.60127, "ax":0.0, "ay":0.0, "alpha":0.87495, "fx":[-0.82984,5.77802,0.82984,-5.77802], "fy":[-5.77802,-0.82984,5.77802,0.82984]}, - {"t":0.83689, "x":0.89363, "y":1.16356, "heading":2.27446, "vx":0.59568, "vy":0.8029, "omega":1.63456, "ax":0.0, "ay":0.0, "alpha":0.78183, "fx":[-0.42584,5.19863,0.42584,-5.19863], "fy":[-5.19863,-0.42584,5.19863,0.42584]}, - {"t":0.87493, "x":0.91629, "y":1.1941, "heading":2.33664, "vx":0.59568, "vy":0.8029, "omega":1.6643, "ax":0.0, "ay":0.0, "alpha":0.6952, "fx":[-0.09068,4.6372,0.09068,-4.63721], "fy":[-4.6372,-0.09068,4.63721,0.09068]}, - {"t":0.91297, "x":0.93895, "y":1.22465, "heading":2.39995, "vx":0.59568, "vy":0.8029, "omega":1.69074, "ax":0.0, "ay":0.0, "alpha":0.61439, "fx":[0.1793,4.09506,-0.17931,-4.09507], "fy":[-4.09506,0.17931,4.09507,-0.1793]}, - {"t":0.95101, "x":0.96161, "y":1.25519, "heading":2.46427, "vx":0.59568, "vy":0.8029, "omega":1.71412, "ax":0.0, "ay":0.0, "alpha":0.53877, "fx":[0.38771,3.57351,-0.38772,-3.57351], "fy":[-3.57351,0.38772,3.57351,-0.38771]}, - {"t":0.98905, "x":0.98427, "y":1.28573, "heading":2.52947, "vx":0.59568, "vy":0.8029, "omega":1.73461, "ax":0.0, "ay":0.0, "alpha":0.46773, "fx":[0.53802,3.0738,-0.53803,-3.0738], "fy":[-3.0738,0.53803,3.0738,-0.53802]}, - {"t":1.02709, "x":1.00693, "y":1.31627, "heading":2.59546, "vx":0.59568, "vy":0.8029, "omega":1.7524, "ax":0.0, "ay":0.0, "alpha":0.4007, "fx":[0.63354,2.59714,-0.63355,-2.59715], "fy":[-2.59715,0.63355,2.59715,-0.63354]}, - {"t":1.06513, "x":1.02959, "y":1.34682, "heading":2.66212, "vx":0.59568, "vy":0.8029, "omega":1.76765, "ax":0.0, "ay":0.0, "alpha":0.33712, "fx":[0.67738,2.14469,-0.67739,-2.1447], "fy":[-2.14469,0.67739,2.1447,-0.67739]}, - {"t":1.10317, "x":1.05225, "y":1.37736, "heading":2.72936, "vx":0.59568, "vy":0.8029, "omega":1.78047, "ax":0.0, "ay":0.0, "alpha":0.27647, "fx":[0.67244,1.71753,-0.67244,-1.71753], "fy":[-1.71753,0.67244,1.71753,-0.67244]}, - {"t":1.14121, "x":1.07491, "y":1.4079, "heading":2.79709, "vx":0.59568, "vy":0.8029, "omega":1.79099, "ax":0.0, "ay":0.0, "alpha":0.21823, "fx":[0.62133,1.31672,-0.62134,-1.31672], "fy":[-1.31672,0.62134,1.31672,-0.62133]}, - {"t":1.17925, "x":1.09757, "y":1.43844, "heading":2.86522, "vx":0.59568, "vy":0.8029, "omega":1.79929, "ax":0.0, "ay":0.0, "alpha":0.16192, "fx":[0.52645,0.94332,-0.52645,-0.94332], "fy":[-0.94332,0.52645,0.94332,-0.52645]}, - {"t":1.21729, "x":1.12023, "y":1.46899, "heading":2.93367, "vx":0.59568, "vy":0.8029, "omega":1.80545, "ax":0.0, "ay":0.0, "alpha":0.10705, "fx":[0.38989,0.59839,-0.38989,-0.59839], "fy":[-0.59839,0.38989,0.59839,-0.38989]}, - {"t":1.25533, "x":1.14288, "y":1.49953, "heading":3.00235, "vx":0.59568, "vy":0.8029, "omega":1.80952, "ax":0.0, "ay":0.0, "alpha":0.05314, "fx":[0.21348,0.28308,-0.21348,-0.28307], "fy":[-0.28308,0.21348,0.28307,-0.21348]}, - {"t":1.29337, "x":1.16554, "y":1.53007, "heading":3.07119, "vx":0.59568, "vy":0.8029, "omega":1.81154, "ax":0.0, "ay":0.0, "alpha":-0.00027, "fx":[-0.00119,-0.00137,0.0012,0.00138], "fy":[0.00137,-0.0012,-0.00138,0.00119]}, - {"t":1.33142, "x":1.1882, "y":1.56061, "heading":3.1401, "vx":0.59568, "vy":0.8029, "omega":1.81153, "ax":0.0, "ay":0.0, "alpha":-0.05367, "fx":[-0.25281,-0.25356,0.25281,0.25357], "fy":[0.25356,-0.25281,-0.25357,0.25281]}, - {"t":1.36946, "x":1.21086, "y":1.59116, "heading":-3.07418, "vx":0.59568, "vy":0.8029, "omega":1.80949, "ax":0.0, "ay":0.0, "alpha":-0.10752, "fx":[-0.54024,-0.4719,0.54025,0.47191], "fy":[0.4719,-0.54025,-0.47191,0.54024]}, - {"t":1.4075, "x":1.23352, "y":1.6217, "heading":-3.00534, "vx":0.59568, "vy":0.8029, "omega":1.8054, "ax":0.0, "ay":0.0, "alpha":-0.1623, "fx":[-0.86256,-0.65456,0.86256,0.65457], "fy":[0.65456,-0.86256,-0.65457,0.86256]}, - {"t":1.44554, "x":1.25618, "y":1.65224, "heading":-2.93666, "vx":0.59568, "vy":0.8029, "omega":1.79923, "ax":0.0, "ay":0.0, "alpha":-0.2185, "fx":[-1.21895,-0.79944,1.21896,0.79944], "fy":[0.79944,-1.21896,-0.79944,1.21895]}, - {"t":1.48358, "x":1.27884, "y":1.68279, "heading":-2.86822, "vx":0.59568, "vy":0.8029, "omega":1.79092, "ax":0.0, "ay":0.0, "alpha":-0.2766, "fx":[-1.60872,-0.90413,1.60872,0.90414], "fy":[0.90414,-1.60872,-0.90414,1.60872]}, - {"t":1.52162, "x":1.3015, "y":1.71333, "heading":-2.80009, "vx":0.59568, "vy":0.8029, "omega":1.78039, "ax":0.0, "ay":0.0, "alpha":-0.33712, "fx":[-2.03118,-0.96593,2.03118,0.96594], "fy":[0.96593,-2.03118,-0.96594,2.03118]}, - {"t":1.55966, "x":1.32416, "y":1.74387, "heading":-2.73237, "vx":0.59568, "vy":0.8029, "omega":1.76757, "ax":0.0, "ay":0.0, "alpha":-0.40058, "fx":[-2.48565,-0.98178,2.48565,0.98179], "fy":[0.98179,-2.48565,-0.98179,2.48565]}, - {"t":1.5977, "x":1.34682, "y":1.77441, "heading":-2.66513, "vx":0.59568, "vy":0.8029, "omega":1.75233, "ax":0.0, "ay":0.0, "alpha":-0.46751, "fx":[-2.97138,-0.94832,2.97138,0.94832], "fy":[0.94832,-2.97138,-0.94832,2.97138]}, - {"t":1.63574, "x":1.36948, "y":1.80496, "heading":-2.59847, "vx":0.59568, "vy":0.8029, "omega":1.73455, "ax":0.0, "ay":0.0, "alpha":-0.53847, "fx":[-3.48754,-0.86187,3.48754,0.86187], "fy":[0.86187,-3.48754,-0.86187,3.48754]}, - {"t":1.67378, "x":1.39214, "y":1.8355, "heading":-2.53249, "vx":0.59568, "vy":0.8029, "omega":1.71406, "ax":0.0, "ay":0.0, "alpha":-0.61404, "fx":[-4.03313,-0.71846,4.03312,0.71846], "fy":[0.71846,-4.03313,-0.71846,4.03313]}, - {"t":1.71182, "x":1.4148, "y":1.86604, "heading":-2.46728, "vx":0.59568, "vy":0.8029, "omega":1.6907, "ax":0.0, "ay":0.0, "alpha":-0.69482, "fx":[-4.60701,-0.5139,4.60701,0.51389], "fy":[0.5139,-4.60701,-0.5139,4.60701]}, - {"t":1.74986, "x":1.43746, "y":1.89658, "heading":-2.40297, "vx":0.59568, "vy":0.8029, "omega":1.66427, "ax":0.0, "ay":0.0, "alpha":-0.78146, "fx":[-5.20788,-0.24376,5.20788,0.24376], "fy":[0.24376,-5.20788,-0.24376,5.20788]}, - {"t":1.7879, "x":1.46012, "y":1.92713, "heading":-2.33966, "vx":0.59568, "vy":0.8029, "omega":1.63455, "ax":0.0, "ay":0.0, "alpha":-0.87461, "fx":[-5.83426,0.09649,5.83426,-0.09649], "fy":[-0.09649,-5.83426,0.09649,5.83426]}, - {"t":1.82594, "x":1.48278, "y":1.95767, "heading":-2.27748, "vx":0.59568, "vy":0.8029, "omega":1.60128, "ax":0.0, "ay":0.0, "alpha":-0.97498, "fx":[-6.48454,0.51149,6.48454,-0.5115], "fy":[-0.51149,-6.48454,0.5115,6.48454]}, - {"t":1.86398, "x":1.50544, "y":1.98821, "heading":-2.21656, "vx":0.59568, "vy":0.8029, "omega":1.56419, "ax":0.0, "ay":0.0, "alpha":-1.0833, "fx":[-7.15703,1.00588,7.15703,-1.00588], "fy":[-1.00588,-7.15703,1.00588,7.15703]}, - {"t":1.90202, "x":1.5281, "y":2.01875, "heading":-2.15706, "vx":0.59568, "vy":0.8029, "omega":1.52298, "ax":0.0, "ay":0.0, "alpha":-1.20035, "fx":[-7.84999,1.58418,7.84999,-1.58418], "fy":[-1.58418,-7.84999,1.58418,7.84999]}, - {"t":1.94006, "x":1.55076, "y":2.0493, "heading":-2.09913, "vx":0.59568, "vy":0.8029, "omega":1.47732, "ax":0.0, "ay":0.0, "alpha":-1.32692, "fx":[-8.56177,2.25074,8.56177,-2.25074], "fy":[-2.25074,-8.56177,2.25074,8.56177]}, - {"t":1.9781, "x":1.57342, "y":2.07984, "heading":-2.04293, "vx":0.59568, "vy":0.8029, "omega":1.42684, "ax":0.0, "ay":0.0, "alpha":-1.46385, "fx":[-9.29094,3.00962,9.29094,-3.00962], "fy":[-3.00962,-9.29094,3.00962,9.29094]}, - {"t":2.01614, "x":1.59608, "y":2.11038, "heading":-1.98865, "vx":0.59568, "vy":0.8029, "omega":1.37115, "ax":0.0, "ay":0.0, "alpha":-1.61201, "fx":[-10.03641,3.8644,10.03642,-3.8644], "fy":[-3.8644,-10.03642,3.8644,10.03642]}, - {"t":2.05418, "x":1.61874, "y":2.14092, "heading":-1.93649, "vx":0.59568, "vy":0.8029, "omega":1.30983, "ax":0.0, "ay":0.0, "alpha":-1.77227, "fx":[-10.79772,4.8181,10.79772,-4.8181], "fy":[-4.8181,-10.79772,4.8181,10.79772]}, - {"t":2.09222, "x":1.6414, "y":2.17147, "heading":-1.88667, "vx":0.59568, "vy":0.8029, "omega":1.24241, "ax":0.0, "ay":0.0, "alpha":-1.94554, "fx":[-11.57523,5.87294,11.57523,-5.87294], "fy":[-5.87294,-11.57523,5.87294,11.57523]}, - {"t":2.13026, "x":1.66406, "y":2.20201, "heading":-1.8394, "vx":0.59568, "vy":0.8029, "omega":1.1684, "ax":0.0, "ay":0.0, "alpha":-2.13271, "fx":[-12.37046,7.03022,12.37046,-7.03021], "fy":[-7.03022,-12.37046,7.03021,12.37046]}, - {"t":2.1683, "x":1.68672, "y":2.23255, "heading":-1.79496, "vx":0.59568, "vy":0.8029, "omega":1.08728, "ax":0.0, "ay":0.0, "alpha":-2.33465, "fx":[-13.18646,8.28997,13.18646,-8.28997], "fy":[-8.28997,-13.18646,8.28997,13.18646]}, - {"t":2.20635, "x":1.70938, "y":2.26309, "heading":-1.7536, "vx":0.59568, "vy":0.8029, "omega":0.99846, "ax":0.0, "ay":0.0, "alpha":-2.55221, "fx":[-14.02822,9.6508,14.02822,-9.6508], "fy":[-9.6508,-14.02822,9.6508,14.02822]}, - {"t":2.24439, "x":1.73204, "y":2.29364, "heading":-1.71562, "vx":0.59568, "vy":0.8029, "omega":0.90138, "ax":0.0, "ay":0.0, "alpha":-2.78618, "fx":[-14.90314,11.10946,14.90314,-11.10946], "fy":[-11.10946,-14.90314,11.10946,14.90314]}, - {"t":2.28243, "x":1.7547, "y":2.32418, "heading":-1.68133, "vx":0.59568, "vy":0.8029, "omega":0.79539, "ax":0.0, "ay":0.0, "alpha":-3.03727, "fx":[-15.82152,12.6605,15.82152,-12.6605], "fy":[-12.6605,-15.82152,12.6605,15.82152]}, - {"t":2.32047, "x":1.77736, "y":2.35472, "heading":-1.65107, "vx":0.59568, "vy":0.8029, "omega":0.67985, "ax":0.0, "ay":0.0, "alpha":-3.3061, "fx":[-16.79708,14.29577,16.79708,-14.29577], "fy":[-14.29577,-16.79708,14.29577,16.79708]}, - {"t":2.35851, "x":1.80002, "y":2.38526, "heading":-1.62521, "vx":0.59568, "vy":0.8029, "omega":0.55409, "ax":0.0, "ay":0.0, "alpha":-3.59313, "fx":[-17.84749,16.00377,17.84749,-16.00377], "fy":[-16.00377,-17.84749,16.00377,17.84749]}, - {"t":2.39655, "x":1.82268, "y":2.41581, "heading":-1.60413, "vx":0.59568, "vy":0.8029, "omega":0.4174, "ax":0.0, "ay":0.0, "alpha":-3.89867, "fx":[-18.99486,17.76889,18.99483,-17.76892], "fy":[-17.76889,-18.99483,17.76892,18.99485]}, - {"t":2.43459, "x":1.84534, "y":2.44635, "heading":-1.58825, "vx":0.59568, "vy":0.8029, "omega":0.26909, "ax":-0.00006, "ay":0.0, "alpha":-4.22282, "fx":[-20.2669,19.56961,20.26507,-19.57145], "fy":[-19.57053,-20.26608,19.57053,20.26589]}, - {"t":2.47263, "x":1.868, "y":2.47689, "heading":-1.57802, "vx":0.59568, "vy":0.8029, "omega":0.10846, "ax":-2.22369, "ay":-2.99736, "alpha":-2.81387, "fx":[-47.44425,-20.37853,-24.62861,-54.60944], "fy":[-58.62488,-65.06062,-40.30922,-34.23173]}, - {"t":2.51067, "x":1.88905, "y":2.50526, "heading":-1.57389, "vx":0.51109, "vy":0.68888, "omega":0.00142, "ax":-4.4721, "ay":-6.02779, "alpha":-0.01929, "fx":[-73.97053,-73.73563,-73.90781,-74.14266], "fy":[-99.63807,-99.81106,-99.6823,-99.50873]}, - {"t":2.54871, "x":1.90526, "y":2.52711, "heading":-1.57384, "vx":0.34097, "vy":0.45958, "omega":0.00068, "ax":-4.48025, "ay":-6.03879, "alpha":-0.01044, "fx":[-74.09083,-73.96357,-74.05712,-74.18437], "fy":[-99.82979,-99.92385,-99.85417,-99.75993]}, - {"t":2.58675, "x":1.91499, "y":2.54022, "heading":-1.57381, "vx":0.17053, "vy":0.22986, "omega":0.00028, "ax":-4.48298, "ay":-6.04247, "alpha":-0.00748, "fx":[-74.13114,-74.03989,-74.10703,-74.19828], "fy":[-99.89403,-99.96156,-99.91161,-99.844]}, - {"t":2.62479, "x":1.91823, "y":2.54459, "heading":-1.5738, "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":0.73218, "y":0.71986, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.10002, "ay":6.30784, "alpha":0.01021, "fx":[67.76368,67.89908,67.81123,67.67586], "fy":[104.30559,104.21767,104.2751,104.36285]}, + {"t":0.0367, "x":0.73494, "y":0.72411, "heading":1.5708, "vx":0.15048, "vy":0.23151, "omega":0.00037, "ax":4.09736, "ay":6.30374, "alpha":0.01427, "fx":[67.7102,67.89937,67.77674,67.58763], "fy":[104.24377,104.12099,104.20138,104.3238]}, + {"t":0.07341, "x":0.74322, "y":0.73685, "heading":1.57081, "vx":0.30086, "vy":0.46288, "omega":0.0009, "ax":4.08943, "ay":6.29151, "alpha":0.02639, "fx":[67.55057,67.89998,67.67408,67.32486], "fy":[104.05914,103.83274,103.98204,104.20724]}, + {"t":0.11011, "x":0.75702, "y":0.75808, "heading":1.57084, "vx":0.45096, "vy":0.69379, "omega":0.00187, "ax":2.55761, "ay":3.93519, "alpha":2.60714, "fx":[31.41257,61.50266,51.61639,24.61302], "fy":[59.48868,49.98538,71.41775,79.35702]}, + {"t":0.14681, "x":0.7753, "y":0.7862, "heading":1.57091, "vx":0.54483, "vy":0.83822, "omega":0.09756, "ax":0.00013, "ay":-0.00001, "alpha":5.28428, "fx":[-24.92379,24.93372,24.92799,-24.92956], "fy":[-24.93192,-24.92583,24.93137,24.92595]}, + {"t":0.18351, "x":0.79529, "y":0.81696, "heading":1.57449, "vx":0.54483, "vy":0.83822, "omega":0.2915, "ax":0.0, "ay":0.0, "alpha":4.90794, "fx":[-23.06761,23.23886,23.06771,-23.23877], "fy":[-23.23884,-23.06769,23.23878,23.06763]}, + {"t":0.22022, "x":0.81529, "y":0.84772, "heading":1.58519, "vx":0.54483, "vy":0.83822, "omega":0.47164, "ax":0.0, "ay":0.0, "alpha":4.54735, "fx":[-21.14127,21.75886,21.14127,-21.75886], "fy":[-21.75886,-21.14127,21.75886,21.14127]}, + {"t":0.25692, "x":0.83529, "y":0.87849, "heading":1.6025, "vx":0.54483, "vy":0.83822, "omega":0.63854, "ax":0.0, "ay":0.0, "alpha":4.20333, "fx":[-19.1908,20.44798,19.1908,-20.44798], "fy":[-20.44798,-19.1908,20.44798,19.1908]}, + {"t":0.29362, "x":0.85528, "y":0.90925, "heading":1.62594, "vx":0.54483, "vy":0.83822, "omega":0.79281, "ax":0.0, "ay":0.0, "alpha":3.87644, "fx":[-17.25159,19.26733,17.25159,-19.26733], "fy":[-19.26733,-17.25159,19.26733,17.25159]}, + {"t":0.33032, "x":0.87528, "y":0.94002, "heading":1.65504, "vx":0.54483, "vy":0.83822, "omega":0.93508, "ax":0.0, "ay":0.0, "alpha":3.56699, "fx":[-15.35189,18.1836,15.35189,-18.18361], "fy":[-18.18361,-15.35189,18.18361,15.35189]}, + {"t":0.36703, "x":0.89528, "y":0.97078, "heading":1.68936, "vx":0.54483, "vy":0.83822, "omega":1.066, "ax":0.0, "ay":0.0, "alpha":3.27504, "fx":[-13.5142,17.16914,13.5142,-17.16914], "fy":[-17.16914,-13.5142,17.16914,13.5142]}, + {"t":0.40373, "x":0.91527, "y":1.00155, "heading":1.72848, "vx":0.54483, "vy":0.83822, "omega":1.1862, "ax":0.0, "ay":0.0, "alpha":3.00043, "fx":[-11.75629,16.20174,11.75629,-16.20173], "fy":[-16.20174,-11.75629,16.20173,11.75629]}, + {"t":0.44043, "x":0.93527, "y":1.03231, "heading":1.77202, "vx":0.54483, "vy":0.83822, "omega":1.29633, "ax":0.0, "ay":0.0, "alpha":2.74279, "fx":[-10.09204,15.26425,10.09204,-15.26425], "fy":[-15.26425,-10.09204,15.26425,10.09204]}, + {"t":0.47713, "x":0.95527, "y":1.06308, "heading":1.8196, "vx":0.54483, "vy":0.83822, "omega":1.39699, "ax":0.0, "ay":0.0, "alpha":2.50162, "fx":[-8.53208,14.34408,8.53208,-14.34408], "fy":[-14.34408,-8.53208,14.34408,8.53208]}, + {"t":0.51384, "x":0.97526, "y":1.09384, "heading":1.87087, "vx":0.54483, "vy":0.83822, "omega":1.48881, "ax":0.0, "ay":0.0, "alpha":2.27624, "fx":[-7.0843,13.43254,7.0843,-13.43254], "fy":[-13.43254,-7.0843,13.43254,7.0843]}, + {"t":0.55054, "x":0.99526, "y":1.12461, "heading":1.92551, "vx":0.54483, "vy":0.83822, "omega":1.57235, "ax":0.0, "ay":0.0, "alpha":2.06591, "fx":[-5.75425,12.52431,5.75426,-12.5243], "fy":[-12.52431,-5.75426,12.5243,5.75425]}, + {"t":0.58724, "x":1.01526, "y":1.15537, "heading":1.98322, "vx":0.54483, "vy":0.83822, "omega":1.64818, "ax":0.0, "ay":0.0, "alpha":1.86978, "fx":[-4.54549,11.61677,4.5455,-11.61676], "fy":[-11.61677,-4.5455,11.61677,4.54549]}, + {"t":0.62394, "x":1.03525, "y":1.18614, "heading":2.04371, "vx":0.54483, "vy":0.83822, "omega":1.7168, "ax":0.0, "ay":0.0, "alpha":1.68693, "fx":[-3.45987,10.70953,3.45988,-10.70953], "fy":[-10.70953,-3.45988,10.70953,3.45987]}, + {"t":0.66065, "x":1.05525, "y":1.2169, "heading":2.10672, "vx":0.54483, "vy":0.83822, "omega":1.77872, "ax":0.0, "ay":0.0, "alpha":1.51644, "fx":[-2.49781,9.8039,2.49781,-9.80389], "fy":[-9.8039,-2.49781,9.80389,2.49781]}, + {"t":0.69735, "x":1.07525, "y":1.24767, "heading":2.17201, "vx":0.54483, "vy":0.83822, "omega":1.83438, "ax":0.0, "ay":0.0, "alpha":1.35734, "fx":[-1.6585,8.90244,1.65851,-8.90244], "fy":[-8.90244,-1.65851,8.90244,1.6585]}, + {"t":0.73405, "x":1.09524, "y":1.27843, "heading":2.23933, "vx":0.54483, "vy":0.83822, "omega":1.88419, "ax":0.0, "ay":0.0, "alpha":1.20866, "fx":[-0.94018,8.00867,0.94018,-8.00867], "fy":[-8.00867,-0.94018,8.00867,0.94018]}, + {"t":0.77075, "x":1.11524, "y":1.3092, "heading":2.30849, "vx":0.54483, "vy":0.83822, "omega":1.92855, "ax":0.0, "ay":0.0, "alpha":1.06943, "fx":[-0.34024,7.12671,0.34024,-7.12671], "fy":[-7.12671,-0.34024,7.12671,0.34024]}, + {"t":0.80746, "x":1.13524, "y":1.33996, "heading":2.37927, "vx":0.54483, "vy":0.83822, "omega":1.9678, "ax":0.0, "ay":0.0, "alpha":0.93872, "fx":[0.14451,6.26109,-0.14452,-6.2611], "fy":[-6.26109,0.14452,6.2611,-0.14451]}, + {"t":0.84416, "x":1.15523, "y":1.37073, "heading":2.45149, "vx":0.54483, "vy":0.83822, "omega":2.00226, "ax":0.0, "ay":0.0, "alpha":0.81559, "fx":[0.51777,5.41659,-0.51778,-5.4166], "fy":[-5.41659,0.51778,5.4166,-0.51777]}, + {"t":0.88086, "x":1.17523, "y":1.40149, "heading":2.52498, "vx":0.54483, "vy":0.83822, "omega":2.03219, "ax":0.0, "ay":0.0, "alpha":0.69914, "fx":[0.78355,4.59807,-0.78356,-4.59809], "fy":[-4.59808,0.78356,4.59809,-0.78355]}, + {"t":0.91756, "x":1.19523, "y":1.43226, "heading":2.59957, "vx":0.54483, "vy":0.83822, "omega":2.05785, "ax":0.0, "ay":0.0, "alpha":0.58848, "fx":[0.94611,3.81041,-0.94612,-3.81043], "fy":[-3.81042,0.94612,3.81043,-0.94611]}, + {"t":0.95427, "x":1.21522, "y":1.46302, "heading":2.6751, "vx":0.54483, "vy":0.83822, "omega":2.07945, "ax":0.0, "ay":0.0, "alpha":0.48277, "fx":[1.00981,3.05844,-1.00982,-3.05846], "fy":[-3.05845,1.00982,3.05846,-1.00981]}, + {"t":0.99097, "x":1.23522, "y":1.49379, "heading":2.75142, "vx":0.54483, "vy":0.83822, "omega":2.09717, "ax":0.0, "ay":0.0, "alpha":0.38116, "fx":[0.97907,2.34691,-0.97908,-2.34692], "fy":[-2.34691,0.97908,2.34692,-0.97907]}, + {"t":1.02767, "x":1.25522, "y":1.52455, "heading":2.82839, "vx":0.54483, "vy":0.83822, "omega":2.11116, "ax":0.0, "ay":0.0, "alpha":0.28283, "fx":[0.85826,1.68046,-0.85827,-1.68046], "fy":[-1.68046,0.85826,1.68046,-0.85826]}, + {"t":1.06437, "x":1.27521, "y":1.55532, "heading":2.90587, "vx":0.54483, "vy":0.83822, "omega":2.12154, "ax":0.0, "ay":0.0, "alpha":0.18698, "fx":[0.65167,1.06367,-0.65167,-1.06367], "fy":[-1.06367,0.65167,1.06367,-0.65167]}, + {"t":1.10108, "x":1.29521, "y":1.58608, "heading":2.98374, "vx":0.54483, "vy":0.83822, "omega":2.1284, "ax":0.0, "ay":0.0, "alpha":0.09278, "fx":[0.36346,0.50108,-0.36346,-0.50107], "fy":[-0.50108,0.36346,0.50107,-0.36346]}, + {"t":1.13778, "x":1.31521, "y":1.61685, "heading":3.06186, "vx":0.54483, "vy":0.83822, "omega":2.13181, "ax":0.0, "ay":0.0, "alpha":-0.00055, "fx":[-0.00237,-0.00278,0.00238,0.00279], "fy":[0.00278,-0.00238,-0.00279,0.00237]}, + {"t":1.17448, "x":1.3352, "y":1.64761, "heading":3.1401, "vx":0.54483, "vy":0.83822, "omega":2.13179, "ax":0.0, "ay":0.0, "alpha":-0.09383, "fx":[-0.44197,-0.4433,0.44199,0.44331], "fy":[0.4433,-0.44199,-0.44331,0.44198]}, + {"t":1.21118, "x":1.3552, "y":1.67838, "heading":-3.06484, "vx":0.54483, "vy":0.83822, "omega":2.12834, "ax":0.0, "ay":0.0, "alpha":-0.18787, "fx":[-0.95164,-0.81573,0.95166,0.81575], "fy":[0.81573,-0.95166,-0.81574,0.95164]}, + {"t":1.24789, "x":1.3752, "y":1.70914, "heading":-2.98673, "vx":0.54483, "vy":0.83822, "omega":2.12145, "ax":0.0, "ay":0.0, "alpha":-0.28351, "fx":[-1.52773,-1.11514,1.52775,1.11516], "fy":[1.11514,-1.52775,-1.11515,1.52774]}, + {"t":1.28459, "x":1.39519, "y":1.73991, "heading":-2.90886, "vx":0.54483, "vy":0.83822, "omega":2.11104, "ax":0.0, "ay":0.0, "alpha":-0.38156, "fx":[-2.16662,-1.33634,2.16664,1.33635], "fy":[1.33634,-2.16664,-1.33635,2.16662]}, + {"t":1.32129, "x":1.41519, "y":1.77067, "heading":-2.83138, "vx":0.54483, "vy":0.83822, "omega":2.09704, "ax":0.0, "ay":0.0, "alpha":-0.48287, "fx":[-2.8646,-1.47387,2.86462,1.47388], "fy":[1.47387,-2.86462,-1.47388,2.86461]}, + {"t":1.35799, "x":1.43519, "y":1.80144, "heading":-2.75442, "vx":0.54483, "vy":0.83822, "omega":2.07932, "ax":0.0, "ay":0.0, "alpha":-0.58831, "fx":[-3.61781,-1.52201,3.61782,1.52202], "fy":[1.52202,-3.61782,-1.52202,3.61782]}, + {"t":1.3947, "x":1.45518, "y":1.8322, "heading":-2.6781, "vx":0.54483, "vy":0.83822, "omega":2.05772, "ax":0.0, "ay":0.0, "alpha":-0.69872, "fx":[-4.42213,-1.47481,4.42214,1.47482], "fy":[1.47481,-4.42214,-1.47482,4.42213]}, + {"t":1.4314, "x":1.47518, "y":1.86296, "heading":-2.60258, "vx":0.54483, "vy":0.83822, "omega":2.03208, "ax":0.0, "ay":0.0, "alpha":-0.815, "fx":[-5.27312,-1.32616,5.27312,1.32615], "fy":[1.32616,-5.27312,-1.32615,5.27312]}, + {"t":1.4681, "x":1.49518, "y":1.89373, "heading":-2.528, "vx":0.54483, "vy":0.83822, "omega":2.00217, "ax":0.0, "ay":0.0, "alpha":-0.93802, "fx":[-6.16599,-1.06987,6.16598,1.06987], "fy":[1.06987,-6.16598,-1.06987,6.16599]}, + {"t":1.5048, "x":1.51517, "y":1.92449, "heading":-2.45451, "vx":0.54483, "vy":0.83822, "omega":1.96774, "ax":0.0, "ay":0.0, "alpha":-1.06871, "fx":[-7.09556,-0.69987,7.09555,0.69987], "fy":[0.69987,-7.09555,-0.69987,7.09556]}, + {"t":1.54151, "x":1.53517, "y":1.95526, "heading":-2.38229, "vx":0.54483, "vy":0.83822, "omega":1.92851, "ax":0.0, "ay":0.0, "alpha":-1.20797, "fx":[-8.05635,-0.21029,8.05634,0.21028], "fy":[0.21029,-8.05634,-0.21028,8.05635]}, + {"t":1.57821, "x":1.55517, "y":1.98602, "heading":-2.31151, "vx":0.54483, "vy":0.83822, "omega":1.88418, "ax":0.0, "ay":0.0, "alpha":-1.35674, "fx":[-9.04261,0.40434,9.0426,-0.40435], "fy":[-0.40434,-9.04261,0.40434,9.04261]}, + {"t":1.61491, "x":1.57516, "y":2.01679, "heading":-2.24235, "vx":0.54483, "vy":0.83822, "omega":1.83438, "ax":0.0, "ay":0.0, "alpha":-1.51597, "fx":[-10.04849,1.14888,10.04848,-1.14888], "fy":[-1.14888,-10.04848,1.14888,10.04849]}, + {"t":1.65161, "x":1.59516, "y":2.04755, "heading":-2.17503, "vx":0.54483, "vy":0.83822, "omega":1.77874, "ax":0.0, "ay":0.0, "alpha":-1.6866, "fx":[-11.06818,2.0274,11.06817,-2.02741], "fy":[-2.0274,-11.06817,2.02741,11.06818]}, + {"t":1.68832, "x":1.61516, "y":2.07832, "heading":-2.10974, "vx":0.54483, "vy":0.83822, "omega":1.71684, "ax":0.0, "ay":0.0, "alpha":-1.86958, "fx":[-12.09621,3.04297,12.09621,-3.04297], "fy":[-3.04297,-12.09621,3.04297,12.09621]}, + {"t":1.72502, "x":1.63515, "y":2.10908, "heading":-2.04673, "vx":0.54483, "vy":0.83822, "omega":1.64822, "ax":0.0, "ay":0.0, "alpha":-2.06583, "fx":[-13.12772,4.19739,13.12772,-4.19739], "fy":[-4.19739,-13.12772,4.19739,13.12772]}, + {"t":1.76172, "x":1.65515, "y":2.13985, "heading":-1.98624, "vx":0.54483, "vy":0.83822, "omega":1.5724, "ax":0.0, "ay":0.0, "alpha":-2.27626, "fx":[-14.15883,5.49098,14.15883,-5.49098], "fy":[-5.49098,-14.15883,5.49098,14.15883]}, + {"t":1.79842, "x":1.67515, "y":2.17061, "heading":-1.92853, "vx":0.54483, "vy":0.83822, "omega":1.48886, "ax":0.0, "ay":0.0, "alpha":-2.5017, "fx":[-15.18711,6.9223,15.18712,-6.9223], "fy":[-6.9223,-15.18711,6.9223,15.18711]}, + {"t":1.83513, "x":1.69514, "y":2.20138, "heading":-1.87388, "vx":0.54483, "vy":0.83822, "omega":1.39704, "ax":0.0, "ay":0.0, "alpha":-2.74291, "fx":[-16.21206,8.48788,16.21207,-8.48788], "fy":[-8.48788,-16.21206,8.48788,16.21206]}, + {"t":1.87183, "x":1.71514, "y":2.23214, "heading":-1.82261, "vx":0.54483, "vy":0.83822, "omega":1.29637, "ax":0.0, "ay":0.0, "alpha":-3.00056, "fx":[-17.23572,10.18193,17.23572,-10.18192], "fy":[-10.18193,-17.23572,10.18192,17.23572]}, + {"t":1.90853, "x":1.73514, "y":2.26291, "heading":-1.77503, "vx":0.54483, "vy":0.83822, "omega":1.18624, "ax":0.0, "ay":0.0, "alpha":-3.27517, "fx":[-18.26325,11.99599,18.26325,-11.99599], "fy":[-11.99599,-18.26325,11.99599,18.26325]}, + {"t":1.94523, "x":1.75513, "y":2.29367, "heading":-1.73149, "vx":0.54483, "vy":0.83822, "omega":1.06603, "ax":0.0, "ay":0.0, "alpha":-3.56711, "fx":[-19.30365,13.91863,19.30365,-13.91863], "fy":[-13.91863,-19.30365,13.91863,19.30365]}, + {"t":1.98194, "x":1.77513, "y":2.32444, "heading":-1.69236, "vx":0.54483, "vy":0.83822, "omega":0.93511, "ax":0.0, "ay":0.0, "alpha":-3.87653, "fx":[-20.3704,15.93499,20.37041,-15.93499], "fy":[-15.93499,-20.3704,15.93499,20.3704]}, + {"t":2.01864, "x":1.79513, "y":2.3552, "heading":-1.65804, "vx":0.54483, "vy":0.83822, "omega":0.79283, "ax":0.0, "ay":0.0, "alpha":-4.20339, "fx":[-21.48208,18.02635,21.48208,-18.02635], "fy":[-18.02635,-21.48208,18.02635,21.48208]}, + {"t":2.05534, "x":1.81512, "y":2.38597, "heading":-1.62894, "vx":0.54483, "vy":0.83822, "omega":0.63856, "ax":0.0, "ay":0.0, "alpha":-4.54738, "fx":[-22.66288,20.16948,22.66288,-20.16949], "fy":[-20.16948,-22.66288,20.16949,22.66288]}, + {"t":2.09204, "x":1.83512, "y":2.41673, "heading":-1.60551, "vx":0.54483, "vy":0.83822, "omega":0.47166, "ax":0.0, "ay":0.0, "alpha":-4.90795, "fx":[-23.94303,22.33592,23.94293,-22.33601], "fy":[-22.33593,-23.94295,22.336,23.943]}, + {"t":2.12875, "x":1.85512, "y":2.4475, "heading":-1.5882, "vx":0.54483, "vy":0.83822, "omega":0.29152, "ax":-0.00013, "ay":0.00001, "alpha":-5.28426, "fx":[-25.36075,24.48905,25.35649,-24.49329], "fy":[-24.49088,-25.35866,24.49148,25.35858]}, + {"t":2.16545, "x":1.87511, "y":2.47826, "heading":-1.5775, "vx":0.54483, "vy":0.83822, "omega":0.09758, "ax":-2.55761, "ay":-3.93519, "alpha":-2.6077, "fx":[-51.72521,-24.69926,-31.26056,-61.45938], "fy":[-71.33006,-79.3859,-59.60579,-49.92723]}, + {"t":2.20215, "x":1.89339, "y":2.50638, "heading":-1.57391, "vx":0.45096, "vy":0.69379, "omega":0.00187, "ax":-4.08943, "ay":-6.29151, "alpha":-0.02639, "fx":[-67.67499,-67.32506,-67.54967,-67.89981], "fy":[-103.98145,-104.20712,-104.05972,-103.83285]}, + {"t":2.23885, "x":1.90719, "y":2.5276, "heading":-1.57385, "vx":0.30086, "vy":0.46288, "omega":0.0009, "ax":-4.09736, "ay":-6.30374, "alpha":-0.01428, "fx":[-67.77723,-67.58773,-67.70973,-67.89928], "fy":[-104.20107,-104.32374,-104.24407,-104.12105]}, + {"t":2.27556, "x":1.91547, "y":2.54035, "heading":-1.57381, "vx":0.15048, "vy":0.23151, "omega":0.00037, "ax":-4.10002, "ay":-6.30784, "alpha":-0.01021, "fx":[-67.81157,-67.67592,-67.76334,-67.89902], "fy":[-104.27488,-104.36281,-104.30581,-104.2177]}, + {"t":2.31226, "x":1.91823, "y":2.54459, "heading":-1.5738, "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 9eb0900..2b8ce92 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -70,6 +70,7 @@ import frc.robot.utils.FieldUtils.ClimbTargets; import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.FieldUtils.TrenchPoses; +import frc.robot.utils.FuelSim; import frc.robot.utils.autoaim.AutoAim; import java.io.File; import java.util.Arrays; @@ -207,6 +208,8 @@ public enum RobotEdition { private final CANdleSubsystem candle = new CANdleSubsystem(new CANdleIOReal(0, CANdleSubsystem.getCandleConfig(), canivore)); + private FuelSim fuelSim = new FuelSim(); + // climber only exists for the comp bot - this is accounted for later private final Superstructure superstructure; @@ -419,7 +422,8 @@ public Robot() { : new CANcoderIOSim(5, TurretSubsystem.getCancoder24tConfigs(), canivore), ROBOT_MODE == RobotMode.REAL ? new CANcoderIO(4, TurretSubsystem.getCancoder26tConfigs(), canivore) - : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore)); + : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore), + fuelSim); break; } climber = @@ -588,6 +592,31 @@ public Robot() { "Interrputing: " + (interrupting.isPresent() ? interrupting.get().getName() : "none")); }); + + // fuelSim.spawnStartingFuel(); + + fuelSim.registerRobot( + Units.inchesToMeters(28), // from left to right in meters + Units.inchesToMeters(28), // from front to back in meters + Units.inchesToMeters(4), // from floor to top of bumpers in meters + swerve::getPose, // Supplier of robot pose + swerve + ::getVelocityFieldRelative); // Supplier of field-centric chassis speeds + + fuelSim.registerIntake( + Units.inchesToMeters(-14), + Units.inchesToMeters(14), + Units.inchesToMeters(14), + Units.inchesToMeters(20), // robot-centric coordinates for bounding box in meters + () -> + Superstructure.getState() + .isAnIntakeState() // (optional) BooleanSupplier for whether the intake should be + // active at a given moment + ); // (optional) Runnable called whenever a fuel is intaked + + fuelSim.setSubticks(5); + + fuelSim.start(); } /** Scales a joystick value for teleop driving */ @@ -697,6 +726,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta // .and( new Trigger(AutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAScoreState()) + .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) .and(() -> superstructure.inScoringArea()) .whileTrue( @@ -713,6 +743,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta new Trigger(AutoAim::targetInTurretDeadzone) .and(() -> Superstructure.getState().isAFeedState()) + .and(() -> !Superstructure.getState().isAFlowState()) .and(() -> !Superstructure.getPoseOverride()) .and(() -> !superstructure.inScoringArea()) .whileTrue( @@ -919,6 +950,7 @@ public void simulationInit() { @Override public void simulationPeriodic() { + fuelSim.updateSim(); // Log zeroed poses for mechs and robot for debugging in sim Logger.recordOutput( "Robot/Zeroed Mechanism Poses", diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f9fdb6b..4943c10 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -68,6 +68,21 @@ public boolean isAScoreState() { public boolean isAFeedState() { return this == FEED || this == SPIN_UP_FEED || this == SPIN_UP_FEED_FLOW || this == FEED_FLOW; } + + public boolean isAnIntakeState() { + return this == INTAKE + || this == SPIN_UP_FEED_FLOW + || this == FEED_FLOW + || this == SPIN_UP_SCORE_FLOW + || this == SCORE_FLOW; + } + + public boolean isAFlowState() { + return this == FEED_FLOW + || this == SCORE_FLOW + || this == SPIN_UP_FEED_FLOW + || this == SPIN_UP_SCORE_FLOW; + } } public enum ShotTarget { @@ -293,7 +308,8 @@ private void addTriggers() { new Trigger(shooter::atFlywheelVelocitySetpoint) // .debounce(0.05) .and(new Trigger(shooter::atHoodSetpoint).debounce(0.05)) - .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)); + .and(new Trigger(shooter::atTurretSetpoint).debounce(0.05)) + .and(new Trigger(AutoAim::targetInTurretDeadzone).negate()); } private void addTransitions() { @@ -325,6 +341,11 @@ private void addTransitions() { bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.SCORE_FLOW, readyTrigger); + bindTransition( + SuperState.SCORE_FLOW, + SuperState.SPIN_UP_SCORE_FLOW, + new Trigger(AutoAim::targetInTurretDeadzone)); + bindTransition( SuperState.SPIN_UP_SCORE_FLOW, SuperState.IDLE, @@ -360,6 +381,11 @@ private void addTransitions() { bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.FEED_FLOW, readyTrigger); + bindTransition( + SuperState.FEED_FLOW, + SuperState.SPIN_UP_FEED_FLOW, + new Trigger(AutoAim::targetInTurretDeadzone)); + bindTransition( SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate())); @@ -455,7 +481,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -469,7 +495,7 @@ private void addCommands() { intake.agitate(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -478,7 +504,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -494,7 +520,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -508,7 +534,7 @@ private void addCommands() { intake.intake(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -517,7 +543,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FeedTargets.getFeedTarget(feedTarget).getTranslation(), swerve.getVelocityFieldRelative(), @@ -533,7 +559,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -549,7 +575,7 @@ private void addCommands() { // intake.restExtended(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -560,7 +586,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -572,12 +598,12 @@ private void addCommands() { bindCommands( SuperState.SPIN_UP_SCORE_FLOW, - intake.restExtended(), + intake.intake(), indexer.rest(), shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -592,7 +618,7 @@ private void addCommands() { intake.intake(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -603,7 +629,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -653,7 +679,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -668,7 +694,7 @@ private void addCommands() { intake.restRetracted(), indexer.kick( () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), @@ -679,7 +705,7 @@ private void addCommands() { shooter.score( swerve::getPose, () -> - AutoAim.getCompensatedSOTMShotData( + AutoAim.getSOTMShotDataNewtonsMethod( shooter.getTurretPose(swerve.getPose()), FieldUtils.getCurrentHubTranslation(), swerve.getVelocityFieldRelative(), diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index b873e0e..1928be5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -4,6 +4,12 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -17,15 +23,20 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; +import frc.robot.utils.FuelSim; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.BooleanSupplier; @@ -46,7 +57,7 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final double HOOD_CURRENT_ZERO_THRESHOLD = 30.0; public static final double TURRET_CURRENT_ZERO_THRESHOLD = 30.0; // TODO find - public static final double FLYWHEEL_DIAMETER_INCHES = 4; + public static final double FLYWHEEL_DIAMETER_INCHES = 4.0; public static final Rotation2d TURRET_LEFT_FIXED_SHOT_ANGLE = Rotation2d.kZero; public static final Rotation2d TURRET_RIGHT_FIXED_SHOT_ANGLE = Rotation2d.kZero; @@ -113,17 +124,21 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { "Turret may have gone past hardstop!! Reoffset cancoders + min/max position", AlertType.kError); + private FuelSim fuelSim; + public TurretSubsystem( FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO, CANcoderIO cancoder24t, - CANcoderIO cancoder26t) { + CANcoderIO cancoder26t, + FuelSim fuelSim) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; this.turretIO = turretIO; this.cancoder24t = cancoder24t; this.cancoder26t = cancoder26t; + this.fuelSim = fuelSim; // assume we start up at min angle and not 0 hoodIO.resetEncoder(HOOD_MIN_ANGLE); @@ -175,8 +190,30 @@ public void periodic() { // && (getCalculatedTurretRotations().getDegrees() // < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees())); // if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert + + if (Superstructure.getState().isAScoreState()) { + System.out.println("launching fuel"); + fuelSim.launchFuel( + // there are few things i despise more than the units library\ + // InchesPerSecond.of( + // flywheelIO.getSetpointRotPerSec() * FLYWHEEL_DIAMETER_INCHES * Math.PI), + // InchesPerSecond.of(200), + angularToLinearVelocity( + RotationsPerSecond.of(flywheelIO.getSetpointRotPerSec()), + Inches.of(FLYWHEEL_DIAMETER_INCHES / 2)), + Rotation2d.fromDegrees(90).minus(hoodIO.getHoodSetpoint()).getMeasure(), + turretIO.getTurretSetpoint().getMeasure(), + Inches.of(13.75)); + } } + public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distance radius) { + return MetersPerSecond.of(vel.in(RadiansPerSecond) * radius.in(Meters) * 0.54 + 1); + } + + @Override + public void simulationPeriodic() {} + @Override public Command feed( Supplier robotPoseSupplier, diff --git a/src/main/java/frc/robot/utils/FuelSim.java b/src/main/java/frc/robot/utils/FuelSim.java new file mode 100644 index 0000000..e3f3405 --- /dev/null +++ b/src/main/java/frc/robot/utils/FuelSim.java @@ -0,0 +1,958 @@ +// https://github.com/hammerheads5000/FuelSim +package frc.robot.utils; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.Timer; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +public class FuelSim { + protected static final double PERIOD = 0.02; // sec + protected static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 + // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air + protected static final double AIR_DENSITY = 1.2041; // kg/m^3 + protected static final double FIELD_COR = + Math.sqrt(22 / 51.5); // coefficient of restitution with the field + protected static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel + protected static final double NET_COR = 0.2; // coefficient of restitution with the net + protected static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot + protected static final double FUEL_RADIUS = 0.075; + protected static final double FIELD_LENGTH = 16.51; + protected static final double FIELD_WIDTH = 8.04; + protected static final double TRENCH_WIDTH = 1.265; + protected static final double TRENCH_BLOCK_WIDTH = 0.305; + protected static final double TRENCH_HEIGHT = 0.565; + protected static final double TRENCH_BAR_HEIGHT = 0.102; + protected static final double TRENCH_BAR_WIDTH = 0.152; + protected static final double FRICTION = + 0.1; // proportion of horizontal vel to lose per sec while on ground + protected static final double FUEL_MASS = 0.448 * 0.45392; // kgs + protected static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; + // Drag coefficient of smooth sphere: + // https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg + protected static final double DRAG_COF = 0.47; // dimensionless + protected static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; + + protected static final Translation3d[] FIELD_XZ_LINE_STARTS = { + new Translation3d(0, 0, 0), + new Translation3d(3.96, 1.57, 0), + new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(4.61, 1.57, 0.165), + new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH - 1.57, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static final Translation3d[] FIELD_XZ_LINE_ENDS = { + new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), + new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static class Fuel { + protected Translation3d pos; + protected Translation3d vel; + + protected Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; + } + + protected Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } + + protected void update(boolean simulateAirResistance, int subticks) { + pos = pos.plus(vel.times(PERIOD / subticks)); + if (pos.getZ() > FUEL_RADIUS) { + Translation3d Fg = GRAVITY.times(FUEL_MASS); + Translation3d Fd = new Translation3d(); + + if (simulateAirResistance) { + double speed = vel.getNorm(); + if (speed > 1e-6) { + Fd = vel.times(-DRAG_FORCE_FACTOR * speed); + } + } + + Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); + vel = vel.plus(accel.times(PERIOD / subticks)); + } + if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { + vel = new Translation3d(vel.getX(), vel.getY(), 0); + vel = vel.times(1 - FRICTION * PERIOD / subticks); + // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS); + } + handleFieldCollisions(subticks); + } + + protected void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { + if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) + return; // not within y range + // Convert into 2D + Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); + Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); + Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); + Translation2d lineVec = end2d.minus(start2d); + + // Get closest point on line + Translation2d projected = + start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); + + if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) + return; // projected point not on line + double dist = pos2d.getDistance(projected); + if (dist > FUEL_RADIUS) return; // not intersecting line + // Back into 3D + Translation3d normal = + new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); + + // Apply collision response + pos = pos.plus(normal.times(FUEL_RADIUS - dist)); + if (vel.dot(normal) > 0) return; // already moving away from line + vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); + } + + protected void handleFieldCollisions(int subticks) { + // floor and bumps + for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); + } + + // edges + if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { + pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { + pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } + + if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { + pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { + pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } + + // hubs + handleHubCollisions(Hub.BLUE_HUB, subticks); + handleHubCollisions(Hub.RED_HUB, subticks); + + handleTrenchCollisions(); + } + + protected void handleHubCollisions(Hub hub, int subticks) { + hub.handleHubInteraction(this, subticks); + hub.fuelCollideSide(this); + + double netCollision = hub.fuelHitNet(this); + if (netCollision != 0) { + pos = pos.plus(new Translation3d(netCollision, 0, 0)); + vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); + } + } + + protected void handleTrenchCollisions() { + fuelCollideRectangle( + this, + new Translation3d(3.96, TRENCH_WIDTH, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d( + FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + } + + protected void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } + } + + protected static void handleFuelCollision(Fuel a, Fuel b) { + Translation3d normal = a.pos.minus(b.pos); + double distance = normal.getNorm(); + if (distance == 0) { + normal = new Translation3d(1, 0, 0); + distance = 1; + } + normal = normal.div(distance); + double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); + double intersection = FUEL_RADIUS * 2 - distance; + a.pos = a.pos.plus(normal.times(intersection / 2)); + b.pos = b.pos.minus(normal.times(intersection / 2)); + a.addImpulse(normal.times(impulse)); + b.addImpulse(normal.times(-impulse)); + } + + protected static final double CELL_SIZE = 0.25; + protected static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); + protected static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); + + @SuppressWarnings("unchecked") + protected final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; + + private final ArrayList> activeCells = new ArrayList<>(); + + protected void handleFuelCollisions(ArrayList fuels) { + // Clear grid + for (ArrayList cell : activeCells) { + cell.clear(); + } + activeCells.clear(); + + // Populate grid + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { + grid[col][row].add(fuel); + if (grid[col][row].size() == 1) { + activeCells.add(grid[col][row]); + } + } + } + + // Check collisions + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + // Check 3x3 neighbor cells + for (int i = col - 1; i <= col + 1; i++) { + for (int j = row - 1; j <= row + 1; j++) { + if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { + for (Fuel other : grid[i][j]) { + if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { + if (fuel.hashCode() < other.hashCode()) { + handleFuelCollision(fuel, other); + } + } + } + } + } + } + } + } + + protected ArrayList fuels = new ArrayList<>(); + protected boolean running = false; + protected boolean simulateAirResistance = false; + protected Supplier robotPoseSupplier = null; + protected Supplier robotFieldSpeedsSupplier = null; + protected double robotWidth; // size along the robot's y axis + protected double robotLength; // size along the robot's x axis + protected double bumperHeight; + protected ArrayList intakes = new ArrayList<>(); + protected int subticks = 5; + protected double loggingFreqHz = 10; + protected Timer loggingTimer = new Timer(); + + /** + * Creates a new instance of FuelSim + * + * @param tableKey NetworkTable to log fuel positions to as an array of {@link Translation3d} + * structs. + */ + public FuelSim(String tableKey) { + // Initialize grid + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j] = new ArrayList(); + } + } + + fuelPublisher = + NetworkTableInstance.getDefault() + .getStructArrayTopic(tableKey + "/Fuels", Translation3d.struct) + .publish(); + } + + /** Creates a new instance of FuelSim with log path "/Fuel Simulation" */ + public FuelSim() { + this("/Fuel Simulation"); + } + + /** Clears the field of fuel */ + public void clearFuel() { + fuels.clear(); + } + + /** Spawns fuel in the neutral zone and depots */ + public void spawnStartingFuel() { + // Center fuel + Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); + for (int i = 0; i < 15; i++) { + for (int j = 0; j < 6; j++) { + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus( + new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + } + } + + // Depots + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); + } + } + + // DEBUG: Log XZ lines + // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; + // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; + // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; + // } + + // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); + } + + protected StructArrayPublisher fuelPublisher; + + /** Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels" */ + public void logFuels() { + fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); + } + + /** Start the simulation. `updateSim` must still be called every loop */ + public void start() { + running = true; + loggingTimer.restart(); + } + + /** Pause the simulation. */ + public void stop() { + running = false; + loggingTimer.stop(); + } + + /** Enables accounting for drag force in physics step * */ + public void enableAirResistance() { + simulateAirResistance = true; + } + + /** + * Sets the number of physics iterations per loop (0.02s) + * + * @param subticks physics iteration per loop (default: 5) + */ + public void setSubticks(int subticks) { + this.subticks = subticks; + } + + /** + * Sets the frequency to publish fuel translations to NetworkTables Used to improve performance in + * AdvantageScope + * + * @param loggingFreqHz update frequency in hertz + */ + public void setLoggingFrequency(double loggingFreqHz) { + this.loggingFreqHz = loggingFreqHz; + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + double width, + double length, + double bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width; + this.robotLength = length; + this.bumperHeight = bumperHeight; + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight from the ground + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + Distance width, + Distance length, + Distance bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width.in(Meters); + this.robotLength = length.in(Meters); + this.bumperHeight = bumperHeight.in(Meters); + } + + /** To be called periodically Will do nothing if sim is not running */ + public void updateSim() { + if (!running) return; + + stepSim(); + } + + /** Run the simulation forward 1 time step (0.02s) */ + public void stepSim() { + for (int i = 0; i < subticks; i++) { + for (Fuel fuel : fuels) { + fuel.update(this.simulateAirResistance, this.subticks); + } + + handleFuelCollisions(fuels); + + if (robotPoseSupplier != null) { + handleRobotCollisions(fuels); + handleIntakes(fuels); + } + } + + if (loggingTimer.advanceIfElapsed(1.0 / loggingFreqHz)) { + logFuels(); + } + } + + /** + * Adds a fuel onto the field + * + * @param pos Position to spawn at + * @param vel Initial velocity vector + */ + public void spawnFuel(Translation3d pos, Translation3d vel) { + fuels.add(new Fuel(pos, vel)); + } + + /** + * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot + * movement + * + * @param launchVelocity Initial launch velocity + * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching + * straight up + * @param turretYaw Robot-relative turret yaw + * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's + * bumper height, or else it will collide with your robot immediately. + * @throws IllegalStateException if robot is not registered + */ + public void launchFuel( + LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { + if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { + throw new IllegalStateException("Robot must be registered before launching fuel."); + } + + Pose3d launchPose = + new Pose3d(this.robotPoseSupplier.get()) + .plus( + new Transform3d( + new Translation3d(Meters.zero(), Meters.zero(), launchHeight), + Rotation3d.kZero)); + ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); + + double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double xVel = + horizontalVel + * Math.cos(turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + double yVel = + horizontalVel + * Math.sin(turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + + xVel += fieldSpeeds.vxMetersPerSecond; + yVel += fieldSpeeds.vyMetersPerSecond; + + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + } + + protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { + Translation2d relativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero).relativeTo(robot).getTranslation(); + + if (fuel.pos.getZ() > bumperHeight) return; // above bumpers + double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); + double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); + double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); + double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); + + // not inside robot + if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) + return; + + Translation2d posOffset; + // find minimum distance to side and send corresponding collision response + if ((distanceToBottom >= distanceToTop + && distanceToBottom >= distanceToRight + && distanceToBottom >= distanceToLeft)) { + posOffset = new Translation2d(distanceToBottom, 0); + } else if ((distanceToTop >= distanceToBottom + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToLeft)) { + posOffset = new Translation2d(-distanceToTop, 0); + } else if ((distanceToRight >= distanceToBottom + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToLeft)) { + posOffset = new Translation2d(0, distanceToRight); + } else { + posOffset = new Translation2d(0, -distanceToLeft); + } + + posOffset = posOffset.rotateBy(robot.getRotation()); + fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); + Translation2d normal = posOffset.div(posOffset.getNorm()); + if (fuel.vel.toTranslation2d().dot(normal) < 0) + fuel.addImpulse( + new Translation3d( + normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); + if (robotVel.dot(normal) > 0) + fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); + } + + protected void handleRobotCollisions(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); + Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + for (Fuel fuel : fuels) { + handleRobotCollision(fuel, robot, robotVel); + } + } + + protected void handleIntakes(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + for (SimIntake intake : intakes) { + for (int i = 0; i < fuels.size(); i++) { + if (intake.shouldIntake(fuels.get(i), robot)) { + fuels.remove(i); + i--; + } + } + } + } + + protected static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { + if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) + return; // above rectangle + double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); + double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; + double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; + double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); + + // not inside hub + if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) + return; + + Translation2d collision; + // find minimum distance to side and send corresponding collision response + if (fuel.pos.getX() < start.getX() + || (distanceToLeft >= distanceToRight + && distanceToLeft >= distanceToTop + && distanceToLeft >= distanceToBottom)) { + collision = new Translation2d(distanceToLeft, 0); + } else if (fuel.pos.getX() >= end.getX() + || (distanceToRight >= distanceToLeft + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToBottom)) { + collision = new Translation2d(-distanceToRight, 0); + } else if (fuel.pos.getY() > end.getY() + || (distanceToTop >= distanceToLeft + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToBottom)) { + collision = new Translation2d(0, -distanceToTop); + } else { + collision = new Translation2d(0, distanceToBottom); + } + + if (collision.getX() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } else if (collision.getY() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); + } + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, + Distance xMax, + Distance yMin, + Distance yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + registerIntake( + xMin.in(Meters), + xMax.in(Meters), + yMin.in(Meters), + yMax.in(Meters), + ableToIntake, + intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake) { + registerIntake( + xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, Runnable intakeCallback) { + registerIntake( + xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters)); + } + + public static class Hub { + public static final Hub BLUE_HUB = + new Hub( + new Translation2d(4.61, FIELD_WIDTH / 2), + new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), + 1); + public static final Hub RED_HUB = + new Hub( + new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), + new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), + -1); + + protected static final double ENTRY_HEIGHT = 1.83; + protected static final double ENTRY_RADIUS = 0.56; + + protected static final double SIDE = 1.2; + + protected static final double NET_HEIGHT_MAX = 3.057; + protected static final double NET_HEIGHT_MIN = 1.5; + protected static final double NET_OFFSET = SIDE / 2 + 0.261; + protected static final double NET_WIDTH = 1.484; + + protected final Translation2d center; + protected final Translation3d exit; + protected final int exitVelXMult; + + protected int score = 0; + + protected Hub(Translation2d center, Translation3d exit, int exitVelXMult) { + this.center = center; + this.exit = exit; + this.exitVelXMult = exitVelXMult; + } + + protected void handleHubInteraction(Fuel fuel, int subticks) { + if (didFuelScore(fuel, subticks)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + } + } + + protected boolean didFuelScore(Fuel fuel, int subticks) { + return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS + && fuel.pos.getZ() <= ENTRY_HEIGHT + && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; + } + + protected Translation3d getDispersalVelocity() { + return new Translation3d( + exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); + } + + /** Reset this hub's score to 0 */ + public void resetScore() { + score = 0; + } + + /** + * Get the current count of fuel scored in this hub + * + * @return + */ + public int getScore() { + return score; + } + + protected void fuelCollideSide(Fuel fuel) { + fuelCollideRectangle( + fuel, + new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), + new Translation3d( + center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); + } + + protected double fuelHitNet(Fuel fuel) { + if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0; + if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 + || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) return 0; + if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { + return Math.max( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); + } else { + return Math.min( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); + } + } + } + + protected class SimIntake { + double xMin, xMax, yMin, yMax; + BooleanSupplier ableToIntake; + Runnable callback; + + protected SimIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + this.xMin = xMin; + this.xMax = xMax; + this.yMin = yMin; + this.yMax = yMax; + this.ableToIntake = ableToIntake; + this.callback = intakeCallback; + } + + protected boolean shouldIntake(Fuel fuel, Pose2d robotPose) { + if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false; + + Translation2d fuelRelativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robotPose) + .getTranslation(); + + boolean result = + fuelRelativePos.getX() >= xMin + && fuelRelativePos.getX() <= xMax + && fuelRelativePos.getY() >= yMin + && fuelRelativePos.getY() <= yMax; + if (result) { + callback.run(); + } + return result; + } + } +} diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index e461d58..9f32c7b 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -20,7 +20,7 @@ public class AutoAim { public static double LATENCY_COMPENSATION_SECS = // new LoggedTunableNumber("Latency time", 0.3).getAsDouble(); // 0.6; // TODO tune latency // comp - 0.3; + 0.0; // public static double SPIN_UP_SECS = 0.0; // TODO tune spinup time public static final InterpolatingShotTree ALPHA_HUB_SHOT_TREE = new InterpolatingShotTree(); @@ -203,12 +203,32 @@ public class AutoAim { // TODO: POPULATE beyond 24 feet and time of flight } + public static Pose2d getTurretPose(Pose2d robotPose) { + Pose2d turretPose = + robotPose.transformBy( + new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + return turretPose; + } + public static double distanceToHub(Pose2d pose) { double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); Logger.recordOutput("Autoaim/Distance To Hub", distance); return distance; } + public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPose) { + Translation2d robotToTarget = target.minus(robotPose.getTranslation()); + Rotation2d rot = Rotation2d.fromRadians(Math.atan2(robotToTarget.getY(), robotToTarget.getX())); + Logger.recordOutput("Autoaim/Target Rotation", rot); + return rot; + } + + public static Rotation2d getVirtualTargetYaw( + Translation2d target, ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose, double tof) { + Translation2d vtarget = getVirtualSOTMTarget(target, fieldRelativeSpeeds, tof); + return getTargetRotation(vtarget, robotPose); + } + // lock in public static Translation2d getVirtualSOTMTarget( Translation2d target, ChassisSpeeds fieldRelativeSpeeds, double timeOfFlightSecs) { @@ -223,16 +243,12 @@ public static Translation2d getVirtualSOTMTarget( } public static Rotation2d getVirtualTargetYaw( - Translation2d target, ChassisSpeeds fieldRelativeSpeeds, Pose2d robotPose, double tof) { - Translation2d vtarget = getVirtualSOTMTarget(target, fieldRelativeSpeeds, tof); - return getTargetRotation(vtarget, robotPose); - } - - public static Rotation2d getTargetRotation(Translation2d target, Pose2d robotPose) { - Translation2d robotToTarget = target.minus(robotPose.getTranslation()); - Rotation2d rot = Rotation2d.fromRadians(Math.atan2(robotToTarget.getY(), robotToTarget.getX())); - Logger.recordOutput("Autoaim/Target Rotation", rot); - return rot; + ChassisSpeeds fieldRelativeSpeeds, + Translation2d targetTranslation, + Pose2d robotPose, + InterpolatingShotTree tree) { + double tof = tree.calculateShot(robotPose, targetTranslation).timeOfFlightSecs(); + return getVirtualTargetYaw(targetTranslation, fieldRelativeSpeeds, robotPose, tof); } // if we have a turret im going to assume we're on comp @@ -241,9 +257,7 @@ public static Rotation2d getTurretTargetRotation( Pose2d robotPose, ChassisSpeeds chassisSpeeds, InterpolatingShotTree shotTree) { - Pose2d turretPose = - robotPose.transformBy( - new Transform2d(TurretSubsystem.ROBOT_TO_TURRET_TRANSLATION, Rotation2d.kZero)); + Pose2d turretPose = getTurretPose(robotPose); // get desired rotation to point at target Rotation2d turretTargetRotation = @@ -292,25 +306,97 @@ public static Rotation2d getTurretFeedTargetRotation( return getTurretTargetRotation(target, robotPose, chassisSpeeds, FEED_SHOT_TREE); } - public static Rotation2d getVirtualTargetYaw( - ChassisSpeeds fieldRelativeSpeeds, - Translation2d targetTranslation, - Pose2d robotPose, - InterpolatingShotTree tree) { - double tof = tree.calculateShot(robotPose, targetTranslation).timeOfFlightSecs(); - return getVirtualTargetYaw(targetTranslation, fieldRelativeSpeeds, robotPose, tof); - } - public static ShotData getSOTMShotData( Pose2d robotPose, Translation2d targetTranslation, ChassisSpeeds fieldRelativeSpeeds, InterpolatingShotTree tree) { + ShotData unadjustedShot = tree.calculateShot(robotPose, targetTranslation); Translation2d virtualTarget = getVirtualSOTMTarget( targetTranslation, fieldRelativeSpeeds, unadjustedShot.timeOfFlightSecs()); - return tree.get(robotPose.getTranslation().getDistance(virtualTarget)); + Pose2d turretPose = getTurretPose(robotPose); + + return tree.get(turretPose.getTranslation().getDistance(virtualTarget)); + } + + public static ShotData getSOTMShotDataNewtonsMethod( + Pose2d robotPose, + Translation2d targetTranslation, + ChassisSpeeds fieldRelativeSpeeds, + InterpolatingShotTree tree) { + + ShotData baseline = tree.calculateShot(robotPose, targetTranslation); + + Pose2d turretPose = getTurretPose(robotPose); + Translation2d turretToTarget = targetTranslation.minus(turretPose.getTranslation()); + + double distance = turretToTarget.getNorm(); + + // get just direction of vector because its vector divded by length + // dont want to account for magnitude bc the speed we are going and shot tree do + // and we just want direction to find dot product + Translation2d shotDirection = turretToTarget.div(distance); + + // dot product! <3 + // get how fast we are going towards where we are shooting + // vectors of robot times direction + // positive if going towrds + // zero is moving perpedicular + // negative is going away + double robotVelocityAlongShot = + fieldRelativeSpeeds.vxMetersPerSecond * shotDirection.getX() + + fieldRelativeSpeeds.vyMetersPerSecond * shotDirection.getY(); + + // required velocity is like velocity the ball must have so it hits the target while the robot + // moving + // because the ball velocity is robot velocity + ball velocity + // so velocity ball needs to go is our distance / tof - the dot product or velocity along that + // shot to account for the robots velocity along the shot + double requiredVelocity = (distance / baseline.timeOfFlightSecs()) - robotVelocityAlongShot; + + return calculateShotAdjustments(distance, baseline, requiredVelocity, tree); + } + + /** + * @param distance distance to target + * @param baseline daseline parameters from tree + * @param requiredVelocity required horizontal velocity magnitude + * @return adjusted shooter command + */ + private static ShotData calculateShotAdjustments( + double distance, ShotData baseline, double requiredVelocity, InterpolatingShotTree tree) { + + ShotData currentParams = baseline; + double currentDistance = distance; + double currentTime = currentParams.timeOfFlightSecs(); + double currentVelocity = currentDistance / currentTime; + + // iterate + for (int i = 0; i < 20; i++) { + final double EPSILON = 0.001; + // get deriv of velocity (dis/time) + double lowVel = + (currentDistance - EPSILON) / tree.get(currentDistance - EPSILON).timeOfFlightSecs(); + double highVel = + (currentDistance + EPSILON) / tree.get(currentDistance + EPSILON).timeOfFlightSecs(); + double velDeriv = (highVel - lowVel) / (EPSILON * 2); + // newtons method: xn+1 = xn - f(xn)/deriv(xn) + // so estimate for new dist is difference between current velocity required velocity over the + // deriv + // this makes sense because if current vel is larger it will lower current distance to account + // for that and if requird is larger it will increase to account for that + currentDistance -= (currentVelocity - requiredVelocity) / velDeriv; + // update + currentParams = tree.get(currentDistance); + currentTime = currentParams.timeOfFlightSecs(); + currentVelocity = currentDistance / currentTime; + } + return new ShotData( + currentParams.hoodAngle(), + currentParams.flywheelVelocityRotPerSec(), + currentParams.timeOfFlightSecs()); } public static ShotData getCompensatedSOTMShotData(