diff --git a/src/main/deploy/choreo/GHtoNI.traj b/src/main/deploy/choreo/GHtoNI.traj index 712dee5a..62989b5a 100644 --- a/src/main/deploy/choreo/GHtoNI.traj +++ b/src/main/deploy/choreo/GHtoNI.traj @@ -3,22 +3,26 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.806319713592529, "y":4.017168045043945, "heading":3.141592653589793, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.510753631591797, "y":4.746592998504639, "heading":-2.866875372751583, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":7.11117696762085, "y":5.479686737060547, "heading":3.490658503988659, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.75}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"GH.x", "val":5.806319713592529}, "y":{"exp":"GH.y", "val":4.017168045043945}, "heading":{"exp":"GH.heading", "val":3.141592653589793}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.510753631591797 m", "val":6.510753631591797}, "y":{"exp":"4.746592998504639 m", "val":4.746592998504639}, "heading":{"exp":"-2.866875372751583 rad", "val":-2.866875372751583}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.11117696762085 m", "val":7.11117696762085}, "y":{"exp":"5.479686737060547 m", "val":5.479686737060547}, "heading":{"exp":"NI.heading", "val":3.490658503988659}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.75 m / s", "val":1.75}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -26,38 +30,51 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.10984], + "waypoints":[0.0,0.61325,1.29213], "samples":[ - {"t":0.0, "x":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.24646, "ay":4.75959, "alpha":1.12862, "fx":[75.89529,69.82763,62.76526,69.19804], "fy":[71.78112,77.70855,83.50697,78.24401]}, - {"t":0.03827, "x":5.80943, "y":4.02065, "heading":3.14159, "vx":0.16251, "vy":0.18215, "omega":0.04319, "ax":4.24611, "ay":4.75919, "alpha":1.12834, "fx":[75.88743,69.82221,62.76182,69.19184], "fy":[71.77674,77.70264,83.49878,78.23681]}, - {"t":0.07654, "x":5.81876, "y":4.03111, "heading":-3.13994, "vx":0.32501, "vy":0.36429, "omega":0.08637, "ax":4.2457, "ay":4.75873, "alpha":1.12813, "fx":[75.87833,69.80587,62.75579,69.19654], "fy":[71.7716,77.70474,83.4907,78.21785]}, - {"t":0.11481, "x":5.83431, "y":4.04854, "heading":-3.13663, "vx":0.4875, "vy":0.54641, "omega":0.12955, "ax":4.24522, "ay":4.75819, "alpha":1.12798, "fx":[75.86762,69.77837,62.74702,69.21184], "fy":[71.76549,77.71457,83.48238,78.18682]}, - {"t":0.15308, "x":5.85607, "y":4.07293, "heading":-3.13168, "vx":0.64996, "vy":0.7285, "omega":0.17272, "ax":4.24464, "ay":4.75753, "alpha":1.1279, "fx":[75.8548,69.73939,62.73532,69.23732], "fy":[71.75817,77.73173,83.47325,78.14326]}, - {"t":0.19135, "x":5.88405, "y":4.1043, "heading":-3.12507, "vx":0.81241, "vy":0.91058, "omega":0.21588, "ax":4.24392, "ay":4.75673, "alpha":1.12787, "fx":[75.8391,69.68842,62.72042,69.27237], "fy":[71.74929,77.75565,83.46251,78.08652]}, - {"t":0.22962, "x":5.91825, "y":4.14263, "heading":-3.1168, "vx":0.97482, "vy":1.09262, "omega":0.25905, "ax":4.24303, "ay":4.75573, "alpha":1.12789, "fx":[75.81942,69.62474,62.70186,69.31609], "fy":[71.73828,77.78548,83.44901,78.01561]}, - {"t":0.26789, "x":5.95867, "y":4.18792, "heading":-3.10689, "vx":1.13721, "vy":1.27462, "omega":0.30221, "ax":4.24189, "ay":4.75444, "alpha":1.12796, "fx":[75.79411,69.54718,62.67887,69.36705], "fy":[71.72418,77.81988,83.43096,77.92899]}, - {"t":0.30616, "x":6.0053, "y":4.24019, "heading":-3.09532, "vx":1.29955, "vy":1.45658, "omega":0.34538, "ax":4.24036, "ay":4.75272, "alpha":1.12806, "fx":[75.76056,69.45384,62.64999,69.42291], "fy":[71.70525,77.85665,83.40552,77.82412]}, - {"t":0.34443, "x":6.05813, "y":4.29941, "heading":-3.08211, "vx":1.46183, "vy":1.63846, "omega":0.38855, "ax":4.23822, "ay":4.75031, "alpha":1.12817, "fx":[75.71427,69.34129,62.61234,69.47946], "fy":[71.67811,77.89181,83.36778,77.69645]}, - {"t":0.3827, "x":6.11718, "y":4.36559, "heading":-3.06724, "vx":1.62402, "vy":1.82026, "omega":0.43173, "ax":4.23501, "ay":4.7467, "alpha":1.12829, "fx":[75.64662,69.20267,62.55977,69.5283], "fy":[71.63564,77.91749,83.30822,77.53691]}, - {"t":0.42097, "x":6.18244, "y":4.43873, "heading":-3.05071, "vx":1.7861, "vy":2.00192, "omega":0.47491, "ax":4.22966, "ay":4.7407, "alpha":1.12838, "fx":[75.53819,69.02204,62.47716,69.55005], "fy":[71.56056,77.91536,83.20524,77.32434]}, - {"t":0.45924, "x":6.25389, "y":4.51882, "heading":-3.03254, "vx":1.94797, "vy":2.18335, "omega":0.51809, "ax":4.21897, "ay":4.72871, "alpha":1.12843, "fx":[75.33208,68.7516,62.3178,69.48686], "fy":[71.39985,77.83077,82.99532,76.99544]}, - {"t":0.49751, "x":6.33153, "y":4.60584, "heading":-3.01271, "vx":2.10943, "vy":2.36432, "omega":0.56127, "ax":4.18703, "ay":4.6929, "alpha":1.12875, "fx":[74.74931,68.15337,61.84491,69.05245], "fy":[70.88599,77.39257,82.36628,76.23525]}, - {"t":0.53579, "x":6.41532, "y":4.69976, "heading":-2.99123, "vx":2.26967, "vy":2.54391, "omega":0.60447, "ax":-0.00061, "ay":-0.00125, "alpha":0.15067, "fx":[0.45973,0.33382,-0.47976,-0.35385], "fy":[-0.37433,0.45675,0.33349,-0.49758]}, - {"t":0.57406, "x":6.50218, "y":4.79711, "heading":-2.9681, "vx":2.26965, "vy":2.54387, "omega":0.61024, "ax":-4.18707, "ay":-4.69297, "alpha":-1.12637, "fx":[-74.68492,-67.88782,-61.87018,-69.35932], "fy":[-70.96195,-77.62436,-82.33994,-75.95815]}, - {"t":0.61233, "x":6.58598, "y":4.89103, "heading":-2.94475, "vx":2.10941, "vy":2.36426, "omega":0.56713, "ax":-4.21897, "ay":-4.72871, "alpha":-1.13065, "fx":[-75.23551,-68.22235,-62.33018,-70.10043], "fy":[-71.50972,-78.29429,-82.97869,-76.43858]}, - {"t":0.6506, "x":6.66362, "y":4.97805, "heading":-2.92304, "vx":1.94795, "vy":2.1833, "omega":0.52386, "ax":-4.22964, "ay":-4.74066, "alpha":-1.13348, "fx":[-75.40372,-68.24941,-62.48719,-70.44614], "fy":[-71.71015,-78.59231,-83.19058,-76.51005]}, - {"t":0.68887, "x":6.73507, "y":5.05813, "heading":-2.90299, "vx":1.78608, "vy":2.00187, "omega":0.48048, "ax":-4.23498, "ay":-4.74664, "alpha":-1.13569, "fx":[-75.47469,-68.20697,-62.57093,-70.68298], "fy":[-71.82441,-78.78989,-83.29295,-76.48691]}, - {"t":0.72714, "x":6.80032, "y":5.13127, "heading":-2.8846, "vx":1.624, "vy":1.82021, "omega":0.43702, "ax":-4.23818, "ay":-4.75023, "alpha":-1.13749, "fx":[-75.50666,-68.14288,-62.6266,-70.86874], "fy":[-71.90412,-78.94171,-83.35043,-76.43247]}, - {"t":0.76541, "x":6.85937, "y":5.19745, "heading":-2.86788, "vx":1.4618, "vy":1.63842, "omega":0.39349, "ax":-4.24031, "ay":-4.75262, "alpha":-1.139, "fx":[-75.51961,-68.07312,-62.66868,-71.02287], "fy":[-71.96602,-79.06608,-83.3851,-76.36785]}, - {"t":0.80368, "x":6.9122, "y":5.25667, "heading":-2.85282, "vx":1.29953, "vy":1.45654, "omega":0.3499, "ax":-4.24184, "ay":-4.75433, "alpha":-1.14027, "fx":[-75.52242,-68.00454,-62.70299,-71.15378], "fy":[-72.01695,-79.1709,-83.40673,-76.30211]}, - {"t":0.84195, "x":6.95883, "y":5.30893, "heading":-2.83943, "vx":1.13719, "vy":1.27459, "omega":0.30626, "ax":-4.24297, "ay":-4.75561, "alpha":-1.14134, "fx":[-75.51975,-67.94054,-62.73215,-71.26579], "fy":[-72.0601,-79.26017,-83.42041,-76.23972]}, - {"t":0.88022, "x":6.99925, "y":5.35423, "heading":-2.82771, "vx":0.97481, "vy":1.09259, "omega":0.26258, "ax":-4.24386, "ay":-4.7566, "alpha":-1.14222, "fx":[-75.51438,-67.883,-62.75739,-71.36135], "fy":[-72.09708,-79.33616,-83.42916,-76.18314]}, - {"t":0.91849, "x":7.03344, "y":5.39256, "heading":-2.81766, "vx":0.81239, "vy":0.91055, "omega":0.21887, "ax":-4.24457, "ay":-4.7574, "alpha":-1.14294, "fx":[-75.50807,-67.83302,-62.77934,-71.442], "fy":[-72.12874,-79.40025,-83.43487,-76.13382]}, - {"t":0.95676, "x":7.06143, "y":5.42392, "heading":-2.80928, "vx":0.64995, "vy":0.72848, "omega":0.17513, "ax":-4.24515, "ay":-4.75805, "alpha":-1.14351, "fx":[-75.50201,-67.7913,-62.79825,-71.50875], "fy":[-72.15554,-79.45334,-83.43884,-76.09266]}, - {"t":0.99503, "x":7.08319, "y":5.44832, "heading":-2.80258, "vx":0.48749, "vy":0.54639, "omega":0.13136, "ax":-4.24563, "ay":-4.7586, "alpha":-1.14393, "fx":[-75.49706,-67.75828,-62.81426,-71.56232], "fy":[-72.17774,-79.49604,-83.44199,-76.06023]}, - {"t":1.0333, "x":7.09874, "y":5.46575, "heading":-2.79755, "vx":0.32501, "vy":0.36428, "omega":0.08758, "ax":-4.24604, "ay":-4.75906, "alpha":-1.14421, "fx":[-75.49383,-67.73429,-62.8274,-71.60321], "fy":[-72.1955,-79.5288,-83.44499,-76.03693]}, - {"t":1.07157, "x":7.10807, "y":5.4762, "heading":-2.7942, "vx":0.16251, "vy":0.18215, "omega":0.04379, "ax":-4.24639, "ay":-4.75946, "alpha":-1.14436, "fx":[-75.49277,-67.71952,-62.83769,-71.6318], "fy":[-72.20892,-79.55194,-83.4483,-76.02301]}, - {"t":1.10984, "x":7.11118, "y":5.47969, "heading":-2.79253, "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":5.80632, "y":4.01717, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.44832, "ay":4.57253, "alpha":0.95227, "fx":[77.95667,72.83283,67.31326,72.78319], "fy":[69.4746,74.84295,79.83107,74.86029]}, + {"t":0.02787, "x":5.80805, "y":4.01894, "heading":3.14159, "vx":0.124, "vy":0.12746, "omega":0.02654, "ax":4.44728, "ay":4.57223, "alpha":0.9688, "fx":[78.02899,72.8203,67.20117,72.76777], "fy":[69.37719,74.84137,79.91147,74.85878]}, + {"t":0.05575, "x":5.81323, "y":4.02427, "heading":-3.14085, "vx":0.24796, "vy":0.25491, "omega":0.05355, "ax":4.44609, "ay":4.57187, "alpha":0.98784, "fx":[78.11188,72.80212,67.07171,72.75442], "fy":[69.26525,74.84327,80.00411,74.85275]}, + {"t":0.08362, "x":5.82187, "y":4.03316, "heading":-3.13936, "vx":0.3719, "vy":0.38235, "omega":0.08109, "ax":4.44469, "ay":4.57145, "alpha":1.00999, "fx":[78.20787,72.77755,66.92067,72.74296], "fy":[69.13523,74.84889,80.11187,74.84173]}, + {"t":0.1115, "x":5.83396, "y":4.04559, "heading":-3.1371, "vx":0.49579, "vy":0.50978, "omega":0.10924, "ax":4.44305, "ay":4.57094, "alpha":1.03608, "fx":[78.32035,72.74559,66.74241,72.73312], "fy":[68.98235,74.85854,80.23858,74.82514]}, + {"t":0.13937, "x":5.84951, "y":4.06158, "heading":-3.13405, "vx":0.61964, "vy":0.63719, "omega":0.13812, "ax":4.44108, "ay":4.57032, "alpha":1.06723, "fx":[78.45397,72.70489,66.52909,72.72455], "fy":[68.79997,74.87266,80.38952,74.80214]}, + {"t":0.16725, "x":5.86851, "y":4.08111, "heading":-3.1302, "vx":0.74344, "vy":0.76459, "omega":0.16787, "ax":4.43867, "ay":4.56956, "alpha":1.10505, "fx":[78.61533,72.65354,66.26948,72.71671], "fy":[68.57862,74.89183,80.57216,74.77155]}, + {"t":0.19512, "x":5.89096, "y":4.1042, "heading":-3.12553, "vx":0.86716, "vy":0.89197, "omega":0.19867, "ax":4.43566, "ay":4.56858, "alpha":1.1519, "fx":[78.81407,72.58874,65.94698,72.70877], "fy":[68.30432,74.91691,80.79746,74.73161]}, + {"t":0.223, "x":5.91685, "y":4.13084, "heading":-3.11999, "vx":0.99081, "vy":1.01931, "omega":0.23078, "ax":4.43181, "ay":4.56729, "alpha":1.21143, "fx":[79.06493,72.50629,65.53585,72.69944], "fy":[67.95541,74.94917,81.08208,74.6796]}, + {"t":0.25087, "x":5.94619, "y":4.16103, "heading":-3.11355, "vx":1.11434, "vy":1.14663, "omega":0.26455, "ax":4.42669, "ay":4.56553, "alpha":1.28957, "fx":[79.39151,72.39942,64.99408,72.68653], "fy":[67.49664,74.99065,81.45275,74.61102]}, + {"t":0.27875, "x":5.97897, "y":4.19476, "heading":-3.10618, "vx":1.23774, "vy":1.27389, "omega":0.3005, "ax":4.41955, "ay":4.56298, "alpha":1.39659, "fx":[79.83418,72.25663,64.24796,72.66615], "fy":[66.86636,75.04475,81.95512,74.51791]}, + {"t":0.30662, "x":6.01519, "y":4.23204, "heading":-3.0978, "vx":1.36093, "vy":1.40108, "omega":0.33943, "ax":4.40893, "ay":4.55898, "alpha":1.55205, "fx":[80.46809,72.05659,63.15558,72.63049], "fy":[65.9464,75.11775,82.67404,74.38454]}, + {"t":0.3345, "x":6.05484, "y":4.27287, "heading":-3.08834, "vx":1.48383, "vy":1.52816, "omega":0.38269, "ax":4.39151, "ay":4.55194, "alpha":1.79828, "fx":[81.45044,71.75459,61.40453,72.56169], "fy":[64.47817,75.22259,83.78636,74.17505]}, + {"t":0.36237, "x":6.09791, "y":4.31724, "heading":-3.07768, "vx":1.60624, "vy":1.65505, "omega":0.43282, "ax":4.35782, "ay":4.53682, "alpha":2.24681, "fx":[83.17221,71.23859,58.14952,72.40767], "fy":[61.76829,75.39112,85.72852,73.78542]}, + {"t":0.39025, "x":6.14438, "y":4.36513, "heading":-3.06561, "vx":1.72772, "vy":1.78151, "omega":0.49545, "ax":4.26733, "ay":4.48829, "alpha":3.31397, "fx":[86.91845,70.1254,50.08083,71.92635], "fy":[55.14178,75.7286,89.89445,72.73533]}, + {"t":0.41812, "x":6.19419, "y":4.41654, "heading":-3.0518, "vx":1.84667, "vy":1.90662, "omega":0.58782, "ax":3.5218, "ay":3.79584, "alpha":9.2402, "fx":[97.98295,65.68955,5.62091,61.00554], "fy":[19.10819,76.90486,99.58998,52.61629]}, + {"t":0.446, "x":6.24704, "y":4.47116, "heading":-3.03541, "vx":1.94484, "vy":2.01243, "omega":0.84539, "ax":-4.28525, "ay":-4.09291, "alpha":5.1111, "fx":[-40.1275,-76.56844,-93.01057,-70.5161], "fy":[-92.57695,-61.78055,-39.80783,-73.48005]}, + {"t":0.47387, "x":6.29959, "y":4.52566, "heading":-3.01185, "vx":1.82539, "vy":1.89834, "omega":0.98786, "ax":-4.44002, "ay":-4.44724, "alpha":1.60702, "fx":[-63.54553,-74.06505,-80.88251,-71.85008], "fy":[-81.18911,-71.45504,-64.00033,-74.17105]}, + {"t":0.50175, "x":6.34874, "y":4.57685, "heading":-2.98431, "vx":1.70162, "vy":1.77438, "omega":1.03266, "ax":-4.45644, "ay":-4.50478, "alpha":0.7283, "fx":[-68.82484,-73.54803,-76.7209,-72.3232], "fy":[-77.51886,-72.98853,-69.73363,-74.33747]}, + {"t":0.52962, "x":6.39444, "y":4.62456, "heading":-2.95553, "vx":1.5774, "vy":1.64881, "omega":1.05296, "ax":-4.46075, "ay":-4.52728, "alpha":0.33202, "fx":[-71.10908,-73.27447,-74.70449,-72.61082], "fy":[-75.77696,-73.66581,-72.24272,-74.36436]}, + {"t":0.5575, "x":6.43668, "y":4.66876, "heading":-2.92618, "vx":1.45306, "vy":1.52261, "omega":1.06221, "ax":-4.46234, "ay":-4.53914, "alpha":0.10662, "fx":[-72.37384,-73.07637,-73.52385,-72.82911], "fy":[-74.77036,-74.07938,-73.64206,-74.33357]}, + {"t":0.58537, "x":6.47545, "y":4.70944, "heading":-2.89657, "vx":1.32867, "vy":1.39608, "omega":1.06519, "ax":-4.46304, "ay":-4.54644, "alpha":-0.03892, "fx":[-73.17067,-72.91116,-72.75326,-73.01386], "fy":[-74.12107,-74.37762,-74.53006,-74.2735]}, + {"t":0.61325, "x":6.51075, "y":4.74659, "heading":-2.86688, "vx":1.20426, "vy":1.26935, "omega":1.0641, "ax":-2.44055, "ay":2.1635, "alpha":-5.42363, "fx":[-54.11864,-55.62183,-23.06998,-26.78293], "fy":[41.85674,13.41314,29.65636,56.55029]}, + {"t":0.64719, "x":6.55023, "y":4.79093, "heading":-2.83076, "vx":1.12142, "vy":1.34279, "omega":0.88, "ax":-0.44111, "ay":0.36427, "alpha":-7.76268, "fx":[-33.16555,-21.69825,20.24332,5.77518], "fy":[19.72552,-21.05323,-8.19369,33.34159]}, + {"t":0.68114, "x":6.58804, "y":4.83672, "heading":-2.80088, "vx":1.10645, "vy":1.35515, "omega":0.61651, "ax":-0.06208, "ay":0.0506, "alpha":-6.79631, "fx":[-24.75657,-12.25733,22.89195,10.06263], "fy":[12.47341,-23.22638,-10.853,24.91507]}, + {"t":0.71508, "x":6.62556, "y":4.88274, "heading":-2.77996, "vx":1.10434, "vy":1.35687, "omega":0.38581, "ax":-0.00836, "ay":0.0068, "alpha":-5.70586, "fx":[-20.33251,-9.08634,20.07509,8.79739], "fy":[9.48355,-20.28626,-9.26386,20.51116]}, + {"t":0.74902, "x":6.66304, "y":4.92881, "heading":-2.76686, "vx":1.10406, "vy":1.3571, "omega":0.19213, "ax":-0.00109, "ay":0.00089, "alpha":-4.60515, "fx":[-16.42116,-7.01913,16.38688,6.98216], "fy":[7.36569,-16.54212,-7.33689,16.57128]}, + {"t":0.78297, "x":6.70051, "y":4.97487, "heading":-2.76034, "vx":1.10402, "vy":1.35713, "omega":0.03582, "ax":-0.00014, "ay":0.00011, "alpha":-3.49927, "fx":[-12.50314,-5.23966,12.49872,5.23504], "fy":[5.50638,-12.61336,-5.50271,12.61705]}, + {"t":0.81691, "x":6.73799, "y":5.02094, "heading":-2.75912, "vx":1.10402, "vy":1.35713, "omega":-0.08296, "ax":-0.00002, "ay":0.00001, "alpha":-2.39061, "fx":[-8.54515,-3.56782,8.54459,3.56726], "fy":[3.7504,-8.62249,-3.74994,8.62295]}, + {"t":0.85085, "x":6.77546, "y":5.067, "heading":-2.76194, "vx":1.10401, "vy":1.35713, "omega":-0.16411, "ax":0.0, "ay":0.0, "alpha":-1.28068, "fx":[-4.57197,-1.92422,4.5719,1.92415], "fy":[2.02193,-4.61388,-2.02188,4.61394]}, + {"t":0.8848, "x":6.81294, "y":5.11307, "heading":-2.76751, "vx":1.10401, "vy":1.35713, "omega":-0.20758, "ax":0.0, "ay":0.0, "alpha":-0.17031, "fx":[-0.6065,-0.25931,0.6065,0.2593], "fy":[0.27227,-0.61215,-0.27227,0.61216]}, + {"t":0.91874, "x":6.85041, "y":5.15914, "heading":-2.77456, "vx":1.10401, "vy":1.35713, "omega":-0.21336, "ax":0.0, "ay":0.0, "alpha":0.9401, "fx":[3.33711,1.45511,-3.33707,-1.45506], "fy":[-1.52644,3.36879,1.52641,-3.36882]}, + {"t":0.95269, "x":6.88789, "y":5.2052, "heading":-2.7818, "vx":1.10401, "vy":1.35713, "omega":-0.18145, "ax":-0.00001, "ay":-0.00003, "alpha":2.05022, "fx":[7.25331,3.22637,-7.25348,-3.22654], "fy":[-3.38201,7.32322,3.38105,-7.32417]}, + {"t":0.98663, "x":6.92536, "y":5.25127, "heading":-2.78796, "vx":1.10401, "vy":1.35713, "omega":-0.11186, "ax":-0.33801, "ay":-0.41547, "alpha":3.11622, "fx":[5.4952,-0.60239,-16.58611,-10.40989], "fy":[-12.18154,4.3763,-1.6329,-17.73043]}, + {"t":1.02057, "x":6.96264, "y":5.2971, "heading":-2.79176, "vx":1.09254, "vy":1.34303, "omega":-0.00608, "ax":-3.99683, "ay":-4.91319, "alpha":0.04395, "fx":[-65.08604,-65.40546,-65.5944,-65.27637], "fy":[-80.52708,-80.26468,-80.11556,-80.37803]}, + {"t":1.05452, "x":6.99742, "y":5.33985, "heading":-2.79196, "vx":0.95687, "vy":1.17626, "omega":-0.00459, "ax":-4.017, "ay":-4.93799, "alpha":0.02753, "fx":[-65.51038,-65.71173,-65.83005,-65.62925], "fy":[-80.85642,-80.69188,-80.59699,-80.76157]}, + {"t":1.08846, "x":7.02759, "y":5.37694, "heading":-2.79212, "vx":0.82052, "vy":1.00864, "omega":-0.00366, "ax":-4.02364, "ay":-4.94615, "alpha":0.02213, "fx":[-65.65014,-65.81238,-65.90753,-65.74565], "fy":[-80.96468,-80.83234,-80.75564,-80.88801]}, + {"t":1.12241, "x":7.05312, "y":5.40832, "heading":-2.79224, "vx":0.68394, "vy":0.84075, "omega":-0.0029, "ax":-4.02695, "ay":-4.95021, "alpha":0.01945, "fx":[-65.71968,-65.86241,-65.94605,-65.80359], "fy":[-81.01851,-80.90217,-80.83458,-80.95094]}, + {"t":1.15635, "x":7.07402, "y":5.43401, "heading":-2.79234, "vx":0.54725, "vy":0.67272, "omega":-0.00224, "ax":-4.02892, "ay":-4.95264, "alpha":0.01785, "fx":[-65.76128,-65.89233,-65.96908,-65.83827], "fy":[-81.0507,-80.94394,-80.88182,-80.98859]}, + {"t":1.19029, "x":7.09027, "y":5.45399, "heading":-2.79242, "vx":0.41049, "vy":0.50461, "omega":-0.00164, "ax":-4.03024, "ay":-4.95426, "alpha":0.01678, "fx":[-65.78897,-65.91223,-65.98441,-65.86135], "fy":[-81.07212,-80.97174,-80.91326,-81.01365]}, + {"t":1.22424, "x":7.10189, "y":5.46827, "heading":-2.79247, "vx":0.27369, "vy":0.33644, "omega":-0.00107, "ax":-4.03118, "ay":-4.95541, "alpha":0.01602, "fx":[-65.80872,-65.92643,-65.99534,-65.87782], "fy":[-81.0874,-80.99156,-80.93568,-81.03153]}, + {"t":1.25818, "x":7.10885, "y":5.47683, "heading":-2.79251, "vx":0.13686, "vy":0.16824, "omega":-0.00052, "ax":-4.03188, "ay":-4.95628, "alpha":0.01545, "fx":[-65.82352,-65.93707,-66.00353,-65.89015], "fy":[-81.09884,-81.00641,-80.95249,-81.04493]}, + {"t":1.29213, "x":7.11118, "y":5.47969, "heading":-2.79253, "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/Autos.java b/src/main/java/frc/robot/Autos.java index 1885b188..8e8e9cc0 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -24,12 +24,14 @@ import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.ManipulatorSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.shoulder.ShoulderSubsystem; +import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.utils.autoaim.AlgaeIntakeTargets; +import frc.robot.subsystems.wrist.WristSubsystem.WristState; +import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.CoralTargets; import java.util.HashMap; import java.util.Optional; import java.util.function.Supplier; @@ -45,7 +47,7 @@ public class Autos { private final ShoulderSubsystem shoulder; private final WristSubsystem wrist; - @AutoLogOutput public static boolean autoPreScore = true; + @AutoLogOutput public static boolean autoPreScore = false; // true; @AutoLogOutput public static boolean autoScore = false; // TODO perhaps this should not be static @AutoLogOutput public static boolean autoGroundCoralIntake = false; @AutoLogOutput public static boolean autoAlgaeIntake = false; @@ -161,7 +163,7 @@ public Command LOtoJ() { routine // run first path .active() - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) .whileTrue(Commands.sequence(steps.get("LOtoJ").resetOdometry(), steps.get("LOtoJ").cmd())); // run middle paths // and puts that name + corresponding traj to the map @@ -188,7 +190,7 @@ public Command LOtoJ() { routine .observe(steps.get("LtoPLM").done()) - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))); + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L2))); return routine.cmd().alongWith(Commands.print("auto :(")); } @@ -209,7 +211,7 @@ public Command ROtoE() { routine // run first path .active() - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) .whileTrue(Commands.sequence(steps.get("ROtoE").resetOdometry(), steps.get("ROtoE").cmd())); // run middle paths // and puts that name + corresponding traj to the map @@ -222,7 +224,7 @@ public Command ROtoE() { routine .observe(steps.get("CtoPRM").done()) - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))); + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L2))); return routine.cmd(); } @@ -338,9 +340,8 @@ public Command CMtoGH() { // algae path .whileTrue( Commands.sequence( steps.get("CMtoG").resetOdometry(), - Commands.waitSeconds(1.5), + // Commands.waitSeconds(1.5), steps.get("CMtoG").cmd())); - routine .observe(steps.get("CMtoG").done()) // TODO change to time based .onTrue(Commands.sequence(scoreCoralInAuto(() -> steps.get("CMtoG").getFinalPose().get()))); @@ -363,7 +364,14 @@ public Command CMtoGH() { // algae path .onTrue( Commands.sequence( intakeAlgaeInAuto(() -> steps.get("NItoIJ").getFinalPose()), - swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2))); + swerve.driveTeleop(() -> new ChassisSpeeds(-0.5, 0, 0)).withTimeout(0.2), + steps.get("IJtoNI").cmd())); + routine.observe( + steps + .get("IJtoNI") + .atTime(steps.get("IJtoNI").getRawTrajectory().getTotalTime() - 0.2) + .onTrue(Commands.sequence(scoreAlgaeInAuto(), steps.get("NItoEF").cmd()))); + return routine.cmd(); // routine // .observe( @@ -426,7 +434,6 @@ public Command CMtoGH() { // algae path // routine // .observe(steps.get("NItoEF").done()) // .onTrue(intakeAlgaeInAuto(() -> steps.get("NItoEF").getFinalPose())); - return routine.cmd(); } public Command LOtoA() { // 2910 @@ -438,11 +445,12 @@ public Command LOtoA() { // 2910 steps.put("AtoB", routine.trajectory("AtoB")); steps.put("BtoB", routine.trajectory("BtoB")); - if (Robot.isSimulation()) manipulator.setSecondBeambreak(true); // gah + // if (Robot.isSimulation()) manipulator.setSimSecondBeambreak(true); // gah //TODO why did i do + // this???? routine // run first path .active() - .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L4))) + .onTrue(Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L4))) .whileTrue(Commands.sequence(steps.get("LOtoA").resetOdometry(), steps.get("LOtoA").cmd())); routine .observe( @@ -453,10 +461,10 @@ public Command LOtoA() { // 2910 AutoAim.translateToPose(swerve, () -> steps.get("AtoB").getInitialPose().get()) .until( () -> - elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + elevator.atExtension(ElevatorState.HP.getExtensionMeters()) && AutoAim.isInTolerance( swerve.getPose(), steps.get("AtoB").getInitialPose().get())), - Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2)), + Commands.runOnce(() -> Robot.setCoralTarget(ReefTarget.L2)), steps.get("AtoB").cmd())); routine .observe( @@ -468,7 +476,7 @@ public Command LOtoA() { // 2910 Commands.runOnce( () -> { autoGroundCoralIntake = false; - Robot.setCurrentTarget(ReefTarget.L4); + Robot.setCoralTarget(ReefTarget.L4); })); routine @@ -480,7 +488,7 @@ public Command LOtoA() { // 2910 AutoAim.translateToPose(swerve, () -> steps.get("BtoB").getInitialPose().get()) .until( () -> - elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS) + elevator.atExtension(ElevatorState.HP.getExtensionMeters()) && AutoAim.isInTolerance( swerve.getPose(), steps.get("BtoB").getInitialPose().get())), Commands.runOnce(() -> autoGroundCoralIntake = true), @@ -527,7 +535,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { () -> AutoAim.isInTolerance( swerve.getPose(), - CoralTargets.getClosestTarget(trajEndPose.get()), + FieldUtils.CoralTargets.getClosestTarget(trajEndPose.get()), swerve.getVelocityFieldRelative(), Units.inchesToMeters(1.0), Units.degreesToRadians(1.0)) @@ -551,7 +559,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { Commands.waitUntil(() -> !manipulator.getSecondBeambreak()) .alongWith( Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setSecondBeambreak(false)) + ? Commands.runOnce(() -> manipulator.setSimSecondBeambreak(false)) : Commands.none()), Commands.runOnce( () -> { @@ -563,7 +571,7 @@ public Command scoreCoralInAuto(Supplier trajEndPose) { .andThen( AutoAim.translateToPose( swerve, - () -> CoralTargets.getClosestTarget(trajEndPose.get()), + () -> FieldUtils.CoralTargets.getClosestTarget(trajEndPose.get()), ChassisSpeeds::new, new Constraints(1.5, 1.0)))); } @@ -574,7 +582,7 @@ public Command intakeCoralInAuto(Supplier> pose) { } else { return Commands.sequence( Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setSecondBeambreak(true)) + ? Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true)) : Commands.none(), Commands.print("intake - 2nd bb" + manipulator.getSecondBeambreak()), AutoAim.translateToPose( @@ -594,7 +602,7 @@ public Command intakeCoralInAuto(Supplier> pose) { } public void bindCoralElevatorExtension(AutoRoutine routine) { - bindCoralElevatorExtension(routine, 3.75); // TODO tune + bindCoralElevatorExtension(routine, 4); // TODO tune } public void bindCoralElevatorExtension(AutoRoutine routine, double toleranceMeters) { @@ -665,13 +673,13 @@ public Command scoreAlgaeInAuto() { // oh good lord Commands.print("Scoring algae"), Commands.runOnce( () -> { - Robot.setCurrentAlgaeScoreTarget(AlgaeScoreTarget.NET); + Robot.setAlgaeScoreTarget(AlgaeScoreTarget.BARGE); autoScore = true; }), Commands.waitUntil(() -> !manipulator.hasAlgae()) .alongWith( Robot.isSimulation() - ? Commands.runOnce(() -> manipulator.setHasAlgae(false)) + ? Commands.runOnce(() -> manipulator.setSimHasAlgae(false)) : Commands.none()) .andThen( Commands.runOnce( @@ -699,30 +707,32 @@ public Command intakeAlgaeInAuto(Supplier> pose) { Commands.runOnce( () -> { autoAlgaeIntake = true; - Robot.setCurrentAlgaeIntakeTarget( - AlgaeIntakeTargets.getClosestTarget(pose.get().get()) // wow + Robot.setAlgaeIntakeTarget( + FieldUtils.AlgaeIntakeTargets.getClosestTarget(pose.get().get()) // wow .height); }), AutoAim.translateToPose( swerve, () -> - AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose(pose.get().get()))) + FieldUtils.AlgaeIntakeTargets.getOffsetLocation( + FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(pose.get().get()))) .until( () -> AutoAim.isInTolerance( swerve.getPose(), - AlgaeIntakeTargets.getOffsetLocation( - AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose())), + FieldUtils.AlgaeIntakeTargets.getOffsetLocation( + FieldUtils.AlgaeIntakeTargets.getClosestTargetPose( + swerve.getPose())), swerve.getVelocityFieldRelative(), Units.inchesToMeters(0.5), Units.degreesToRadians(1.0)) - && elevator.isNearTarget() - && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), + && elevator.atExtension() + && shoulder.isNearAngle(ShoulderState.INTAKE_ALGAE_REEF.getAngle()) + && wrist.isNearAngle(WristState.INTAKE_ALGAE_REEF.getAngle())), AutoAim.approachAlgae( - swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), 1) + swerve, + () -> FieldUtils.AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), + 1) .withTimeout(0.5)) .andThen( Commands.runOnce( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cd62681c..7e007484 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -29,8 +29,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -42,19 +42,20 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.ExtensionKinematics; -import frc.robot.subsystems.ExtensionKinematics.ExtensionState; -import frc.robot.subsystems.ExtensionPathing; import frc.robot.subsystems.FunnelSubsystem; import frc.robot.subsystems.ManipulatorSubsystem; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.Superstructure.SuperState; import frc.robot.subsystems.beambreak.BeambreakIOReal; +import frc.robot.subsystems.camera.CameraIO; +import frc.robot.subsystems.camera.CameraIOReal; +import frc.robot.subsystems.camera.CameraIOSim; import frc.robot.subsystems.climber.ClimberIOReal; import frc.robot.subsystems.climber.ClimberIOSim; import frc.robot.subsystems.climber.ClimberSubsystem; @@ -69,18 +70,30 @@ import frc.robot.subsystems.shoulder.ShoulderIOReal; import frc.robot.subsystems.shoulder.ShoulderIOSim; import frc.robot.subsystems.shoulder.ShoulderSubsystem; -import frc.robot.subsystems.swerve.*; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOReal; -import frc.robot.subsystems.vision.VisionIOSim; -import frc.robot.subsystems.wrist.*; +import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; +import frc.robot.subsystems.swerve.AlphaSwerveConstants; +import frc.robot.subsystems.swerve.BansheeSwerveConstants; +import frc.robot.subsystems.swerve.GyroIOPigeon2; +import frc.robot.subsystems.swerve.GyroIOSim; +import frc.robot.subsystems.swerve.KelpieSwerveConstants; +import frc.robot.subsystems.swerve.ModuleIO; +import frc.robot.subsystems.swerve.ModuleIOMapleSim; +import frc.robot.subsystems.swerve.ModuleIOReal; +import frc.robot.subsystems.swerve.PhoenixOdometryThread; +import frc.robot.subsystems.swerve.SwerveConstants; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.wrist.WristIOReal; +import frc.robot.subsystems.wrist.WristIOSim; +import frc.robot.subsystems.wrist.WristSubsystem; +import frc.robot.subsystems.wrist.WristSubsystem.WristState; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.FieldUtils; +import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; +import frc.robot.utils.FieldUtils.CageTargets; +import frc.robot.utils.FieldUtils.CoralTargets; +import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.Tracer; -import frc.robot.utils.autoaim.AlgaeIntakeTargets; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.CageTargets; -import frc.robot.utils.autoaim.CoralTargets; -import frc.robot.utils.autoaim.L1Targets; import java.util.HashMap; import java.util.Optional; import java.util.Set; @@ -126,53 +139,20 @@ private RobotHardware(SwerveConstants swerveConstants) { public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; // For replay to work properly this should match the hardware used in the log public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE; - // for testing class loading - public static final ExtensionState test = - ExtensionPathing.getNearest(new ExtensionState(0.0, Rotation2d.kZero, Rotation2d.kZero)); + public static final boolean TUNING_MODE = true; public static enum ReefTarget { - L1( - ElevatorSubsystem.L1_EXTENSION_METERS, - 3.0, - WristSubsystem.WRIST_SCORE_L1_POS, - ShoulderSubsystem.SHOULDER_SCORE_POS), - L2( - ElevatorSubsystem.L2_EXTENSION_METERS, - -15.0, - WristSubsystem.WRIST_SCORE_L2_POS, - ShoulderSubsystem.SHOULDER_SCORE_POS), - L3( - ElevatorSubsystem.L3_EXTENSION_METERS, - -15.0, - WristSubsystem.WRIST_SCORE_L3_POS, - ShoulderSubsystem.SHOULDER_SCORE_POS), - L4( - ElevatorSubsystem.L4_EXTENSION_METERS, - -20.0, - WristSubsystem.WRIST_SCORE_L4_POS, - ShoulderSubsystem.SHOULDER_SCORE_L4_POS); - - public final double elevatorHeight; + L1(3.0, SuperState.PRE_L1), + L2(-15.0, SuperState.L2), + L3(-15.0, SuperState.L3), + L4(-20.0, SuperState.L4); + public final double outtakeSpeed; - public final Rotation2d wristAngle; - public final Rotation2d shoulderAngle; - - private ReefTarget( - double elevatorHeight, - double outtakeSpeed, - Rotation2d wristAngle, - Rotation2d shoulderAngle) { - this.elevatorHeight = elevatorHeight; - this.outtakeSpeed = outtakeSpeed; - this.wristAngle = wristAngle; - this.shoulderAngle = shoulderAngle; - } + public final SuperState state; - private ReefTarget(double elevatorHeight, Rotation2d wristAngle, Rotation2d shoulderAngle) { - this.elevatorHeight = elevatorHeight; - this.outtakeSpeed = 15.0; - this.wristAngle = wristAngle; - this.shoulderAngle = shoulderAngle; + private ReefTarget(double outtakeSpeed, SuperState state) { + this.outtakeSpeed = outtakeSpeed; + this.state = state; } } @@ -184,25 +164,106 @@ public static enum AlgaeIntakeTarget { } public static enum AlgaeScoreTarget { - NET, + BARGE, PROCESSOR } - @AutoLogOutput private static ReefTarget currentTarget = ReefTarget.L4; + @AutoLogOutput private static ReefTarget coralTarget = ReefTarget.L4; @AutoLogOutput private static AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; - @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; + @AutoLogOutput private static AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.BARGE; private boolean leftHandedTarget = false; - @AutoLogOutput private boolean killVisionIK = true; - @AutoLogOutput private boolean haveAutosGenerated = false; private static CANBus canivore = new CANBus("*"); private static CANBusStatus canivoreStatus = canivore.getStatus(); - private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); - private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + private static final CommandXboxControllerSubsystem driver = + new CommandXboxControllerSubsystem(0); + private static final CommandXboxControllerSubsystem operator = + new CommandXboxControllerSubsystem(1); + + private static Supplier pose = () -> new Pose2d(); + public static Supplier state = () -> SuperState.IDLE; + + @AutoLogOutput(key = "Superstructure/Pre Score Request") + public static Trigger preScoreReq = + driver + .rightTrigger() + .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) + .or( + () -> + pose.get() + .getTranslation() + .minus( + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_REEF_CENTER + : AutoAim.RED_REEF_CENTER) + .getNorm() + < 3.25 + && DriverStation.isAutonomous()); + + @AutoLogOutput(key = "Superstructure/Score Request") + public static Trigger scoreReq = + driver + .rightTrigger() + .negate() + .and(() -> DriverStation.isTeleop()) + .or(() -> Autos.autoScore && DriverStation.isAutonomous()); + + @AutoLogOutput(key = "Superstructure/Algae Intake Request") + public static Trigger intakeAlgaeReq = + driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()); + + @AutoLogOutput(key = "Superstructure/Coral Intake Request") + public static Trigger intakeCoralReq = + driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()); + + @AutoLogOutput(key = "Superstructure/Pre Climb Request") + public static Trigger preClimbReq = + driver + .x() + .and(driver.pov(-1).negate()) + .debounce(0.25) + .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)); + + @AutoLogOutput(key = "Superstructure/Climb Confirm Request") + public static Trigger climbConfReq = driver.rightTrigger(); + + @AutoLogOutput(key = "Superstructure/Climb Cancel Request") + public static Trigger climbCancelReq = + driver.y().debounce(0.5).or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)); + + @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") + public static final Trigger antiJamCoralReq = driver.a(); + + @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") + public static final Trigger antiJamAlgaeReq = driver.b(); + + @AutoLogOutput(key = "Superstructure/Home Request") + public static Trigger homeReq = driver.start(); + + @AutoLogOutput(key = "Superstructure/Rev Funnel Req") + public static Trigger revFunnelReq = operator.rightBumper(); + + @AutoLogOutput(key = "Superstructure/Force Funnel Req") + public static Trigger forceFunnelReq = operator.leftBumper(); + + // @AutoLogOutput(key = "Superstructure/Force Index Req") // TODO what? + // public static Trigger forceIndexReq = operator.povDown(); + + @AutoLogOutput(key = "Superstructure/Jog Coral Up Req") // i dont like it either + public static Trigger jogCoralUpReq = operator.povUp(); + + @AutoLogOutput(key = "Superstructure/Jog Coral Down Req") // i dont like it either + public static Trigger jogCoralDownReq = operator.povDown(); + + // killVisionIK, DoubleSupplier coralAdjust) + // new Trigger(() -> killVisionIK) + // .or(() -> coralTarget == ReefTarget.L1) + // .or(() -> DriverStation.isAutonomous()), + // () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); // Create and configure a drivetrain simulation configuration private Optional driveTrainSimulationConfig = @@ -252,13 +313,13 @@ public static enum AlgaeScoreTarget { ROBOT_TYPE != RobotType.SIM ? new GyroIOPigeon2(ROBOT_HARDWARE.swerveConstants.getGyroID()) : new GyroIOSim(swerveDriveSimulation.get().getGyroSimulation()), - Stream.of(ROBOT_HARDWARE.swerveConstants.getVisionConstants()) + Stream.of(ROBOT_HARDWARE.swerveConstants.getCameraConstants()) .map( (constants) -> ROBOT_TYPE == RobotType.REAL - ? new VisionIOReal(constants) - : new VisionIOSim(constants)) - .toArray(VisionIO[]::new), + ? new CameraIOReal(constants) + : new CameraIOSim(constants)) + .toArray(CameraIO[]::new), ROBOT_TYPE != RobotType.SIM ? new ModuleIO[] { new ModuleIOReal( @@ -293,14 +354,10 @@ public static enum AlgaeScoreTarget { swerveDriveSimulation.get().getModules()[3]) }, PhoenixOdometryThread.getInstance(), - swerveDriveSimulation, - ROBOT_TYPE != RobotType.SIM - ? new VisionIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants()) - : new VisionIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants())); - - private final ElevatorSubsystem elevator = - new ElevatorSubsystem( - ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim()); + swerveDriveSimulation); + // ROBOT_TYPE != RobotType.SIM + // ? new CameraIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants()) + // : new CameraIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeCameraConstants())); private final ManipulatorSubsystem manipulator = new ManipulatorSubsystem( @@ -356,7 +413,9 @@ public static enum AlgaeScoreTarget { .withForwardSoftLimitEnable(true) .withForwardSoftLimitThreshold(0.5))) : new WristIOSim()); - + private final ElevatorSubsystem elevator = + new ElevatorSubsystem( + ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim()); private final FunnelSubsystem funnel = new FunnelSubsystem( ROBOT_TYPE != RobotType.SIM @@ -381,126 +440,7 @@ public static enum AlgaeScoreTarget { new ClimberSubsystem(ROBOT_TYPE != RobotType.SIM ? new ClimberIOReal() : new ClimberIOSim()); private final Superstructure superstructure = - new Superstructure( - elevator, - manipulator, - shoulder, - wrist, - funnel, - climber, - swerve::getPose, - swerve::getVelocityFieldRelative, - () -> currentTarget, - () -> algaeIntakeTarget, - () -> algaeScoreTarget, - driver - .rightTrigger() - .negate() - .and(() -> DriverStation.isTeleop()) - .or( - new Trigger( - () -> { - final var state = - new ExtensionState( - elevator.getExtensionMeters(), - shoulder.getAngle(), - wrist.getAngle()); - final var branch = - ExtensionKinematics.getBranchPose( - swerve.getPose(), state, currentTarget); - final var manipulatorPose = - ExtensionKinematics.getManipulatorPose(swerve.getPose(), state); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("IK/Manipulator Pose", manipulatorPose); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("IK/Branch", branch); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "IK/Extension Check", - manipulatorPose, - manipulatorPose.transformBy( - new Transform3d( - Units.inchesToMeters(3.0), 0.0, 0.0, new Rotation3d()))); - return false; - // return branch - // .getTranslation() - // .getDistance(manipulatorPose.getTranslation()) - // < Units.inchesToMeters(1.5) - // || branch - // .getTranslation() - // .getDistance( - // manipulatorPose - // .transformBy( - // new Transform3d( - // Units.inchesToMeters(3.0), - // 0.0, - // 0.0, - // new Rotation3d())) - // .getTranslation()) - // < Units.inchesToMeters(1.5); - }) - .debounce(0.15)) - // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) - .or(() -> Autos.autoScore && DriverStation.isAutonomous()) - .or( - new Trigger( - () -> - AutoAim.isInToleranceCoral( - swerve.getPose(), - Units.inchesToMeters(1.5), - Units.degreesToRadians(1.5)) - && MathUtil.isNear( - 0, - Math.hypot( - swerve.getVelocityRobotRelative().vxMetersPerSecond, - swerve.getVelocityRobotRelative().vyMetersPerSecond), - AutoAim.VELOCITY_TOLERANCE_METERSPERSECOND) - && MathUtil.isNear( - 0.0, - swerve.getVelocityRobotRelative().omegaRadiansPerSecond, - 3.0) - && currentTarget != ReefTarget.L4 - && currentTarget != ReefTarget.L1) - .debounce(0.08) - .and(() -> swerve.hasFrontTags)), - driver - .rightTrigger() - .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) - .or( - () -> - swerve - .getPose() - .getTranslation() - .minus( - DriverStation.getAlliance().orElse(Alliance.Blue) - == Alliance.Blue - ? AutoAim.BLUE_REEF_CENTER - : AutoAim.RED_REEF_CENTER) - .getNorm() - < 3.25 - && DriverStation.isAutonomous()), - driver.leftTrigger().or(() -> Autos.autoAlgaeIntake && DriverStation.isAutonomous()), - driver.leftBumper().or(() -> Autos.autoGroundCoralIntake && DriverStation.isAutonomous()), - driver - .x() - .and(driver.pov(-1).negate()) - .debounce(0.5) - .or(operator.x().and(operator.pov(-1).negate()).debounce(0.5)), - driver.rightTrigger(), - driver - .y() - .debounce(0.5) - .or(operator.leftStick().and(operator.rightTrigger()).debounce(0.5)), - driver.a(), - driver.b(), - driver.start(), - operator.rightBumper(), - operator.leftBumper(), - operator.povDown(), - new Trigger(() -> killVisionIK) - .or(() -> currentTarget == ReefTarget.L1) - .or(() -> DriverStation.isAutonomous()), - () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); + new Superstructure(elevator, shoulder, wrist, manipulator, funnel, swerve); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); @@ -516,20 +456,29 @@ public static enum AlgaeScoreTarget { new LoggedMechanism2d(3.0, Units.feetToMeters(4.0)); private final LoggedMechanismRoot2d elevatorRoot = // CAD distance from origin to center of carriage at full retraction - elevatorMech2d.getRoot("Elevator", Units.inchesToMeters(21.5), 0.0); + elevatorMech2d.getRoot( + "Elevator", Units.inchesToMeters(21.5), 0.0); // now what on earth is this number + // doesn't get updated or actually do anything it's just so i remember there's actually an + // elevator there when i'm looking at glass + private final LoggedMechanismLigament2d firstStage = + new LoggedMechanismLigament2d( + "First Stage", Units.inchesToMeters(41.925), ELEVATOR_ANGLE.getDegrees()); private final LoggedMechanismLigament2d carriageLigament = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); private final LoggedMechanismLigament2d shoulderLigament = - new LoggedMechanismLigament2d( - "Arm", Units.inchesToMeters(15.7), ShoulderSubsystem.SHOULDER_RETRACTED_POS.getDegrees()); + new LoggedMechanismLigament2d("Arm", Units.inchesToMeters(15.7), 90.0); private final LoggedMechanismLigament2d wristLigament = new LoggedMechanismLigament2d( "Wrist", Units.inchesToMeters(14.9), WristSubsystem.WRIST_RETRACTED_POS.getDegrees()); - public static Supplier state = - () -> SuperState.IDLE; // TODO i feel like im breaking a rule + private final LoggedMechanismRoot2d climberRoot = + elevatorMech2d.getRoot("Climber", Units.inchesToMeters(2), 0); + private final LoggedMechanismLigament2d climberBase = + new LoggedMechanismLigament2d("Climber Base", Units.inchesToMeters(9.5), 90); + private final LoggedMechanismLigament2d climberLigament = + new LoggedMechanismLigament2d("Climber", Units.inchesToMeters(12), 0.0); - @SuppressWarnings("resource") + @SuppressWarnings({"resource", "unlikely-arg-type"}) public Robot() { DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); @@ -586,100 +535,35 @@ public Robot() { SimulatedArena.getInstance().addDriveTrainSimulation(swerveDriveSimulation.orElse(null)); swerve.resetPose(swerveDriveSimulation.get().getSimulatedDriveTrainPose()); // global static is mildly questionable - VisionIOSim.pose = () -> new Pose3d(swerveDriveSimulation.get().getSimulatedDriveTrainPose()); + CameraIOSim.pose = () -> new Pose3d(swerveDriveSimulation.get().getSimulatedDriveTrainPose()); } else { // this should never be called? - VisionIOSim.pose = () -> new Pose3d(); + CameraIOSim.pose = () -> new Pose3d(); } // Add the arms and stuff + elevatorRoot.append(firstStage); elevatorRoot.append(carriageLigament); carriageLigament.append(shoulderLigament); shoulderLigament.append(wristLigament); + shoulderLigament.setColor(new Color8Bit(Color.kBlue)); + wristLigament.setColor(new Color8Bit(Color.kGreen)); + + climberRoot.append(climberBase); + climberBase.append(climberLigament); + climberLigament.setColor(new Color8Bit(Color.kPurple)); autos = new Autos(swerve, manipulator, funnel, elevator, shoulder, wrist); autoChooser.addDefaultOption("None", autos.getNoneAuto()); - SmartDashboard.putData( - "Run Elevator Sysid", - elevator - .runSysid() - .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); - - SmartDashboard.putData( - "Step Elevator Current", - elevator - .setCurrent(60.0) - .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); - - SmartDashboard.putData( - "Check Clear", - Commands.parallel( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))); - SmartDashboard.putData( "Manual Zero Extension", Commands.runOnce( () -> { elevator.resetExtension(0.0); - wrist.resetPosition(Rotation2d.k180deg); + wrist.rezero(Rotation2d.k180deg); }) .ignoringDisable(true)); - System.out.println("Node Count " + ExtensionPathing.graph.nodes().size()); - - SmartDashboard.putData( - "Traverse Extension Graph", - superstructure - .extendWithClearance( - () -> - new ExtensionState( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) - .until( - () -> - elevator.isNearExtension(ElevatorSubsystem.HP_EXTENSION_METERS) - && shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS)) - .andThen( - Commands.sequence( - ExtensionPathing.graph.nodes().stream() - .map( - (node) -> - superstructure - .extendWithClearance(() -> node) - .alongWith( - Commands.print("Traversing to " + node), - Commands.runOnce( - () -> Logger.recordOutput("Traversal Target", node))) - .until( - () -> - elevator.isNearExtension(node.elevatorHeightMeters()) - && shoulder.isNearAngle(node.shoulderAngle()) - && wrist.isNearAngle(node.wristAngle())) - .finallyDo(() -> System.out.println("done")) - .andThen( - Commands.waitSeconds(0.5), - superstructure - .extendWithClearance( - () -> - new ExtensionState( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) - .alongWith(Commands.print("Retracting")) - .until( - () -> - elevator.isNearExtension( - ElevatorSubsystem.HP_EXTENSION_METERS) - && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_HP_POS) - && wrist.isNearAngle( - WristSubsystem.WRIST_HP_POS)), - Commands.waitSeconds(0.5))) - .toArray(Command[]::new)))); - // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); @@ -695,6 +579,7 @@ public Robot() { || superstructure.getState() == SuperState.READY_CORAL) .onTrue(driver.rumbleCmd(1.0, 1.0).withTimeout(0.5)); + // Set auto scoring, etc bindings to false at the start of teleop new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoScore = false)); @@ -704,14 +589,20 @@ public Robot() { new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoGroundCoralIntake = false)); + new Trigger(Robot::isSimulation) + .and(() -> DriverStation.isAutonomousEnabled()) + .onTrue(Commands.runOnce(() -> manipulator.setSimSecondBeambreak(true))); + + // Zero elevator/wrist at the start of auto new Trigger(() -> DriverStation.isAutonomousEnabled() && !wrist.hasZeroed) .onTrue( Commands.runOnce( () -> { - wrist.resetPosition(Rotation2d.fromRadians(3.094)); + wrist.rezero(Rotation2d.fromRadians(3.094)); elevator.resetExtension(0.0); })); + // Add autos on alliance change new Trigger( () -> { var allianceChange = !DriverStation.getAlliance().equals(lastAlliance); @@ -723,6 +614,7 @@ public Robot() { .alongWith(leds.setBlinkingCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); + // Add autos when first connecting to DS new Trigger( () -> DriverStation.isDSAttached() @@ -734,6 +626,7 @@ public Robot() { .alongWith(leds.setBlinkingCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); + // Disable swerve current limits in auto I think? new Trigger(() -> DriverStation.isAutonomousEnabled()) .onTrue( Commands.runOnce( @@ -750,6 +643,12 @@ public Robot() { ROBOT_HARDWARE.swerveConstants.getDriveConfig().CurrentLimits)) .ignoringDisable(true)); + // Rumble controller when climber is fully extended + new Trigger(() -> state.get() == SuperState.PRE_CLIMB) + .and(superstructure::atExtension) + .debounce(0.1) + .onTrue(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()); + SmartDashboard.putData( "Add Autos", Commands.runOnce( @@ -759,27 +658,72 @@ public Robot() { } }) .ignoringDisable(true)); - elevator.setDefaultCommand( - Commands.sequence( - elevator.runCurrentZeroing().onlyIf(() -> !elevator.hasZeroed), - elevator.setExtension(0.0).until(() -> elevator.isNearExtension(0.0)), - elevator.setVoltage(0.0)) - .withName("Elevator Default Command")); - - manipulator.setDefaultCommand(manipulator.hold()); - - shoulder.setDefaultCommand(shoulder.hold()); - - wrist.setDefaultCommand(wrist.hold()); - - funnel.setDefaultCommand(funnel.setVoltage(0.0)); - - climber.setDefaultCommand(climber.setPosition(0.0)); + SmartDashboard.putData( + "[SIM ONLY] Toggle First Beambreak", + Robot.isSimulation() + ? Commands.runOnce( + () -> manipulator.setSimFirstBeambreak(!manipulator.getFirstBeambreak())) + : Commands.none()); + SmartDashboard.putData( + "[SIM ONLY] Toggle Second Beambreak", + Robot.isSimulation() + ? Commands.runOnce( + () -> manipulator.setSimSecondBeambreak(!manipulator.getSecondBeambreak())) + : Commands.none()); + SmartDashboard.putData( + "[SIM ONLY] Toggle Has Algae", + Robot.isSimulation() + ? Commands.runOnce(() -> manipulator.setSimHasAlgae(!manipulator.hasAlgae())) + : Commands.none()); + + new Trigger(wrist::atSetpoint) + .negate() + .debounce(2) + .whileTrue( + Commands.runOnce( + () -> + SmartDashboard.putString( + "Wrist has not hit the setpoint for 2 seconds", "FF0000"))) + .whileFalse( + Commands.runOnce( + () -> + SmartDashboard.putString( + "Wrist has not hit the setpoint for 2 seconds", + "00FF00"))); // tune specific time maybe + + elevator.setDefaultCommand(elevator.setStateExtension()); + shoulder.setDefaultCommand(shoulder.setStateAngle()); + wrist.setDefaultCommand(wrist.setStateAngle()); + manipulator.setDefaultCommand(manipulator.setRollerVelocity(0.0)); + funnel.setDefaultCommand( + funnel.setRollerVoltage( + () -> + superstructure.getState() == SuperState.IDLE && revFunnelReq.getAsBoolean() + ? -2.0 + : (forceFunnelReq.getAsBoolean() + || (Stream.of(FieldUtils.HumanPlayerTargets.values()) + .map( + (t) -> + t.location + .minus(swerve.getPose()) + .getTranslation() + .getNorm()) + .min(Double::compare) + .get() + < 1.0) + ? 1.0 + : (antiJamCoralReq.getAsBoolean() + ? -10.0 + : 0.0)))); // at what point do ternary operators do more harm than good + climber.setDefaultCommand( + climber.setPosition( + () -> superstructure.getState().climberPosition, + () -> superstructure.getState().climberSpeed)); // why does it need to be slow leds.setDefaultCommand( Commands.either( leds.setBlinkingCmd( - () -> LEDSubsystem.getReefTargetColor(currentTarget), + () -> LEDSubsystem.getReefTargetColor(coralTarget), () -> superstructure.getState() == SuperState.IDLE ? Color.kBlack @@ -811,10 +755,16 @@ public Robot() { * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed()) .times(-1))); + // ----Controller bindings---- + // Auto align for coral scoring L2/3 driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike() && currentTarget != ReefTarget.L1) + .and( + () -> + superstructure.stateIsCoralAlike() + && coralTarget != ReefTarget.L1 + && coralTarget != ReefTarget.L4) .whileTrue( Commands.parallel( AutoAim.autoAimWithIntermediatePose( @@ -835,10 +785,11 @@ public Robot() { Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align for coral scoring L1 driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike() && currentTarget == ReefTarget.L1) + .and(() -> superstructure.stateIsCoralAlike() && coralTarget == ReefTarget.L1) .whileTrue( Commands.parallel( AutoAim.alignToLine( @@ -860,6 +811,60 @@ public Robot() { L1Targets.getNearestLine(swerve.getPose()).getRotation()))) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to intermediate pose to score L4 + driver + .rightBumper() + .or(driver.leftBumper()) + .and(() -> superstructure.stateIsCoralAlike() && coralTarget == ReefTarget.L4) + .and(() -> !superstructure.atExtension(SuperState.L4)) + .whileTrue( + Commands.parallel( + AutoAim.translateToPose( + swerve, + () -> { + var twist = swerve.getVelocityFieldRelative().toTwist2d(0.3); + return CoralTargets.getHandedClosestTarget( + swerve + .getPose() + .plus( + new Transform2d( + twist.dx, + twist.dy, + Rotation2d.fromRadians(twist.dtheta))), + driver.leftBumper().getAsBoolean()) + .exp(new Twist2d(-0.25, 0, 0)); + }), + Commands.waitUntil( + () -> + AutoAim.isInToleranceCoral( + swerve.getPose())) // don't know if this does anything + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + + // Auto align to score L4 + driver + .rightBumper() + .or(driver.leftBumper()) + .and(() -> superstructure.stateIsCoralAlike() && coralTarget == ReefTarget.L4) + .and(() -> superstructure.atExtension(SuperState.L4)) + .debounce(0.25) + .whileTrue( + Commands.parallel( + AutoAim.translateToPose( + swerve, + () -> { + var twist = swerve.getVelocityFieldRelative().toTwist2d(0.3); + return CoralTargets.getHandedClosestTarget( + swerve + .getPose() + .plus( + new Transform2d( + twist.dx, twist.dy, Rotation2d.fromRadians(twist.dtheta))), + driver.leftBumper().getAsBoolean()); + }), + Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + + // Auto align to intake algae (high, low, stack) driver .rightBumper() .and( @@ -891,11 +896,10 @@ public Robot() { swerve.getVelocityFieldRelative(), Units.inchesToMeters(1.0), Units.degreesToRadians(1.0)) - && elevator.isNearTarget() + && elevator.atExtension() && shoulder.isNearAngle( - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) - && wrist.isNearAngle( - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), + ShoulderState.INTAKE_ALGAE_REEF.getAngle()) + && wrist.isNearAngle(WristState.INTAKE_ALGAE_REEF.getAngle())), AutoAim.approachAlgae( swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), @@ -919,28 +923,30 @@ public Robot() { .and(() -> swerve.hasFrontTags)) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); - driver - .rightBumper() - .or(driver.leftBumper()) - .and(() -> superstructure.getState() == SuperState.INTAKE_ALGAE_GROUND) - .whileTrue( - swerve.driveToAlgae( - () -> - modifyJoystick(driver.getLeftY()) - * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), - () -> - modifyJoystick(driver.getLeftX()) - * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), - () -> - modifyJoystick(driver.getRightX()) - * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())); + // driver + // .rightBumper() + // .or(driver.leftBumper()) + // .and(() -> superstructure.getState() == SuperState.INTAKE_ALGAE_GROUND) + // .whileTrue( + // swerve.driveToAlgae( + // () -> + // modifyJoystick(driver.getLeftY()) + // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + // () -> + // modifyJoystick(driver.getLeftX()) + // * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + // () -> + // modifyJoystick(driver.getRightX()) + // * ROBOT_HARDWARE.swerveConstants.getMaxAngularSpeed())) + + // Auto align to processor driver .rightBumper() .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.READY_ALGAE - || superstructure.getState() == SuperState.PRE_PROCESSOR) + || superstructure.getState() == SuperState.PROCESSOR) .and(() -> algaeScoreTarget == AlgaeScoreTarget.PROCESSOR) .whileTrue( Commands.parallel( @@ -967,6 +973,7 @@ public Robot() { swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to cage driver .rightBumper() .and( @@ -984,13 +991,14 @@ public Robot() { swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Auto align to barge driver .rightBumper() .and( () -> superstructure.getState() == SuperState.READY_ALGAE - || superstructure.getState() == SuperState.PRE_NET) - .and(() -> algaeScoreTarget == AlgaeScoreTarget.NET) + || superstructure.getState() == SuperState.PRE_BARGE) + .and(() -> algaeScoreTarget == AlgaeScoreTarget.BARGE) .whileTrue( Commands.parallel( AutoAim.translateToXCoord( @@ -1021,22 +1029,30 @@ public Robot() { }) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + // Set sim beambreak/algae driver .povUp() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setFirstBeambreak(true)).ignoringDisable(true)); + .onTrue( + Commands.runOnce(() -> manipulator.setSimFirstBeambreak(true)).ignoringDisable(true)); driver .povDown() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setFirstBeambreak(false)).ignoringDisable(true)); + .onTrue( + Commands.runOnce(() -> manipulator.setSimFirstBeambreak(false)).ignoringDisable(true)); driver .povRight() .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setHasAlgae(!manipulator.hasAlgae()))); + .onTrue(Commands.runOnce(() -> manipulator.setSimHasAlgae(!manipulator.hasAlgae()))); - RobotModeTriggers.autonomous() - .and(() -> ROBOT_TYPE == RobotType.SIM) - .onTrue(Commands.runOnce(() -> manipulator.setSecondBeambreak(true)).ignoringDisable(true)); + // RobotModeTriggers.autonomous() + // .and(() -> ROBOT_TYPE == RobotType.SIM) + // .onTrue( + // Commands.runOnce(() -> + // manipulator.setSimSecondBeambreak(true)).ignoringDisable(true)); + + // Reset sim pose (?) + // Has this literally ever been used driver .start() .onTrue( @@ -1046,14 +1062,17 @@ public Robot() { swerveDriveSimulation.get().setSimulationWorldPose(swerve.getPose()); } })); + + // Rezero shoulder driver.x().onTrue(Commands.runOnce(() -> shoulder.rezero()).ignoringDisable(true)); + // Operator - Set scoring/intaking levels operator .a() .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L1; + coralTarget = ReefTarget.L1; algaeIntakeTarget = AlgaeIntakeTarget.GROUND; })); operator @@ -1061,49 +1080,50 @@ public Robot() { .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L2; - algaeIntakeTarget = AlgaeIntakeTarget.LOW; + coralTarget = ReefTarget.L2; + algaeIntakeTarget = AlgaeIntakeTarget.STACK; })); operator .b() .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L3; - algaeIntakeTarget = AlgaeIntakeTarget.HIGH; + coralTarget = ReefTarget.L3; + algaeIntakeTarget = AlgaeIntakeTarget.LOW; })); operator .y() .onTrue( Commands.runOnce( () -> { - currentTarget = ReefTarget.L4; - algaeIntakeTarget = AlgaeIntakeTarget.STACK; + coralTarget = ReefTarget.L4; + algaeIntakeTarget = AlgaeIntakeTarget.HIGH; })); - operator.leftTrigger().onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.NET)); + operator + .leftTrigger() + .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.BARGE)); operator .rightTrigger() .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.PROCESSOR)); + // Enable/disable left handed auto align + // TODO isn't this already accounted for by the autoaim method? operator.povLeft().onTrue(Commands.runOnce(() -> leftHandedTarget = true)); operator.povRight().onTrue(Commands.runOnce(() -> leftHandedTarget = false)); - operator - .back() - .and(operator.start()) - .onTrue(Commands.runOnce(() -> killVisionIK = !killVisionIK)); - + // Set LEDs when the robot has an algae (alga?) new Trigger(() -> superstructure.stateIsAlgaeAlike()) .whileTrue( leds.setBlinkingSplitCmd( () -> LEDSubsystem.getAlgaeIntakeTargetColor(algaeIntakeTarget), () -> LEDSubsystem.getAlgaeScoringTargetColor( - algaeScoreTarget == AlgaeScoreTarget.NET), + algaeScoreTarget == AlgaeScoreTarget.BARGE), () -> Color.kBlack, 5.0)); + // heading reset driver .leftStick() @@ -1142,15 +1162,6 @@ public Robot() { Stream.of(AlgaeIntakeTargets.values()) .map((target) -> AlgaeIntakeTargets.getRobotTargetLocation(target.location)) .toArray(Pose2d[]::new)); - - Logger.recordOutput("IK/L1 FK Pose", ExtensionKinematics.L1_POSE); - System.out.println("ExtensionKinematics.L1_POSE: " + ExtensionKinematics.L1_POSE); - Logger.recordOutput("IK/L2 FK Pose", ExtensionKinematics.L2_POSE); - System.out.println("ExtensionKinematics.L2_POSE: " + ExtensionKinematics.L2_POSE); - Logger.recordOutput("IK/L3 FK Pose", ExtensionKinematics.L3_POSE); - System.out.println("ExtensionKinematics.L3_POSE: " + ExtensionKinematics.L3_POSE); - Logger.recordOutput("IK/L4 FK Pose", ExtensionKinematics.L4_POSE); - System.out.println("ExtensionKinematics.L4_POSE: " + ExtensionKinematics.L4_POSE); } private void addAutos() { @@ -1212,8 +1223,7 @@ public void robotPeriodic() { "MapleSim/Pose", swerveDriveSimulation.get().getSimulatedDriveTrainPose()); } - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Targets/Reef Target", currentTarget); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Targets/Reef Target", coralTarget); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Targets/Algae Intake Target", algaeIntakeTarget); if (Robot.ROBOT_TYPE != RobotType.REAL) @@ -1322,6 +1332,8 @@ public void robotPeriodic() { // Minus 90 to make it relative to horizontal shoulderLigament.setAngle(shoulder.getAngle().getDegrees() - 90); wristLigament.setAngle(wrist.getAngle().getDegrees() + shoulderLigament.getAngle()); + climberLigament.setAngle(climber.getAngle() - 90 - 18); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Mechanism/Elevator", elevatorMech2d); superstructure.periodic(); @@ -1330,52 +1342,34 @@ public void robotPeriodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Pre Score", Autos.autoPreScore); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Score", Autos.autoScore); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "IK/Manipulator FK Pose", - ExtensionKinematics.getManipulatorPose( - swerve.getPose(), superstructure.getExtensionState())); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "IK/Extension FK Pose", - ExtensionKinematics.solveFK( - new ExtensionState( - elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle()))); state = superstructure::getState; + pose = swerve::getPose; } - public static void setCurrentCoralTarget(ReefTarget target) { - currentTarget = target; + public static void setCoralTarget(ReefTarget target) { + coralTarget = target; } - public ReefTarget getCurrentCoralTarget() { - return currentTarget; + public static ReefTarget getCoralTarget() { + return coralTarget; } - public static void setCurrentAlgaeIntakeTarget(AlgaeIntakeTarget target) { + public static void setAlgaeIntakeTarget(AlgaeIntakeTarget target) { algaeIntakeTarget = target; } - public AlgaeIntakeTarget getCurrentAlgaeIntakeTarget() { + public static AlgaeIntakeTarget getAlgaeIntakeTarget() { return algaeIntakeTarget; } - public static void setCurrentAlgaeScoreTarget(AlgaeScoreTarget target) { + public static void setAlgaeScoreTarget(AlgaeScoreTarget target) { algaeScoreTarget = target; } - public AlgaeScoreTarget getCurrentAlgaeScoreTarget() { + public static AlgaeScoreTarget getAlgaeScoreTarget() { return algaeScoreTarget; } - public static void setCurrentTarget(ReefTarget target) { - currentTarget = target; - } - - public ReefTarget getCurrentTarget() { - return currentTarget; - } - @Override public void disabledInit() {} diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java deleted file mode 100644 index cfebf6c7..00000000 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ /dev/null @@ -1,239 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.LinearFilter; -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.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.Robot.ReefTarget; -import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.shoulder.ShoulderSubsystem; -import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.utils.autoaim.CoralTargets; -import java.util.function.Supplier; - -public class ExtensionKinematics { - - // These need to be here bc their main constants arent loaded when we need the constants in this - // class - private static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - static final Transform2d IK_WRIST_TO_CORAL = - new Transform2d( - Units.inchesToMeters(12.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); - private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); - - // Not super accurate bc of whack - public static final Pose2d L1_POSE = - new Pose2d(0.33, 0.50, Rotation2d.fromDegrees(20.0)); // solveFK(L1_EXTENSION); - public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); - public static final ExtensionState L2_EXTENSION = - new ExtensionState( - 0.23 + Units.inchesToMeters(1.5), - Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20)), - Rotation2d.fromRadians(2.447)); - public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); - public static final ExtensionState L3_EXTENSION = - new ExtensionState( - 0.60 + Units.inchesToMeters(2.0), - Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)), - Rotation2d.fromRadians(2.427)); - public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); - public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.20, 2.03), Rotation2d.fromDegrees(115.0)); - - public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); - - public static final ExtensionState LOW_ALGAE_EXTENSION = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - public static final Pose2d LOW_ALGAE_POSE = solveFK(LOW_ALGAE_EXTENSION); - - public static final ExtensionState HIGH_ALGAE_EXTENSION = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - public static final Pose2d HIGH_ALGAE_POSE = solveFK(HIGH_ALGAE_EXTENSION); - - public record ExtensionState( - double elevatorHeightMeters, Rotation2d shoulderAngle, Rotation2d wristAngle) {} - - private ExtensionKinematics() {} - - /** - * @param target pose where +x is robot +x from elevator, +y is robot +z from elevator min, and - * rotation is coral angle from horizontal - */ - public static ExtensionState solveIK(Pose2d target) { - // Offset wrist pose from target - final var wristPose = target.transformBy(IK_WRIST_TO_CORAL.inverse()); - // Find shoulder angle from needed horizontal extension - var shoulderAngle = Math.acos(wristPose.getX() / ARM_LENGTH_METERS); - // Set angle to horizontal if we can't reach - if (Double.isNaN(shoulderAngle)) shoulderAngle = 0.0; - // Elevator goes to remaining needed height - var elevatorHeight = - wristPose - .getTranslation() - .minus( - new Translation2d( - ARM_LENGTH_METERS * Math.cos(shoulderAngle), - ARM_LENGTH_METERS * Math.sin(shoulderAngle))) - .getY(); - // If we're extending higher than we can reach, prioritize matching Z instead of X - if (elevatorHeight > MAX_EXTENSION_METERS) { - elevatorHeight = MAX_EXTENSION_METERS - Units.inchesToMeters(1.0); - shoulderAngle = Math.asin((wristPose.getY() - MAX_EXTENSION_METERS) / ARM_LENGTH_METERS); - // Limit shoulder angle - if (Double.isNaN(shoulderAngle) || shoulderAngle > Units.degreesToRadians(85.0)) { - shoulderAngle = Units.degreesToRadians(85.0); - } - } - - return new ExtensionState( - MathUtil.clamp(elevatorHeight, 0.0, ElevatorSubsystem.MAX_EXTENSION_METERS), - Rotation2d.fromRadians(shoulderAngle), - Rotation2d.fromDegrees(MathUtil.clamp(wristPose.getRotation().getDegrees(), -45.0, 120.0))); - } - - public static Pose2d solveFK(ExtensionState state) { - return new Pose2d( - state.shoulderAngle().getCos() * ARM_LENGTH_METERS, - state.elevatorHeightMeters() + state.shoulderAngle().getSin() * ARM_LENGTH_METERS, - state.wristAngle()) - .transformBy(IK_WRIST_TO_CORAL); - } - - public static ExtensionState getPoseCompensatedExtension(Pose2d pose, ExtensionState target) { - final var fk = ExtensionKinematics.solveFK(target); - final var diff = pose.minus(CoralTargets.getClosestTarget(pose)); - final var adjustedFk = new Pose2d(fk.getX() - diff.getX(), fk.getY(), fk.getRotation()); - return ExtensionKinematics.solveIK(adjustedFk); - } - - public static Pose3d getManipulatorPose(Pose2d robotPose, ExtensionState state) { - final var fk = solveFK(state); - return new Pose3d( - robotPose.transformBy( - new Transform2d( - fk.getX() + ElevatorSubsystem.X_OFFSET_METERS, 0.0, Rotation2d.kZero))) - .transformBy( - new Transform3d( - 0, - 0, - fk.getY() + ElevatorSubsystem.Z_OFFSET_METERS, - new Rotation3d(0, -state.wristAngle().getRadians(), 0))); - } - - public static Pose3d getBranchPose(Pose2d pose, ExtensionState state, ReefTarget level) { - return new Pose3d(CoralTargets.getClosestTarget(pose)) - .transformBy( - new Transform3d( - ElevatorSubsystem.X_OFFSET_METERS - + switch (level) { - case L1 -> L1_POSE.getX(); - case L2 -> L2_POSE.getX(); - case L3 -> L3_POSE.getX(); - case L4 -> L4_POSE.getX(); - }, - 0, - ElevatorSubsystem.Z_OFFSET_METERS - + switch (level) { - case L1 -> L1_POSE.getY(); - case L2 -> L2_POSE.getY(); - case L3 -> L3_POSE.getY(); - case L4 -> L4_POSE.getY(); - }, - new Rotation3d())); - } - - public static Command holdStateCommand( - ElevatorSubsystem elevator, - ShoulderSubsystem shoulder, - WristSubsystem wrist, - Supplier target) { - final LinearFilter elevatorFilter = LinearFilter.movingAverage(8); - final LinearFilter shoulderFilter = LinearFilter.movingAverage(8); - final LinearFilter wristFilter = LinearFilter.movingAverage(8); - return Commands.runOnce( - () -> { - elevatorFilter.reset( - new double[] { - // i hate java surely theres a better way to do this - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters(), - elevator.getExtensionMeters() - }, - new double[0]); - shoulderFilter.reset( - new double[] { - // i hate java surely theres a better way to do this - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations(), - shoulder.getAngle().getRotations() - }, - new double[0]); - wristFilter.reset( - new double[] { - // i hate java surely theres a better way to do this - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations(), - wrist.getAngle().getRotations() - }, - new double[0]); - }) - .andThen( - Commands.parallel( - elevator.setExtension( - () -> elevatorFilter.calculate(target.get().elevatorHeightMeters())), - shoulder.setTargetAngle( - () -> - Rotation2d.fromRotations( - shoulderFilter.calculate(target.get().shoulderAngle().getRotations()))), - wrist.setTargetAngle( - () -> - Rotation2d.fromRotations( - wristFilter.calculate(target.get().wristAngle().getRotations()))))); - } - - public static ExtensionState getExtensionForLevel(ReefTarget target) { - return switch (target) { - case L2 -> ExtensionKinematics.L2_EXTENSION; - case L3 -> ExtensionKinematics.L3_EXTENSION; - case L4 -> ExtensionKinematics.L4_EXTENSION; - default -> // shouldnt be reachable - new ExtensionState( - ElevatorSubsystem.L1_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - WristSubsystem.WRIST_SCORE_L1_POS); - }; - } -} diff --git a/src/main/java/frc/robot/subsystems/ExtensionPathing.java b/src/main/java/frc/robot/subsystems/ExtensionPathing.java deleted file mode 100644 index b445fc03..00000000 --- a/src/main/java/frc/robot/subsystems/ExtensionPathing.java +++ /dev/null @@ -1,243 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import com.google.common.collect.Sets; -import com.google.common.graph.GraphBuilder; -import com.google.common.graph.MutableGraph; -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.ExtensionKinematics.ExtensionState; -import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.shoulder.ShoulderSubsystem; -import frc.robot.subsystems.wrist.WristSubsystem; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.Set; - -public class ExtensionPathing { - public static final MutableGraph graph = - GraphBuilder.undirected().allowsSelfLoops(true).build(); - // TODO make this cache distances so we can do partial caches - private static final Map, List> cache = - new HashMap<>(); - - static { - final var hp = - new ExtensionState( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS); - graph.addNode(hp); - final var tucked = - new ExtensionState( - 0.0, - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(tucked); - graph.putEdge(hp, tucked); - final var l2Tucked = - new ExtensionState( - ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(l2Tucked); - graph.putEdge(tucked, l2Tucked); - final var l3Tucked = - new ExtensionState( - ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(l3Tucked); - graph.putEdge(tucked, l3Tucked); - graph.putEdge(l3Tucked, l2Tucked); - final var l3 = ExtensionKinematics.L3_EXTENSION; - graph.addNode(l3); - graph.putEdge(l3, l3Tucked); - final var l4Tucked = - new ExtensionState( - ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(l4Tucked); - graph.putEdge(tucked, l4Tucked); - graph.putEdge(l4Tucked, l3Tucked); - graph.putEdge(l4Tucked, l2Tucked); - final var l4TuckedOut = - new ExtensionState( - ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(), - Rotation2d.fromDegrees(25.0), - Rotation2d.fromDegrees(120.0)); - graph.addNode(l4TuckedOut); - graph.putEdge(l4Tucked, l4TuckedOut); - - final var betweenTucked = - new ExtensionState(0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS); - graph.addNode(betweenTucked); - graph.putEdge(tucked, betweenTucked); - - final var untucked = - new ExtensionState( - 0.0, ShoulderSubsystem.SHOULDER_CLEARANCE_POS, WristSubsystem.WRIST_CLEARANCE_POS); - graph.addNode(untucked); - graph.putEdge(betweenTucked, untucked); - - final var preCoralGround = - new ExtensionState( - ElevatorSubsystem.GROUND_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, - Rotation2d.kCCW_90deg); - graph.addNode(preCoralGround); - graph.putEdge(tucked, preCoralGround); - - final var coralGround = - new ExtensionState( - ElevatorSubsystem.GROUND_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, - WristSubsystem.WRIST_CORAL_GROUND); - graph.addNode(coralGround); - graph.putEdge(preCoralGround, coralGround); - - final var retracted = - new ExtensionState( - 0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS); - graph.addNode(retracted); - graph.putEdge(untucked, retracted); - graph.putEdge(betweenTucked, retracted); - - final var l1 = ExtensionKinematics.L1_EXTENSION; - graph.addNode(l1); - graph.putEdge(l1, betweenTucked); - - final var algaeGround = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS); - graph.addNode(algaeGround); - graph.putEdge(algaeGround, betweenTucked); - graph.putEdge(algaeGround, untucked); - - final var algaeLow = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - graph.addNode(algaeLow); - graph.putEdge(betweenTucked, algaeLow); - - final var algaeHigh = - new ExtensionState( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS); - graph.addNode(algaeHigh); - graph.putEdge(betweenTucked, algaeHigh); - graph.putEdge(algaeLow, algaeHigh); - - graph.putEdge(l4Tucked, algaeHigh); - graph.putEdge(l4Tucked, algaeLow); - - final var algaeHalfTucked = - new ExtensionState( - Units.inchesToMeters(27.6), - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS, - WristSubsystem.WRIST_TUCKED_CLEARANCE_POS); - graph.addNode(algaeHalfTucked); - graph.putEdge(algaeHalfTucked, algaeLow); - graph.putEdge(algaeHalfTucked, algaeHigh); - } - - private ExtensionPathing() {} - - public static ExtensionState getNearest(ExtensionState state) { - return graph.nodes().stream() - .min( - (a, b) -> { - var aFK = ExtensionKinematics.solveFK(a); - var bFK = ExtensionKinematics.solveFK(b); - var stateFK = ExtensionKinematics.solveFK(state); - return (int) - (1000 - * (aFK.getTranslation().getDistance(stateFK.getTranslation()) - + Math.abs( - aFK.getRotation().getRotations() - - stateFK.getRotation().getRotations())) - - 1000 - * (bFK.getTranslation().getDistance(stateFK.getTranslation()) - + Math.abs( - bFK.getRotation().getRotations() - - stateFK.getRotation().getRotations()))); - }) - .get(); - } - - private record TotalMotion(double elevator, double shoulderRotations, double wristRotations) { - public TotalMotion plus(double elevator, double shoulderRotations, double wristRotations) { - return new TotalMotion( - this.elevator + elevator, - this.shoulderRotations + shoulderRotations, - this.wristRotations + wristRotations); - } - } - - private static Pair, TotalMotion> search( - ExtensionState current, ExtensionState target, Set visited) { - if (current == target) { - List result = new ArrayList<>(); - result.add(target); - return Pair.of(result, new TotalMotion(0, 0, 0)); - } - - final var edges = Sets.difference(graph.successors(current), visited); - final List, TotalMotion>> result = new ArrayList<>(); - for (var edge : edges) { - final var path = search(edge, target, Sets.union(visited, Set.of(current))); - if (path != null) result.add(path); - } - if (result.size() == 0) return null; - final var best = - result.stream() - .min( - Comparator.comparing( - (Pair, TotalMotion> s) -> s.getFirst().size()) - .thenComparing( - (Pair, TotalMotion> s) -> s.getSecond().elevator) - .thenComparing((s) -> s.getSecond().wristRotations) - .thenComparing((s) -> s.getSecond().shoulderRotations)) - .get(); - final List newList = new ArrayList<>(); - newList.add(current); - newList.addAll(best.getFirst()); - return Pair.of( - newList, - best.getSecond() - .plus( - Math.abs( - current.elevatorHeightMeters() - best.getFirst().get(0).elevatorHeightMeters()), - Math.abs( - current.shoulderAngle().getRotations() - - best.getFirst().get(0).shoulderAngle().getRotations()), - Math.abs( - current.wristAngle().getRotations() - - best.getFirst().get(0).wristAngle().getRotations()))); - } - - public static List getPath(ExtensionState current, ExtensionState target) { - final var nearestCurrent = getNearest(current); - final var nearestTarget = getNearest(target); - final var path = - cache.containsKey(Pair.of(nearestCurrent, nearestTarget)) - ? cache.get(Pair.of(nearestCurrent, nearestTarget)) - : search(nearestCurrent, nearestTarget, Set.of()).getFirst(); - path.add(target); - cache.putIfAbsent(Pair.of(nearestCurrent, nearestTarget), path); - return path; - } -} diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index c5eaa3cf..6c338443 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -4,10 +4,7 @@ package frc.robot.subsystems; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -18,20 +15,19 @@ import frc.robot.subsystems.beambreak.BeambreakIOInputsAutoLogged; import frc.robot.subsystems.roller.RollerIO; import frc.robot.subsystems.roller.RollerSubsystem; -import frc.robot.utils.Tracer; -import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; - public static final double CORAL_INTAKE_VELOCITY = -18.0; + public static final double MAX_VELOCITY = 20; // holy cooked + + public static final double CORAL_INTAKE_VELOCITY = -10.0; public static final double JOG_POS = 0.75; - public static final double ALGAE_INTAKE_VOLTAGE = 10.0; - public static final double ALGAE_HOLDING_VOLTAGE = 1.0; - public static final double ALGAE_CURRENT_THRESHOLD = 6.0; - public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; + public static final double ALGAE_INTAKE_VOLTAGE = 8.0; + public static final double ALGAE_HOLDING_VOLTAGE = 2.0; + public static final double ALGAE_CURRENT_THRESHOLD = 30.0; public static final double CORAL_HOLD_POS = 0.6; @@ -41,12 +37,13 @@ public class ManipulatorSubsystem extends RollerSubsystem { private final BeambreakIOInputsAutoLogged firstBBInputs = new BeambreakIOInputsAutoLogged(), secondBBInputs = new BeambreakIOInputsAutoLogged(); - private boolean bb1 = false; - private boolean bb2 = false; - @AutoLogOutput private boolean hasAlgae = false; + private boolean bb1Sim = false; + private boolean bb2Sim = false; + @AutoLogOutput private boolean hasAlgaeSim = false; + @AutoLogOutput public boolean hasAlgaeReal = false; - private LinearFilter currentFilter = LinearFilter.movingAverage(20); - private double currentFilterValue = 0.0; + private LinearFilter currentFilter = LinearFilter.movingAverage(10); + @AutoLogOutput private double currentFilterValue = 0.0; private Timer zeroTimer = new Timer(); @@ -69,143 +66,98 @@ public void periodic() { Logger.processInputs(NAME + "/First Beambreak", firstBBInputs); Logger.processInputs(NAME + "/Second Beambreak", secondBBInputs); - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Has Algae", hasAlgae); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Has Algae", hasAlgaeSim); if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput(NAME + "/Sim First Beambreak Override", bb1); + Logger.recordOutput(NAME + "/Sim First Beambreak Override", bb1Sim); if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput(NAME + "/Sim Second Beambreak Override", bb2); + Logger.recordOutput(NAME + "/Sim Second Beambreak Override", bb2Sim); currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Filtered Current", currentFilterValue); if (getFirstBeambreak() && !getSecondBeambreak()) { - Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.0)); zeroTimer.reset(); } if (!getFirstBeambreak() && getSecondBeambreak()) { - // Number calculated from coral length, may need tuning - Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(1.0)); zeroTimer.reset(); } } - /** For the old ee */ - @Deprecated - public Command index() { - return Commands.sequence( - setVelocity(9.0) - .until(() -> getFirstBeambreak() || getSecondBeambreak()) - .unless(() -> getFirstBeambreak()), - setVelocity(3.0).until(() -> getSecondBeambreak()).unless(() -> getSecondBeambreak()), - setVelocity(-3.0) - .until(() -> getFirstBeambreak() && !getSecondBeambreak()) - .unless(() -> zeroTimer.get() < 0.25), - // TODO tune timeout - // Commands.runOnce(() -> io.resetEncoder(0.0)), - Commands.run(() -> io.setPosition(Rotation2d.fromRotations(1.1))) - .until(() -> !getFirstBeambreak() && !getSecondBeambreak())); - } // TODO check if anything got lost in merge? - - public Command jog(double rotations) { - return Commands.sequence( - // this.runOnce(() -> io.resetEncoder(0.0)), - this.run( - () -> { - io.setPosition(Rotation2d.fromRotations(rotations)); - positionSetpoint = rotations; - })); - } - - public Command jog(DoubleSupplier rotations) { - return Commands.sequence( - // this.runOnce(() -> io.resetEncoder(0.0)), - this.run( - () -> { - io.setPosition(Rotation2d.fromRotations(rotations.getAsDouble())); - positionSetpoint = rotations.getAsDouble(); - })); - } - - public Command hold() { - return this.jog(() -> inputs.positionRotations) - .until(() -> true) - .andThen(this.run(() -> {})) - .until(() -> !MathUtil.isNear(positionSetpoint, inputs.positionRotations, 2.0)) - .repeatedly(); - } - - public void resetPosition(final double rotations) { - io.resetEncoder(rotations); - } - - public Command intakeCoral() { - return intakeCoral(CORAL_INTAKE_VELOCITY); - } - - public Command intakeCoralAir(double vel) { - return Commands.sequence( - setVelocity(vel) - .until(() -> getSecondBeambreak()) - .finallyDo( - () -> { - io.setPosition(Rotation2d.fromRotations(0.63)); - positionSetpoint = 0.63; - }), - setVoltage(2.0).until(() -> !getFirstBeambreak()), - jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); - } - - public Command intakeCoral(double vel) { - return Commands.sequence( - setVelocity(vel).until(new Trigger(() -> getSecondBeambreak()).debounce(0.5)), - Commands.runOnce( - () -> { - io.setPosition(Rotation2d.fromRotations(0.5)); - positionSetpoint = 0.5; - }), - setVelocity(1.0).until(() -> !getFirstBeambreak()), - jog(CORAL_HOLD_POS).until(() -> !getSecondBeambreak() && !getFirstBeambreak())); - } - public Command intakeAlgae() { return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) .until( new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) - .debounce(0.75)) - .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE))); + .debounce(0.25)) + .andThen(Commands.runOnce(() -> hasAlgaeReal = true)); + } + + public Command holdAlgae() { + return this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE)); + } + + public Command holdAlgaeExtra() { + return this.run(() -> io.setVoltage(3 * ALGAE_HOLDING_VOLTAGE)); + } + + public Command scoreAlgaeProcessor() { + return this.run(() -> io.setVoltage(-2.0)); + } + + public Command scoreAlgaeBarge() { + return this.run(() -> io.setVoltage(-13.0)); + } + + public Command intakeCoral() { + return setRollerVelocity(CORAL_INTAKE_VELOCITY); } public double getStatorCurrentAmps() { return currentFilterValue; } - public double getTimeSinceZero() { - return zeroTimer.get(); + @AutoLogOutput(key = "Manipulator/Has Algae") + public boolean hasAlgae() { // TODO icky + // return new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + // .debounce(0.75) + // .getAsBoolean() + return hasAlgaeReal || hasAlgaeSim; } public boolean getFirstBeambreak() { - return firstBBInputs.get || bb1; + return firstBBInputs.get || bb1Sim; } public boolean getSecondBeambreak() { - return secondBBInputs.get || bb2; + return secondBBInputs.get || bb2Sim; } - public void setFirstBeambreak(boolean state) { - bb1 = state; + public boolean eitherBeambreak() { + return getFirstBeambreak() || getSecondBeambreak(); } - public void setSecondBeambreak(boolean state) { - bb2 = state; + public boolean bothBeambreaks() { + return getFirstBeambreak() && getSecondBeambreak(); } - public void setHasAlgae(boolean state) { - hasAlgae = state; + public boolean neitherBeambreak() { + return !eitherBeambreak(); } - public boolean hasAlgae() { // TODO icky - return hasAlgae; + public void setSimFirstBeambreak(boolean b) { + bb1Sim = b; + } + + public void setSimSecondBeambreak(boolean b) { + bb2Sim = b; + } + + public void setSimHasAlgae(boolean state) { + hasAlgaeSim = state; + } + + public double getTimeSinceZero() { + return zeroTimer.get(); } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a0a9ede8..e7a56044 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,125 +1,213 @@ package frc.robot.subsystems; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.AlgaeIntakeTarget; import frc.robot.Robot.AlgaeScoreTarget; import frc.robot.Robot.ReefTarget; -import frc.robot.Robot.RobotType; -import frc.robot.subsystems.ExtensionKinematics.ExtensionState; -import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorState; import frc.robot.subsystems.shoulder.ShoulderSubsystem; +import frc.robot.subsystems.shoulder.ShoulderSubsystem.ShoulderState; +import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; -import frc.robot.utils.autoaim.AlgaeIntakeTargets; +import frc.robot.subsystems.wrist.WristSubsystem.WristState; +import frc.robot.utils.FieldUtils; +import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; +import frc.robot.utils.FieldUtils.L1Targets; import frc.robot.utils.autoaim.AutoAim; -import frc.robot.utils.autoaim.HumanPlayerTargets; -import frc.robot.utils.autoaim.L1Targets; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import java.util.concurrent.atomic.AtomicInteger; -import java.util.concurrent.atomic.AtomicReference; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import java.util.stream.Stream; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Superstructure { - public static enum SuperState { - IDLE, - HOME, - INTAKE_CORAL_GROUND, - READY_CORAL, - SPIT_CORAL, - PRE_L1, - PRE_L2, - PRE_L3, - PRE_L4, - SCORE_CORAL, - ANTI_CORAL_JAM, - ANTI_ALGAE_JAM, - INTAKE_ALGAE_GROUND, - INTAKE_ALGAE_HIGH, - INTAKE_ALGAE_LOW, - INTAKE_ALGAE_STACK, - CHECK_ALGAE, - READY_ALGAE, - SPIT_ALGAE, - PRE_PROCESSOR, - PRE_NET, - SCORE_ALGAE_NET, - SCORE_ALGAE_PROCESSOR, - PRE_CLIMB, - CLIMB - } - - private final Supplier pose; - private final Supplier chassisVel; - private final Supplier reefTarget; - private final Supplier algaeIntakeTarget; - private final Supplier algaeScoreTarget; - - @AutoLogOutput(key = "Superstructure/Pre Score Request") - private final Trigger preScoreReq; - - @AutoLogOutput(key = "Superstructure/Score Request") - private final Trigger scoreReq; - - @AutoLogOutput(key = "Superstructure/Algae Intake Request") - private final Trigger intakeAlgaeReq; - - @AutoLogOutput(key = "Superstructure/Coral Intake Request") - private final Trigger intakeCoralReq; - - @AutoLogOutput(key = "Superstructure/Pre Climb Request") - private final Trigger preClimbReq; - - @AutoLogOutput(key = "Superstructure/Climb Confirm Request") - private final Trigger climbConfReq; - - @AutoLogOutput(key = "Superstructure/Climb Cancel Request") - private final Trigger climbCancelReq; - - @AutoLogOutput(key = "Superstructure/Anti Coral Jam Request") - private final Trigger antiCoralJamReq; - - @AutoLogOutput(key = "Superstructure/Anti Algae Jam Request") - private final Trigger antiAlgaeJamReq; - - @AutoLogOutput(key = "Superstructure/Home Request") - private final Trigger homeReq; - - @AutoLogOutput(key = "Superstructure/Rev Funnel Req") - private final Trigger revFunnelReq; - - @AutoLogOutput(key = "Superstructure/Force Funnel Req") - private final Trigger forceFunnelReq; - - @AutoLogOutput(key = "Superstructure/Force Funnel Req") - private final Trigger forceIndexReq; - - @AutoLogOutput(key = "Superstructure/Kill Vision and IK") - private final Trigger killVisionIK; + /** + * We should have a state for every single "pose" the robot will hit. Hopefully we can get named + * positions set up in cad to make this easier? + */ + public enum SuperState { + IDLE(ElevatorState.HP, ShoulderState.HP, WristState.HP, -7.0), + PRE_INTAKE_CORAL_GROUND( + ElevatorState.INTAKE_CORAL_GROUND, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.PRE_INTAKE_CORAL_GROUND, + -5.0), + INTAKE_CORAL_GROUND( + ElevatorState.INTAKE_CORAL_GROUND, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.INTAKE_CORAL_GROUND, + -5.0), + POST_INTAKE_CORAL_GROUND( + ElevatorState.INTAKE_CORAL_GROUND, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.POST_INTAKE_CORAL_GROUND, + 0.0), + CHECK_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), + READY_CORAL(ElevatorState.HP, ShoulderState.HP, WristState.HP, 0.0), + PRE_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), + L1(ElevatorState.L1, ShoulderState.L1, WristState.L1, 3.0), + POST_L1(ElevatorState.L1, ShoulderState.PRE_L1, WristState.L1, 0.0), + + PRE_L2(ElevatorState.L2, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), + L2(ElevatorState.L2, ShoulderState.L2, WristState.L2, -15.0), + POST_L2(ElevatorState.HP, ShoulderState.PRE_L2, WristState.PRE_L2, 0.0), + + PRE_L3(ElevatorState.L3, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), + L3(ElevatorState.L3, ShoulderState.L3, WristState.L3, -15.0), + POST_L3(ElevatorState.HP, ShoulderState.PRE_L3, WristState.PRE_L3, 0.0), + + // PRE_L4(ElevatorState.HP, ShoulderState.PRE_L4, WristState.L4, 0.0), + // L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, 20.0), + // PRE_PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.HP, 0.0), + // PRE_L4(ElevatorState.L4, ShoulderState.L4, WristState.PRE_L4, 0.0), + // YAP_L4(ElevatorState.L4, ShoulderState.L4REAL, WristState.PRE_L4, 0.0), + PRE_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), + L4(ElevatorState.L4, ShoulderState.L4, WristState.L4, -20.0), + // POST_L4(ElevatorState.L4, ShoulderState.PRE_L4, WristState.HP, 0.0), + POST_L4(ElevatorState.HP, ShoulderState.L4, WristState.L4, 0.0), + // POST_POST_L4( + // ElevatorState.HP, ShoulderState.PRE_L4, WristState.HP, 0.0), // like do we see the vision + + PRE_PRE_INTAKE_ALGAE( + ElevatorState.HP, + ShoulderState.PRE_INTAKE_ALGAE_REEF, + WristState.PRE_INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + PRE_INTAKE_ALGAE( + ElevatorState.HP, + ShoulderState.INTAKE_ALGAE_REEF, + WristState.INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_HIGH( + ElevatorState.INTAKE_ALGAE_HIGH, + ShoulderState.INTAKE_ALGAE_REEF, + WristState.INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_LOW( + ElevatorState.INTAKE_ALGAE_LOW, + ShoulderState.INTAKE_ALGAE_REEF, + WristState.INTAKE_ALGAE_REEF, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_STACK( + ElevatorState.INTAKE_ALGAE_STACK, + ShoulderState.INTAKE_ALGAE_STACK, + WristState.INTAKE_ALGAE_STACK, + ManipulatorSubsystem.MAX_VELOCITY * 10.0 / 12.0), + INTAKE_ALGAE_GROUND( + ElevatorState.INTAKE_ALGAE_GROUND, + ShoulderState.INTAKE_ALGAE_GROUND, + WristState.INTAKE_ALGAE_GROUND, + ManipulatorSubsystem.MAX_VELOCITY), // ?? lmao + READY_ALGAE( + ElevatorState.HP, + ShoulderState.READY_ALGAE, + WristState.READY_ALGAE, + ManipulatorSubsystem.MAX_VELOCITY * 1.0 / 12.0), + PRE_BARGE( + ElevatorState.BARGE, + ShoulderState.PRE_BARGE, + WristState.PRE_BARGE, + ManipulatorSubsystem.MAX_VELOCITY * 3.0 / 12.0), + BARGE( + ElevatorState.BARGE, + ShoulderState.SCORE_BARGE, + WristState.SCORE_BARGE, + ManipulatorSubsystem.MAX_VELOCITY * -13.0 / 12.0), // TODO HELLO??? + POST_BARGE(ElevatorState.BARGE, ShoulderState.READY_ALGAE, WristState.PRE_BARGE, 0.0), + POST_POST_BARGE(ElevatorState.HP, ShoulderState.PRE_BARGE, WristState.PRE_BARGE, 0.0), + + // SPIT_ALGAE, + PROCESSOR( + ElevatorState.PROCESSOR, + ShoulderState.PROCESSOR, + WristState.PROCESSOR, + ManipulatorSubsystem.MAX_VELOCITY * -2.0 / 12.0), + PRE_CLIMB( + ElevatorState.INTAKE_ALGAE_GROUND, + ShoulderState.INTAKE_ALGAE_GROUND, + WristState.INTAKE_ALGAE_GROUND, + 10.0, + 3.0 + 0.35, + 2.0), + CLIMB( + ElevatorState.INTAKE_ALGAE_GROUND, + ShoulderState.INTAKE_ALGAE_GROUND, + WristState.INTAKE_ALGAE_GROUND, + 0.0, + 1.0, // TODO + 0.35, + 0.5), // lowkey why is this so slow + HOME_ELEVATOR(ElevatorState.HOME, ShoulderState.HOME, WristState.HP, 0.0), + HOME_WRIST(ElevatorState.HP, ShoulderState.HOME, WristState.HOME, 0.0), + PRE_ANTIJAM_ALGAE( + ElevatorState.PRE_ANTIJAM_ALGAE, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.INTAKE_CORAL_GROUND, + 0.0), + ANTIJAM_ALGAE( + ElevatorState.ANTIJAM_ALGAE, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.INTAKE_CORAL_GROUND, + 0.0), + POST_ANTIJAM_ALGAE( + ElevatorState.HP, + ShoulderState.INTAKE_CORAL_GROUND, + WristState.POST_INTAKE_CORAL_GROUND, + 0.0) + // SPIT_CORAL(), + // ANTI_JAM, + // L4_TUCKED(ElevatorState.HP, ShoulderState.L4_TUCKED, WristState.L4_TUCKED), + // L4_TUCKED_EXTENDED(ElevatorState.L4, ShoulderState.L4_TUCKED, WristState.L4_TUCKED), + // L4_TUCKED_OUT(ElevatorState.L4, ShoulderState.L4_TUCKED_OUT, WristState.L4_TUCKED_OUT), + + ; + + public final ElevatorState elevatorState; + public final ShoulderState shoulderState; + public final WristState wristState; + public final double + manipulatorVelocity; // negative is scoring coral l2-4 positive is intaking algae + public final double climberPosition; + public final double climberSpeed; + + private SuperState( + ElevatorState elevatorState, + ShoulderState shoulderState, + WristState wristState, + double manipulatorVelocity) { + this.elevatorState = elevatorState; + this.shoulderState = shoulderState; + this.wristState = wristState; + this.manipulatorVelocity = manipulatorVelocity; + this.climberPosition = 0.0; // TODO is this right? lol + this.climberSpeed = 0.0; + } - @AutoLogOutput(key = "Superstructure/Coral Score Adjust") - private final DoubleSupplier coralAdjust; + private SuperState( + ElevatorState elevatorState, + ShoulderState shoulderState, + WristState wristState, + double manipulatorVelocity, + double climberPosition, + double climberSpeed) { + this.elevatorState = elevatorState; + this.shoulderState = shoulderState; + this.wristState = wristState; + this.manipulatorVelocity = manipulatorVelocity; + this.climberPosition = climberPosition; + this.climberSpeed = climberSpeed; + } + } private SuperState state = SuperState.IDLE; private SuperState prevState = SuperState.IDLE; - private Map stateTriggers = new HashMap(); private Timer stateTimer = new Timer(); @@ -128,991 +216,767 @@ public static enum SuperState { private final WristSubsystem wrist; private final ManipulatorSubsystem manipulator; private final FunnelSubsystem funnel; - private final ClimberSubsystem climber; - // Intake would be included here, but is cut from cad as of rn + private final SwerveSubsystem swerve; + @AutoLogOutput public static boolean coralIndexed; + + /** Creates a new Superstructure. */ public Superstructure( ElevatorSubsystem elevator, - ManipulatorSubsystem manipulator, ShoulderSubsystem shoulder, WristSubsystem wrist, + ManipulatorSubsystem manipulator, FunnelSubsystem funnel, - ClimberSubsystem climber, - Supplier pose, - Supplier chassisVel, - Supplier reefTarget, - Supplier algaeIntakeTarget, - Supplier algaeScoreTarget, - Trigger scoreReq, - Trigger preScoreReq, - Trigger intakeAlgaeReq, - Trigger intakeCoralReq, - Trigger climbReq, - Trigger climbConfReq, - Trigger climbCancelReq, - Trigger antiCoralJamReq, - Trigger antiAlgaeJamReq, - Trigger homeReq, - Trigger revFunnelReq, - Trigger forceFunnelReq, - Trigger forceIndexReq, - Trigger killVisionIK, - DoubleSupplier coralAdjust) { + SwerveSubsystem swerve) { this.elevator = elevator; - this.manipulator = manipulator; this.shoulder = shoulder; this.wrist = wrist; + this.manipulator = manipulator; this.funnel = funnel; - this.climber = climber; - - this.pose = pose; - this.chassisVel = chassisVel; - this.reefTarget = reefTarget; - this.algaeIntakeTarget = algaeIntakeTarget; - this.algaeScoreTarget = algaeScoreTarget; + this.swerve = swerve; - this.preScoreReq = preScoreReq; - this.scoreReq = scoreReq; + addTransitions(); + addManipulatorStates(); - this.intakeAlgaeReq = intakeAlgaeReq; - this.intakeCoralReq = intakeCoralReq; - - this.preClimbReq = climbReq; - this.climbConfReq = climbConfReq; - this.climbCancelReq = climbCancelReq; - - this.antiCoralJamReq = antiCoralJamReq; - this.antiAlgaeJamReq = antiAlgaeJamReq; - - this.homeReq = homeReq; - - this.revFunnelReq = revFunnelReq; - this.forceFunnelReq = forceFunnelReq; + stateTimer.start(); + } - this.forceIndexReq = forceIndexReq; + /** This file is not a subsystem, so this MUST be called manually. */ + public void periodic() { + Logger.recordOutput("Superstructure/Superstructure State", state); + Logger.recordOutput("Superstructure/State Timer", stateTimer.get()); + } - this.killVisionIK = killVisionIK; + /** + * @param start first state + * @param end second state + * @param trigger trigger to make it go from the first state to the second (assuming it's already + * in the first state) + */ + private void bindTransition(SuperState start, SuperState end, Trigger trigger) { + // maps triggers to the transitions + trigger.and(new Trigger(() -> state == start)).onTrue(changeStateTo(end)); + } + /** + * @param start first state + * @param end second state + * @param trigger trigger to make it go from the first state to the second (assuming it's already + * in the first state) + * @param cmd some command to run while making the transition + */ + private void bindTransition(SuperState start, SuperState end, Trigger trigger, Command cmd) { + // maps triggers to the transitions + trigger + .and(new Trigger(() -> state == start)) + .onTrue(Commands.parallel(changeStateTo(end), cmd)); + } - this.coralAdjust = coralAdjust; + public boolean atExtension(SuperState state) { + return elevator.atExtension(state.elevatorState.getExtensionMeters()) + && shoulder.isNearAngle(state.shoulderState.getAngle()) + && wrist.isNearAngle(state.wristState.getAngle()); + } - stateTimer.start(); + public boolean atExtension() { + return atExtension(state); + } - for (var state : SuperState.values()) { - stateTriggers.put(state, new Trigger(() -> this.state == state && DriverStation.isEnabled())); - } + private Command changeStateTo(SuperState nextState) { + return Commands.runOnce( + () -> { + System.out.println("Changing state from " + state + " to " + nextState); + stateTimer.reset(); + this.prevState = this.state; + this.state = nextState; + setSubstates(); + }) + .ignoringDisable(true); + } - configureStateTransitionCommands(); + public Command changeStateTo(Supplier state) { + return changeStateTo(state.get()); } - /** This file is not a subsystem, so this MUST be called manually. */ - public void periodic() { - Logger.recordOutput("Superstructure/Superstructure State", state); + private void setSubstates() { + elevator.setState(state.elevatorState); + shoulder.setState(state.shoulderState); + wrist.setState(state.wristState); + + // funnel and climber don't have states per se } - private void configureStateTransitionCommands() { - // Prob a better way to impl this - // Vaughn says he wants this available anytime - forceIndexReq.whileTrue(manipulator.setVelocity(1.0)); - - stateTriggers - .get(SuperState.IDLE) - .whileTrue( - // extendWithClearance( - // Units.inchesToMeters(5.0), - // ShoulderSubsystem.SHOULDER_HP_POS, - // WristSubsystem.WRIST_HP_POS) - // .until(() -> elevator.isNearExtension(Units.inchesToMeters(5.0))) - // .andThen( - extendWithClearance( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS) - .repeatedly()) // ) - .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) - .whileTrue( - funnel.setVoltage( + private void addTransitions() { + // ---Funnel--- + bindTransition( + SuperState.IDLE, + SuperState.CHECK_CORAL, + new Trigger(manipulator::getSecondBeambreak).debounce(0.1)); + + // first beambreak is the one closest to the outside when scoring l2-4 + // second beambreak is the one closer to the funnel when scoring l2-4 + bindTransition( + SuperState.CHECK_CORAL, + SuperState.READY_CORAL, + // new Trigger(manipulator::bothBeambreaks).debounce(0.5)); + new Trigger(manipulator::bothBeambreaks).debounce(0.2).or(() -> Robot.isSimulation())); + // .and(() -> manipulator.getTimeSinceZero() < 1.0), + + // ---Intake coral ground--- + bindTransition(SuperState.IDLE, SuperState.PRE_INTAKE_CORAL_GROUND, Robot.intakeCoralReq); + + bindTransition( + SuperState.PRE_INTAKE_CORAL_GROUND, + SuperState.INTAKE_CORAL_GROUND, + new Trigger(this::atExtension)); + + // bindTransition( + // SuperState.PRE_INTAKE_CORAL_GROUND, SuperState.IDLE, Robot.intakeCoralReq.negate()); + + bindTransition( + SuperState.INTAKE_CORAL_GROUND, + SuperState.POST_INTAKE_CORAL_GROUND, + Robot.intakeCoralReq + .negate() + .debounce(0.060) + .and(manipulator::bothBeambreaks) + .debounce(0.12)); // TODO i'm lowkey losing my shit + + bindTransition( + SuperState.INTAKE_CORAL_GROUND, + SuperState.POST_INTAKE_CORAL_GROUND, + Robot.intakeCoralReq.negate().and(manipulator::neitherBeambreak)); + + // has coral + bindTransition( + SuperState.POST_INTAKE_CORAL_GROUND, + SuperState.READY_CORAL, + new Trigger(this::atExtension).and(manipulator::bothBeambreaks)); + + // cancel + bindTransition( + SuperState.POST_INTAKE_CORAL_GROUND, + SuperState.IDLE, + new Trigger(this::atExtension).and(manipulator::neitherBeambreak)); + + bindTransition( + SuperState.READY_CORAL, + SuperState.IDLE, + new Trigger(manipulator::neitherBeambreak).debounce(0.1)); + + bindTransition( + SuperState.CHECK_CORAL, + SuperState.IDLE, + new Trigger(manipulator::neitherBeambreak).debounce(0.1)); + // ---L1--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_L1, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L1).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_L1, SuperState.L1, new Trigger(this::atExtension)); + + // cancel + bindTransition( + SuperState.PRE_L1, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L1)); + + bindTransition( + SuperState.L1, + SuperState.POST_L1, + new Trigger(manipulator::neitherBeambreak) + .and(this::atExtension) + .and( () -> - revFunnelReq.getAsBoolean() - ? -2.0 - : (forceFunnelReq.getAsBoolean() - || (Stream.of(HumanPlayerTargets.values()) - .map( - (t) -> - t.location - .minus(pose.get()) - .getTranslation() - .getNorm()) - .min(Double::compare) - .get() - < 1.0) - ? 1.0 - : 0.0))) - .and(manipulator::getFirstBeambreak) - .and(() -> manipulator.getTimeSinceZero() < 1.0) - .onTrue(this.forceState(SuperState.READY_CORAL)); - - stateTriggers - .get(SuperState.IDLE) - .and(intakeCoralReq) - .onTrue(this.forceState(SuperState.INTAKE_CORAL_GROUND)); - - // IDLE -> INTAKE_ALGAE_{location} - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.GROUND) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_GROUND)); - - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_HIGH)); - - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.LOW) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_LOW)); - - stateTriggers - .get(SuperState.IDLE) - .and(intakeAlgaeReq) - .and(() -> algaeIntakeTarget.get() == AlgaeIntakeTarget.STACK) - .onTrue(this.forceState(SuperState.INTAKE_ALGAE_STACK)); - // IDLE -> PRE_CLIMB - stateTriggers - .get(SuperState.IDLE) - .and(preClimbReq) - .onTrue(this.forceState(SuperState.PRE_CLIMB)); - - stateTriggers - .get(SuperState.IDLE) - .and(() -> !elevator.hasZeroed || !wrist.hasZeroed) - .and(() -> DriverStation.isEnabled()) - // .and(() -> Robot.ROBOT_TYPE != RobotType.SIM) - .onTrue(this.forceState(SuperState.HOME)); - - // We might want to make this work when we have a piece as well? - stateTriggers - .get(SuperState.IDLE) - .and(homeReq) - .onTrue(this.forceState(SuperState.HOME)) - .onTrue( - Commands.runOnce( - () -> { - elevator.hasZeroed = false; - wrist.hasZeroed = false; - })); - - stateTriggers - .get(SuperState.HOME) - .whileTrue( - Commands.parallel( - shoulder.setTargetAngle(Rotation2d.fromDegrees(55.0)), - elevator.runCurrentZeroing(), - Commands.waitUntil(() -> shoulder.getAngle().getDegrees() > 20.0) - .andThen(wrist.currentZero(() -> shoulder.getInputs())))) - .and(() -> elevator.hasZeroed && wrist.hasZeroed && !homeReq.getAsBoolean()) - .onTrue(this.forceState(prevState)); - - stateTriggers - .get(SuperState.INTAKE_CORAL_GROUND) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.GROUND_EXTENSION_METERS, - Rotation2d.fromDegrees(35.0), - WristSubsystem.WRIST_CLEARANCE_POS) - .until(() -> elevator.isNearExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS)) - .andThen( - Commands.parallel( - shoulder - .setVoltage(-10.0) - .until(() -> shoulder.getAngle().getDegrees() < 10.0) - .andThen(shoulder.setVoltage(-1.0)), - elevator.setExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS), - wrist - .setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND) - .until( - () -> - wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) - .andThen(wrist.setVoltage(-1.0))))) - .whileTrue(manipulator.intakeCoral().repeatedly().until(intakeCoralReq.negate())); - - stateTriggers - .get(SuperState.INTAKE_CORAL_GROUND) - .and(() -> manipulator.getSecondBeambreak() && manipulator.getFirstBeambreak()) - .and(intakeCoralReq.negate()) - .debounce(0.060) - .onTrue( - Commands.parallel( - manipulator.setVoltage(0.0), - Commands.waitSeconds(0.12) - .andThen( - Commands.runOnce(() -> manipulator.resetPosition(0.792)), - this.forceState(SuperState.READY_CORAL)))); - - stateTriggers - .get(SuperState.INTAKE_CORAL_GROUND) - .and(intakeCoralReq.negate()) - .and(() -> !manipulator.getFirstBeambreak() || !manipulator.getSecondBeambreak()) - .onTrue(this.forceState(SuperState.IDLE)); - - // READY_CORAL logic - stateTriggers - .get(SuperState.READY_CORAL) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) - .whileTrue( - manipulator - .hold() - .until( - () -> - shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS)) - .andThen(manipulator.jog(ManipulatorSubsystem.JOG_POS))); - // keep indexing to make sure its chilling - - stateTriggers - .get(SuperState.READY_CORAL) - .or(stateTriggers.get(SuperState.PRE_L1)) - .or(stateTriggers.get(SuperState.PRE_L2)) - .or(stateTriggers.get(SuperState.PRE_L3)) - .or(stateTriggers.get(SuperState.PRE_L4)) - .and(() -> (!manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak())) - .onTrue(this.forceState(SuperState.IDLE)); - - // SPIT_CORAL logic + -> IDLE - stateTriggers - .get(SuperState.SPIT_CORAL) - .whileTrue(manipulator.setVelocity(10)) - .and(() -> !manipulator.getFirstBeambreak()) - .and(() -> !manipulator.getSecondBeambreak()) - .and(preClimbReq.negate()) - .onTrue(this.forceState(SuperState.IDLE)); - // SPIT_CORAL -> PRE_CLIMB - stateTriggers - .get(SuperState.SPIT_CORAL) - .and(() -> !manipulator.getFirstBeambreak()) - .and(() -> !manipulator.getSecondBeambreak()) - .and(preClimbReq) - .onTrue(forceState(SuperState.PRE_CLIMB)); - // READY_CORAL -> PRE_L{1-4} - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L1) - .onTrue(this.forceState(SuperState.PRE_L1)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L2) - .onTrue(this.forceState(SuperState.PRE_L2)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L3) - .onTrue(this.forceState(SuperState.PRE_L3)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(preScoreReq) - .and(() -> reefTarget.get() == ReefTarget.L4) - .onTrue(this.forceState(SuperState.PRE_L4)); - - stateTriggers - .get(SuperState.READY_CORAL) - .and(() -> !wrist.hasZeroed || !elevator.hasZeroed) - .onTrue(this.forceState(SuperState.HOME)); - // READY_CORAL -> SPIT_CORAL - stateTriggers - .get(SuperState.READY_CORAL) - .and(preClimbReq) - .onTrue(this.forceState(SuperState.SPIT_CORAL)); - // PRE_L{1-4} logic + -> SCORE_CORAL - stateTriggers - .get(SuperState.PRE_L1) - .and( - () -> - L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) - // .whileTrue( - // this.extendWithClearance( - // ElevatorSubsystem.L1_EXTENSION_METERS, - // ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - // WristSubsystem.WRIST_SCORE_L1_POS)) - .whileTrue( - this.extendWithClearance( + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); + + bindTransition( + SuperState.POST_L1, + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L1_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())); - - stateTriggers - .get(SuperState.PRE_L1) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) - .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SCORE_L1_POS)) - .whileTrue( - this.extendWithClearance( + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); + + // go straight to intaking algae from reef + // def needs tuning + bindTransition( + SuperState.POST_L1, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); + + // ---L2--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_L2, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L2).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_L2, SuperState.L2, new Trigger(this::atExtension)); + + // cancel + bindTransition( + SuperState.PRE_L2, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L2)); + + bindTransition( + SuperState.L2, + SuperState.POST_L2, + new Trigger(manipulator::neitherBeambreak) + .and(this::atExtension) + .and( () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L1_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L1_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.25 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); - - stateTriggers - .get(SuperState.PRE_L1) - .and(() -> reefTarget.get() != ReefTarget.L1) - .onTrue(this.forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.PRE_L2) - .whileTrue( - this.extendWithClearance( + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); + bindTransition( + SuperState.POST_L2, + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L2_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L2_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); - - stateTriggers - .get(SuperState.PRE_L2) - .and(() -> reefTarget.get() != ReefTarget.L2) - .onTrue(this.forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.PRE_L3) - .whileTrue( - this.extendWithClearance( + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); + // go straight to intaking algae from reef + bindTransition( + SuperState.POST_L2, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); + + // ---L3--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_L3, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L3).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_L3, SuperState.L3, new Trigger(this::atExtension)); + + // cancel + bindTransition( + SuperState.PRE_L3, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L3)); + + bindTransition( + SuperState.L3, + SuperState.POST_L3, + new Trigger(manipulator::neitherBeambreak) + .and(this::atExtension) + .and( () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L3_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L3_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.05 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); - - stateTriggers - .get(SuperState.PRE_L3) - .and(() -> reefTarget.get() != ReefTarget.L3) - .onTrue(this.forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.PRE_L4) - .whileTrue( - this.extendWithClearance( + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); + + bindTransition( + SuperState.POST_L3, + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.L4_EXTENSION - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), ExtensionKinematics.L4_EXTENSION))) - .whileTrue( - manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + 0.125 + coralAdjust.getAsDouble())) - .and(scoreReq) - .onTrue(this.forceState(SuperState.SCORE_CORAL)); - - stateTriggers - .get(SuperState.PRE_L4) - .and(() -> reefTarget.get() != ReefTarget.L4) - .onTrue(this.forceState(SuperState.IDLE)); - - // SCORE_CORAL -> IDLE - stateTriggers - .get(SuperState.SCORE_CORAL) - .whileTrue( - // Commands.either( - // Commands.parallel( - // ExtensionKinematics.holdStateCommand( - // elevator, - // shoulder, - // wrist, - // () -> - // killVisionIK.getAsBoolean() - // ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - // : ExtensionKinematics.getPoseCompensatedExtension( - // pose.get(), - // ExtensionKinematics.getExtensionForLevel( - // reefTarget.get())))), - Commands.either( - this.holdExtension( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), - this.extendWithClearance( - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); + + // go straight to intaking algae from reef + bindTransition( + SuperState.POST_L3, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); + + // ---L4--- + bindTransition( + SuperState.READY_CORAL, + SuperState.PRE_L4, + new Trigger(() -> Robot.getCoralTarget() == ReefTarget.L4).and(Robot.preScoreReq)); + + bindTransition(SuperState.PRE_L4, SuperState.L4, new Trigger(this::atExtension)); + + // TODO i don't think any of the cancels work + // cancel + bindTransition( + SuperState.PRE_L4, + SuperState.IDLE, + new Trigger(() -> Robot.getCoralTarget() != ReefTarget.L4)); + + bindTransition( + SuperState.L4, + SuperState.POST_L4, + new Trigger(manipulator::neitherBeambreak) + .and(this::atExtension) + .and( () -> - elevator.isNearExtension( - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - .elevatorHeightMeters() - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get())) - .elevatorHeightMeters()) - && shoulder.isNearAngle( - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - .shoulderAngle() - : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get())) - .shoulderAngle())) - // // End - // () -> - // MathUtil.isNear( - // elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0)))) - ) - .whileTrue( - manipulator - .hold() - .until( - () -> - elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) - .andThen(manipulator.setVelocity(() -> reefTarget.get().outtakeSpeed))) - // .and(() -> reefTarget.get() == ReefTarget.L1) - // .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) - // .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) - // .whileTrue( - // Commands.waitSeconds(0.1) - // .andThen(wrist.setTargetAngle(WristSubsystem.WRIST_WHACK_L1_POS))) - ; - - stateTriggers - .get(SuperState.SCORE_CORAL) - .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) - .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) - .and( - () -> - L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) - .debounce(0.15) - .onTrue(forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.SCORE_CORAL) - .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) - .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) - .and(killVisionIK) - .and( - () -> - L1Targets.getNearestLine(pose.get()).getDistance(pose.get().getTranslation()) > 0.3) - .onTrue(forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.SCORE_CORAL) - .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) - .and(intakeAlgaeReq) - .and(() -> intakeTargetOnReef()) - .onTrue( - forceState( - algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH - ? SuperState.INTAKE_ALGAE_HIGH - : SuperState.INTAKE_ALGAE_LOW)); - - antiCoralJamReq - .onTrue(this.forceState(SuperState.ANTI_CORAL_JAM)) - .onFalse(this.forceState(SuperState.IDLE)); - antiAlgaeJamReq - .onTrue(this.forceState(SuperState.ANTI_ALGAE_JAM)) - .onFalse(this.forceState(SuperState.IDLE)); - // ANTI_JAM logic - stateTriggers - .get(SuperState.ANTI_CORAL_JAM) - .whileTrue(elevator.hold()) - .whileTrue(wrist.hold()) - .whileTrue(shoulder.hold()) - .whileTrue(manipulator.setVelocity(-10)) - .whileTrue(funnel.setVoltage(-10.0)); - - stateTriggers - .get(SuperState.ANTI_ALGAE_JAM) - .whileTrue( - Commands.parallel( - elevator.hold(), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND)) - .until(() -> wrist.isNearTarget() && shoulder.getAngle().getDegrees() < 10.0) - .andThen( - Commands.parallel( - wrist.hold(), - shoulder.hold(), - elevator.setExtension(Units.inchesToMeters(40)).andThen(elevator.hold())))); - - stateTriggers - .get(SuperState.CHECK_ALGAE) - .and(() -> stateTimer.hasElapsed(1.0)) - .and(() -> manipulator.getStatorCurrentAmps() <= 20.0 && Robot.ROBOT_TYPE != RobotType.SIM) - .onTrue(this.forceState(SuperState.IDLE)); - - // change intake target - stateTriggers - .get(SuperState.INTAKE_ALGAE_GROUND) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.GROUND) - .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.INTAKE_ALGAE_LOW) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.LOW) - .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.INTAKE_ALGAE_HIGH) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.HIGH) - .onTrue(this.forceState(SuperState.IDLE)); - stateTriggers - .get(SuperState.INTAKE_ALGAE_STACK) - .and(() -> algaeIntakeTarget.get() != AlgaeIntakeTarget.STACK) - .onTrue(this.forceState(SuperState.IDLE)); - - // INTAKE_ALGAE_{location} -> READY_ALGAE - stateTriggers - .get(SuperState.INTAKE_ALGAE_GROUND) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) - .whileTrue( - Commands.waitUntil( - () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS)) - .andThen(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE))); - - stateTriggers - .get(SuperState.INTAKE_ALGAE_LOW) - .whileTrue( - this.extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) - .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); - - stateTriggers - .get(SuperState.INTAKE_ALGAE_HIGH) - .whileTrue( - this.extendWithClearance( - new ExtensionState( - 0.0, Rotation2d.fromDegrees(35.0), WristSubsystem.WRIST_CLEARANCE_POS)) - .until( - () -> - shoulder.isNearAngle(Rotation2d.fromDegrees(35.0)) - && wrist.isNearAngle(WristSubsystem.WRIST_CLEARANCE_POS)) - .andThen( - this.extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS))) - .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); - - stateTriggers - .get(SuperState.INTAKE_ALGAE_STACK) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) - .whileTrue( - Commands.waitUntil( - () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .andThen(manipulator.setVoltage(12.0))); - - // leave intake - stateTriggers - .get(SuperState.INTAKE_ALGAE_GROUND) - .or(stateTriggers.get(SuperState.INTAKE_ALGAE_LOW)) - .or(stateTriggers.get(SuperState.INTAKE_ALGAE_HIGH)) - .or(stateTriggers.get(SuperState.INTAKE_ALGAE_STACK)) - .and(intakeAlgaeReq.negate()) - .onTrue(this.forceState(SuperState.CHECK_ALGAE)); - - stateTriggers - .get(SuperState.CHECK_ALGAE) - .whileTrue(elevator.hold()) - .whileTrue(manipulator.intakeAlgae()) - .whileTrue( - shoulder.setTargetAngle( + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.2) // TODO tune + .debounce(0.15) + // .and(Robot.scoreReq.negate()) + ); + + // bindTransition(SuperState.POST_L4, SuperState.POST_POST_L4, new Trigger(this::atExtension)); + + bindTransition( + // SuperState.POST_POST_L4, + SuperState.POST_L4, + SuperState.IDLE, + Robot.intakeAlgaeReq + .negate() + .or(() -> !intakeAlgaeFromReef()) + .and(this::atExtension) + .and( () -> - (prevState == SuperState.INTAKE_ALGAE_HIGH - || prevState == SuperState.INTAKE_ALGAE_LOW) - ? ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS - : ShoulderSubsystem.SHOULDER_RETRACTED_POS)) - .whileTrue( - wrist.setTargetAngle( + L1Targets.getNearestLine(swerve.getPose()) + .getDistance(swerve.getPose().getTranslation()) + > 0.3) + .debounce(0.15)); + + // go straight to intaking algae from reef + bindTransition( + // SuperState.POST_POST_L4, + SuperState.POST_L4, + SuperState.PRE_PRE_INTAKE_ALGAE, + new Trigger(this::atExtension).and(Robot.intakeAlgaeReq).and(() -> intakeAlgaeFromReef())); + + // ---Intake Algae--- + bindTransition(SuperState.IDLE, SuperState.PRE_PRE_INTAKE_ALGAE, Robot.intakeAlgaeReq); + + bindTransition( + SuperState.PRE_PRE_INTAKE_ALGAE, + SuperState.PRE_INTAKE_ALGAE, + new Trigger(this::atExtension)); + + // ---Intake Low Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_LOW, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.LOW))); + + // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_LOW, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + + bindTransition( + SuperState.INTAKE_ALGAE_LOW, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.LOW)); + + // seems like the post pickup state is different for reef/ground?? why would you do this + bindTransition( + SuperState.INTAKE_ALGAE_LOW, + SuperState.READY_ALGAE, + new Trigger(() -> stateTimer.hasElapsed(1.0)) + .and(manipulator::hasAlgae) + .and( () -> - (algaeIntakeTarget.get() == AlgaeIntakeTarget.GROUND - || algaeIntakeTarget.get() == AlgaeIntakeTarget.STACK) - ? WristSubsystem.WRIST_RETRACTED_POS - : WristSubsystem.WRIST_INTAKE_ALGAE_REEF_RETRACT_POS)) - .and(() -> stateTimer.hasElapsed(1.0)) - .and( - () -> - manipulator.getStatorCurrentAmps() > ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD - || Robot.ROBOT_TYPE == RobotType.SIM) - .and( - () -> - AlgaeIntakeTargets.getClosestTargetPose(pose.get()) + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) .getTranslation() - .minus(pose.get().getTranslation()) + .minus(swerve.getPose().getTranslation()) .getNorm() - > 0.3 - || (algaeIntakeTarget.get() == AlgaeIntakeTarget.GROUND - || algaeIntakeTarget.get() == AlgaeIntakeTarget.STACK)) - .onTrue( - this.forceState(SuperState.READY_ALGAE) - .alongWith(Commands.runOnce(() -> manipulator.setHasAlgae(true)))); // gahahaha - - // READY_ALGAE logic - stateTriggers - .get(SuperState.READY_ALGAE) - .whileTrue( - extendWithClearanceSlow( - () -> 0.1, - () -> ShoulderSubsystem.SHOULDER_RETRACTED_POS, - () -> WristSubsystem.WRIST_READY_ALGAE)) - .whileTrue(manipulator.intakeAlgae()); - // READY_ALGAE -> PRE_NET - stateTriggers - .get(SuperState.READY_ALGAE) - .and(preScoreReq) - .and(() -> algaeScoreTarget.get() == AlgaeScoreTarget.NET) - .onTrue(forceState(SuperState.PRE_NET)); - // READY_ALGAE -> PRE_PROCESSOR - stateTriggers - .get(SuperState.READY_ALGAE) - .and(preScoreReq) - .and(() -> algaeScoreTarget.get() == AlgaeScoreTarget.PROCESSOR) - .onTrue(forceState(SuperState.PRE_PROCESSOR)); - - // READY_ALGAE -> SPIT_ALGAE - stateTriggers - .get(SuperState.READY_ALGAE) - .and(preClimbReq) - .onTrue(forceState(SuperState.SPIT_ALGAE)); - - stateTriggers - .get(SuperState.READY_ALGAE) - .and(() -> manipulator.getStatorCurrentAmps() < 20.0 && Robot.ROBOT_TYPE != RobotType.SIM) - .onTrue(forceState(SuperState.CHECK_ALGAE)); - // SPIT_ALGAE -> PRE_CLIMB - stateTriggers - .get(SuperState.SPIT_ALGAE) - // Positive bc algae is backwards - .whileTrue(manipulator.setVelocity(10)) - // Wait 1 second - .and(() -> stateTimer.hasElapsed(1)) - .and(preClimbReq) - .onTrue(forceState(SuperState.PRE_CLIMB)); - - // PRE_PROCESSOR logic - stateTriggers - .get(SuperState.PRE_PROCESSOR) - .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) - .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .and(scoreReq) - .onTrue(forceState(SuperState.SCORE_ALGAE_PROCESSOR)); - // PRE_NET logic - stateTriggers - .get(SuperState.PRE_NET) - .whileTrue(manipulator.setVoltage(3 * ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) - .whileTrue( - Commands.parallel( - elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), - // Make it initially extend to the full 90 degrees - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS), - wrist.setSlowTargetAngle(WristSubsystem.WRIST_PRE_NET_POS))) - .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_PRE_NET_POS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_PRE_NET_POS)) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) - .and(scoreReq) - .onTrue(forceState(SuperState.SCORE_ALGAE_NET)); - - stateTriggers - .get(SuperState.SCORE_ALGAE_NET) - .onTrue(Commands.runOnce(() -> stateTimer.reset())) - .whileTrue( - manipulator - .hold() - .until( - () -> - shoulder.getVelocity() - > ShoulderSubsystem.TOSS_CONFIGS.MotionMagicCruiseVelocity - 0.1) - .andThen(manipulator.setVoltage(-13.0))) - .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) - .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) - .and(() -> stateTimer.hasElapsed(0.5)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS)) - .and(() -> stateTimer.hasElapsed(1.0)) - .onTrue( - forceState(SuperState.IDLE) - .alongWith(Commands.runOnce(() -> manipulator.setHasAlgae(false)))); - - stateTriggers - .get(SuperState.SCORE_ALGAE_PROCESSOR) - .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) - .whileTrue(manipulator.setVoltage(-2.0)) - .and( + > 0.3)); + + // ---Intake High Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_HIGH, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.HIGH))); + + // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_HIGH, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + + bindTransition( + SuperState.INTAKE_ALGAE_HIGH, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.HIGH)); + + // seems like the post pickup state is different for reef/ground?? why would you do this + bindTransition( + SuperState.INTAKE_ALGAE_HIGH, + SuperState.READY_ALGAE, + new Trigger(() -> stateTimer.hasElapsed(1.0)) + .and(manipulator::hasAlgae) + .and( + () -> + AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()) + .getTranslation() + .minus(swerve.getPose().getTranslation()) + .getNorm() + > 0.4)); + + // ---Intake Stack Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_STACK, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.STACK))); + + // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_STACK, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + + bindTransition( + SuperState.INTAKE_ALGAE_STACK, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.STACK)); + + // seems like the post pickup state is different for reef/ground?? why would you do this + bindTransition( + SuperState.INTAKE_ALGAE_STACK, + SuperState.READY_ALGAE, + new Trigger(() -> stateTimer.hasElapsed(1.0)).and(manipulator::hasAlgae)); + + // ---Intake Ground Algae--- + bindTransition( + SuperState.PRE_INTAKE_ALGAE, + SuperState.INTAKE_ALGAE_GROUND, + new Trigger(this::atExtension) + .and(new Trigger(() -> Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.GROUND))); + + // cancel + // bindTransition( + // SuperState.INTAKE_ALGAE_GROUND, + // SuperState.IDLE, + // new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND) + // .or(Robot.intakeAlgaeReq.negate()) + // .and(() -> !manipulator.hasAlgae()) + // .debounce(0.25)); + + bindTransition( + SuperState.INTAKE_ALGAE_GROUND, + SuperState.IDLE, + new Trigger(() -> Robot.getAlgaeIntakeTarget() != AlgaeIntakeTarget.GROUND)); + + // seems like the post pickup state is different for reef/ground?? why would you do this + // TODO tune intake algae ground + bindTransition( + SuperState.INTAKE_ALGAE_GROUND, + SuperState.READY_ALGAE, + new Trigger(() -> stateTimer.hasElapsed(1.0)).and(manipulator::hasAlgae)); + + // ---Score in barge--- + bindTransition( + SuperState.READY_ALGAE, + SuperState.PRE_BARGE, + new Trigger(() -> Robot.getAlgaeScoreTarget() == AlgaeScoreTarget.BARGE) + .and(Robot.preScoreReq)); + + bindTransition( + SuperState.PRE_BARGE, SuperState.BARGE, Robot.scoreReq.and(new Trigger(this::atExtension))); + + bindTransition( + SuperState.BARGE, + SuperState.POST_BARGE, + new Trigger(() -> stateTimer.hasElapsed(0.5)) // TODO i don't trust this state timer stuff + ); + + bindTransition( + SuperState.POST_BARGE, + SuperState.POST_POST_BARGE, + // new Trigger(() -> stateTimer.hasElapsed(1.0))); + new Trigger(this::atExtension)); + + bindTransition(SuperState.POST_POST_BARGE, SuperState.IDLE, new Trigger(this::atExtension)); + + // ---Score in processor--- + bindTransition( + SuperState.READY_ALGAE, + SuperState.PROCESSOR, + new Trigger(() -> Robot.getAlgaeScoreTarget() == AlgaeScoreTarget.PROCESSOR) + .and(Robot.preScoreReq) + .and( + () -> + MathUtil.isNear( + swerve.getPose().getX(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getX() + : AutoAim.RED_PROCESSOR_POS.getX(), + 2) + || MathUtil.isNear( + swerve.getPose().getY(), + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? AutoAim.BLUE_PROCESSOR_POS.getY() + : AutoAim.RED_PROCESSOR_POS.getY(), + 2))); + + // TODO manipulator voltage gets set elsewhere i guess + bindTransition( + SuperState.PROCESSOR, + SuperState.IDLE, + new Trigger( () -> !MathUtil.isNear( - pose.get().getX(), + swerve.getPose().getX(), DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? AutoAim.BLUE_PROCESSOR_POS.getX() : AutoAim.RED_PROCESSOR_POS.getX(), - 0.5) + 2) || !MathUtil.isNear( - pose.get().getY(), + swerve.getPose().getY(), DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? AutoAim.BLUE_PROCESSOR_POS.getY() : AutoAim.RED_PROCESSOR_POS.getY(), - 0.5)) - .onTrue(this.forceState(SuperState.IDLE)); - - stateTriggers - .get(SuperState.PRE_CLIMB) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) - .whileTrue(climber.setPosition(ClimberSubsystem.CLIMB_EXTENDED_POSITION)) - .onTrue(funnel.unlatch()) // !! - .and(climbConfReq) - .onTrue(forceState(SuperState.CLIMB)); - - stateTriggers - .get(SuperState.CLIMB) - .whileTrue( - extendWithClearance( - ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, - ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, - WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) - .whileTrue( - climber - .setPositionSlow(1.35) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); + 2))); + + // bindTransition( + // SuperState.IDLE, + // SuperState.HOME_ELEVATOR, + // Robot.homeReq.and(() -> prevState != SuperState.HOME_ELEVATOR), + // Commands.runOnce( + // () -> { + // ElevatorSubsystem.hasZeroed = false; + // WristSubsystem.hasZeroed = false; + // wrist.resetEncoder(SuperState.IDLE.wristState.getAngle()); + // })); + + // zero on startup + bindTransition( + SuperState.IDLE, + SuperState.HOME_ELEVATOR, + new Trigger(() -> !ElevatorSubsystem.hasZeroed || !WristSubsystem.hasZeroed) + .and(() -> DriverStation.isEnabled())); + + // manual request + bindTransition( + SuperState.IDLE, + SuperState.HOME_ELEVATOR, + Robot.homeReq, + Commands.runOnce( + () -> { + elevator.hasZeroed = false; + wrist.hasZeroed = false; + })); + + // bindTransition( + // SuperState.READY_CORAL, + // SuperState.HOME_ELEVATOR, + // Robot.homeReq, + // Commands.runOnce(() -> elevator.hasZeroed = false)); + + bindTransition( + SuperState.HOME_ELEVATOR, + SuperState.HOME_WRIST, + // SuperState.IDLE, + new Trigger(() -> Math.abs(elevator.currentFilterValue) > 50.0).debounce(0.1), + Commands.runOnce(() -> elevator.resetExtension(0.0)) + // .andThen(Commands.runOnce(() -> elevator.resetExtension(0.0))) + ); + + bindTransition( + SuperState.HOME_WRIST, + SuperState.IDLE, + Robot.homeReq.negate().and(() -> Math.abs(wrist.currentFilterValue) > 7.0).debounce(0.5), + Commands.runOnce( + () -> wrist.rezero(Rotation2d.fromDegrees(160).minus(Rotation2d.fromRadians(3.357))))); + + // getting rid of SPIT_CORAL and SPIT_ALGAE as explicit states- all they do is run the + // manipulator wheels + bindTransition( + SuperState.READY_CORAL, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); + + // the manipulator wheels run at the same speed to spit algae and coral + bindTransition( + SuperState.READY_ALGAE, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); + + // ---Climb--- + bindTransition(SuperState.IDLE, SuperState.PRE_CLIMB, Robot.preClimbReq, funnel.unlatch()); + + bindTransition( + SuperState.PRE_CLIMB, + SuperState.CLIMB, + Robot.climbConfReq.and(manipulator::neitherBeambreak)); // May need more checks to see if canceling is safe - stateTriggers - .get(SuperState.CLIMB) - .and(climbCancelReq) - .onTrue(forceState(SuperState.PRE_CLIMB)); - } + bindTransition(SuperState.CLIMB, SuperState.PRE_CLIMB, Robot.climbCancelReq); - public ExtensionState getExtensionState() { - return new ExtensionState(elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle()); - } + bindTransition(SuperState.PRE_CLIMB, SuperState.IDLE, Robot.preClimbReq.negate()); - private Command extendWithClearance(ExtensionState extension) { - return extendWithClearance( - extension.elevatorHeightMeters(), extension.shoulderAngle(), extension.wristAngle()); - } + // Algae antijam + Robot.antiJamAlgaeReq.onTrue(this.changeStateTo(SuperState.PRE_ANTIJAM_ALGAE)); - public Command extendWithClearance(Supplier extension) { - return extendWithClearance( - () -> extension.get().elevatorHeightMeters(), - () -> extension.get().shoulderAngle(), - () -> extension.get().wristAngle()) - .until( + bindTransition( + SuperState.PRE_ANTIJAM_ALGAE, + SuperState.ANTIJAM_ALGAE, + new Trigger( () -> - elevator.isNearExtension(extension.get().elevatorHeightMeters()) - && shoulder.isNearAngle(extension.get().shoulderAngle()) - && wrist.isNearAngle(extension.get().wristAngle())) - .andThen(ExtensionKinematics.holdStateCommand(elevator, shoulder, wrist, extension)); - } + shoulder.isNearAngle(state.shoulderState.getAngle()) + && wrist.isNearAngle(state.wristState.getAngle()))); - private Command extendWithClearance( - double elevatorExtension, Rotation2d shoulderAngle, Rotation2d wristAngle) { - return extendWithClearance(() -> elevatorExtension, () -> shoulderAngle, () -> wristAngle); - } + bindTransition( + SuperState.ANTIJAM_ALGAE, SuperState.POST_ANTIJAM_ALGAE, Robot.antiJamAlgaeReq.negate()); - private boolean shouldntTuck(Rotation2d shoulderAngle) { - return wrist.getAngle().getDegrees() < 100.0 - || elevator.getExtensionMeters() > 0.75 - || shoulderAngle.getDegrees() > 60.0; + bindTransition( + SuperState.POST_ANTIJAM_ALGAE, + SuperState.IDLE, + Robot.antiJamAlgaeReq.negate().and(new Trigger(this::atExtension))); } - private Command extendWithClearance( - DoubleSupplier elevatorExtension, - Supplier shoulderAngle, - Supplier wristAngle) { - final AtomicReference> path = new AtomicReference<>(); - final AtomicInteger index = new AtomicInteger(0); - return Commands.sequence( - Commands.runOnce( - () -> { - index.set(0); - path.set( - ExtensionPathing.getPath( - getExtensionState(), - new ExtensionState( - elevatorExtension.getAsDouble(), shoulderAngle.get(), wristAngle.get()))); - }), - holdExtension(() -> path.get().get(index.get())) - .until( + private void addManipulatorStates() { + // Score coral + Robot.scoreReq + .and(() -> stateIsScoreCoral(state)) + .whileTrue(manipulator.setRollerVelocity(() -> state.manipulatorVelocity)); + + // Intake coral + Robot.intakeCoralReq + .and(() -> !manipulator.bothBeambreaks()) + .onTrue(manipulator.setRollerVelocity(-10)) + .onFalse(manipulator.setRollerVelocity(0)); + + // intake coral funnel + Robot.forceFunnelReq + .or( // near hp station + new Trigger( () -> - elevator.isNearExtension( - path.get().get(index.get()).elevatorHeightMeters(), 0.2) - && shoulder.isNearAngle( - path.get().get(index.get()).shoulderAngle(), - Rotation2d.fromDegrees(10.0)) - && wrist.isNearAngle( - path.get().get(index.get()).wristAngle(), Rotation2d.fromDegrees(20.0))) - .finallyDo(() -> index.set(index.get() + 1)) - .repeatedly() - .until(() -> index.get() == path.get().size() - 1), - holdExtension( + (Stream.of(FieldUtils.HumanPlayerTargets.values()) + .map( + (t) -> + t.location.minus(swerve.getPose()).getTranslation().getNorm()) + .min(Double::compare) + .get() + < 1.0))) + .and(manipulator::neitherBeambreak) + .whileTrue(manipulator.setRollerVelocity(-7.0)) + .whileFalse(manipulator.setRollerVelocity(0.0)); // TODO well that seems wrong + + // intake algae + new Trigger(this::stateIsIntakeAlgae).whileTrue(manipulator.intakeAlgae()); + + // hold algae + new Trigger(() -> state == SuperState.READY_ALGAE).whileTrue(manipulator.holdAlgae()); + + // hold algae extra hard + new Trigger(() -> state == SuperState.PRE_BARGE).whileTrue(manipulator.holdAlgaeExtra()); + + // score algae processor + new Trigger(() -> state == SuperState.PROCESSOR).whileTrue(manipulator.scoreAlgaeProcessor()); + + // score algae barge + new Trigger(() -> state == SuperState.BARGE) + .and( () -> - new ExtensionState( - elevatorExtension.getAsDouble(), shoulderAngle.get(), wristAngle.get()))); - } - - private Command holdExtension(Supplier state) { - return Commands.parallel( - elevator.setExtension(() -> state.get().elevatorHeightMeters()), - shoulder.setTargetAngle(() -> state.get().shoulderAngle()), - wrist.setTargetAngle(() -> state.get().wristAngle())); - } + shoulder.getVelocity() + > ShoulderSubsystem.TOSS_CONFIGS.MotionMagicCruiseVelocity - 0.1) + .whileTrue(manipulator.scoreAlgaeBarge()); - private Command extendWithClearanceSlow( - DoubleSupplier elevatorExtension, - Supplier shoulderAngle, - Supplier wristAngle) { - return Commands.sequence( - // Retract shoulder + wrist - Commands.parallel( - shoulder - .run(() -> {}) - .until( - () -> - wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 - > shoulder.getAngle().getDegrees()) - .andThen( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle( - ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get()))), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - elevator.hold()) - // .unless( - // () -> - // shoulder.getAngle().getDegrees() - // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() - // && wrist.getAngle().getDegrees() < 90.0) - .until( - () -> - shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 - || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) - || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() - && wrist.isNearTarget()) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), - // extend elevator - Commands.parallel( - Commands.either( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - Commands.either( - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), - () -> shouldntTuck(shoulderAngle.get())), - elevator.setExtensionSlow(elevatorExtension)) - .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) - .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), - // re-extend joints - Commands.parallel( - shoulder - .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) - .unless( - () -> - shouldntTuck(shoulderAngle.get()) - || shoulderAngle.get().getDegrees() - < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .until(() -> wrist.isNearTarget()) - .andThen(shoulder.setTargetAngle(shoulderAngle)), - wrist - .hold() - .until(() -> shoulder.isNearTarget()) - .unless( - () -> - wristAngle.get().getDegrees() < 90.0 - || shoulderAngle.get().getDegrees() - > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) - .andThen(wrist.setTargetAngle(wristAngle)), - elevator.setExtensionSlow(elevatorExtension))); + // in case algae drops + // I'm not convinced this works but wtv + new Trigger(this::stateIsAlgaeAlike) + .and( + () -> + manipulator.getStatorCurrentAmps() + < ManipulatorSubsystem + .ALGAE_CURRENT_THRESHOLD) // TODO might be 20? need to check what the normal + // holding amperage is + .debounce(1.0) + .onTrue(Commands.runOnce(() -> manipulator.hasAlgaeReal = false)); + + // Antijam coral + Robot.antiJamCoralReq + .whileTrue(manipulator.setRollerVelocity(-10.0)) + .whileFalse(manipulator.setRollerVelocity(0.0)); + // ,whileFalse(manipulator.setRollerVelocity(0.0)); + + // goofy ahh jog + new Trigger(Robot.jogCoralUpReq) + .whileTrue(manipulator.setRollerVoltage(-2.0).withTimeout(0.05)) + .whileFalse(manipulator.setRollerVoltage(0.0)); + + new Trigger(Robot.jogCoralDownReq) + .whileTrue(manipulator.setRollerVoltage(2.0).withTimeout(0.05)) + .whileFalse(manipulator.setRollerVoltage(0.0)); } public SuperState getState() { return state; } + public boolean intakeAlgaeFromReef() { + return Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.HIGH + || Robot.getAlgaeIntakeTarget() == AlgaeIntakeTarget.LOW; + } + public boolean stateIsCoralAlike() { - return this.state == SuperState.READY_CORAL - || this.state == SuperState.PRE_L1 - || this.state == SuperState.PRE_L2 - || this.state == SuperState.PRE_L3 - || this.state == SuperState.PRE_L4 - || this.state == SuperState.SCORE_CORAL; + return state == SuperState.READY_CORAL + || state == SuperState.PRE_L1 + || state == SuperState.PRE_L2 + || state == SuperState.PRE_L3 + || state == SuperState.PRE_L4 + || state == SuperState.L1 + || state == SuperState.L2 + || state == SuperState.L3 + || state == SuperState.L4; } - public boolean stateIsAlgaeAlike() { - return this.state == SuperState.READY_ALGAE - || this.state == SuperState.INTAKE_ALGAE_GROUND - || this.state == SuperState.INTAKE_ALGAE_LOW - || this.state == SuperState.INTAKE_ALGAE_HIGH - || this.state == SuperState.INTAKE_ALGAE_STACK - || this.state == SuperState.CHECK_ALGAE - || this.state == SuperState.PRE_NET - || this.state == SuperState.PRE_PROCESSOR - || this.state == SuperState.SCORE_ALGAE_NET - || this.state == SuperState.SCORE_ALGAE_PROCESSOR; + public static boolean stateIsScoreCoral(SuperState state) { + return state == SuperState.L1 + || state == SuperState.L2 + || state == SuperState.L3 + || state == SuperState.L4; } - public boolean intakeTargetOnReef() { - return this.algaeIntakeTarget.get() == AlgaeIntakeTarget.HIGH - || this.algaeIntakeTarget.get() == AlgaeIntakeTarget.LOW; + public boolean stateIsAlgaeAlike() { + return state == SuperState.PRE_PRE_INTAKE_ALGAE + || state == SuperState.PRE_INTAKE_ALGAE + || state == SuperState.INTAKE_ALGAE_HIGH + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.INTAKE_ALGAE_GROUND + || state == SuperState.READY_ALGAE + || state == SuperState.PRE_BARGE + || state == SuperState.BARGE + || state == SuperState.POST_BARGE + || state == SuperState.POST_POST_BARGE + || state == SuperState.PROCESSOR; + // SPIT_ALGAE, } - private Command forceState(SuperState nextState) { - return Commands.runOnce( - () -> { - System.out.println("Changing state to " + nextState); - stateTimer.reset(); - this.prevState = this.state; - this.state = nextState; - }) - .ignoringDisable(true); + public boolean stateIsIntakeAlgae() { + return state == SuperState.INTAKE_ALGAE_HIGH + || state == SuperState.INTAKE_ALGAE_LOW + || state == SuperState.INTAKE_ALGAE_STACK + || state == SuperState.INTAKE_ALGAE_GROUND; } } diff --git a/src/main/java/frc/robot/subsystems/camera/Camera.java b/src/main/java/frc/robot/subsystems/camera/Camera.java new file mode 100644 index 00000000..0785bdaf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/Camera.java @@ -0,0 +1,248 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N8; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Robot; +import frc.robot.Robot.RobotType; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.utils.Tracer; +import java.util.NoSuchElementException; +import java.util.Optional; +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; + +/** Add your docs here. */ +public class Camera { + + // TODO add doc comment about how the matrices are actually only used for sim cause i'd forgotten + // that! + public record CameraConstants( + String name, + Transform3d robotToCamera, + Matrix intrinsicsMatrix, + Matrix distCoeffs) {} + + public static final Matrix visionPointBlankDevs = + new Matrix(Nat.N3(), Nat.N1(), new double[] {0.6, 0.6, 0.5}); + public static final Matrix infiniteDevs = + new Matrix( + Nat.N3(), + Nat.N1(), + new double[] { + Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY + }); + public static final double distanceFactor = 3.0; + + private final CameraIO io; + private final CameraIOInputsAutoLogged inputs = new CameraIOInputsAutoLogged(); + private final PhotonPoseEstimator estimator = + new PhotonPoseEstimator( + Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(), + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + null); + + private Alert futureVisionData; + + private Pose3d pose; + + public Camera(CameraIO io) { + this.io = io; + estimator.setRobotToCameraTransform(io.getCameraConstants().robotToCamera); + io.updateInputs(inputs); + futureVisionData = + new Alert( + getName() + " Vision Data Coming from ✨The Future✨", + AlertType.kError); // what the hell is this + } + + public void updateInputs() { + io.updateInputs(inputs); + } + + public void processApriltagInputs() { + Logger.processInputs("Apriltag Vision/" + io.getName(), inputs); + } + + public Optional update(PhotonPipelineResult result) { + // Skip if we have no targets (could/should switch to 1?) + if (result.getTargets().size() < 1) { + return Optional.empty(); + } + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "Vision/" + io.getName() + " Best Distance", + result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); + Optional estPose = estimator.update(result); + return estPose; + } + + public void setSimPose(Optional simEst, boolean newResult) { + this.io.setSimPose(simEst, newResult); + } + + public String getName() { + return io.getName(); + } + + public static Matrix findVisionMeasurementStdDevs(EstimatedRobotPose estimation) { + double sumDistance = 0; + for (var target : estimation.targetsUsed) { + var t3d = target.getBestCameraToTarget(); + sumDistance += + Math.sqrt(Math.pow(t3d.getX(), 2) + Math.pow(t3d.getY(), 2) + Math.pow(t3d.getZ(), 2)); + } + double avgDistance = sumDistance / estimation.targetsUsed.size(); + + var deviation = visionPointBlankDevs.times(Math.max(avgDistance, 0.0) * distanceFactor); + if (estimation.targetsUsed.size() == 1) { + deviation = deviation.times(3); + } + if (estimation.targetsUsed.size() == 1 && estimation.targetsUsed.get(0).poseAmbiguity > 0.15) { + return infiniteDevs; + } + // Reject if estimated pose is in the air or ground + if (Math.abs(estimation.estimatedPose.getZ()) > 0.125) { + return infiniteDevs; + } + // TAG_COUNT_DEVIATION_PARAMS + // .get( + // MathUtil.clamp( + // estimation.targetsUsed.size() - 1, 0, TAG_COUNT_DEVIATION_PARAMS.size() - 1)) + // .computeDeviation(avgDistance); + return deviation; + } + + public void updateCamera() { + var hasFutureData = false; + try { + if (!inputs.stale) { + var estPose = Tracer.trace("Update Camera", () -> update(inputs.result)); + var visionPose = estPose.get().estimatedPose; + pose = visionPose; + // Sets the pose on the sim field + setSimPose(estPose, !inputs.stale); + + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/Vision Pose From " + getName(), visionPose); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/Vision Pose2d From " + getName(), visionPose.toPose2d()); + final var deviations = findVisionMeasurementStdDevs(estPose.get()); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData()); + + Tracer.trace( + "Add Measurement From " + getName(), + () -> { + SwerveSubsystem + .addVisionMeasurement( // TODO this makes me uncomfortable but i don't really know + // if it's an issue + visionPose.toPose2d(), + inputs.result.metadata.captureTimestampMicros / 1.0e6, + deviations + // .times(DriverStation.isAutonomous() ? 2.0 : 1.0) + .times( + getName().equals("Front_Left_Camera") + || getName().equals("Front_Right_Camera") + ? 0.75 + : 2.0) + // reef positions + .times( + (getName().equals("Front_Left_Camera") + || getName().equals("Front_Right_Camera")) + && (Robot.state.get().toString().startsWith("PRE_L") + || Superstructure.stateIsScoreCoral(Robot.state.get()) + || Robot.state.get() == SuperState.INTAKE_ALGAE_HIGH + || Robot.state.get() == SuperState.INTAKE_ALGAE_LOW) + ? 0.5 + : 1.5) // TODO tune these sorts of numbers + // hp tags + .times( + // !camera.getName().equals("Front_Camera") + // && + estPose.get().targetsUsed.stream() + .anyMatch( + t -> + t.getFiducialId() == 12 + || t.getFiducialId() == 13 + || t.getFiducialId() == 2 + || t.getFiducialId() == 1) + ? 1.5 + : 1.0) + // barge tags + .times( + // !camera.getName().equals("Front_Right_Camera") + // && + estPose.get().targetsUsed.stream() + .anyMatch( + t -> + t.getFiducialId() == 4 + || t.getFiducialId() == 5 + || t.getFiducialId() == 15 + || t.getFiducialId() == 14) + ? 1.2 + : 1.0) + .times(Robot.state.get() == SuperState.PRE_BARGE ? 0.5 : 1.0)); + // the sussifier + }); + + hasFutureData |= inputs.result.metadata.captureTimestampMicros > RobotController.getTime(); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update"); + + Tracer.trace( + "Log Tag Poses", + () -> { + Pose3d[] targetPose3ds = new Pose3d[inputs.result.targets.size()]; + for (int j = 0; j < inputs.result.targets.size(); j++) { + targetPose3ds[j] = + Robot.ROBOT_HARDWARE + .swerveConstants + .getFieldTagLayout() + .getTagPose(inputs.result.targets.get(j).getFiducialId()) + .get(); + } + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds); + }); + + } else { + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Stale"); + } + } catch (NoSuchElementException e) { + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Bad Estimate"); + } + futureVisionData.set(hasFutureData); + } + + public CameraConstants getCameraConstants() { + return io.getCameraConstants(); + } + + public Pose3d getPose() { + return pose; + } + + public boolean hasFrontTags() { + return getName().equals("Front_Left_Camera") + || getName().equals("Front_Right_Camera") && inputs.result.targets.size() > 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIO.java b/src/main/java/frc/robot/subsystems/camera/CameraIO.java new file mode 100644 index 00000000..7a2fbf26 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/CameraIO.java @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import frc.robot.subsystems.camera.Camera.CameraConstants; +import java.util.Optional; +import org.littletonrobotics.junction.AutoLog; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.targeting.PhotonPipelineResult; + +/** Add your docs here. */ +public interface CameraIO { + @AutoLog + public static class CameraIOInputs { + public PhotonPipelineResult result = new PhotonPipelineResult(); + public boolean stale = true; + } + + public void updateInputs(CameraIOInputs inputs); + + public void setSimPose(Optional simEst, boolean newResult); + + public String getName(); + + public CameraConstants getCameraConstants(); +} diff --git a/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java b/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java new file mode 100644 index 00000000..e8615cbe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera/CameraIOReal.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.camera; + +import frc.robot.subsystems.camera.Camera.CameraConstants; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; + +/** Add your docs here. */ +public class CameraIOReal implements CameraIO { + private final CameraConstants constants; + + public PhotonCamera camera; + + public CameraIOReal(CameraConstants constants) { + this.constants = constants; + this.camera = new PhotonCamera(constants.name()); + } + + @Override + public void updateInputs(CameraIOInputs inputs) { + var results = camera.getAllUnreadResults(); + + if (results.size() > 0) { + inputs.result = results.get(results.size() - 1); + inputs.stale = false; + } else { + // else leave stale data + inputs.stale = true; + } + } + + @Override + public String getName() { + return constants.name(); + } + + @Override + public void setSimPose(Optional simEst, boolean newResult) {} + + @Override + public CameraConstants getCameraConstants() { + return constants; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java similarity index 58% rename from src/main/java/frc/robot/subsystems/vision/VisionIOSim.java rename to src/main/java/frc/robot/subsystems/camera/CameraIOSim.java index a1d2aa37..ec77f154 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ b/src/main/java/frc/robot/subsystems/camera/CameraIOSim.java @@ -2,12 +2,12 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.subsystems.vision; +package frc.robot.subsystems.camera; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.geometry.Pose3d; import frc.robot.Robot; -import frc.robot.subsystems.vision.Vision.VisionConstants; +import frc.robot.subsystems.camera.Camera.CameraConstants; import java.util.Optional; import java.util.function.Supplier; import org.photonvision.EstimatedRobotPose; @@ -17,16 +17,17 @@ import org.photonvision.simulation.VisionSystemSim; /** Add your docs here. */ -public class VisionIOSim implements VisionIO { +public class CameraIOSim implements CameraIO { + + private final CameraConstants constants; private final VisionSystemSim sim; private final PhotonCamera camera; private final PhotonCameraSim simCamera; - private final VisionConstants constants; - public static Supplier pose = () -> Pose3d.kZero; + public static Supplier pose = () -> Pose3d.kZero; // TODO wtf - public VisionIOSim(VisionConstants constants) { - this.sim = new VisionSystemSim(constants.cameraName()); + public CameraIOSim(CameraConstants constants) { + this.sim = new VisionSystemSim(constants.name()); var cameraProp = new SimCameraProperties(); // TODO Fix these constants cameraProp.setCalibration(1080, 960, constants.intrinsicsMatrix(), constants.distCoeffs()); @@ -34,7 +35,7 @@ public VisionIOSim(VisionConstants constants) { cameraProp.setFPS(50.0); cameraProp.setAvgLatencyMs(30.0); cameraProp.setLatencyStdDevMs(5.0); - this.camera = new PhotonCamera(constants.cameraName()); + this.camera = new PhotonCamera(constants.name()); this.simCamera = new PhotonCameraSim(camera, cameraProp); simCamera.enableDrawWireframe(true); simCamera.setMaxSightRange(7.0); @@ -51,32 +52,11 @@ public VisionIOSim(VisionConstants constants) { } @Override - public void setSimPose(Optional simEst, Vision camera, boolean newResult) { - simEst.ifPresentOrElse( - est -> - VisionHelper.getSimDebugField(sim) - .getObject("VisionEstimation") - .setPose(est.estimatedPose.toPose2d()), - () -> { - if (newResult) - VisionHelper.getSimDebugField(sim).getObject("VisionEstimation").setPoses(); - }); - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - // TODO add sphere for algae sim + public void updateInputs(CameraIOInputs inputs) { sim.update(pose.get()); var results = camera.getAllUnreadResults(); - inputs.constants = constants; if (results.size() > 0) { - final var result = results.get(results.size() - 1); - inputs.latency = result.metadata.getLatencyMillis(); - inputs.targets = result.targets; - inputs.sequenceID = result.metadata.getSequenceID(); - inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); - inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); - inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; + inputs.result = results.get(results.size() - 1); inputs.stale = false; } else { // else leave stale data @@ -84,8 +64,23 @@ public void updateInputs(VisionIOInputs inputs) { } } + @Override + public void setSimPose(Optional simEst, boolean newResult) { + simEst.ifPresentOrElse( + est -> + sim.getDebugField().getObject("VisionEstimation").setPose(est.estimatedPose.toPose2d()), + () -> { + if (newResult) sim.getDebugField().getObject("VisionEstimation").setPoses(); + }); + } + @Override public String getName() { - return constants.cameraName(); + return constants.name(); + } + + @Override + public CameraConstants getCameraConstants() { + return constants; } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 80e6666a..b316422c 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -17,8 +17,6 @@ class ClimberIOInputs { public void setVoltage(final double volts); - public void setPosition(final double position); - public void setPosition(final double position, final double vel); public void resetEncoder(final double position); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java index 691a94de..b084ff01 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java @@ -62,7 +62,7 @@ public ClimberIOReal() { @Override public void updateInputs(ClimberIOInputsAutoLogged inputs) { Logger.recordOutput( - "Climber refreshall statuscode", + "Climber/Signal Refresh Status Code", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, @@ -84,11 +84,6 @@ public void setVoltage(double volts) { motor.setControl(voltageOut.withOutput(volts)); } - @Override - public void setPosition(final double position) { - motor.setControl(motionMagic.withPosition(position)); - } - @Override public void setPosition(final double position, final double vel) { motor.setControl(motionMagic.withPosition(position).withVelocity(vel)); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index f4b56ccb..861d2cff 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -49,18 +49,6 @@ public void setVoltage(final double voltage) { armSim.setInputVoltage(voltage); } - @Override - public void setPosition(final double position) { - setVoltage( - pid.calculate( - armSim.getAngleRads(), - Math.asin( - (Units.rotationsToRadians(position) - * ClimberSubsystem.CLIMBER_DRUM_RADIUS_METERS) - / ClimberSubsystem.CLIMBER_ARM_LENGTH_METERS)) - + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); - } - @Override public void resetEncoder(double position) { armSim.setState(position, 0); diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index f9df6c29..cf21e05d 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; public class ClimberSubsystem extends SubsystemBase { @@ -28,7 +29,7 @@ public ClimberSubsystem(ClimberIO io) { this.io = io; SmartDashboard.putData( - "rezero Climber", Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true)); + "Rezero Climber", Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true)); SmartDashboard.putData("Reset Climber (MANUAL STOP)", resetClimber()); } @@ -38,12 +39,13 @@ public void periodic() { Logger.processInputs("Climber", inputs); } - public Command setPosition(double position) { - return this.run(() -> io.setPosition(position, FAST_VEL)); - } - - public Command setPositionSlow(double position) { - return this.run(() -> io.setPosition(position, SLOW_VEL)); + public Command setPosition(DoubleSupplier position, DoubleSupplier vel) { + return this.run( + () -> { + Logger.recordOutput("Climber/Setpoint/Pos", position); + Logger.recordOutput("Climber/Setpoint/Velocity", vel); + io.setPosition(position.getAsDouble(), vel.getAsDouble()); + }); } public Command resetClimber() { @@ -53,4 +55,8 @@ public Command resetClimber() { public Command zeroClimber() { return Commands.runOnce(() -> io.resetEncoder(0.0)).ignoringDisable(true); } + + public double getAngle() { + return inputs.position; + } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 64e55c8c..b294f15d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -19,14 +19,10 @@ public static class ElevatorIOInputs { public void updateInputs(final ElevatorIOInputsAutoLogged inputs); - public void setTarget(final double meters, final double maxAccel); - - public void setTarget(final double meters); + public void setPosition(final double meters, final double maxAccel); public void setVoltage(final double voltage); - public void setCurrent(final double amps); - public default void stop() { setVoltage(0); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java index b8a6d8af..523e1198 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java @@ -9,7 +9,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -28,10 +27,10 @@ public class ElevatorIOReal implements ElevatorIO { private final TalonFX follower = new TalonFX(17, "*"); private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final TorqueCurrentFOC torque = new TorqueCurrentFOC(0.0); private final DynamicMotionMagicVoltage positionVoltage; // misusing type system here - these correspond to linear meters, NOT rotations + // i hate this so much private final StatusSignal position = motor.getPosition(); private final StatusSignal velocity = motor.getVelocity(); private final StatusSignal voltage = motor.getMotorVoltage(); @@ -40,6 +39,7 @@ public class ElevatorIOReal implements ElevatorIO { private final StatusSignal temp = motor.getDeviceTemp(); public ElevatorIOReal() { + // i'm just going to trust this for chezy i guess var config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -109,9 +109,10 @@ public ElevatorIOReal() { @Override public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { Logger.recordOutput( - "Elevator refreshall statuscode", + "Elevator/Signal Refresh Status Code", BaseStatusSignal.refreshAll( position, velocity, voltage, statorCurrent, supplyCurrent, temp)); + inputs.positionMeters = position.getValueAsDouble(); inputs.velocityMetersPerSec = velocity.getValueAsDouble(); inputs.appliedVolts = voltage.getValueAsDouble(); @@ -121,25 +122,15 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { } @Override - public void setTarget(final double meters, final double maxAccel) { + public void setPosition(final double meters, final double maxAccel) { motor.setControl(positionVoltage.withPosition(meters).withAcceleration(maxAccel)); } - @Override - public void setTarget(final double meters) { - motor.setControl(positionVoltage.withPosition(meters)); - } - @Override public void setVoltage(final double voltage) { motor.setControl(voltageOut.withOutput(voltage)); } - @Override - public void setCurrent(final double amps) { - motor.setControl(torque.withOutput(amps)); - } - @Override public void resetEncoder(final double position) { motor.setPosition(position); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java index 73f4afd4..2f54c9de 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java @@ -50,33 +50,19 @@ public void updateInputs(final ElevatorIOInputsAutoLogged inputs) { } @Override - public void setTarget(final double meters) { + public void setPosition(double meters, double maxAccel) { + pid.setConstraints(new Constraints(5.0, maxAccel)); setVoltage( pid.calculate(physicsSim.getPositionMeters(), meters) + ff.calculate(pid.getSetpoint().velocity)); } - @Override - public void setTarget(double meters, double maxAccel) { - pid.setConstraints(new Constraints(5.0, maxAccel)); - setTarget(meters); - } - @Override public void setVoltage(final double voltage) { volts = voltage; physicsSim.setInputVoltage(MathUtil.clamp(voltage, -12.0, 12.0)); } - @Override - public void setCurrent(double amps) { - setVoltage( - DCMotor.getKrakenX60Foc(2) - .getVoltage( - amps, - physicsSim.getVelocityMetersPerSecond() / ElevatorSubsystem.DRUM_RADIUS_METERS)); - } - @Override public void resetEncoder(final double position) { // sim always has a perfectly accurate encoder diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 2b9e6d34..2cbca8e4 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -4,26 +4,23 @@ package frc.robot.subsystems.elevator; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; 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.util.Units; +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.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.ExtensionKinematics; +import frc.robot.utils.LoggedTunableNumber; import java.util.function.DoubleSupplier; -import java.util.function.Function; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.mechanism.LoggedMechanism2d; import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d; @@ -41,45 +38,60 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); - public static final double L1_EXTENSION_METERS = - ExtensionKinematics.L1_EXTENSION.elevatorHeightMeters(); - public static final double L1_WHACK_CORAL_EXTENSION_METERS = Units.inchesToMeters(24.0); - public static final double L2_EXTENSION_METERS = - ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(); - public static final double L3_EXTENSION_METERS = - ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(); - public static final double L4_EXTENSION_METERS = - ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); - - public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.14 - Units.inchesToMeters(0.75); - public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(9.0); - public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); - public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); - - // 1 inch high (maybe not needed?) - public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(62.5); - - public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; - - public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); - public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); - public static final double MAX_ACCELERATION = 10.0; public static final double SLOW_ACCELERATION = 5.0; public static final double MEDIUM_ACCELERATION = 8.5; + public static final double CHECK_ZERO_SECONDS = 2; + + public enum ElevatorState { + HP(Units.inchesToMeters(0.0)), + INTAKE_CORAL_GROUND(Units.inchesToMeters(0.0)), + L1(0.217), + L2(0.23 + Units.inchesToMeters(1.5)), + L3(0.60 + Units.inchesToMeters(2.0)), + PRE_L4(1), + L4(1.383), + INTAKE_ALGAE_LOW(Units.inchesToMeters(20.0)), + INTAKE_ALGAE_HIGH(Units.inchesToMeters(35.0)), + INTAKE_ALGAE_STACK(Units.inchesToMeters(9.0)), + INTAKE_ALGAE_GROUND(0.14 - Units.inchesToMeters(0.75)), + READY_ALGAE(0.1), + BARGE(Units.inchesToMeters(62.5)), + PROCESSOR(Units.inchesToMeters(0.01)), // lmao + HOME(-0.3), // i'm quite scared + PRE_ANTIJAM_ALGAE(Units.inchesToMeters(20)), + ANTIJAM_ALGAE(Units.inchesToMeters(45)); + + private final DoubleSupplier extensionMeters; + + private ElevatorState(double defaultExtensionMeters) { + // this.extensionMeters = extensionMeters; + this.extensionMeters = + new LoggedTunableNumber("Elevator/" + this.name(), defaultExtensionMeters); + } + + public double getExtensionMeters() { + return extensionMeters.getAsDouble(); + } + } + + @AutoLogOutput(key = "Elevator/State") + public ElevatorState state = ElevatorState.HP; + + private ElevatorState prevState = ElevatorState.HP; + + @AutoLogOutput(key = "Elevator/Setpoint") + private double setpoint = 0.0; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); private final ElevatorIO io; private final LinearFilter currentFilter = LinearFilter.movingAverage(5); public double currentFilterValue = 0.0; - public boolean hasZeroed = false; - - private double setpoint = 0.0; - - private final SysIdRoutine voltageSysid; - private final SysIdRoutine currentSysid; + @AutoLogOutput(key = "Elevator/Has Zeroed") + public static boolean hasZeroed = false; // For dashboard private final LoggedMechanism2d mech2d = new LoggedMechanism2d(3.0, Units.feetToMeters(4.0)); @@ -90,26 +102,18 @@ public class ElevatorSubsystem extends SubsystemBase { private final LoggedMechanismLigament2d carriage = new LoggedMechanismLigament2d("Carriage", 0, ELEVATOR_ANGLE.getDegrees()); + private final Alert notZeroedAlert = new Alert("Elevator may not be zeroed!", AlertType.kWarning); + /** Creates a new ElevatorSubsystem. */ public ElevatorSubsystem(ElevatorIO io) { this.io = io; - voltageSysid = - new SysIdRoutine( - new Config( - null, - null, - null, - (state) -> Logger.recordOutput("Elevator/SysIdTestStateVolts", state.toString())), - new Mechanism((volts) -> io.setVoltage(volts.in(Volts)), null, this)); - - currentSysid = - new SysIdRoutine( - new Config( - Volts.of(10.0).per(Second), - Volts.of(60.0), - null, - (state) -> Logger.recordOutput("Elevator/SysIdTestStateCurrent", state.toString())), - new Mechanism((volts) -> io.setCurrent(volts.in(Volts)), null, this)); + + new Trigger(this::atExtension) + .negate() + .debounce(CHECK_ZERO_SECONDS) + .or(() -> !hasZeroed) + .onTrue(Commands.runOnce(() -> notZeroedAlert.set(true))) + .onFalse(Commands.runOnce(() -> notZeroedAlert.set(false))); } @Override @@ -119,20 +123,22 @@ public void periodic() { currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); carriage.setLength(inputs.positionMeters); - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Mechanism2d", mech2d); + if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Mechanism2d", mech2d); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Carriage Pose", getCarriagePose()); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Elevator/Filtered Current", currentFilterValue); + } - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Elevator/Has Zeroed", hasZeroed); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Filtered Current", currentFilterValue); + public void setState(ElevatorState state) { + this.state = state; } public Command setExtension(DoubleSupplier meters) { return this.run( () -> { - io.setTarget(meters.getAsDouble(), MAX_ACCELERATION); + io.setPosition(meters.getAsDouble(), MAX_ACCELERATION); setpoint = meters.getAsDouble(); Logger.recordOutput("Elevator/Setpoint", setpoint); }); @@ -142,96 +148,36 @@ public Command setExtension(double meters) { return this.setExtension(() -> meters); } - public Command setExtensionSlow(DoubleSupplier meters) { - return this.run( - () -> { - io.setTarget(meters.getAsDouble(), SLOW_ACCELERATION); - setpoint = meters.getAsDouble(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", setpoint); - }); - } - - public Command setExtensionSlow(double meters) { - return this.setExtensionSlow(() -> meters); - } - - public Command setExtensionMedium(DoubleSupplier meters) { - return this.run( - () -> { - io.setTarget(meters.getAsDouble(), MEDIUM_ACCELERATION); - setpoint = meters.getAsDouble(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", setpoint); - }); + public Command setStateExtension() { + return setExtension(() -> state.getExtensionMeters()); } - public Command setExtensionMedium(double meters) { - return this.setExtensionMedium(() -> meters); + public boolean atExtension(double expected) { + return MathUtil.isNear(expected, inputs.positionMeters, 0.05); } - public Command hold() { - return Commands.sequence( - setExtension(() -> inputs.positionMeters).until(() -> true), this.run(() -> {})); + public boolean atExtension() { + return atExtension(setpoint); } public Command runCurrentZeroing() { - return this.run( - () -> { - io.setVoltage(-2.0); - setpoint = 0.0; - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", Double.NaN); - }) - .until(() -> Math.abs(currentFilterValue) > 50.0) - .finallyDo( - (interrupted) -> { - if (!interrupted) { - io.resetEncoder(0.0); - hasZeroed = true; - } - }); - } - - public Command runSysid() { - final Function runSysid = - (routine) -> - Commands.sequence( - routine - .quasistatic(SysIdRoutine.Direction.kForward) - .until(() -> inputs.positionMeters > Units.inchesToMeters(50.0)), - Commands.waitUntil(() -> inputs.velocityMetersPerSec < 0.1), - routine - .quasistatic(SysIdRoutine.Direction.kReverse) - .until(() -> inputs.positionMeters < Units.inchesToMeters(10.0)), - Commands.waitUntil(() -> Math.abs(inputs.velocityMetersPerSec) < 0.1), - routine - .dynamic(SysIdRoutine.Direction.kForward) - .until(() -> inputs.positionMeters > Units.inchesToMeters(50.0)), - Commands.waitUntil(() -> inputs.velocityMetersPerSec < 0.1), - routine - .dynamic(SysIdRoutine.Direction.kReverse) - .until(() -> inputs.positionMeters < Units.inchesToMeters(10.0))); - return Commands.sequence( - runCurrentZeroing(), runSysid.apply(voltageSysid), runSysid.apply(currentSysid)); - } - - public Command setVoltage(double voltage) { - return this.run( - () -> { - io.setVoltage(voltage); - }); - } - - public Command setVoltage(DoubleSupplier voltage) { - return this.setVoltage(voltage.getAsDouble()); - } - - public Command setCurrent(double amps) { - return this.run( - () -> { - io.setCurrent(amps); - }); + return Commands.print("Elevator Zeroing") + .andThen( + this.run( + () -> { + io.setVoltage(-2.0); + setpoint = 0.0; + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Elevator/Setpoint", Double.NaN); + }) + .until(() -> Math.abs(currentFilterValue) > 50.0) + .finallyDo( + (interrupted) -> { + if (!interrupted) { + io.resetEncoder(0.0); + hasZeroed = true; + } + })); } public Pose3d getCarriagePose() { @@ -242,16 +188,6 @@ public Pose3d getCarriagePose() { new Rotation3d()); } - public Pose3d getFirstStagePose() { - return new Pose3d( - Units.inchesToMeters(2.25) - + (inputs.positionMeters / 2.0) * Math.cos(ELEVATOR_ANGLE.getRadians()), - 0.0, - Units.inchesToMeters(4.25) - + (inputs.positionMeters / 2.0) * Math.sin(ELEVATOR_ANGLE.getRadians()), - new Rotation3d()); - } - public double getExtensionMeters() { return inputs.positionMeters; } @@ -260,20 +196,10 @@ public double getSetpoint() { return setpoint; } - public boolean isNearTarget() { - return isNearExtension(setpoint); - } - - public boolean isNearExtension(double expected) { - return MathUtil.isNear(expected, inputs.positionMeters, 0.05); - } - - public boolean isNearExtension(double expected, double toleranceMeters) { - return MathUtil.isNear(expected, inputs.positionMeters, toleranceMeters); - } - public void resetExtension(double extension) { + System.out.println("Elevator zeroing"); io.resetEncoder(extension); hasZeroed = true; + System.out.println("Elevator zeroed!"); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotIO.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotIO.java deleted file mode 100644 index b752b562..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotIO.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; - -public interface IntakePivotIO { - @AutoLog - public static class IntakePivotIOInputs { - public double angularVelocityRotsPerSec = 0.0; - public Rotation2d position = new Rotation2d(); - public double tempDegreesC = 0.0; - public double supplyCurrentAmps = 0.0; - public double appliedVoltage = 0.0; - public double statorCurrentAmps = 0.0; - } - - void updateInputs(IntakePivotIOInputs inputs); - - void setMotorVoltage(double voltage); - - void setMotorPosition(Rotation2d targetPosition); -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotIOReal.java deleted file mode 100644 index 17000933..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOReal.java +++ /dev/null @@ -1,94 +0,0 @@ -package frc.robot.subsystems.intake; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; - -public class IntakePivotIOReal implements IntakePivotIO { - private final TalonFX motor = new TalonFX(14, "*"); - - private final StatusSignal angularVelocityRotsPerSec = motor.getVelocity(); - private final StatusSignal temp = motor.getDeviceTemp(); - private final StatusSignal supplyCurrentAmps = motor.getSupplyCurrent(); - private final StatusSignal statorCurrentAmps = motor.getStatorCurrent(); - private final StatusSignal appliedVoltage = motor.getMotorVoltage(); - private final StatusSignal motorPositionRotations = motor.getPosition(); - - private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0.0).withEnableFOC(true); - - public IntakePivotIOReal() { - var config = new TalonFXConfiguration(); - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - // Gains currently from the sim. Need to be updated with our own robot's - config.Slot0.kV = 0.543; - config.Slot0.kG = 0.3856; - config.Slot0.kS = 0.0; - config.Slot0.kP = 5.0; - config.Slot0.kI = 0.0; - config.Slot0.kD = 2.6; - - config.CurrentLimits.SupplyCurrentLimit = 20.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - - config.Feedback.SensorToMechanismRatio = IntakePivotSubsystem.PIVOT_RATIO; - - motor.getConfigurator().apply(config); - motor.optimizeBusUtilization(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - angularVelocityRotsPerSec, - temp, - supplyCurrentAmps, - statorCurrentAmps, - appliedVoltage, - motorPositionRotations); - } - - @Override - public void updateInputs(IntakePivotIOInputs inputs) { - BaseStatusSignal.refreshAll( - angularVelocityRotsPerSec, - temp, - supplyCurrentAmps, - statorCurrentAmps, - appliedVoltage, - motorPositionRotations); - - inputs.angularVelocityRotsPerSec = angularVelocityRotsPerSec.getValueAsDouble(); - inputs.tempDegreesC = temp.getValue().in(Units.Celsius); - inputs.supplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); - inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); - inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); - inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); - } - - @Override - public void setMotorVoltage(double voltage) { - motor.setControl(voltageOut.withOutput(voltage)); - } - - @Override - public void setMotorPosition(Rotation2d targetPosition) { - motor.setControl(motionMagic.withPosition(targetPosition.getRotations())); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotIOSim.java deleted file mode 100644 index 616bf549..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotIOSim.java +++ /dev/null @@ -1,66 +0,0 @@ -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; - -public class IntakePivotIOSim implements IntakePivotIO { - // TODO: SET TO ACTUAL VALUES WHEN CAD IS FINISHED - // Taken from Citrus circuits 2024 CAD - private final SingleJointedArmSim intakePivotSim = - new SingleJointedArmSim( - DCMotor.getKrakenX60Foc(1), - IntakePivotSubsystem.PIVOT_RATIO, - 0.07, - Units.inchesToMeters(11.5), - IntakePivotSubsystem.MIN_ANGLE.getRadians(), - IntakePivotSubsystem.MAX_ANGLE.getRadians(), - true, - 0.0); - - // TODO: TUNE - private final ProfiledPIDController pivotPid = - new ProfiledPIDController(5.0, 0.0, 2.6, new TrapezoidProfile.Constraints(10.0, 10.0)); - // TODO: TUNE - private final ArmFeedforward pivotFf = new ArmFeedforward(0.0, 0.3856, 0.543); // 0.543 - - private double appliedVoltage = 0.0; - - @Override - public void updateInputs(IntakePivotIOInputs inputs) { - intakePivotSim.update(0.02); - - inputs.position = Rotation2d.fromRadians(intakePivotSim.getAngleRads()); - inputs.angularVelocityRotsPerSec = - RadiansPerSecond.of(intakePivotSim.getVelocityRadPerSec()).in(RotationsPerSecond); - inputs.statorCurrentAmps = intakePivotSim.getCurrentDrawAmps(); - inputs.supplyCurrentAmps = 0.0; - inputs.tempDegreesC = 0.0; - inputs.appliedVoltage = appliedVoltage; - } - - @Override - public void setMotorVoltage(double voltage) { - appliedVoltage = voltage; - intakePivotSim.setInputVoltage(MathUtil.clamp(voltage, -12, 12)); - } - - @Override - public void setMotorPosition(Rotation2d targetPosition) { - setMotorVoltage( - pivotPid.calculate(intakePivotSim.getAngleRads(), targetPosition.getRadians()) - + pivotFf.calculate(pivotPid.getSetpoint().position, pivotPid.getSetpoint().velocity)); - } - - public void setResetSimState() { - intakePivotSim.setState(0.0, 0.0); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java deleted file mode 100644 index f0289a74..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; -import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - -public class IntakePivotSubsystem extends SubsystemBase { - // TODO: SET TO ACTUAL RATIO WHEN CAD IS FINISHED - public static double PIVOT_RATIO = 20.0; - public static Rotation2d RETRACTED_ANGLE = Rotation2d.fromDegrees(90); - public static Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(120); - public static Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(0); - - private IntakePivotIO io; - private IntakePivotIOInputsAutoLogged inputs = new IntakePivotIOInputsAutoLogged(); - - public IntakePivotSubsystem(IntakePivotIO io) { - this.io = io; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Intake/Pivot", inputs); - } - - public Command setTargetAngle(Rotation2d target) { - return setTargetAngle(() -> target); - } - - public Command setTargetAngle(Supplier target) { - return this.runOnce( - () -> { - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Intake/PivotSetpoint", target.get()); - }) - .andThen(this.run(() -> io.setMotorPosition(target.get()))); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java deleted file mode 100644 index 92ba7039..00000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.subsystems.intake; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.roller.RollerIO; -import frc.robot.subsystems.roller.RollerSubsystem; - -public class IntakeRollerSubsystem extends RollerSubsystem { - // TODO: SET TO GOOD VALUE - static final double INTAKE_VELOCITY = 80.0; - - public IntakeRollerSubsystem(RollerIO io) { - super(io, "Intake/Roller"); - } - - /** Runs intake */ - public Command intake() { - return intake(INTAKE_VELOCITY); - } - - /** Runs intake at specified velocity */ - public Command intake(double velocity) { - return setVelocity(velocity); - } - - /** Runs intake in reverse */ - public Command outtake() { - return intake(-INTAKE_VELOCITY); - } -} diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/roller/RollerIO.java index 6fa26234..dbcdcf2b 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIO.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIO.java @@ -5,7 +5,6 @@ package frc.robot.subsystems.roller; import edu.wpi.first.math.geometry.Rotation2d; -import java.util.function.Consumer; import org.littletonrobotics.junction.AutoLog; public interface RollerIO { @@ -23,20 +22,12 @@ public static class RollerIOInputs { public void setVoltage(double voltage); - public void setCurrent(double amps); - public default void stop() { setVoltage(0.0); } public void setVelocity(double velocityRPS); - /** - * This method is meant to set a function to be called alongside updateInput to update a - * simulation, such as for routing simulation - */ - public void registerSimulationCallback(Consumer callback); - public void setPosition(Rotation2d rot); public void resetEncoder(double position); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java index 0f917ba7..ce990e75 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOReal.java @@ -16,7 +16,6 @@ import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -28,15 +27,12 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; -import java.util.Optional; -import java.util.function.Consumer; import org.littletonrobotics.junction.Logger; public class RollerIOReal implements RollerIO { private final TalonFX motor; private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); - private final TorqueCurrentFOC currentFOC = new TorqueCurrentFOC(0.0); private final VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true).withSlot(0); private final PositionVoltage positionVoltage = @@ -49,8 +45,6 @@ public class RollerIOReal implements RollerIO { private final StatusSignal temp; private final StatusSignal position; - private Optional> callback = Optional.empty(); - public RollerIOReal(final int motorID, final TalonFXConfiguration config) { this.motor = new TalonFX(motorID, "*"); @@ -86,7 +80,7 @@ public static TalonFXConfiguration getDefaultConfig() { @Override public void updateInputs(RollerIOInputsAutoLogged inputs) { Logger.recordOutput( - "Roller" + motor.getDeviceID() + "refreshall statuscode", + "Roller" + motor.getDeviceID() + " Signal Refresh Status Code", BaseStatusSignal.refreshAll( velocity, voltage, statorCurrent, supplyCurrent, temp, position)); inputs.velocityRotationsPerSec = velocity.getValue().in(RotationsPerSecond); @@ -95,8 +89,6 @@ public void updateInputs(RollerIOInputsAutoLogged inputs) { inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); inputs.tempCelsius = temp.getValue().in(Celsius); inputs.positionRotations = position.getValueAsDouble(); - - callback.ifPresent((cb) -> cb.accept(inputs)); } @Override @@ -104,21 +96,11 @@ public void setVoltage(double voltage) { motor.setControl(voltageOut.withOutput(voltage)); } - @Override - public void setCurrent(double amps) { - motor.setControl(currentFOC.withOutput(amps)); - } - @Override public void setVelocity(double velocityRPS) { motor.setControl(velocityVoltage.withVelocity(velocityRPS)); } - @Override - public void registerSimulationCallback(Consumer callback) { - this.callback = Optional.of(callback); - } - @Override public void setPosition(Rotation2d rot) { motor.setControl(positionVoltage.withPosition(rot.getRotations())); diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java index 3a37e16b..f95e3bd8 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java @@ -59,11 +59,6 @@ public void setVelocity(double velocityRPS) { + pid.calculate(motorSim.getAngularVelocityRPM() / 60, velocityRPS)); } - @Override - public void registerSimulationCallback(Consumer callback) { - this.callback = Optional.of(callback); - } - @Override public void setPosition(Rotation2d rot) { setVelocity(0.0); @@ -75,13 +70,4 @@ public void setPosition(Rotation2d rot) { public void resetEncoder(double position) { motorSim.setAngle(position); } - - @Override - public void setCurrent(double amps) { - setVoltage( - DCMotor.getKrakenX60Foc(1) - .getVoltage( - DCMotor.getKrakenX60Foc(1).getTorque(amps), - motorSim.getAngularVelocityRadPerSec())); - } } diff --git a/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java index 9fc42bf9..111e35c2 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerSubsystem.java @@ -14,7 +14,8 @@ public class RollerSubsystem extends SubsystemBase { protected final RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); private final String name; - public static final double INDEXING_VELOCITY = 10; // TODO + + private double setpoint = 0.0; /** * Creates a new RollerSubsystem. @@ -34,23 +35,24 @@ public void periodic() { Logger.processInputs(name, inputs); } - public Command setVelocity(DoubleSupplier vel) { - return this.run(() -> io.setVelocity(vel.getAsDouble())); + public Command setRollerVoltage(DoubleSupplier voltage) { + return this.runOnce( + () -> { + io.setVoltage(voltage.getAsDouble()); + setpoint = voltage.getAsDouble(); + Logger.recordOutput(name + "/Roller Voltage Setpoint", setpoint); + }); } - public Command setVelocity(double vel) { - return this.setVelocity(() -> vel); + public Command setRollerVoltage(double voltage) { + return setRollerVoltage(() -> voltage); } - public Command setVoltage(DoubleSupplier volts) { - return this.run(() -> io.setVoltage(volts.getAsDouble())); - } - - public Command setVoltage(double vel) { - return this.setVoltage(() -> vel); + public Command setRollerVelocity(DoubleSupplier vel) { + return this.run(() -> io.setVelocity(vel.getAsDouble())); } - public Command setCurrent(DoubleSupplier amps) { - return this.run(() -> io.setCurrent(amps.getAsDouble())); + public Command setRollerVelocity(double vel) { + return setRollerVelocity(() -> vel); } } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java index d7d2fe49..02cafd19 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java @@ -8,7 +8,7 @@ public interface ShoulderIO { @AutoLog class ShoulderIOInputs { public double angularVelocityRPS = 0.0; - public Rotation2d position = new Rotation2d(); + public Rotation2d motorPosition = new Rotation2d(); public double cancoderPosition = 0.0; public double appliedVoltage = 0.0; public double tempDegreesC = 0.0; @@ -20,12 +20,12 @@ class ShoulderIOInputs { public void setMotorVoltage(final double voltage); - public void setMotorPosition(final Rotation2d targetPosition); + public void setPivotAngle(final Rotation2d targetPosition); - public default void resetEncoder(final Rotation2d rotation) {} + public default void setEncoderPosition(final Rotation2d rotation) {} public default void resetEncoder() { - resetEncoder(Rotation2d.kZero); + setEncoderPosition(Rotation2d.kZero); } public void setMotionMagicConfigs(final MotionMagicConfigs configs); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 54677024..fdd9fc8d 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -81,7 +81,8 @@ public ShoulderIOReal() { final CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - cancoderConfig.MagnetSensor.MagnetOffset = -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; + cancoderConfig.MagnetSensor.MagnetOffset = + -0.524; // -0.36; // -0.385174; // 0.2307 + 0.25; // 0.6323; // 0.779; cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.9; cancoder.getConfigurator().apply(cancoderConfig); @@ -104,7 +105,7 @@ public ShoulderIOReal() { @Override public void updateInputs(ShoulderIOInputs inputs) { Logger.recordOutput( - "shoulder refreshall statuscode", + "Shoulder/Signal Refresh Status Code", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, @@ -114,7 +115,7 @@ public void updateInputs(ShoulderIOInputs inputs) { cancoderPositionRotations, appliedVoltage)); - inputs.position = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorPosition = Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); inputs.cancoderPosition = cancoderPositionRotations.getValueAsDouble(); inputs.tempDegreesC = temp.getValue().in(Units.Celsius); inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); @@ -129,16 +130,17 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setPivotAngle(final Rotation2d targetPosition) { motor.setControl(motionMagic.withPosition(targetPosition.getRotations())); } @Override - public void resetEncoder(final Rotation2d rotation) { + public void setEncoderPosition(final Rotation2d rotation) { motor.setPosition(rotation.getRotations()); } @Override + // TODO i hate this can we get rid of this public void setMotionMagicConfigs(final MotionMagicConfigs configs) { motor.getConfigurator().apply(configs); } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java index 03ee8bab..be9bf62a 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java @@ -39,7 +39,7 @@ public void updateInputs(final ShoulderIOInputs inputs) { inputs.angularVelocityRPS = RadiansPerSecond.of(armSim.getVelocityRadPerSec()).in(RotationsPerSecond); - inputs.position = Rotation2d.fromRadians(armSim.getAngleRads()); + inputs.motorPosition = Rotation2d.fromRadians(armSim.getAngleRads()); inputs.statorCurrentAmps = armSim.getCurrentDrawAmps(); inputs.supplyCurrentAmps = 0.0; inputs.tempDegreesC = 0.0; @@ -53,7 +53,7 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setPivotAngle(final Rotation2d targetPosition) { setMotorVoltage( pid.calculate(armSim.getAngleRads(), targetPosition.getRadians()) + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 00977b67..e2e0d5b9 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.ExtensionKinematics; +import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.Tracer; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -18,42 +18,16 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; public class ShoulderSubsystem extends SubsystemBase { - // TODO: UPDATE WITH CAD public static final double SHOULDER_FINAL_STAGE_RATIO = 3.0; public static final double SHOULDER_GEAR_RATIO = 25.0 * (34.0 / 28.0) * SHOULDER_FINAL_STAGE_RATIO; public static final int CANCODER_ID = 5; public static final Rotation2d MAX_SHOULDER_ROTATION = Rotation2d.fromDegrees(120.0); public static final Rotation2d MIN_SHOULDER_ROTATION = Rotation2d.fromDegrees(-5.0); - public static final Rotation2d SHOULDER_RETRACTED_POS = Rotation2d.fromDegrees(90.0); public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(50.0); - public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); - - public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = - Rotation2d.fromRadians(0.505) - .plus(Rotation2d.fromDegrees(-5.0)) - .minus(Rotation2d.fromDegrees(2)); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = - Rotation2d.fromDegrees(60.0); - // may be incorrect as l2-3 poses are derived from ExtensionKinematics now - public static final Rotation2d SHOULDER_SCORE_POS = - ExtensionKinematics.L2_EXTENSION.shoulderAngle(); - public static final Rotation2d SHOULDER_WHACK_L1_POS = Rotation2d.fromDegrees(45); - public static final Rotation2d SHOULDER_SCORE_L1_POS = - ExtensionKinematics.L1_EXTENSION.shoulderAngle(); - public static final Rotation2d SHOULDER_SCORE_L4_POS = - ExtensionKinematics.L4_EXTENSION.shoulderAngle(); - public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(30); - public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); - public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); - public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); - public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); public static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(6.0); @@ -62,14 +36,58 @@ public class ShoulderSubsystem extends SubsystemBase { .withMotionMagicCruiseVelocity(0.275) .withMotionMagicAcceleration(4.0); - private final ShoulderIO io; - private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); + public enum ShoulderState { + HP(57.0), + PRE_INTAKE_CORAL_GROUND(15.0), + INTAKE_CORAL_GROUND(0.0), + PRE_L1(35.0), + L1(Units.radiansToDegrees(1.617)), // not sure about units tbh + PRE_L2(35.0), + L2(Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20)).getDegrees()), + PRE_L3(40.0), + L3(Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)).getDegrees()), + // PRE_L4(8.0), + PRE_L4(30.0), + L4(50), + PRE_INTAKE_ALGAE_REEF(35.0), + INTAKE_ALGAE_REEF(45.0), + INTAKE_ALGAE_STACK(30.0), + INTAKE_ALGAE_GROUND( + Rotation2d.fromRadians(0.505) + .plus(Rotation2d.fromDegrees(-5.0)) + .minus(Rotation2d.fromDegrees(2)) + .getDegrees()), // hello?? + READY_ALGAE(60.0), + PRE_BARGE(30), + SCORE_BARGE(90), + PROCESSOR(60.0), + HOME(50.0); + + private final Supplier angle; + + private ShoulderState(double defaultAngle) { + LoggedTunableNumber ltn = new LoggedTunableNumber("Shoulder/" + this.name(), defaultAngle); + // we're in real life!! use degrees + this.angle = () -> Rotation2d.fromDegrees(ltn.get()); + } + public Rotation2d getAngle() { + return angle.get(); + } + } + + @AutoLogOutput(key = "Carriage/Shoulder/Setpoint") private Rotation2d setpoint = Rotation2d.kZero; + @AutoLogOutput(key = "Carriage/Shoulder/State") + private ShoulderState state = ShoulderState.HP; + + private final ShoulderIO io; + private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); + private LoggedNetworkBoolean dashboardZero = new LoggedNetworkBoolean("Zero Shoulder"); - public ShoulderSubsystem(final ShoulderIO io) { + public ShoulderSubsystem(ShoulderIO io) { this.io = io; io.updateInputs(inputs); rezero(); @@ -83,7 +101,7 @@ public void periodic() { Logger.processInputs("Carriage/Shoulder", inputs); if (dashboardZero.get()) { Tracer.trace( - "Shoulder/Zero", + "Carriage/Shoulder/Zero", () -> { rezero(); dashboardZero.set(false); @@ -93,80 +111,58 @@ public void periodic() { Logger.recordOutput("Carriage/Shoulder/Cancoder Pos", getZeroingAngle()); } - @AutoLogOutput(key = "Shoulder/Zeroing Angle") + public void setState(ShoulderState state) { + this.state = state; + } + + public Command setStateAngle() { + return setAngle(() -> state.getAngle()); + } + + @AutoLogOutput(key = "Carriage/Shoulder/Zeroing Angle") + // what public Rotation2d getZeroingAngle() { return Rotation2d.fromRotations(inputs.cancoderPosition).div(SHOULDER_FINAL_STAGE_RATIO); } public void rezero() { - io.resetEncoder(getZeroingAngle()); + io.setEncoderPosition(getZeroingAngle()); } - public Command setTargetAngle(final Supplier target) { + public Command setAngle(Supplier target) { return this.run( () -> { - io.setMotorPosition(target.get()); + io.setPivotAngle(target.get()); setpoint = target.get(); Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); }); } - public Command setTargetAngle(final Rotation2d target) { - return setTargetAngle(() -> target); + public Command setAngle(Rotation2d target) { + return setAngle(() -> target); } - public Command setTargetAngleSlow(final Supplier target) { - return Commands.sequence( - this.runOnce(() -> io.setMotionMagicConfigs(TOSS_CONFIGS)), - this.run( - () -> { - io.setMotorPosition(target.get()); - setpoint = target.get(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); - })) - .finallyDo(() -> io.setMotionMagicConfigs(DEFAULT_CONFIGS)); + public Command setVoltage(double volts) { + return this.run(() -> io.setMotorVoltage(volts)); } - public Command setTargetAngleSlow(final Rotation2d target) { - return setTargetAngleSlow(() -> target); + public boolean isNearAngle(Rotation2d target) { + return MathUtil.isNear(target.getDegrees(), inputs.motorPosition.getDegrees(), 10.0); } - public Command setVoltage(final double volts) { - return this.run(() -> io.setMotorVoltage(volts)); + public ShoulderIOInputsAutoLogged getInputs() { + return inputs; } - public Command hold() { - return Commands.sequence( - setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); + public double getVelocity() { + return inputs.angularVelocityRPS; } public Rotation2d getAngle() { - return inputs.position; + return inputs.motorPosition; } public Rotation2d getSetpoint() { return setpoint; } - - public boolean isNearTarget() { - return isNearAngle(setpoint); - } - - public boolean isNearAngle(Rotation2d target) { - return MathUtil.isNear(target.getDegrees(), inputs.position.getDegrees(), 10.0); - } - - public boolean isNearAngle(Rotation2d target, Rotation2d tolerance) { - return MathUtil.isNear( - target.getDegrees(), inputs.position.getDegrees(), tolerance.getDegrees()); - } - - public ShoulderIOInputsAutoLogged getInputs() { - return inputs; - } - - public double getVelocity() { - return inputs.angularVelocityRPS; - } } diff --git a/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java index 30932277..abfb1356 100644 --- a/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/AlphaSwerveConstants.java @@ -21,7 +21,7 @@ import edu.wpi.first.math.numbers.N8; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; -import frc.robot.subsystems.vision.Vision.VisionConstants; +import frc.robot.subsystems.camera.Camera.CameraConstants; public class AlphaSwerveConstants extends SwerveConstants { private static boolean instantiated = false; @@ -201,7 +201,7 @@ public Mass getMass() { } @Override - public VisionConstants[] getVisionConstants() { + public CameraConstants[] getCameraConstants() { // Stolen from banshee for now for testing, fix later once alphabot camera config is known final Matrix CAMERA_MATRIX = MatBuilder.fill( @@ -229,8 +229,8 @@ public VisionConstants[] getVisionConstants() { 0.012202835675939619, 0.0034143496721838872); - return new VisionConstants[] { - new VisionConstants( + return new CameraConstants[] { + new CameraConstants( "Camera", new Transform3d( new Translation3d( @@ -254,8 +254,8 @@ public double getBumperLength() { } @Override - public VisionConstants getAlgaeVisionConstants() { + public CameraConstants getAlgaeCameraConstants() { // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getAlgaeVisionConstants'"); + throw new UnsupportedOperationException("Unimplemented method 'getAlgaeCameraConstants'"); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java index 1e928567..b66a8a9d 100644 --- a/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/BansheeSwerveConstants.java @@ -21,8 +21,8 @@ import edu.wpi.first.math.numbers.N8; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; +import frc.robot.subsystems.camera.Camera.CameraConstants; import frc.robot.subsystems.swerve.Module.ModuleConstants; -import frc.robot.subsystems.vision.Vision.VisionConstants; public class BansheeSwerveConstants extends SwerveConstants { private static boolean instantiated = false; @@ -98,7 +98,7 @@ public AprilTagFieldLayout getFieldTagLayout() { } @Override - public VisionConstants[] getVisionConstants() { + public CameraConstants[] getCameraConstants() { final Matrix LEFT_CAMERA_MATRIX = MatBuilder.fill( Nat.N3(), @@ -149,8 +149,8 @@ public VisionConstants[] getVisionConstants() { 0, 0, 0); - final VisionConstants leftCamConstants = - new VisionConstants( + final CameraConstants leftCamConstants = + new CameraConstants( "Left_Camera", new Transform3d( new Translation3d( @@ -163,8 +163,8 @@ public VisionConstants[] getVisionConstants() { Units.degreesToRadians(120))), LEFT_CAMERA_MATRIX, LEFT_DIST_COEFFS); - final VisionConstants rightCamConstants = - new VisionConstants( + final CameraConstants rightCamConstants = + new CameraConstants( "Right_Camera", new Transform3d( new Translation3d( @@ -174,7 +174,7 @@ public VisionConstants[] getVisionConstants() { new Rotation3d(0, Units.degreesToRadians(-28.125), Units.degreesToRadians(210))), RIGHT_CAMERA_MATRIX, RIGHT_DIST_COEFFS); - return new VisionConstants[] {leftCamConstants, rightCamConstants}; + return new CameraConstants[] {leftCamConstants, rightCamConstants}; } @Override @@ -269,8 +269,8 @@ public double getBumperLength() { } @Override - public VisionConstants getAlgaeVisionConstants() { + public CameraConstants getAlgaeCameraConstants() { // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getAlgaeVisionConstants'"); + throw new UnsupportedOperationException("Unimplemented method 'getAlgaeCameraConstants'"); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java index e4d032c0..93e0355e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/KelpieSwerveConstants.java @@ -21,7 +21,7 @@ import edu.wpi.first.math.numbers.N8; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Mass; -import frc.robot.subsystems.vision.Vision.VisionConstants; +import frc.robot.subsystems.camera.Camera.CameraConstants; public class KelpieSwerveConstants extends SwerveConstants { private static boolean instantiated = false; @@ -178,7 +178,7 @@ public TalonFXConfiguration getTurnConfig(int cancoderID) { } @Override - public VisionConstants[] getVisionConstants() { + public CameraConstants[] getCameraConstants() { final Matrix BACK_LEFT_CAMERA_MATRIX = MatBuilder.fill( Nat.N3(), Nat.N3(), 906.46, 0.0, 675.30, 0.0, 907.49, 394.45, 0.0, 0.0, 1.0); @@ -203,8 +203,8 @@ public VisionConstants[] getVisionConstants() { final Matrix FRONT_LEFT_DIST_COEFFS = MatBuilder.fill( Nat.N8(), Nat.N1(), 0.057, -0.09, -0.001, 0.002, 0.043, -0.002, 0.004, -0.002); - final VisionConstants backLeftCamConstants = - new VisionConstants( + final CameraConstants backLeftCamConstants = + new CameraConstants( "Back_Left", new Transform3d( new Translation3d( @@ -217,8 +217,8 @@ public VisionConstants[] getVisionConstants() { Units.degreesToRadians(150))), BACK_LEFT_CAMERA_MATRIX, BACK_LEFT_DIST_COEFFS); - final VisionConstants backRightCamConstants = - new VisionConstants( + final CameraConstants backRightCamConstants = + new CameraConstants( "Back_Right", new Transform3d( new Translation3d( @@ -229,8 +229,8 @@ public VisionConstants[] getVisionConstants() { 0, Units.degreesToRadians(-(90 - 76.875000)), Units.degreesToRadians(210))), BACK_RIGHT_CAMERA_MATRIX, BACK_RIGHT_DIST_COEFFS); - final VisionConstants frontRightCamConstants = - new VisionConstants( + final CameraConstants frontRightCamConstants = + new CameraConstants( "Front_Right_Camera", new Transform3d( new Translation3d( @@ -240,8 +240,8 @@ public VisionConstants[] getVisionConstants() { new Rotation3d(0, Units.degreesToRadians(-10), Units.degreesToRadians(30))), FRONT_RIGHT_CAMERA_MATRIX, FRONT_RIGHT_DIST_COEFFS); - final VisionConstants frontLeftCamConstants = - new VisionConstants( + final CameraConstants frontLeftCamConstants = + new CameraConstants( "Front_Left_Camera", new Transform3d( new Translation3d( @@ -251,18 +251,18 @@ public VisionConstants[] getVisionConstants() { new Rotation3d(0, Units.degreesToRadians(-10), Units.degreesToRadians(-30))), FRONT_LEFT_CAMERA_MATRIX, FRONT_LEFT_DIST_COEFFS); - return new VisionConstants[] {frontRightCamConstants, frontLeftCamConstants}; + return new CameraConstants[] {frontRightCamConstants, frontLeftCamConstants}; } @Override - public VisionConstants getAlgaeVisionConstants() { // TODO calibrate + public CameraConstants getAlgaeCameraConstants() { // TODO calibrate final Matrix ALGAE_CAMERA_MATRIX = MatBuilder.fill( Nat.N3(), Nat.N3(), 906.46, 0.0, 675.30, 0.0, 907.49, 394.45, 0.0, 0.0, 1.0); final Matrix ALGAE_DIST_COEFFS = MatBuilder.fill( Nat.N8(), Nat.N1(), 0.039, -0.057, -0.005, 0.001, -0.004, -0.001, 0.003, 0.001); - return new VisionConstants( + return new CameraConstants( "Algae_Camera", new Transform3d( new Translation3d( diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index bae0cb59..414b0acb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -15,8 +15,8 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.subsystems.camera.Camera.CameraConstants; import frc.robot.subsystems.swerve.Module.ModuleConstants; -import frc.robot.subsystems.vision.Vision.VisionConstants; import java.io.File; /** @@ -95,7 +95,7 @@ public Translation2d[] getModuleTranslations() { public abstract AprilTagFieldLayout getFieldTagLayout(); - public abstract VisionConstants[] getVisionConstants(); + public abstract CameraConstants[] getCameraConstants(); public double getWheelRadiusMeters() { return Units.inchesToMeters(2.0); @@ -135,5 +135,5 @@ public double getVisionDistanceFactor() { public abstract double getBumperLength(); - public abstract VisionConstants getAlgaeVisionConstants(); + public abstract CameraConstants getAlgaeCameraConstants(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 37a6afcc..6772b2fb 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -14,6 +14,7 @@ import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -26,38 +27,33 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; -import frc.robot.subsystems.Superstructure.SuperState; +import frc.robot.subsystems.camera.Camera; +import frc.robot.subsystems.camera.CameraIO; import frc.robot.subsystems.swerve.OdometryThreadIO.OdometryThreadIOInputs; import frc.robot.subsystems.swerve.PhoenixOdometryThread.Samples; import frc.robot.subsystems.swerve.PhoenixOdometryThread.SignalID; import frc.robot.subsystems.swerve.PhoenixOdometryThread.SignalType; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionHelper; -import frc.robot.subsystems.vision.VisionIO; import frc.robot.utils.Tracer; import java.util.Arrays; import java.util.List; import java.util.Map; -import java.util.NoSuchElementException; import java.util.Optional; import java.util.function.Consumer; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import org.photonvision.targeting.PhotonPipelineResult; public class SwerveSubsystem extends SubsystemBase { private final SwerveConstants constants; @@ -66,11 +62,11 @@ public class SwerveSubsystem extends SubsystemBase { private final Module[] modules; // FL, FR, BL, BR private final OdometryThreadIO odoThread; private final OdometryThreadIOInputs odoThreadInputs = new OdometryThreadIOInputs(); - private final Vision algaeCamera; + // private final Camera algaeCamera; private SwerveDriveKinematics kinematics; - private final Vision[] cameras; + private final Camera[] cameras; /** For delta tracking with PhoenixOdometryThread* */ private SwerveModulePosition[] lastModulePositions = @@ -84,8 +80,7 @@ public class SwerveSubsystem extends SubsystemBase { private Rotation2d rawGyroRotation = new Rotation2d(); private Rotation2d lastGyroRotation = new Rotation2d(); - private SwerveDrivePoseEstimator estimator; - private double lastEstTimestamp = 0.0; + private static SwerveDrivePoseEstimator estimator; // TODO i forgot why this is static private double lastOdometryUpdateTimestamp = 0.0; final Pose3d[] cameraPoses; @@ -94,8 +89,6 @@ public class SwerveSubsystem extends SubsystemBase { private Alert usingSyncOdometryAlert = new Alert("Using Sync Odometry", AlertType.kInfo); private Alert missingModuleData = new Alert("Missing Module Data", AlertType.kError); private Alert missingGyroData = new Alert("Missing Gyro Data", AlertType.kWarning); - private Alert futureVisionData = - new Alert("Vision Data Coming from ✨The Future✨", AlertType.kError); private boolean useModuleForceFF = !Robot.isSimulation(); @@ -104,14 +97,15 @@ public class SwerveSubsystem extends SubsystemBase { public SwerveSubsystem( SwerveConstants constants, GyroIO gyroIO, - VisionIO[] visionIOs, + CameraIO[] CameraIOs, ModuleIO[] moduleIOs, OdometryThreadIO odoThread, - Optional simulation, - VisionIO algaeCameraIO) { + Optional simulation + // CameraIO algaeCameraIO) { + ) { this.constants = constants; this.kinematics = new SwerveDriveKinematics(constants.getModuleTranslations()); - this.estimator = + estimator = new SwerveDrivePoseEstimator( kinematics, rawGyroRotation, @@ -122,15 +116,15 @@ public SwerveSubsystem( this.gyroIO = gyroIO; this.odoThread = odoThread; this.simulation = simulation; - this.algaeCamera = new Vision(algaeCameraIO); - cameras = new Vision[visionIOs.length]; + // this.algaeCamera = new Camera(algaeCameraIO); + cameras = new Camera[CameraIOs.length]; modules = new Module[moduleIOs.length]; for (int i = 0; i < moduleIOs.length; i++) { modules[i] = new Module(moduleIOs[i]); } - for (int i = 0; i < visionIOs.length; i++) { - cameras[i] = new Vision(visionIOs[i]); + for (int i = 0; i < CameraIOs.length; i++) { + cameras[i] = new Camera(CameraIOs[i]); } cameraPoses = new Pose3d[cameras.length]; @@ -152,8 +146,8 @@ public void periodic() { Tracer.trace("Update cam inputs", camera::updateInputs); Tracer.trace("Process cam inputs", camera::processApriltagInputs); } - Tracer.trace("Update algae cam inputs", algaeCamera::updateInputs); - Tracer.trace("Process algae cam inputs", algaeCamera::processAlgaeInputs); + // Tracer.trace("Update algae cam inputs", algaeCamera::updateInputs); + // Tracer.trace("Process algae cam inputs", algaeCamera::processAlgaeInputs); Tracer.trace( "Update odo inputs", () -> odoThread.updateInputs(odoThreadInputs, lastOdometryUpdateTimestamp)); @@ -194,28 +188,28 @@ public void periodic() { Tracer.trace("Update odometry", this::updateOdometry); Tracer.trace("Update tag vision", this::updateVision); - Tracer.trace("Update algae vision", this::updateAlgaeVision); + // Tracer.trace("Update algae vision", this::updateAlgaeVision); }); } - private void updateAlgaeVision() { - final PhotonPipelineResult result = - new PhotonPipelineResult( - algaeCamera.inputs.sequenceID, - algaeCamera.inputs.captureTimestampMicros, - algaeCamera.inputs.publishTimestampMicros, - algaeCamera.inputs.timeSinceLastPong, - algaeCamera.inputs.targets); - try { - if (!algaeCamera.inputs.stale) { - Logger.recordOutput("Vision/Algae Result", result); - } else { - Logger.recordOutput("Vision/Algae Camera/Invalid Result", "Stale"); - } - } catch (NullPointerException e) { - Logger.recordOutput("Vision/Algae Camera/Invalid Result", "No Targets"); - } - } + // private void updateAlgaeVision() { + // final PhotonPipelineResult result = + // new PhotonPipelineResult( + // algaeCamera.inputs.sequenceID, + // algaeCamera.inputs.captureTimestampMicros, + // algaeCamera.inputs.publishTimestampMicros, + // algaeCamera.inputs.timeSinceLastPong, + // algaeCamera.inputs.targets); + // try { + // if (!algaeCamera.inputs.stale) { + // Logger.recordOutput("Vision/Algae Result", result); + // } else { + // Logger.recordOutput("Vision/Algae Camera/Invalid Result", "Stale"); + // } + // } catch (NullPointerException e) { + // Logger.recordOutput("Vision/Algae Camera/Invalid Result", "No Targets"); + // } + // } private void updateOdometry() { if (Robot.ROBOT_TYPE != RobotType.REAL) @@ -332,128 +326,28 @@ private void updateOdometry() { } private void updateVision() { - var i = 0; hasFrontTags = false; - var hasFutureData = false; - for (var camera : cameras) { - if ((camera.getName().equals("Front_Left_Camera") - || camera.getName().equals("Front_Right_Camera")) - && camera.inputs.targets.size() > 0) { - hasFrontTags = true; - } - final PhotonPipelineResult result = - new PhotonPipelineResult( - camera.inputs.sequenceID, - camera.inputs.captureTimestampMicros, - camera.inputs.publishTimestampMicros, - camera.inputs.timeSinceLastPong, - camera.inputs.targets); - try { - if (!camera.inputs.stale) { - var estPose = Tracer.trace("Update Camera", () -> camera.update(result)); - var visionPose = estPose.get().estimatedPose; - // Sets the pose on the sim field - camera.setSimPose(estPose, camera, !camera.inputs.stale); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/Vision Pose From " + camera.getName(), visionPose); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/Vision Pose2d From " + camera.getName(), visionPose.toPose2d()); - final var deviations = VisionHelper.findVisionMeasurementStdDevs(estPose.get()); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Deviations", deviations.getData()); - Tracer.trace( - "Add Measurement From " + camera.getName(), - () -> { - estimator.addVisionMeasurement( - visionPose.toPose2d(), - camera.inputs.captureTimestampMicros / 1.0e6, - deviations - // .times(DriverStation.isAutonomous() ? 2.0 : 1.0) - .times( - camera.getName().equals("Front_Left_Camera") - || camera.getName().equals("Front_Right_Camera") - ? 0.75 - : 2.0) - // reef positions - .times( - (camera.getName().equals("Front_Left_Camera") - || camera.getName().equals("Front_Right_Camera")) - && (Robot.state.get().toString().startsWith("PRE_L") - || Robot.state.get() == SuperState.SCORE_CORAL - || Robot.state.get() == SuperState.INTAKE_ALGAE_HIGH - || Robot.state.get() == SuperState.INTAKE_ALGAE_LOW) - ? 0.5 - : 1.5) // TODO tune these sorts of numbers - // hp tags - .times( - // !camera.getName().equals("Front_Camera") - // && - estPose.get().targetsUsed.stream() - .anyMatch( - t -> - t.getFiducialId() == 12 - || t.getFiducialId() == 13 - || t.getFiducialId() == 2 - || t.getFiducialId() == 1) - ? 1.5 - : 1.0) - // barge tags - .times( - // !camera.getName().equals("Front_Right_Camera") - // && - estPose.get().targetsUsed.stream() - .anyMatch( - t -> - t.getFiducialId() == 4 - || t.getFiducialId() == 5 - || t.getFiducialId() == 15 - || t.getFiducialId() == 14) - ? 1.2 - : 1.0) - .times(Robot.state.get() == SuperState.PRE_NET ? 0.5 : 1.0)); - // the sussifier - }); - lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; - hasFutureData |= camera.inputs.captureTimestampMicros > RobotController.getTime(); - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Good Update"); - cameraPoses[i] = visionPose; - Tracer.trace( - "Log Tag Poses", - () -> { - Pose3d[] targetPose3ds = new Pose3d[result.targets.size()]; - for (int j = 0; j < result.targets.size(); j++) { - targetPose3ds[j] = - Robot.ROBOT_HARDWARE - .swerveConstants - .getFieldTagLayout() - .getTagPose(result.targets.get(j).getFiducialId()) - .get(); - } - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Target Poses", targetPose3ds); - }); - - } else { - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Stale"); - } - } catch (NoSuchElementException e) { - // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + camera.getName() + "/Invalid Pose Result", "Bad Estimate"); - } - i++; + for (int i = 0; i < cameras.length; i++) { + if (cameras[i].hasFrontTags()) hasFrontTags = true; + cameras[i].updateCamera(); + cameraPoses[i] = cameras[i].getPose(); } Logger.recordOutput("Vision/Front Cameras Have Tags", hasFrontTags); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/Camera Poses", cameraPoses); Pose3d[] arr = new Pose3d[cameras.length]; for (int k = 0; k < cameras.length; k++) { - arr[k] = getPose3d().transformBy(cameras[k].inputs.constants.robotToCamera()); + arr[k] = getPose3d().transformBy(cameras[k].getCameraConstants().robotToCamera()); } if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Vision/Camera Poses on Robot", arr); - futureVisionData.set(hasFutureData); + } + + public static void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + estimator.addVisionMeasurement( + visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } /** @@ -788,53 +682,56 @@ public void setCurrentLimits(CurrentLimitsConfigs configs) { } } - @SuppressWarnings("resource") - public Command driveToAlgae(DoubleSupplier xVel, DoubleSupplier yVel, DoubleSupplier theta) { - final PIDController xController = new PIDController(1, 0.0, 0.0); // TODO tune - final PIDController yController = new PIDController(9, 0.0, 0.8); // TODO tune - return this.run( - () -> { - var target = - new PhotonPipelineResult( - algaeCamera.inputs.sequenceID, - algaeCamera.inputs.captureTimestampMicros, - algaeCamera.inputs.publishTimestampMicros, - algaeCamera.inputs.timeSinceLastPong, - algaeCamera.inputs.targets) - .getBestTarget(); - if (target != null) { - double yaw = -target.getYaw(); - double pitch = target.getPitch(); - double r = Units.inchesToMeters(36.990 - 8.125) / Math.tan(Math.toRadians(pitch - 35)); - double yOffset = r * Math.sin(Math.toRadians(yaw)); - double xOffset = r * Math.cos(Math.toRadians(yaw)); - Logger.recordOutput("AutoAim/Algae Detection/X Offset", xOffset); - Logger.recordOutput("AutoAim/Algae Detection/Y Offset", yOffset); - Logger.recordOutput("AutoAim/Algae Detection/R", r); - Logger.recordOutput("AutoAim/Algae Detection/Yaw", yaw); - Logger.recordOutput("AutoAim/Algae Detection/Pitch", pitch); - drive( - ChassisSpeeds.fromFieldRelativeSpeeds( - new ChassisSpeeds( - xVel.getAsDouble(), yVel.getAsDouble(), theta.getAsDouble()), - getRotation()) - .plus( - new ChassisSpeeds( - (1 / (1 + Math.abs(yOffset))) * xController.calculate(xOffset, 0.0), - yController.calculate(yOffset, 0.0), - 0.0)), - false, - new double[4], - new double[4]); - } else { - drive( - ChassisSpeeds.fromFieldRelativeSpeeds( - new ChassisSpeeds(xVel.getAsDouble(), yVel.getAsDouble(), theta.getAsDouble()), - getRotation()), - false, - new double[4], - new double[4]); - } - }); - } + // it's fine this was cooked anyway + // @SuppressWarnings("resource") + // public Command driveToAlgae(DoubleSupplier xVel, DoubleSupplier yVel, DoubleSupplier theta) { + // final PIDController xController = new PIDController(1, 0.0, 0.0); // TODO tune + // final PIDController yController = new PIDController(9, 0.0, 0.8); // TODO tune + // return this.run( + // () -> { + // var target = + // new PhotonPipelineResult( + // algaeCamera.inputs.sequenceID, + // algaeCamera.inputs.captureTimestampMicros, + // algaeCamera.inputs.publishTimestampMicros, + // algaeCamera.inputs.timeSinceLastPong, + // algaeCamera.inputs.targets) + // .getBestTarget(); + // if (target != null) { + // double yaw = -target.getYaw(); + // double pitch = target.getPitch(); + // double r = Units.inchesToMeters(36.990 - 8.125) / Math.tan(Math.toRadians(pitch - + // 35)); + // double yOffset = r * Math.sin(Math.toRadians(yaw)); + // double xOffset = r * Math.cos(Math.toRadians(yaw)); + // Logger.recordOutput("AutoAim/Algae Detection/X Offset", xOffset); + // Logger.recordOutput("AutoAim/Algae Detection/Y Offset", yOffset); + // Logger.recordOutput("AutoAim/Algae Detection/R", r); + // Logger.recordOutput("AutoAim/Algae Detection/Yaw", yaw); + // Logger.recordOutput("AutoAim/Algae Detection/Pitch", pitch); + // drive( + // ChassisSpeeds.fromFieldRelativeSpeeds( + // new ChassisSpeeds( + // xVel.getAsDouble(), yVel.getAsDouble(), theta.getAsDouble()), + // getRotation()) + // .plus( + // new ChassisSpeeds( + // (1 / (1 + Math.abs(yOffset))) * xController.calculate(xOffset, 0.0), + // yController.calculate(yOffset, 0.0), + // 0.0)), + // false, + // new double[4], + // new double[4]); + // } else { + // drive( + // ChassisSpeeds.fromFieldRelativeSpeeds( + // new ChassisSpeeds(xVel.getAsDouble(), yVel.getAsDouble(), + // theta.getAsDouble()), + // getRotation()), + // false, + // new double[4], + // new double[4]); + // } + // }); + // } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index a48e94bb..00000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N8; -import frc.robot.Robot; -import frc.robot.Robot.RobotType; -import java.util.Optional; -import org.littletonrobotics.junction.Logger; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; - -/** Add your docs here. */ -public class Vision { - public static final Matrix visionPointBlankDevs = - new Matrix(Nat.N3(), Nat.N1(), new double[] {0.6, 0.6, 0.5}); - public static final Matrix infiniteDevs = - new Matrix( - Nat.N3(), - Nat.N1(), - new double[] { - Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY - }); - public static final double distanceFactor = 3.0; - - public record VisionConstants( - String cameraName, - Transform3d robotToCamera, - Matrix intrinsicsMatrix, - Matrix distCoeffs) {} - - private final VisionIO io; - public final VisionIOInputsLogged inputs = new VisionIOInputsLogged(); - - public Vision(final VisionIO io) { - this.io = io; - io.updateInputs(inputs); - } - - public void setSimPose(Optional simEst, Vision camera, boolean newResult) { - this.io.setSimPose(simEst, camera, newResult); - } - - public void updateInputs() { - io.updateInputs(inputs); - } - - public void processApriltagInputs() { - Logger.processInputs("Apriltag Vision/" + inputs.constants.cameraName, inputs); - } - - public void processAlgaeInputs() { - Logger.processInputs("Algae Vision/" + inputs.constants.cameraName, inputs); - } - - public Optional update(PhotonPipelineResult result) { - // Skip if we have no targets (could/should switch to 1?) - if (result.getTargets().size() < 1) { - return Optional.empty(); - } - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput( - "Vision/" + inputs.constants.cameraName + " Best Distance", - result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); - Optional estPose = - VisionHelper.update( - result, - inputs.constants.intrinsicsMatrix(), - inputs.constants.distCoeffs(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - inputs.constants.robotToCamera(), - inputs.coprocPNPTransform); - - return estPose; - } - - public String getName() { - return inputs.constants.cameraName; - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionHelper.java b/src/main/java/frc/robot/subsystems/vision/VisionHelper.java deleted file mode 100644 index e47773b3..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionHelper.java +++ /dev/null @@ -1,473 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N8; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Robot; -import frc.robot.subsystems.vision.Vision.VisionConstants; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import org.littletonrobotics.junction.LogTable; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -public class VisionHelper { - - /*** - * To be added - */ - public static final List TAG_COUNT_DEVIATION_PARAMS = - List.of( - // 1 tag - new TagCountDeviation( - new UnitDeviationParams(.25, .4, .9), new UnitDeviationParams(.5, .7, 1.5)), - - // 2 tags - new TagCountDeviation( - new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5)), - - // 3+ tags - new TagCountDeviation( - new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5))); - - public class Logging { - public static void logPhotonTrackedTarget( - PhotonTrackedTarget target, LogTable table, String name) { - // System.out.println(target); - logTransform3d(target.getBestCameraToTarget(), table, name); - logTransform3d(target.getAlternateCameraToTarget(), table, "Alt " + name); - logCorners(target, table, name, target.getFiducialId()); - - table.put("Targets/Yaw " + name, target.getYaw()); - table.put("Targets/Pitch " + name, target.getPitch()); - table.put("Targets/Area " + name, target.getArea()); - table.put("Targets/Skew " + name, target.getSkew()); - table.put("Targets/Fiducial ID " + name, target.getFiducialId()); - table.put("Targets/Pose Ambiguity " + name, target.getPoseAmbiguity()); - } - - public static void logCorners(PhotonTrackedTarget target, LogTable table, String name, int id) { - double[] detectedCornersX = new double[4]; - double[] detectedCornersY = new double[4]; - double[] minAreaRectCornersX = new double[4]; - double[] minAreaRectCornersY = new double[4]; - for (int i = 0; i < 4; i++) { - if (id > -1) { - detectedCornersX[i] = target.getDetectedCorners().get(i).x; - detectedCornersY[i] = target.getDetectedCorners().get(i).y; - } else { - detectedCornersX[i] = 0; - detectedCornersY[i] = 0; - } - minAreaRectCornersX[i] = target.getMinAreaRectCorners().get(i).x; - minAreaRectCornersY[i] = target.getMinAreaRectCorners().get(i).y; - } - table.put("Targets/Detected Corners X " + name, detectedCornersX); - table.put("Targets/Detected Corners Y " + name, detectedCornersY); - table.put("Targets/Min Area Rect Corners X " + name, minAreaRectCornersX); - table.put("Targets/Min Area Rect Corners Y " + name, minAreaRectCornersY); - } - - public static void logTransform3d(Transform3d transform3d, LogTable table, String name) { - double rotation[] = new double[4]; - rotation[0] = transform3d.getRotation().getQuaternion().getW(); - rotation[1] = transform3d.getRotation().getQuaternion().getX(); - rotation[2] = transform3d.getRotation().getQuaternion().getY(); - rotation[3] = transform3d.getRotation().getQuaternion().getZ(); - table.put("Targets/Rotation " + name, rotation); - - double translation[] = new double[3]; - translation[0] = transform3d.getTranslation().getX(); - translation[1] = transform3d.getTranslation().getY(); - translation[2] = transform3d.getTranslation().getZ(); - table.put("Targets/Translation " + name, translation); - } - - public static void logVisionConstants(VisionConstants constants, LogTable table) { - table.put("Vision Constants/Name ", constants.cameraName()); - table.put("Vision Constants/Transform ", constants.robotToCamera()); - table.put("Vision Constants/Intrinsics ", constants.intrinsicsMatrix().getData()); - table.put("Vision Constants/Distortion ", constants.distCoeffs().getData()); - } - - public static Transform3d getLoggedTransform3d(double[] translation, double[] rotation) { - Transform3d transform3d = - new Transform3d( - new Translation3d(translation[0], translation[1], translation[2]), - new Rotation3d(new Quaternion(rotation[0], rotation[1], rotation[2], rotation[3]))); - return transform3d; - } - - public static Transform3d getLoggedTransform3d(LogTable table, String name) { - double[] rotation = table.get("Targets/Rotation " + name, new double[4]); - double[] translation = table.get("Targets/Translation " + name, new double[3]); - return getLoggedTransform3d(translation, rotation); - } - - public static PhotonTrackedTarget getLoggedPhotonTrackedTarget(LogTable table, String name) { - double[] translation = table.get("Targets/Translation " + name, new double[3]); - double[] rotation = table.get("Targets/Rotation " + name, new double[4]); - double[] altTranslation = table.get("Targets/Translation Alt " + name, new double[3]); - double[] altRotation = table.get("Targets/Rotation Alt " + name, new double[4]); - double[] detectedCornersX = table.get("Targets/Detected Corners X " + name, new double[4]); - double[] detectedCornersY = table.get("Targets/Detected Corners Y " + name, new double[4]); - double[] minAreaRectCornersX = - table.get("Targets/Min Area Rect Corners X " + name, new double[4]); - double[] minAreaRectCornersY = - table.get("Targets/Min Area Rect Corners Y " + name, new double[4]); - List detectedCorners = new ArrayList<>(); - List minAreaRectCorners = new ArrayList<>(); - - for (int i = 0; i < 4; i++) { - detectedCorners.add(new TargetCorner(detectedCornersX[i], detectedCornersY[i])); - minAreaRectCorners.add(new TargetCorner(minAreaRectCornersX[i], minAreaRectCornersY[i])); - } - Transform3d pose = getLoggedTransform3d(translation, rotation); - Transform3d altPose = getLoggedTransform3d(altTranslation, altRotation); - return (new PhotonTrackedTarget( - table.get("Targets/Yaw " + name, -1), - table.get("Targets/Pitch " + name, -1), - table.get("Targets/Area " + name, -1), - table.get("Targets/Skew " + name, -1), - (int) (table.get("Targets/Fiducial ID " + name, -1)), - -1, // for obj detection - -1, - pose, - altPose, - table.get("Targets/Pose Ambiguity " + name, -1), - minAreaRectCorners, - detectedCorners)); - } - - public static VisionConstants getLoggedVisionConstants(LogTable table) { - return new VisionConstants( - table.get("Vision Constants Name ", "Default"), - table.get("Vision Constants Transform ", new Transform3d()), - new Matrix( - Nat.N3(), - Nat.N3(), - table.get("Vision Constants Intrinsics ", Matrix.eye(Nat.N3()).getData())), - new Matrix( - Nat.N8(), - Nat.N1(), - table.get( - "Vision Constants Distortion ", - new double[] {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}))); - } - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera. - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @param strat The selected pose solving strategy. - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public static Optional update( - PhotonPipelineResult cameraResult, - Matrix cameraMatrix, - Matrix distCoeffs, - PoseStrategy strat, - Transform3d robotToCamera, - Transform3d bestTF) { - Optional estimatedPose; - switch (strat) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult, robotToCamera); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = - multiTagOnRioStrategy( - cameraResult, - Optional.of(cameraMatrix), - Optional.of(distCoeffs), - PoseStrategy.LOWEST_AMBIGUITY, - robotToCamera); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = - multiTagOnCoprocStrategy( - cameraResult, - Optional.of(cameraMatrix), - Optional.of(distCoeffs), - robotToCamera, - PoseStrategy.LOWEST_AMBIGUITY, - bestTF); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - return estimatedPose; - } - - /** - * Runs SolvePNP on the roborio - * - * @param result The latest pipeline result from the camera. - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - * @param multiTagFallbackStrategy Fallback strategy in case the rio fails. Should usually be - * lowest ambiguity - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - private static Optional multiTagOnRioStrategy( - PhotonPipelineResult result, - Optional> cameraMatrix, - Optional> distCoeffs, // TODO FIX THE 5 VS 8 THING!! - PoseStrategy multiTagFallbackStrategy, - Transform3d robotToCamera) { - boolean hasCalibData = cameraMatrix.isPresent() && distCoeffs.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update( - result, - cameraMatrix.get(), - distCoeffs.get(), - multiTagFallbackStrategy, - robotToCamera, - new Transform3d()); - } - - var pnpResult = - VisionEstimation.estimateCamPosePNP( - cameraMatrix.get(), - distCoeffs.get(), - result.getTargets(), - Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout(), - TargetModel.kAprilTag36h11); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent()) - return update( - result, - cameraMatrix.get(), - distCoeffs.get(), - multiTagFallbackStrategy, - robotToCamera, - new Transform3d()); - var best = - new Pose3d() - .plus(pnpResult.get().best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Runs SolvePNP on a coprocessor - * - * @param result The latest pipeline result from the camera. - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - * @param multiTagFallbackStrategy Fallback strategy in case the coproc fails. Should usually be - * the rio or lowest ambiguity - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - private static Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result, - Optional> cameraMatrix, - Optional> distCoeffs, - Transform3d robotToCamera, - PoseStrategy multiTagFallbackStrategy, - Transform3d bestTF) { - if (!bestTF.equals(new Transform3d())) { - var best_tf = bestTF; - var best = - new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout().getOrigin()) - .plus(robotToCamera.inverse()); // field-to-robot - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update( - result, - cameraMatrix.get(), - distCoeffs.get(), - multiTagFallbackStrategy, - robotToCamera, - new Transform3d()); - } - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private static Optional lowestAmbiguityStrategy( - PhotonPipelineResult result, Transform3d robotToCamera) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - - // reject if not a tag on this field - if (target.getFiducialId() < 1 || target.getFiducialId() > 22) continue; - - // reject if too far - if (target.getBestCameraToTarget().getTranslation().getNorm() > 5) continue; - - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = - Robot.ROBOT_HARDWARE.swerveConstants.getFieldTagLayout().getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + targetFiducialId, - false); - return Optional.empty(); - } - - var estimatedRobotPose = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY); - return Optional.of(estimatedRobotPose); - } - - /*** 5026 - to be tuned if necessary*/ - public static record UnitDeviationParams( - double distanceMultiplier, double eulerMultiplier, double minimum) { - private double computeUnitDeviation(double averageDistance) { - return Math.max(minimum, eulerMultiplier * Math.exp(averageDistance * distanceMultiplier)); - } - } - - /*** 5026 - to be tuned if necessary*/ - public static record TagCountDeviation( - UnitDeviationParams xParams, UnitDeviationParams yParams, UnitDeviationParams thetaParams) { - private Matrix computeDeviation(double averageDistance) { - return MatBuilder.fill( - Nat.N3(), - Nat.N1(), - xParams.computeUnitDeviation(averageDistance), - yParams.computeUnitDeviation(averageDistance), - thetaParams.computeUnitDeviation(averageDistance)); - } - - public TagCountDeviation(UnitDeviationParams xyParams, UnitDeviationParams thetaParams) { - this(xyParams, xyParams, thetaParams); - } - } - - public static record VisionMeasurement( - EstimatedRobotPose estimation, Matrix confidence) {} - - public static Matrix findVisionMeasurementStdDevs(EstimatedRobotPose estimation) { - double sumDistance = 0; - for (var target : estimation.targetsUsed) { - var t3d = target.getBestCameraToTarget(); - sumDistance += - Math.sqrt(Math.pow(t3d.getX(), 2) + Math.pow(t3d.getY(), 2) + Math.pow(t3d.getZ(), 2)); - } - double avgDistance = sumDistance / estimation.targetsUsed.size(); - - var deviation = - Vision.visionPointBlankDevs.times(Math.max(avgDistance, 0.0) * Vision.distanceFactor); - if (estimation.targetsUsed.size() == 1) { - deviation = deviation.times(3); - } - if (estimation.targetsUsed.size() == 1 && estimation.targetsUsed.get(0).poseAmbiguity > 0.15) { - return Vision.infiniteDevs; - } - // Reject if estimated pose is in the air or ground - if (Math.abs(estimation.estimatedPose.getZ()) > 0.125) { - return Vision.infiniteDevs; - } - // TAG_COUNT_DEVIATION_PARAMS - // .get( - // MathUtil.clamp( - // estimation.targetsUsed.size() - 1, 0, TAG_COUNT_DEVIATION_PARAMS.size() - 1)) - // .computeDeviation(avgDistance); - return deviation; - } - - /** A Field2d for visualizing our robot and objects on the field. */ - public static Field2d getSimDebugField(VisionSystemSim visionSim) { - if (!Robot.isSimulation()) return null; - return visionSim.getDebugField(); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java deleted file mode 100644 index a6bd59bc..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.MatBuilder; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.Nat; -import edu.wpi.first.math.geometry.Transform3d; -import frc.robot.subsystems.vision.Vision.VisionConstants; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.targeting.PhotonTrackedTarget; - -/** Add your docs here. */ -public interface VisionIO { - - public static class VisionIOInputs { - // latency could just be calculated from the timestamp, do we need it as an input or could it be - // an output? - public double latency = 0.0; - // We could use protobuf serialization for this instead of custom - // There are som alleged performance considerations for protobuf - // Could be worth testing? This works for now - public List targets = new ArrayList<>(); - public double numTags = 0; // Helps with deserialization - public Transform3d coprocPNPTransform = new Transform3d(); - public VisionConstants constants = - new VisionConstants( - "Default", - new Transform3d(), - Matrix.eye(Nat.N3()), - MatBuilder.fill(Nat.N8(), Nat.N1(), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)); - public long sequenceID = 0; - public long captureTimestampMicros = 0; - public long publishTimestampMicros = 0; - public long timeSinceLastPong = 0; - public boolean stale = true; - } - - public void updateInputs(VisionIOInputs inputs); - - public void setSimPose(Optional simEst, Vision camera, boolean newResult); - - public String getName(); -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java b/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java deleted file mode 100644 index 28cc4d75..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOInputsLogged.java +++ /dev/null @@ -1,57 +0,0 @@ -package frc.robot.subsystems.vision; - -import org.littletonrobotics.junction.LogTable; -import org.littletonrobotics.junction.inputs.LoggableInputs; - -/** - * AutoLog does not work with PhotonTrackedTargets as of yet which is very sad but until it does - * here is a manual implementation - */ -public class VisionIOInputsLogged extends VisionIO.VisionIOInputs - implements LoggableInputs, Cloneable { - @Override - public void toLog(LogTable table) { - table.put("Latency", latency); - - for (int i = 0; i < targets.size(); i++) { - VisionHelper.Logging.logPhotonTrackedTarget(targets.get(i), table, String.valueOf(i)); - } - table.put("NumTags", targets.size()); - table.put("Pose", coprocPNPTransform); - table.put("Stale", stale); - VisionHelper.Logging.logVisionConstants(constants, table); - table.put("SequenceID", sequenceID); - table.put("Capture Timestamp Micros", captureTimestampMicros); - table.put("Publish Timestamp Micros", publishTimestampMicros); - table.put("Time Since Last Pong", timeSinceLastPong); - } - - @Override - public void fromLog(LogTable table) { - latency = table.get("Latency", latency); - for (int i = 0; i < table.get("NumTags", numTags); i++) { - this.targets.add(VisionHelper.Logging.getLoggedPhotonTrackedTarget(table, String.valueOf(i))); - } - numTags = table.get("NumTags", numTags); - coprocPNPTransform = table.get("Pose", coprocPNPTransform); - constants = VisionHelper.Logging.getLoggedVisionConstants(table); - sequenceID = table.get("SequenceID", sequenceID); - captureTimestampMicros = table.get("Capture Timestamp Micros", captureTimestampMicros); - publishTimestampMicros = table.get("Publish Timestamp Micros", publishTimestampMicros); - timeSinceLastPong = table.get("Time Since Last Pong", timeSinceLastPong); - stale = table.get("Stale", stale); - } - - public VisionIOInputsLogged clone() { - VisionIOInputsLogged copy = new VisionIOInputsLogged(); - copy.latency = this.latency; - copy.targets = this.targets; - copy.numTags = this.numTags; - copy.coprocPNPTransform = this.coprocPNPTransform; - copy.sequenceID = this.sequenceID; - copy.captureTimestampMicros = this.captureTimestampMicros; - copy.publishTimestampMicros = this.publishTimestampMicros; - copy.timeSinceLastPong = this.timeSinceLastPong; - return copy; - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java deleted file mode 100644 index 418dc178..00000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N8; -import frc.robot.subsystems.vision.Vision.VisionConstants; -import java.util.Optional; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; - -/** Add your docs here. */ -public class VisionIOReal implements VisionIO { - // constants - public String cameraName; - public PhotonCamera camera; - public Matrix cameraMatrix; - public Matrix distCoeffs; - private final VisionConstants constants; - - /*** Transform3d from the center of the robot to the camera mount position (ie, - * robot ➔ camera) in the Robot - * Coordinate System. - ***/ - public Transform3d robotToCamera; - - public VisionIOReal(VisionConstants constants) { - cameraName = constants.cameraName(); - camera = new PhotonCamera(cameraName); - robotToCamera = constants.robotToCamera(); - cameraMatrix = constants.intrinsicsMatrix(); - distCoeffs = constants.distCoeffs(); - this.constants = constants; - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - var results = camera.getAllUnreadResults(); - if (results.size() > 0) { - final var result = results.get(results.size() - 1); - inputs.latency = result.metadata.getLatencyMillis(); - inputs.targets = result.targets; - inputs.constants = constants; - inputs.coprocPNPTransform = - result - .multitagResult - .map((pnpResult) -> pnpResult.estimatedPose.best) - .orElse(Transform3d.kZero); - inputs.sequenceID = result.metadata.getSequenceID(); - inputs.captureTimestampMicros = result.metadata.getCaptureTimestampMicros(); - inputs.publishTimestampMicros = result.metadata.getPublishTimestampMicros(); - inputs.timeSinceLastPong = result.metadata.timeSinceLastPong; - inputs.stale = false; - } else { - // else leave stale data - inputs.stale = true; - } - } - - @Override - public String getName() { - return cameraName; - } - - @Override - public void setSimPose(Optional simEst, Vision camera, boolean newResult) {} -} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java index 89ce07e0..0ebcaed0 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIO.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIO.java @@ -19,7 +19,7 @@ class ArmIOInputs { public void setMotorVoltage(final double voltage); - public void setMotorPosition(final Rotation2d targetPosition); + public void setAngle(final Rotation2d targetPosition); public default void resetEncoder(final Rotation2d rotation) {} diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index 639a4131..3927470e 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -64,7 +64,7 @@ public WristIOReal(final int motorId, final TalonFXConfiguration config) { @Override public void updateInputs(ArmIOInputs inputs) { Logger.recordOutput( - "wrist refreshall statuscode", + "Wrist/Signal Refresh Status Code", BaseStatusSignal.refreshAll( angularVelocityRPS, temp, @@ -87,7 +87,7 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setAngle(final Rotation2d targetPosition) { motor.setControl(motionMagic.withPosition(targetPosition.getRotations())); } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java index 42e5c350..0a76f97c 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; public class WristIOSim implements WristIO { - // TODO: UPDATE WITH VALUES WHEN CAD IS DONE private final SingleJointedArmSim armSim = new SingleJointedArmSim( DCMotor.getKrakenX60Foc(1), @@ -52,7 +51,7 @@ public void setMotorVoltage(final double voltage) { } @Override - public void setMotorPosition(final Rotation2d targetPosition) { + public void setAngle(final Rotation2d targetPosition) { setMotorVoltage( pid.calculate(armSim.getAngleRads(), targetPosition.getRadians()) + feedforward.calculate(pid.getSetpoint().position, pid.getSetpoint().velocity)); diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index aaec10d3..b7cede94 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -4,45 +4,27 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +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 frc.robot.Robot; -import frc.robot.Robot.RobotType; -import frc.robot.subsystems.ExtensionKinematics; -import frc.robot.subsystems.shoulder.ShoulderIOInputsAutoLogged; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.utils.LoggedTunableNumber; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class WristSubsystem extends SubsystemBase { public static final double WRIST_GEAR_RATIO = 4.0 * 4.0 * (64.0 / 34.0); - // TODO: UPDATE WHEN CAD IS FINISHED - public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); - public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); + public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(160.0); + public static final Rotation2d MIN_ARM_ROTATION = + // Rotation2d.fromRadians(-0.687); // Rotation2d.fromDegrees(-90.0); //TODO find?? + Rotation2d.fromDegrees(178).minus(Rotation2d.fromRadians(3.357)); public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); - public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); - public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(178.0); - public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-65); - public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); - public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); - public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); - public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); - public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); - public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); - - public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); - public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(170.0); - - public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = - Rotation2d.fromDegrees(-20.0); - - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(110); - public static final Rotation2d WRIST_PRE_NET_POS = Rotation2d.fromDegrees(100); - public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); + public static final double CHECK_ZERO_SECONDS = 2; public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(5); @@ -53,62 +35,95 @@ public class WristSubsystem extends SubsystemBase { public static MotionMagicConfigs CRAWL_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); + public enum WristState { + PRE_INTAKE_CORAL_GROUND(30.0), // formerly WRIST_CLEARANCE_POS + INTAKE_CORAL_GROUND(0.0), + POST_INTAKE_CORAL_GROUND(40), + HP(172.0), + L1(Units.radiansToDegrees(0.349 - 0.1)), + PRE_L2(170.0), + L2(Units.radiansToDegrees(2.447)), + PRE_L3(170.0), + L3(Units.radiansToDegrees(2.427)), + PRE_L4(140), + L4(98.0), // ?? + PRE_INTAKE_ALGAE_REEF(30.0), + INTAKE_ALGAE_REEF(-20.0), + INTAKE_ALGAE_STACK(-10), + INTAKE_ALGAE_GROUND(-65), + READY_ALGAE(20), + PRE_BARGE(100), + SCORE_BARGE(110), + PROCESSOR(-30.0), + HOME(Units.radiansToDegrees(-0.687 - 6.0)) // i dunno + ; + + private final Supplier angle; + + private WristState(double defaultAngle) { + LoggedTunableNumber ltn = new LoggedTunableNumber("Wrist/" + this.name(), defaultAngle); + // we're in real life!! use degrees + this.angle = () -> Rotation2d.fromDegrees(ltn.get()); + } + + public Rotation2d getAngle() { + return angle.get(); + } + } + + @AutoLogOutput(key = "Carriage/Wrist/Setpoint") + private Rotation2d setpoint = Rotation2d.kZero; + + @AutoLogOutput(key = "Carriage/Wrist/State") + private WristState state = WristState.HP; + private final WristIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private Rotation2d setpoint = Rotation2d.kZero; - private final LinearFilter currentFilter = LinearFilter.movingAverage(10); + public double currentFilterValue = 0.0; - public boolean hasZeroed = false; + @AutoLogOutput(key = "Carriage/Wrist/Has Zeroed") + public static boolean hasZeroed = false; + + private final Alert notZeroedAlert = new Alert("Wrist may not be zeroed!", AlertType.kWarning); public WristSubsystem(WristIO io) { this.io = io; + + new Trigger(this::atSetpoint) + .negate() + .debounce(CHECK_ZERO_SECONDS) + .or(() -> !hasZeroed) + .onTrue(Commands.runOnce(() -> notZeroedAlert.set(true))) + .onFalse(Commands.runOnce(() -> notZeroedAlert.set(false))); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Carriage/Wrist", inputs); - if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Wrist/Has Zeroed", hasZeroed); - } - - public Command setTargetAngle(final Supplier target) { - return this.run( - () -> { - io.setMotorPosition(target.get()); - setpoint = target.get(); - Logger.recordOutput("Carriage/Wrist/Setpoint", setpoint); - }); - } + currentFilterValue = currentFilter.calculate(inputs.statorCurrentAmps); - public Command setTargetAngle(final Rotation2d target) { - return setTargetAngle(() -> target); + // if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("Carriage/Wrist/Filtered Current", currentFilterValue); } - public Command setSlowTargetAngle(final Supplier target) { - return this.runOnce(() -> io.setMotionMagicConfigs(SLOW_MOTION_MAGIC)) - .andThen(this.setTargetAngle(target)) - .finallyDo((interrupted) -> io.setMotionMagicConfigs(DEFAULT_MOTION_MAGIC)); + public void setState(WristState state) { + this.state = state; } - public Command setSlowTargetAngle(final Rotation2d target) { - return this.setSlowTargetAngle(() -> target); + public Command setStateAngle() { + return setAngle(() -> state.getAngle()); } - public Command setCrawlTargetAngle(final Supplier target) { - return this.runOnce(() -> io.setMotionMagicConfigs(CRAWL_MOTION_MAGIC)) - .andThen(this.setTargetAngle(target)) - .finallyDo((interrupted) -> io.setMotionMagicConfigs(DEFAULT_MOTION_MAGIC)); - } - - public Command setCrawlTargetAngle(final Rotation2d target) { - return this.setCrawlTargetAngle(() -> target); - } - - public Command hold() { - return Commands.sequence( - setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); + public Command setAngle(final Supplier target) { + return this.run( + () -> { + io.setAngle(target.get()); + setpoint = target.get(); + Logger.recordOutput("Carriage/Wrist/Setpoint", setpoint); + }); } public Command setVoltage(final double volts) { @@ -123,43 +138,78 @@ public Rotation2d getSetpoint() { return setpoint; } - public boolean isNearTarget() { - return isNearAngle(setpoint); - } - public boolean isNearAngle(Rotation2d target) { return MathUtil.isNear(target.getDegrees(), inputs.position.getDegrees(), 10.0); } - public boolean isNearAngle(Rotation2d target, Rotation2d tolerance) { - return MathUtil.isNear( - target.getDegrees(), inputs.position.getDegrees(), tolerance.getDegrees()); + public boolean atSetpoint() { + return isNearAngle(setpoint); } - public Command currentZero(Supplier shoulderInputs) { - return Commands.sequence( - this.runOnce( - () -> { - currentFilter.reset(); - System.out.println("Wrist Zeroing"); - }), - this.run(() -> io.setMotorVoltage(-1.0)) - .raceWith( - Commands.waitSeconds(0.5) - .andThen( - Commands.waitUntil( - () -> - Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) - > 7.0))), - this.runOnce( - () -> { - hasZeroed = true; - io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); - })); + public Command currentZero() { + // return Commands.sequence( + // this.runOnce( + // () -> { + // currentFilter.reset(); + // System.out.println("Wrist Zeroing"); + // }), + // this.run(() -> io.setMotorVoltage(-1.0)) + // .raceWith( + // Commands.waitSeconds(0.5) + // .andThen( + // Commands.waitUntil( + // () -> + // Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) + // > 7.0))), + // this.runOnce( + // () -> { + // // Logger.recordOutput( + // // "shoulder zero pos", + // shoulderInputs.get().position.minus(ZEROING_OFFSET)); + // hasZeroed = true; + // // io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); + // io.resetEncoder(Rotation2d.fromRadians(-0.687)); + // })) + // .finallyDo(() -> Commands.print("DONE")); + return Commands.print("Wrist Zeroing") + .andThen( + this.run(() -> io.setMotorVoltage(-1.0)) + .until(() -> Math.abs(currentFilterValue) > 7.0) + .finallyDo( + (interrupted) -> { + if (!interrupted) { + io.resetEncoder(Rotation2d.fromRadians(-0.687)); + hasZeroed = true; + } + })); + + // return Commands.print("Elevator Zeroing") + // .andThen( + // this.run( + // () -> { + // io.setVoltage(-2.0); + // setpoint = 0.0; + // if (Robot.ROBOT_TYPE != RobotType.REAL) + // Logger.recordOutput("Elevator/Setpoint", Double.NaN); + // }) + // .until(() -> Math.abs(currentFilterValue) > 50.0) + // .finallyDo( + // (interrupted) -> { + // if (!interrupted) { + // io.resetEncoder(0.0); + // hasZeroed = true; + // } + // })); + } + + public void rezero(Rotation2d angle) { + System.out.println("Wrist rezeroing"); + io.resetEncoder(angle); + hasZeroed = true; + System.out.println("Wrist rezeroed!"); } - public void resetPosition(Rotation2d angle) { + public void resetEncoder(Rotation2d angle) { io.resetEncoder(angle); - hasZeroed = true; } } diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java new file mode 100644 index 00000000..8d80fd5e --- /dev/null +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -0,0 +1,381 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils; + +import choreo.util.ChoreoAllianceFlipUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import frc.robot.Robot; +import frc.robot.Robot.AlgaeIntakeTarget; +import frc.robot.utils.autoaim.AutoAim; +import java.util.Arrays; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; +import java.util.stream.Stream; + +/** Add your docs here. */ +public class FieldUtils { + public enum AlgaeIntakeTargets { + // All coordinates are global coordinates from the lower, blue alliance side corner, if the + // walls + // were extended beyond the coral station + // All angles from the center of the coral with 0° across the width of the field, + // counterclockwise + BLUE_AB(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AlgaeIntakeTarget.HIGH), + BLUE_CD(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AlgaeIntakeTarget.LOW), + BLUE_EF(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AlgaeIntakeTarget.HIGH), + BLUE_GH(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), AlgaeIntakeTarget.LOW), + BLUE_IJ(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), AlgaeIntakeTarget.HIGH), + BLUE_KL(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), AlgaeIntakeTarget.LOW), + + RED_AB(ChoreoAllianceFlipUtil.flip(BLUE_AB.location), AlgaeIntakeTarget.HIGH), + RED_CD(ChoreoAllianceFlipUtil.flip(BLUE_CD.location), AlgaeIntakeTarget.LOW), + RED_EF(ChoreoAllianceFlipUtil.flip(BLUE_EF.location), AlgaeIntakeTarget.HIGH), + RED_GH(ChoreoAllianceFlipUtil.flip(BLUE_GH.location), AlgaeIntakeTarget.LOW), + RED_IJ(ChoreoAllianceFlipUtil.flip(BLUE_IJ.location), AlgaeIntakeTarget.HIGH), + RED_KL(ChoreoAllianceFlipUtil.flip(BLUE_KL.location), AlgaeIntakeTarget.LOW); + + public final Pose2d location; + public final AlgaeIntakeTarget height; + + private AlgaeIntakeTargets(Pose2d location, AlgaeIntakeTarget height) { + this.location = location; + this.height = height; + } + + private static final List transformedPoses = + Arrays.stream(values()) + .map( + (AlgaeIntakeTargets targets) -> { + return AlgaeIntakeTargets.getRobotTargetLocation(targets.location); + }) + .toList(); + + public static Pose2d getRobotTargetLocation(Pose2d original) { + return original.transformBy( + new Transform2d( + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Pose2d getOffsetLocation(Pose2d original) { + return original.transformBy( + new Transform2d((-0.3 - Units.inchesToMeters(6)), 0, Rotation2d.kZero)); + } + + /** Gets the closest offset target to the given pose. */ + public static Pose2d getClosestTargetPose(Pose2d pose) { + return pose.nearest(transformedPoses); + } + + public static AlgaeIntakeTargets getClosestTarget(Pose2d pose) { + return Collections.min( + Stream.of(AlgaeIntakeTargets.values()).toList(), + Comparator.comparing( + (AlgaeIntakeTargets other) -> + pose.getTranslation().getDistance(other.location.getTranslation())) + .thenComparing( + (AlgaeIntakeTargets other) -> + Math.abs( + pose.getRotation().minus(other.location.getRotation()).getRadians()))); + } + } + + public enum CageTargets { + RED_OUTSIDE(new Pose2d(8.760, 0.799, Rotation2d.fromDegrees(0)), Alliance.Red), + RED_MIDDLE(new Pose2d(8.760, 1.889 + 0.15, Rotation2d.fromDegrees(0)), Alliance.Red), + RED_INSIDE(new Pose2d(8.760, 2.980, Rotation2d.fromDegrees(0)), Alliance.Red), + + BLUE_OUTSIDE(ChoreoAllianceFlipUtil.flip(RED_OUTSIDE.getLocation()), Alliance.Blue), + BLUE_MIDDLE(ChoreoAllianceFlipUtil.flip(RED_MIDDLE.getLocation()), Alliance.Blue), + BLUE_INSIDE(ChoreoAllianceFlipUtil.flip(RED_INSIDE.getLocation()), Alliance.Blue); + + private static final List poses = + Arrays.stream(values()).map((CageTargets target) -> target.getLocation()).toList(); + + private final Pose2d location; + private final Alliance alliance; + + private CageTargets(Pose2d location, Alliance alliance) { + this.location = location; + this.alliance = alliance; + } + + public static Pose2d getOffsetClosestTarget(Pose2d robotPose) { + if (DriverStation.getAlliance().isPresent()) { + // If it's across the field, x > 8.76 on blue and x < 8.76 on red + return getOffsetClosestTarget( + robotPose, + (DriverStation.getAlliance().get() == Alliance.Blue && robotPose.getX() > 8.76) + || (DriverStation.getAlliance().get() == Alliance.Red && robotPose.getX() < 8.76)); + } + return getOffsetClosestTarget(robotPose, false); + } + + public static Pose2d getOffsetClosestTarget(Pose2d robotPose, boolean far) { + Pose2d nearestPose; + if (DriverStation.getAlliance().isPresent()) { + nearestPose = + robotPose.nearest( + Arrays.stream(values()) + .filter(target -> target.getAlliance() == DriverStation.getAlliance().get()) + .map(target -> target.getLocation()) + .toList()); + } else { + nearestPose = robotPose.nearest(poses); + } + if (far) { + return getFarRobotTargetLocation(nearestPose); + } else { + return getCloseRobotTargetLocation(nearestPose); + } + } + + public static Pose2d getCloseRobotTargetLocation(Pose2d pose) { + return pose.transformBy( + new Transform2d( + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) + 0.087, + 0, + Rotation2d.kZero)); + } + + public static Pose2d getFarRobotTargetLocation(Pose2d pose) { + return pose.transformBy( + new Transform2d( + -(Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) - 0.087, + 0, + Rotation2d.k180deg)); + } + + public Pose2d getLocation() { + return this.location; + } + + public Alliance getAlliance() { + return this.alliance; + } + } + + public enum CoralTargets { + // All coordinates are global coordinates from the lower, blue alliance side corner, if the + // walls + // were extended beyond the coral station + // All angles from the center of the coral with 0° across the width of the field, + // counterclockwise + BLUE_A(new Pose2d(3.95, 4.20, Rotation2d.fromDegrees(180)), true), + BLUE_B(new Pose2d(3.95, 3.87, Rotation2d.fromDegrees(180)), false), + BLUE_C(new Pose2d(4.07, 3.66, Rotation2d.fromDegrees(240)), true), + BLUE_D(new Pose2d(4.35, 3.49, Rotation2d.fromDegrees(240)), false), + BLUE_E(new Pose2d(4.60, 3.50, Rotation2d.fromDegrees(300)), false), + BLUE_F(new Pose2d(4.88, 3.66, Rotation2d.fromDegrees(300)), true), + BLUE_G(new Pose2d(5.00, 3.86, Rotation2d.fromDegrees(0)), false), + BLUE_H(new Pose2d(5.00, 4.18, Rotation2d.fromDegrees(0)), true), + BLUE_I(new Pose2d(4.88, 4.41, Rotation2d.fromDegrees(60)), false), + BLUE_J(new Pose2d(4.60, 4.57, Rotation2d.fromDegrees(60)), true), + BLUE_K(new Pose2d(4.36, 4.57, Rotation2d.fromDegrees(120)), true), + BLUE_L(new Pose2d(4.06, 4.41, Rotation2d.fromDegrees(120)), false), + + RED_A(ChoreoAllianceFlipUtil.flip(BLUE_A.location), true), + RED_B(ChoreoAllianceFlipUtil.flip(BLUE_B.location), false), + RED_C(ChoreoAllianceFlipUtil.flip(BLUE_C.location), true), + RED_D(ChoreoAllianceFlipUtil.flip(BLUE_D.location), false), + RED_E(ChoreoAllianceFlipUtil.flip(BLUE_E.location), false), + RED_F(ChoreoAllianceFlipUtil.flip(BLUE_F.location), true), + RED_G(ChoreoAllianceFlipUtil.flip(BLUE_G.location), false), + RED_H(ChoreoAllianceFlipUtil.flip(BLUE_H.location), true), + RED_I(ChoreoAllianceFlipUtil.flip(BLUE_I.location), false), + RED_J(ChoreoAllianceFlipUtil.flip(BLUE_J.location), true), + RED_K(ChoreoAllianceFlipUtil.flip(BLUE_K.location), true), + RED_L(ChoreoAllianceFlipUtil.flip(BLUE_L.location), false); + + public final Pose2d location; + public final boolean leftHanded; + + private CoralTargets(Pose2d location, boolean leftHanded) { + this.location = location; + this.leftHanded = leftHanded; + } + + private static final List transformedPoses = + Arrays.stream(values()) + .map( + (CoralTargets targets) -> { + return CoralTargets.getRobotTargetLocation(targets.location); + }) + .toList(); + + public static Pose2d getRobotTargetLocation(Pose2d original) { + // 0.248 for trough + return original.transformBy( + new Transform2d( + 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Pose2d getBranchLocation(Pose2d transformed) { + // 0.248 for trough + return transformed.transformBy( + new Transform2d( + 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), + 0, + Rotation2d.fromDegrees(180.0)) + .inverse()); + } + + /** Gets the closest offset target to the given pose. */ + public static Pose2d getClosestTarget(Pose2d pose) { + return pose.nearest(transformedPoses); + } + + /** Gets the closest offset target to the given pose. */ + public static Pose2d getHandedClosestTarget(Pose2d pose, boolean leftHandeed) { + return pose.nearest( + Arrays.stream(values()) + .filter((target) -> target.leftHanded == leftHandeed) + .map( + (CoralTargets targets) -> { + return CoralTargets.getRobotTargetLocation(targets.location); + }) + .toList()); + } + } + + public enum HumanPlayerTargets { + BLUE_RIGHT_OUTSIDE( + new Pose2d( + 1.6333351135253906, 0.6246773600578308, Rotation2d.fromRadians(0.9420001549844138))), + BLUE_RIGHT_MIDDLE( + new Pose2d( + 1.1213834285736084, 0.9940196871757507, Rotation2d.fromRadians(0.9940196871757508))), + BLUE_RIGHT_INSIDE( + new Pose2d( + 0.5838082432746887, 1.3407007455825806, Rotation2d.fromRadians(0.9420001549844138))), + + BLUE_LEFT_OUTSIDE( + new Pose2d( + 1.666144609451294, 7.431143760681152, Rotation2d.fromRadians(-0.9350057865774469))), + BLUE_LEFT_MIDDLE( + new Pose2d( + 1.179524540901184, 7.083498477935791, Rotation2d.fromRadians(-0.9350057865774469))), + BLUE_LEFT_INSIDE( + new Pose2d( + 0.6153400540351868, 6.673182487487793, Rotation2d.fromRadians(-0.9350057865774469))), + + RED_RIGHT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_OUTSIDE.location)), + RED_RIGHT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_MIDDLE.location)), + RED_RIGHT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_INSIDE.location)), + RED_LEFT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_OUTSIDE.location)), + RED_LEFT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_MIDDLE.location)), + RED_LEFT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_INSIDE.location)); + + public final Pose2d location; + + private HumanPlayerTargets(Pose2d location) { + this.location = location; + } + } + + public enum L1Targets { + BLUE_AB( + new Rectangle2d( + new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_CD( + new Rectangle2d( + new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_EF( + new Rectangle2d( + new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_GH( + new Rectangle2d( + new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_IJ( + new Rectangle2d( + new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_KL( + new Rectangle2d( + new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + + RED_AB( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_CD( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_EF( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_GH( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_IJ( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_KL( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)); + + public Rectangle2d line; + + private L1Targets(Rectangle2d line) { + this.line = line; + } + + private static final List transformedLines = + Arrays.stream(values()) + .map( + (L1Targets targets) -> { + return L1Targets.getRobotTargetLine(targets.line); + }) + .toList(); + + public static Rectangle2d getRobotTargetLine(Rectangle2d original) { + return original.transformBy( + new Transform2d( + Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2, + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Rectangle2d getNearestLine(Pose2d pose) { + // It feels like there should be a better way to do this + return new Rectangle2d( + pose.nearest(transformedLines.stream().map(line -> line.getCenter()).toList()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS); + } + } +} diff --git a/src/main/java/frc/robot/utils/LoggedTunableNumber.java b/src/main/java/frc/robot/utils/LoggedTunableNumber.java new file mode 100644 index 00000000..229fc5ea --- /dev/null +++ b/src/main/java/frc/robot/utils/LoggedTunableNumber.java @@ -0,0 +1,124 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.utils; + +import frc.robot.Robot; +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +/** + * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or + * value not in dashboard. Taken from 6328's 2025 code - with the modification that we don't disable + * the HAL and don't put TUNING_MODE in a Constants file + */ +public class LoggedTunableNumber implements DoubleSupplier { + private static final String tableKey = "/Tuning"; + + private final String key; + private boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private Map lastHasChangedValues = new HashMap<>(); + + /** + * Create a new LoggedTunableNumber + * + * @param dashboardKey Key on dashboard + */ + public LoggedTunableNumber(String dashboardKey) { + this.key = tableKey + "/" + dashboardKey; + } + + /** + * Create a new LoggedTunableNumber with the default value + * + * @param dashboardKey Key on dashboard + * @param defaultValue Default value + */ + public LoggedTunableNumber(String dashboardKey, double defaultValue) { + this(dashboardKey); + initDefault(defaultValue); + } + + /** + * Set the default value of the number. The default value can only be set once. + * + * @param defaultValue The default value + */ + public void initDefault(double defaultValue) { + if (!hasDefault) { + hasDefault = true; + this.defaultValue = defaultValue; + if (Robot.TUNING_MODE) { + dashboardNumber = new LoggedNetworkNumber(key, defaultValue); + } + } + } + + /** + * Get the current value, from dashboard if available and in tuning mode. + * + * @return The current value + */ + public double get() { + if (!hasDefault) { + return 0.0; + } else { + return Robot.TUNING_MODE ? dashboardNumber.get() : defaultValue; + } + } + + /** + * Checks whether the number has changed since our last check + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple + * objects. Recommended approach is to pass the result of "hashCode()" + * @return True if the number has changed since the last time this method was called, false + * otherwise. + */ + public boolean hasChanged(int id) { + double currentValue = get(); + Double lastValue = lastHasChangedValues.get(id); + if (lastValue == null || currentValue != lastValue) { + lastHasChangedValues.put(id, currentValue); + return true; + } + + return false; + } + + /** + * Runs action if any of the tunableNumbers have changed + * + * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * + * objects. Recommended approach is to pass the result of "hashCode()" + * @param action Callback to run when any of the tunable numbers have changed. Access tunable + * numbers in order inputted in method + * @param tunableNumbers All tunable numbers to check + */ + public static void ifChanged( + int id, Consumer action, LoggedTunableNumber... tunableNumbers) { + if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { + action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); + } + } + + /** Runs action if any of the tunableNumbers have changed */ + public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) { + ifChanged(id, values -> action.run(), tunableNumbers); + } + + @Override + public double getAsDouble() { + return get(); + } +} diff --git a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java b/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java deleted file mode 100644 index 105e35a4..00000000 --- a/src/main/java/frc/robot/utils/autoaim/AlgaeIntakeTargets.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.util.Units; -import frc.robot.Robot; -import frc.robot.Robot.AlgaeIntakeTarget; -import java.util.Arrays; -import java.util.Collections; -import java.util.Comparator; -import java.util.List; -import java.util.stream.Stream; - -public enum AlgaeIntakeTargets { - // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls - // were extended beyond the coral station - // All angles from the center of the coral with 0° across the width of the field, counterclockwise - BLUE_AB(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AlgaeIntakeTarget.HIGH), - BLUE_CD(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AlgaeIntakeTarget.LOW), - BLUE_EF(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AlgaeIntakeTarget.HIGH), - BLUE_GH(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), AlgaeIntakeTarget.LOW), - BLUE_IJ(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), AlgaeIntakeTarget.HIGH), - BLUE_KL(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), AlgaeIntakeTarget.LOW), - - RED_AB(ChoreoAllianceFlipUtil.flip(BLUE_AB.location), AlgaeIntakeTarget.HIGH), - RED_CD(ChoreoAllianceFlipUtil.flip(BLUE_CD.location), AlgaeIntakeTarget.LOW), - RED_EF(ChoreoAllianceFlipUtil.flip(BLUE_EF.location), AlgaeIntakeTarget.HIGH), - RED_GH(ChoreoAllianceFlipUtil.flip(BLUE_GH.location), AlgaeIntakeTarget.LOW), - RED_IJ(ChoreoAllianceFlipUtil.flip(BLUE_IJ.location), AlgaeIntakeTarget.HIGH), - RED_KL(ChoreoAllianceFlipUtil.flip(BLUE_KL.location), AlgaeIntakeTarget.LOW); - - public final Pose2d location; - public final AlgaeIntakeTarget height; - - private AlgaeIntakeTargets(Pose2d location, AlgaeIntakeTarget height) { - this.location = location; - this.height = height; - } - - private static final List transformedPoses = - Arrays.stream(values()) - .map( - (AlgaeIntakeTargets targets) -> { - return AlgaeIntakeTargets.getRobotTargetLocation(targets.location); - }) - .toList(); - - public static Pose2d getRobotTargetLocation(Pose2d original) { - return original.transformBy( - new Transform2d( - (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), - 0, - Rotation2d.fromDegrees(180.0))); - } - - public static Pose2d getOffsetLocation(Pose2d original) { - return original.transformBy( - new Transform2d((-0.3 - Units.inchesToMeters(6)), 0, Rotation2d.kZero)); - } - - /** Gets the closest offset target to the given pose. */ - public static Pose2d getClosestTargetPose(Pose2d pose) { - return pose.nearest(transformedPoses); - } - - public static AlgaeIntakeTargets getClosestTarget(Pose2d pose) { - return Collections.min( - Stream.of(AlgaeIntakeTargets.values()).toList(), - Comparator.comparing( - (AlgaeIntakeTargets other) -> - pose.getTranslation().getDistance(other.location.getTranslation())) - .thenComparing( - (AlgaeIntakeTargets other) -> - Math.abs(pose.getRotation().minus(other.location.getRotation()).getRadians()))); - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index f1644467..0de94b96 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -18,6 +18,8 @@ import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.utils.FieldUtils.AlgaeIntakeTargets; +import frc.robot.utils.FieldUtils.CoralTargets; import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/utils/autoaim/CageTargets.java b/src/main/java/frc/robot/utils/autoaim/CageTargets.java deleted file mode 100644 index f585a615..00000000 --- a/src/main/java/frc/robot/utils/autoaim/CageTargets.java +++ /dev/null @@ -1,86 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import frc.robot.Robot; -import java.util.Arrays; -import java.util.List; - -public enum CageTargets { - RED_OUTSIDE(new Pose2d(8.760, 0.799, Rotation2d.fromDegrees(0)), Alliance.Red), - RED_MIDDLE(new Pose2d(8.760, 1.889 + 0.15, Rotation2d.fromDegrees(0)), Alliance.Red), - RED_INSIDE(new Pose2d(8.760, 2.980, Rotation2d.fromDegrees(0)), Alliance.Red), - - BLUE_OUTSIDE(ChoreoAllianceFlipUtil.flip(RED_OUTSIDE.getLocation()), Alliance.Blue), - BLUE_MIDDLE(ChoreoAllianceFlipUtil.flip(RED_MIDDLE.getLocation()), Alliance.Blue), - BLUE_INSIDE(ChoreoAllianceFlipUtil.flip(RED_INSIDE.getLocation()), Alliance.Blue); - - private static final List poses = - Arrays.stream(values()).map((CageTargets target) -> target.getLocation()).toList(); - - private final Pose2d location; - private final Alliance alliance; - - private CageTargets(Pose2d location, Alliance alliance) { - this.location = location; - this.alliance = alliance; - } - - public static Pose2d getOffsetClosestTarget(Pose2d robotPose) { - if (DriverStation.getAlliance().isPresent()) { - // If it's across the field, x > 8.76 on blue and x < 8.76 on red - return getOffsetClosestTarget( - robotPose, - (DriverStation.getAlliance().get() == Alliance.Blue && robotPose.getX() > 8.76) - || (DriverStation.getAlliance().get() == Alliance.Red && robotPose.getX() < 8.76)); - } - return getOffsetClosestTarget(robotPose, false); - } - - public static Pose2d getOffsetClosestTarget(Pose2d robotPose, boolean far) { - Pose2d nearestPose; - if (DriverStation.getAlliance().isPresent()) { - nearestPose = - robotPose.nearest( - Arrays.stream(values()) - .filter(target -> target.getAlliance() == DriverStation.getAlliance().get()) - .map(target -> target.getLocation()) - .toList()); - } else { - nearestPose = robotPose.nearest(poses); - } - if (far) { - return getFarRobotTargetLocation(nearestPose); - } else { - return getCloseRobotTargetLocation(nearestPose); - } - } - - public static Pose2d getCloseRobotTargetLocation(Pose2d pose) { - return pose.transformBy( - new Transform2d( - (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) + 0.087, - 0, - Rotation2d.kZero)); - } - - public static Pose2d getFarRobotTargetLocation(Pose2d pose) { - return pose.transformBy( - new Transform2d( - -(Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2) - 0.087, - 0, - Rotation2d.k180deg)); - } - - public Pose2d getLocation() { - return this.location; - } - - public Alliance getAlliance() { - return this.alliance; - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/CoralTargets.java b/src/main/java/frc/robot/utils/autoaim/CoralTargets.java deleted file mode 100644 index 04893800..00000000 --- a/src/main/java/frc/robot/utils/autoaim/CoralTargets.java +++ /dev/null @@ -1,92 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import frc.robot.Robot; -import java.util.Arrays; -import java.util.List; - -public enum CoralTargets { - // All coordinates are global coordinates from the lower, blue alliance side corner, if the walls - // were extended beyond the coral station - // All angles from the center of the coral with 0° across the width of the field, counterclockwise - BLUE_A(new Pose2d(3.95, 4.20, Rotation2d.fromDegrees(180)), true), - BLUE_B(new Pose2d(3.95, 3.87, Rotation2d.fromDegrees(180)), false), - BLUE_C(new Pose2d(4.07, 3.66, Rotation2d.fromDegrees(240)), true), - BLUE_D(new Pose2d(4.35, 3.49, Rotation2d.fromDegrees(240)), false), - BLUE_E(new Pose2d(4.60, 3.50, Rotation2d.fromDegrees(300)), false), - BLUE_F(new Pose2d(4.88, 3.66, Rotation2d.fromDegrees(300)), true), - BLUE_G(new Pose2d(5.00, 3.86, Rotation2d.fromDegrees(0)), false), - BLUE_H(new Pose2d(5.00, 4.18, Rotation2d.fromDegrees(0)), true), - BLUE_I(new Pose2d(4.88, 4.41, Rotation2d.fromDegrees(60)), false), - BLUE_J(new Pose2d(4.60, 4.57, Rotation2d.fromDegrees(60)), true), - BLUE_K(new Pose2d(4.36, 4.57, Rotation2d.fromDegrees(120)), true), - BLUE_L(new Pose2d(4.06, 4.41, Rotation2d.fromDegrees(120)), false), - - RED_A(ChoreoAllianceFlipUtil.flip(BLUE_A.location), true), - RED_B(ChoreoAllianceFlipUtil.flip(BLUE_B.location), false), - RED_C(ChoreoAllianceFlipUtil.flip(BLUE_C.location), true), - RED_D(ChoreoAllianceFlipUtil.flip(BLUE_D.location), false), - RED_E(ChoreoAllianceFlipUtil.flip(BLUE_E.location), false), - RED_F(ChoreoAllianceFlipUtil.flip(BLUE_F.location), true), - RED_G(ChoreoAllianceFlipUtil.flip(BLUE_G.location), false), - RED_H(ChoreoAllianceFlipUtil.flip(BLUE_H.location), true), - RED_I(ChoreoAllianceFlipUtil.flip(BLUE_I.location), false), - RED_J(ChoreoAllianceFlipUtil.flip(BLUE_J.location), true), - RED_K(ChoreoAllianceFlipUtil.flip(BLUE_K.location), true), - RED_L(ChoreoAllianceFlipUtil.flip(BLUE_L.location), false); - - public final Pose2d location; - public final boolean leftHanded; - - private CoralTargets(Pose2d location, boolean leftHanded) { - this.location = location; - this.leftHanded = leftHanded; - } - - private static final List transformedPoses = - Arrays.stream(values()) - .map( - (CoralTargets targets) -> { - return CoralTargets.getRobotTargetLocation(targets.location); - }) - .toList(); - - public static Pose2d getRobotTargetLocation(Pose2d original) { - // 0.248 for trough - return original.transformBy( - new Transform2d( - 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), - 0, - Rotation2d.fromDegrees(180.0))); - } - - public static Pose2d getBranchLocation(Pose2d transformed) { - // 0.248 for trough - return transformed.transformBy( - new Transform2d( - 0.291 + (Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2), - 0, - Rotation2d.fromDegrees(180.0)) - .inverse()); - } - - /** Gets the closest offset target to the given pose. */ - public static Pose2d getClosestTarget(Pose2d pose) { - return pose.nearest(transformedPoses); - } - - /** Gets the closest offset target to the given pose. */ - public static Pose2d getHandedClosestTarget(Pose2d pose, boolean leftHandeed) { - return pose.nearest( - Arrays.stream(values()) - .filter((target) -> target.leftHanded == leftHandeed) - .map( - (CoralTargets targets) -> { - return CoralTargets.getRobotTargetLocation(targets.location); - }) - .toList()); - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/HumanPlayerTargets.java b/src/main/java/frc/robot/utils/autoaim/HumanPlayerTargets.java deleted file mode 100644 index dbd9085f..00000000 --- a/src/main/java/frc/robot/utils/autoaim/HumanPlayerTargets.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public enum HumanPlayerTargets { - BLUE_RIGHT_OUTSIDE( - new Pose2d( - 1.6333351135253906, 0.6246773600578308, Rotation2d.fromRadians(0.9420001549844138))), - BLUE_RIGHT_MIDDLE( - new Pose2d( - 1.1213834285736084, 0.9940196871757507, Rotation2d.fromRadians(0.9940196871757508))), - BLUE_RIGHT_INSIDE( - new Pose2d( - 0.5838082432746887, 1.3407007455825806, Rotation2d.fromRadians(0.9420001549844138))), - - BLUE_LEFT_OUTSIDE( - new Pose2d( - 1.666144609451294, 7.431143760681152, Rotation2d.fromRadians(-0.9350057865774469))), - BLUE_LEFT_MIDDLE( - new Pose2d( - 1.179524540901184, 7.083498477935791, Rotation2d.fromRadians(-0.9350057865774469))), - BLUE_LEFT_INSIDE( - new Pose2d( - 0.6153400540351868, 6.673182487487793, Rotation2d.fromRadians(-0.9350057865774469))), - - RED_RIGHT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_OUTSIDE.location)), - RED_RIGHT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_MIDDLE.location)), - RED_RIGHT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_RIGHT_INSIDE.location)), - RED_LEFT_OUTSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_OUTSIDE.location)), - RED_LEFT_MIDDLE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_MIDDLE.location)), - RED_LEFT_INSIDE(ChoreoAllianceFlipUtil.flip(BLUE_LEFT_INSIDE.location)); - - public final Pose2d location; - - private HumanPlayerTargets(Pose2d location) { - this.location = location; - } -} diff --git a/src/main/java/frc/robot/utils/autoaim/L1Targets.java b/src/main/java/frc/robot/utils/autoaim/L1Targets.java deleted file mode 100644 index d06bbfe9..00000000 --- a/src/main/java/frc/robot/utils/autoaim/L1Targets.java +++ /dev/null @@ -1,100 +0,0 @@ -package frc.robot.utils.autoaim; - -import choreo.util.ChoreoAllianceFlipUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rectangle2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import frc.robot.Robot; -import java.util.Arrays; -import java.util.List; - -public enum L1Targets { - BLUE_AB( - new Rectangle2d( - new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_CD( - new Rectangle2d( - new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_EF( - new Rectangle2d( - new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_GH( - new Rectangle2d( - new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_IJ( - new Rectangle2d( - new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), - BLUE_KL( - new Rectangle2d( - new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - - RED_AB( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_CD( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_EF( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_GH( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_IJ( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)), - RED_KL( - new Rectangle2d( - ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS)); - - public Rectangle2d line; - - private L1Targets(Rectangle2d line) { - this.line = line; - } - - private static final List transformedLines = - Arrays.stream(values()) - .map( - (L1Targets targets) -> { - return L1Targets.getRobotTargetLine(targets.line); - }) - .toList(); - - public static Rectangle2d getRobotTargetLine(Rectangle2d original) { - return original.transformBy( - new Transform2d( - Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2, - 0, - Rotation2d.fromDegrees(180.0))); - } - - public static Rectangle2d getNearestLine(Pose2d pose) { - // It feels like there should be a better way to do this - return new Rectangle2d( - pose.nearest(transformedLines.stream().map(line -> line.getCenter()).toList()), - 0.0, - AutoAim.L1_TROUGH_WIDTH_METERS); - } -}