diff --git a/src/main/deploy/choreo/RCleanuptoRPreTrenchReversed.traj b/src/main/deploy/choreo/RCleanuptoRPreTrenchReversed.traj new file mode 100644 index 00000000..70b4b9fb --- /dev/null +++ b/src/main/deploy/choreo/RCleanuptoRPreTrenchReversed.traj @@ -0,0 +1,173 @@ +{ + "name":"RCleanuptoRPreTrenchReversed", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":5.824512958526611, "y":4.3513383865356445, "heading":1.5707963267948966, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.107110023498535, "y":3.480527400970459, "heading":1.5707963267948966, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.613850116729736, "y":1.745917558670044, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.418950080871582, "y":0.6934576034545898, "heading":3.141592653589793, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.32966947555542, "y":0.688210129737854, "heading":3.141592653589793, "intervals":82, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":3, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.7}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":6.0}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":2.7}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":6.0}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"RCleanup.x", "val":5.824512958526611}, "y":{"exp":"RCleanup.y", "val":4.3513383865356445}, "heading":{"exp":"RCleanup.heading", "val":1.5707963267948966}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.107110023498535 m", "val":6.107110023498535}, "y":{"exp":"3.480527400970459 m", "val":3.480527400970459}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"6.613850116729736 m", "val":6.613850116729736}, "y":{"exp":"1.745917558670044 m", "val":1.745917558670044}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"6.418950080871582 m", "val":6.418950080871582}, "y":{"exp":"0.6934576034545898 m", "val":0.6934576034545898}, "heading":{"exp":"3.141592653589793 rad", "val":3.141592653589793}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"RPreTrenchReversed.x", "val":3.32966947555542}, "y":{"exp":"RPreTrenchReversed.y", "val":0.688210129737854}, "heading":{"exp":"RPreTrenchReversed.heading", "val":3.141592653589793}, "intervals":82, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":3, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.7}}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"6 rad / s", "val":6.0}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":2.7}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":1, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"6 rad / s", "val":6.0}}}, "enabled":true}, + {"from":2, "to":3, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2761996, + "y":0.2761996 + }, + "backLeft":{ + "x":-0.2761996, + "y":0.2761996 + }, + "mass":66.133767546, + "inertia":10.42384825260472, + "gearing":7.03, + "radius":0.0508, + "vmax":485.8996637552213, + "tmax":0.9, + "cof":2.25, + "bumper":{ + "front":0.44196, + "side":0.44196, + "back":0.44196 + }, + "differentialTrackWidth":0.5427218 + }, + "sampleType":"Swerve", + "waypoints":[0.0,0.33927,1.04306,1.5926,2.78433], + "samples":[ + {"t":0.0, "x":5.82451, "y":4.35134, "heading":1.5708, "vx":0.84524, "vy":-2.55956, "omega":0.0, "ax":0.04356, "ay":-0.13193, "alpha":0.0, "fx":[0.72027,0.72027,0.72027,0.72027], "fy":[-2.18126,-2.18126,-2.18126,-2.18126]}, + {"t":0.02423, "x":5.84501, "y":4.28927, "heading":1.5708, "vx":0.84629, "vy":-2.56276, "omega":0.0, "ax":0.0, "ay":-0.00004, "alpha":0.0, "fx":[0.00007,0.00007,0.00007,0.00007], "fy":[-0.0007,-0.0007,-0.0007,-0.0007]}, + {"t":0.04847, "x":5.86552, "y":4.22717, "heading":1.5708, "vx":0.84629, "vy":-2.56276, "omega":0.0, "ax":-0.00003, "ay":-0.00001, "alpha":0.0, "fx":[-0.00046,-0.00046,-0.00046,-0.00046], "fy":[-0.00015,-0.00015,-0.00015,-0.00015]}, + {"t":0.0727, "x":5.88603, "y":4.16506, "heading":1.5708, "vx":0.84629, "vy":-2.56276, "omega":0.0, "ax":-0.00008, "ay":-0.00003, "alpha":0.0, "fx":[-0.0014,-0.0014,-0.0014,-0.0014], "fy":[-0.00046,-0.00046,-0.00046,-0.00046]}, + {"t":0.09694, "x":5.90654, "y":4.10296, "heading":1.5708, "vx":0.84629, "vy":-2.56276, "omega":0.0, "ax":-0.00026, "ay":-0.00009, "alpha":0.0, "fx":[-0.00428,-0.00428,-0.00428,-0.00428], "fy":[-0.00141,-0.00141,-0.00141,-0.00141]}, + {"t":0.12117, "x":5.92704, "y":4.04085, "heading":1.5708, "vx":0.84628, "vy":-2.56276, "omega":0.0, "ax":-0.00079, "ay":-0.00026, "alpha":0.0, "fx":[-0.01309,-0.01309,-0.01309,-0.01309], "fy":[-0.00432,-0.00432,-0.00432,-0.00432]}, + {"t":0.1454, "x":5.94755, "y":3.97874, "heading":1.5708, "vx":0.84626, "vy":-2.56277, "omega":0.0, "ax":-0.00242, "ay":-0.0008, "alpha":0.0, "fx":[-0.04003,-0.04003,-0.04003,-0.04003], "fy":[-0.01322,-0.01322,-0.01322,-0.01322]}, + {"t":0.16964, "x":5.96806, "y":3.91664, "heading":1.5708, "vx":0.8462, "vy":-2.56279, "omega":0.0, "ax":-0.00741, "ay":-0.00245, "alpha":0.0, "fx":[-0.12245,-0.12245,-0.12245,-0.12245], "fy":[-0.04043,-0.04043,-0.04043,-0.04043]}, + {"t":0.19387, "x":5.98856, "y":3.85453, "heading":1.5708, "vx":0.84602, "vy":-2.56285, "omega":0.0, "ax":-0.02265, "ay":-0.00747, "alpha":0.0, "fx":[-0.37451,-0.37451,-0.37451,-0.37451], "fy":[-0.12359,-0.12359,-0.12359,-0.12359]}, + {"t":0.2181, "x":6.00906, "y":3.79242, "heading":1.5708, "vx":0.84548, "vy":-2.56303, "omega":0.0, "ax":-0.06928, "ay":-0.02283, "alpha":0.0, "fx":[-1.1455,-1.1455,-1.1455,-1.1455], "fy":[-0.37745,-0.37745,-0.37745,-0.37745]}, + {"t":0.24234, "x":6.02953, "y":3.7303, "heading":1.5708, "vx":0.8438, "vy":-2.56358, "omega":0.0, "ax":-0.21182, "ay":-0.06948, "alpha":0.0, "fx":[-3.50214,-3.50214,-3.50214,-3.50214], "fy":[-1.14882,-1.14882,-1.14882,-1.14882]}, + {"t":0.26657, "x":6.04992, "y":3.66816, "heading":1.5708, "vx":0.83866, "vy":-2.56527, "omega":0.0, "ax":-0.64378, "ay":-0.2083, "alpha":0.0, "fx":[-10.6439,-10.6439,-10.6439,-10.6439], "fy":[-3.44393,-3.44393,-3.44393,-3.44393]}, + {"t":0.29081, "x":6.07005, "y":3.60593, "heading":1.5708, "vx":0.82306, "vy":-2.57032, "omega":0.0, "ax":-1.85336, "ay":-0.57565, "alpha":0.0, "fx":[-30.64249,-30.64249,-30.64249,-30.64249], "fy":[-9.51743,-9.51743,-9.51743,-9.51743]}, + {"t":0.31504, "x":6.08945, "y":3.54347, "heading":1.5708, "vx":0.77815, "vy":-2.58427, "omega":0.0, "ax":-4.08438, "ay":-1.08472, "alpha":0.0, "fx":[-67.52883,-67.52883,-67.52883,-67.52883], "fy":[-17.93408,-17.93408,-17.93408,-17.93408]}, + {"t":0.33927, "x":6.10711, "y":3.48053, "heading":1.5708, "vx":0.67917, "vy":-2.61055, "omega":0.0, "ax":-0.06644, "ay":-0.05711, "alpha":18.51645, "fx":[-87.52904,85.30929,87.19074,-89.36488], "fy":[-87.23035,-89.36929,87.5035,85.31893]}, + {"t":0.36127, "x":6.12203, "y":3.4231, "heading":1.5708, "vx":0.67771, "vy":-2.61181, "omega":0.40724, "ax":-0.0855, "ay":-0.02554, "alpha":18.48667, "fx":[-88.1969,85.35677,86.19859,-89.01267], "fy":[-86.27461,-89.03342,88.20192,85.417]}, + {"t":0.38326, "x":6.13692, "y":3.36565, "heading":1.57975, "vx":0.67583, "vy":-2.61237, "omega":0.81382, "ax":-0.10231, "ay":-0.02657, "alpha":18.44127, "fx":[-87.48431,85.64461,84.90015,-89.82659], "fy":[-86.59211,-88.33388,89.02873,84.14013]}, + {"t":0.40525, "x":6.15175, "y":3.30819, "heading":1.59765, "vx":0.67358, "vy":-2.61296, "omega":1.21941, "ax":-0.13025, "ay":-0.03331, "alpha":18.36566, "fx":[-85.97601,86.30205,82.51148,-91.45123], "fy":[-87.43701,-86.98152,90.55319,81.66249]}, + {"t":0.42725, "x":6.16654, "y":3.25071, "heading":1.62447, "vx":0.67071, "vy":-2.61369, "omega":1.62333, "ax":-0.18778, "ay":-0.04743, "alpha":18.21499, "fx":[-83.78408,86.76602,78.43022,-93.83087], "fy":[-88.30232,-85.08481,92.76544,77.48478]}, + {"t":0.44924, "x":6.18124, "y":3.19322, "heading":1.66017, "vx":0.66658, "vy":-2.61473, "omega":2.02393, "ax":-0.36088, "ay":-0.0858, "alpha":17.77147, "fx":[-81.31711,84.31858,70.01239,-96.88026], "fy":[-87.26271,-83.32161,95.48689,69.42286]}, + {"t":0.47123, "x":6.19582, "y":3.13569, "heading":1.70469, "vx":0.65865, "vy":-2.61662, "omega":2.41479, "ax":-3.09195, "ay":-0.29005, "alpha":9.63329, "fx":[-82.88588,-5.93144,-23.43787,-92.22686], "fy":[-49.30181,-67.6922,68.05106,29.76102]}, + {"t":0.49323, "x":6.20955, "y":3.07807, "heading":1.75779, "vx":0.59064, "vy":-2.623, "omega":2.62666, "ax":3.46004, "ay":1.42323, "alpha":3.74932, "fx":[50.01218,75.8521,65.83463,37.1266], "fy":[-1.35865,6.26705,42.96894,46.24613]}, + {"t":0.51522, "x":6.22338, "y":3.02073, "heading":1.81556, "vx":0.66674, "vy":-2.5917, "omega":2.70912, "ax":5.73304, "ay":2.10422, "alpha":1.65767, "fx":[96.48099,101.45111,94.10411,87.11165], "fy":[21.78749,23.45622,45.46856,48.44763]}, + {"t":0.53721, "x":6.23943, "y":2.96424, "heading":1.87515, "vx":0.79283, "vy":-2.54542, "omega":2.74557, "ax":5.28253, "ay":2.78476, "alpha":1.96858, "fx":[90.39737,97.11462,86.07294,75.76848], "fy":[31.94794,33.30007,57.37901,61.53964]}, + {"t":0.55921, "x":6.25815, "y":2.90893, "heading":1.93553, "vx":0.90901, "vy":-2.48417, "omega":2.78887, "ax":3.95216, "ay":3.30365, "alpha":3.23356, "fx":[68.67122,85.39712,65.94683,41.35605], "fy":[34.75171,38.59423,70.20807,74.92913]}, + {"t":0.5812, "x":6.27909, "y":2.85509, "heading":1.99687, "vx":0.99593, "vy":-2.41151, "omega":2.85999, "ax":1.92522, "ay":3.50441, "alpha":4.38093, "fx":[27.24974,64.57354,37.6525,-2.15387], "fy":[34.75473,44.72887,77.85263,74.42358]}, + {"t":0.60319, "x":6.30146, "y":2.8029, "heading":2.05977, "vx":1.03827, "vy":-2.33444, "omega":2.95634, "ax":-0.1351, "ay":3.16913, "alpha":4.44903, "fx":[-15.46365,32.67647,6.75387,-32.90116], "fy":[27.15361,47.1368,74.87605,60.42034]}, + {"t":0.62519, "x":6.32427, "y":2.75233, "heading":2.12479, "vx":1.0353, "vy":-2.26474, "omega":3.05419, "ax":-1.69085, "ay":2.39951, "alpha":3.38532, "fx":[-38.13019,-4.00013,-20.85896,-48.83277], "fy":[17.16876,38.92586,60.2856,42.3081]}, + {"t":0.64718, "x":6.34663, "y":2.7031, "heading":2.19196, "vx":0.99811, "vy":-2.21197, "omega":3.12864, "ax":-2.69804, "ay":1.43745, "alpha":1.79931, "fx":[-48.34005,-33.74738,-41.555,-54.78929], "fy":[10.40374,23.45117,36.74068,24.46864]}, + {"t":0.66917, "x":6.36793, "y":2.6548, "heading":2.26077, "vx":0.93878, "vy":-2.18035, "omega":3.16821, "ax":-3.21546, "ay":0.49493, "alpha":0.22746, "fx":[-53.37037,-51.92476,-52.96319,-54.39249], "fy":[6.39514,8.09784,9.96982,8.269]}, + {"t":0.69117, "x":6.38779, "y":2.60696, "heading":2.33045, "vx":0.86806, "vy":-2.16947, "omega":3.17322, "ax":-3.37665, "ay":-0.3283, "alpha":-1.08905, "fx":[-56.05161,-61.57119,-55.78501,-49.90283], "fy":[3.31159,-4.96387,-14.073,-5.98645]}, + {"t":0.71316, "x":6.40607, "y":2.55917, "heading":2.40024, "vx":0.79379, "vy":-2.17669, "omega":3.14926, "ax":-3.30764, "ay":-1.01323, "alpha":-2.12331, "fx":[-57.26319,-65.95366,-52.92182,-42.60836], "fy":[0.21188,-16.10323,-33.05635,-18.06106]}, + {"t":0.73515, "x":6.42273, "y":2.51105, "heading":2.4695, "vx":0.72105, "vy":-2.19897, "omega":3.10256, "ax":-3.08633, "ay":-1.57388, "alpha":-2.93991, "fx":[-57.19738,-66.93679,-46.74992,-33.22664], "fy":[-3.10037,-25.84187,-47.382,-27.76263]}, + {"t":0.75715, "x":6.43784, "y":2.46231, "heading":2.53773, "vx":0.65317, "vy":-2.23359, "omega":3.03791, "ax":-2.74758, "ay":-2.02896, "alpha":-3.61748, "fx":[-55.77233,-65.46362,-38.44588,-22.02617], "fy":[-6.49981,-34.53995,-58.12084,-35.02224]}, + {"t":0.77914, "x":6.45154, "y":2.4127, "heading":2.60455, "vx":0.59274, "vy":-2.27821, "omega":2.95835, "ax":-2.29355, "ay":-2.39226, "alpha":-4.22508, "fx":[-52.66657,-61.89354,-28.29625,-8.82484], "fy":[-9.768,-42.48086,-66.16052,-39.79951]}, + {"t":0.80113, "x":6.46402, "y":2.36201, "heading":2.66961, "vx":0.5423, "vy":-2.33082, "omega":2.86542, "ax":-1.69532, "ay":-2.66756, "alpha":-4.81666, "fx":[-47.10026,-56.05824,-15.93343,6.97425], "fy":[-12.63179,-49.937,-71.99853,-41.84874]}, + {"t":0.82313, "x":6.47554, "y":2.3101, "heading":2.73263, "vx":0.50501, "vy":-2.38949, "omega":2.75949, "ax":-0.88578, "ay":-2.83726, "alpha":-5.41534, "fx":[-37.22653,-47.06408,-0.38477,26.09539], "fy":[-14.52566,-57.14206,-75.57888,-40.39223]}, + {"t":0.84512, "x":6.48643, "y":2.25687, "heading":2.79332, "vx":0.48553, "vy":-2.45189, "omega":2.64039, "ax":0.25263, "ay":-2.83357, "alpha":-5.961, "fx":[-18.73998,-32.77541,19.78525,48.43779], "fy":[-13.57023,-64.00285,-75.89095,-33.93071]}, + {"t":0.86711, "x":6.49717, "y":2.20226, "heading":2.85139, "vx":0.49109, "vy":-2.51421, "omega":2.50928, "ax":1.86095, "ay":-2.49948, "alpha":-6.21716, "fx":[15.3024,-8.98869,45.2801,71.47809], "fy":[-4.44705,-69.02927,-70.55854,-21.26548]}, + {"t":0.88911, "x":6.50842, "y":2.14635, "heading":2.90658, "vx":0.53201, "vy":-2.56918, "omega":2.37255, "ax":3.7293, "ay":-1.73592, "alpha":-5.90836, "fx":[55.83172,27.71264,72.50994,90.57842], "fy":[15.22366,-67.30051,-58.16763,-4.55868]}, + {"t":0.9111, "x":6.52102, "y":2.08943, "heading":2.95876, "vx":0.61403, "vy":-2.60736, "omega":2.2426, "ax":3.67999, "ay":0.00435, "alpha":-10.4331, "fx":[34.83183,13.80274,93.71599,101.02119], "fy":[86.40733,-73.46791,-48.56258,35.91078]}, + {"t":0.93309, "x":6.53542, "y":2.03209, "heading":3.00808, "vx":0.69497, "vy":-2.60727, "omega":2.01315, "ax":0.49143, "ay":0.12872, "alpha":-17.89671, "fx":[-59.63187,-90.5197,83.69338,98.95819], "fy":[103.86465,-77.7619,-86.29224,68.70255]}, + {"t":0.95509, "x":6.55082, "y":1.97478, "heading":3.05236, "vx":0.70578, "vy":-2.60444, "omega":1.61954, "ax":0.2917, "ay":0.07919, "alpha":-18.26956, "fx":[-70.97722,-90.67344,84.75381,96.18812], "fy":[99.24176,-81.47023,-87.99274,75.45834]}, + {"t":0.97708, "x":6.56642, "y":1.91751, "heading":3.08798, "vx":0.71219, "vy":-2.60269, "omega":1.21773, "ax":0.22225, "ay":0.06107, "alpha":-18.39914, "fx":[-76.8075,-88.94138,86.88102,93.56604], "fy":[95.84579,-84.63158,-86.94666,79.77099]}, + {"t":0.99907, "x":6.58213, "y":1.86029, "heading":3.11476, "vx":0.71708, "vy":-2.60135, "omega":0.81307, "ax":0.18773, "ay":0.0536, "alpha":-18.4648, "fx":[-80.47569,-87.28313,88.747,91.42735], "fy":[93.34187,-86.96572,-85.60308,82.77185]}, + {"t":1.02107, "x":6.59795, "y":1.80309, "heading":3.13264, "vx":0.72121, "vy":-2.60017, "omega":0.40697, "ax":0.16311, "ay":0.06631, "alpha":-18.50429, "fx":[-82.58629,-86.47033,90.23413,89.60958], "fy":[91.82979,-88.14212,-84.37969,85.07723]}, + {"t":1.04306, "x":6.61385, "y":1.74592, "heading":3.14159, "vx":0.7248, "vy":-2.59871, "omega":0.0, "ax":5.51415, "ay":1.71875, "alpha":0.0, "fx":[91.16793,91.16793,91.16793,91.16793], "fy":[28.41687,28.41687,28.41687,28.41687]}, + {"t":1.0774, "x":6.642, "y":1.65768, "heading":3.14159, "vx":0.91419, "vy":-2.53968, "omega":0.0, "ax":-3.53676, "ay":-1.17893, "alpha":0.0, "fx":[-58.47474,-58.47474,-58.47474,-58.47474], "fy":[-19.49174,-19.49174,-19.49174,-19.49174]}, + {"t":1.11175, "x":6.67131, "y":1.56975, "heading":3.14159, "vx":0.79271, "vy":-2.58017, "omega":0.0, "ax":-6.6129, "ay":-1.72024, "alpha":0.0, "fx":[-109.33399,-109.33399,-109.33399,-109.33399], "fy":[-28.44153,-28.44153,-28.44153,-28.44153]}, + {"t":1.1461, "x":6.69464, "y":1.48012, "heading":3.14159, "vx":0.56559, "vy":-2.63926, "omega":0.0, "ax":-7.15322, "ay":-1.18943, "alpha":0.0, "fx":[-118.26739,-118.26739,-118.26739,-118.26739], "fy":[-19.66537,-19.66537,-19.66537,-19.66537]}, + {"t":1.18044, "x":6.70984, "y":1.38877, "heading":3.14159, "vx":0.3199, "vy":-2.68011, "omega":0.0, "ax":-7.36782, "ay":-0.47064, "alpha":0.0, "fx":[-121.81536,-121.81536,-121.81536,-121.81536], "fy":[-7.78123,-7.78123,-7.78123,-7.78123]}, + {"t":1.21479, "x":6.71648, "y":1.29644, "heading":3.14159, "vx":0.06685, "vy":-2.69627, "omega":0.0, "ax":-6.36134, "ay":3.88646, "alpha":0.0, "fx":[-105.17479,-105.17479,-105.17479,-105.17479], "fy":[64.25655,64.25655,64.25655,64.25655]}, + {"t":1.24914, "x":6.71503, "y":1.20613, "heading":3.14159, "vx":-0.15164, "vy":-2.56279, "omega":0.0, "ax":-5.05636, "ay":5.52381, "alpha":0.0, "fx":[-83.599,-83.599,-83.599,-83.599], "fy":[91.32752,91.32752,91.32752,91.32752]}, + {"t":1.28348, "x":6.70684, "y":1.12136, "heading":3.14159, "vx":-0.32531, "vy":-2.37307, "omega":0.0, "ax":-4.44001, "ay":6.0479, "alpha":0.0, "fx":[-73.40865,-73.40865,-73.40865,-73.40865], "fy":[99.99266,99.99266,99.99266,99.99266]}, + {"t":1.31783, "x":6.69304, "y":1.04342, "heading":3.14159, "vx":-0.4778, "vy":-2.16535, "omega":0.0, "ax":-4.10063, "ay":6.29183, "alpha":0.0, "fx":[-67.79748,-67.79748,-67.79748,-67.79748], "fy":[104.02559,104.02559,104.02559,104.02559]}, + {"t":1.35217, "x":6.67422, "y":0.97276, "heading":3.14159, "vx":-0.61864, "vy":-1.94925, "omega":0.0, "ax":-3.88824, "ay":6.43055, "alpha":0.0, "fx":[-64.28596,-64.28596,-64.28596,-64.28596], "fy":[106.31913,106.31913,106.31913,106.31913]}, + {"t":1.38652, "x":6.65067, "y":0.90961, "heading":3.14159, "vx":-0.75219, "vy":-1.72838, "omega":0.0, "ax":-3.74336, "ay":6.51946, "alpha":0.0, "fx":[-61.89065,-61.89065,-61.89065,-61.89065], "fy":[107.78919,107.78919,107.78919,107.78919]}, + {"t":1.42087, "x":6.62263, "y":0.85409, "heading":3.14159, "vx":-0.88076, "vy":-1.50447, "omega":0.0, "ax":-3.6384, "ay":6.58111, "alpha":0.0, "fx":[-60.15529,-60.15529,-60.15529,-60.15529], "fy":[108.80848,108.80848,108.80848,108.80848]}, + {"t":1.45521, "x":6.59023, "y":0.8063, "heading":3.14159, "vx":-1.00572, "vy":-1.27843, "omega":0.0, "ax":-3.55893, "ay":6.62629, "alpha":0.0, "fx":[-58.84137,-58.84137,-58.84137,-58.84137], "fy":[109.55541,109.55541,109.55541,109.55541]}, + {"t":1.48956, "x":6.55359, "y":0.7663, "heading":3.14159, "vx":-1.12796, "vy":-1.05084, "omega":0.0, "ax":-3.49671, "ay":6.66078, "alpha":0.0, "fx":[-57.81261,-57.81261,-57.81261,-57.81261], "fy":[110.12563,110.12563,110.12563,110.12563]}, + {"t":1.5239, "x":6.51279, "y":0.73414, "heading":3.14159, "vx":-1.24806, "vy":-0.82207, "omega":0.0, "ax":-3.44669, "ay":6.68795, "alpha":0.0, "fx":[-56.98567,-56.98567,-56.98567,-56.98567], "fy":[110.57485,110.57485,110.57485,110.57485]}, + {"t":1.55825, "x":6.46789, "y":0.70985, "heading":3.14159, "vx":-1.36644, "vy":-0.59237, "omega":0.0, "ax":-3.40563, "ay":6.70989, "alpha":0.0, "fx":[-56.30679,-56.30679,-56.30679,-56.30679], "fy":[110.93761,110.93761,110.93761,110.93761]}, + {"t":1.5926, "x":6.41895, "y":0.69346, "heading":3.14159, "vx":-1.48341, "vy":-0.36191, "omega":0.0, "ax":-3.86565, "ay":6.4549, "alpha":0.0, "fx":[-63.91257,-63.91257,-63.91257,-63.91257], "fy":[106.72175,106.72175,106.72175,106.72175]}, + {"t":1.63104, "x":6.35907, "y":0.68431, "heading":3.14159, "vx":-1.63202, "vy":-0.11376, "omega":0.0, "ax":-6.03636, "ay":4.47481, "alpha":0.0, "fx":[-99.80173,-99.80173,-99.80173,-99.80173], "fy":[73.98402,73.98402,73.98402,73.98402]}, + {"t":1.66948, "x":6.29187, "y":0.68325, "heading":3.14159, "vx":-1.86407, "vy":0.05826, "omega":0.0, "ax":-7.49801, "ay":0.04458, "alpha":0.0, "fx":[-123.96785,-123.96785,-123.96785,-123.96785], "fy":[0.737,0.737,0.737,0.737]}, + {"t":1.70792, "x":6.21467, "y":0.68552, "heading":3.14159, "vx":-2.15232, "vy":0.05998, "omega":0.0, "ax":-7.46625, "ay":-0.01078, "alpha":0.0, "fx":[-123.44283,-123.44283,-123.44283,-123.44283], "fy":[-0.17828,-0.17828,-0.17828,-0.17828]}, + {"t":1.74637, "x":6.12641, "y":0.68782, "heading":3.14159, "vx":-2.43934, "vy":0.05956, "omega":0.0, "ax":-6.75443, "ay":-0.64476, "alpha":0.0, "fx":[-111.67393,-111.67393,-111.67393,-111.67393], "fy":[-10.66018,-10.66018,-10.66018,-10.66018]}, + {"t":1.78481, "x":6.02764, "y":0.68963, "heading":3.14159, "vx":-2.699, "vy":0.03477, "omega":0.0, "ax":-0.00707, "ay":-0.61163, "alpha":0.0, "fx":[-0.11695,-0.11695,-0.11695,-0.11695], "fy":[-10.11231,-10.11231,-10.11231,-10.11231]}, + {"t":1.82325, "x":5.92388, "y":0.69052, "heading":3.14159, "vx":-2.69927, "vy":0.01126, "omega":0.0, "ax":-0.00045, "ay":-0.14367, "alpha":0.0, "fx":[-0.00748,-0.00748,-0.00748,-0.00748], "fy":[-2.37537,-2.37537,-2.37537,-2.37537]}, + {"t":1.8617, "x":5.82011, "y":0.69084, "heading":3.14159, "vx":-2.69929, "vy":0.00574, "omega":0.0, "ax":-0.00011, "ay":-0.06541, "alpha":0.0, "fx":[-0.00179,-0.00179,-0.00179,-0.00179], "fy":[-1.08143,-1.08143,-1.08143,-1.08143]}, + {"t":1.90014, "x":5.71634, "y":0.69102, "heading":3.14159, "vx":-2.69929, "vy":0.00322, "omega":0.0, "ax":-0.00004, "ay":-0.04461, "alpha":0.0, "fx":[-0.00065,-0.00065,-0.00065,-0.00065], "fy":[-0.73753,-0.73753,-0.73753,-0.73753]}, + {"t":1.93858, "x":5.61257, "y":0.69111, "heading":3.14159, "vx":-2.6993, "vy":0.00151, "omega":0.0, "ax":-0.00001, "ay":-0.03403, "alpha":0.0, "fx":[-0.00018,-0.00018,-0.00018,-0.00018], "fy":[-0.56257,-0.56257,-0.56257,-0.56257]}, + {"t":1.97703, "x":5.5088, "y":0.69114, "heading":3.14159, "vx":-2.6993, "vy":0.0002, "omega":0.0, "ax":0.0, "ay":-0.02657, "alpha":0.0, "fx":[0.00005,0.00005,0.00005,0.00005], "fy":[-0.43929,-0.43929,-0.43929,-0.43929]}, + {"t":2.01547, "x":5.40503, "y":0.69113, "heading":3.14159, "vx":-2.6993, "vy":-0.00082, "omega":0.0, "ax":0.00001, "ay":-0.02086, "alpha":0.0, "fx":[0.00016,0.00016,0.00016,0.00016], "fy":[-0.34483,-0.34483,-0.34483,-0.34483]}, + {"t":2.05391, "x":5.30126, "y":0.69108, "heading":3.14159, "vx":-2.6993, "vy":-0.00162, "omega":0.0, "ax":0.00001, "ay":-0.0164, "alpha":0.0, "fx":[0.00019,0.00019,0.00019,0.00019], "fy":[-0.2711,-0.2711,-0.2711,-0.2711]}, + {"t":2.09235, "x":5.1975, "y":0.69101, "heading":3.14159, "vx":-2.6993, "vy":-0.00225, "omega":0.0, "ax":0.00001, "ay":-0.0129, "alpha":0.0, "fx":[0.0002,0.0002,0.0002,0.0002], "fy":[-0.21328,-0.21328,-0.21328,-0.21328]}, + {"t":2.1308, "x":5.09373, "y":0.69091, "heading":3.14159, "vx":-2.6993, "vy":-0.00275, "omega":0.0, "ax":0.00001, "ay":-0.01015, "alpha":0.0, "fx":[0.00018,0.00018,0.00018,0.00018], "fy":[-0.16784,-0.16784,-0.16784,-0.16784]}, + {"t":2.16924, "x":4.98996, "y":0.6908, "heading":3.14159, "vx":-2.6993, "vy":-0.00314, "omega":0.0, "ax":0.00001, "ay":-0.00799, "alpha":0.0, "fx":[0.00016,0.00016,0.00016,0.00016], "fy":[-0.1321,-0.1321,-0.1321,-0.1321]}, + {"t":2.20768, "x":4.88619, "y":0.69067, "heading":3.14159, "vx":-2.69929, "vy":-0.00345, "omega":0.0, "ax":0.00001, "ay":-0.00629, "alpha":0.0, "fx":[0.00014,0.00014,0.00014,0.00014], "fy":[-0.10397,-0.10397,-0.10397,-0.10397]}, + {"t":2.24613, "x":4.78242, "y":0.69053, "heading":3.14159, "vx":-2.69929, "vy":-0.00369, "omega":0.0, "ax":0.00001, "ay":-0.00495, "alpha":0.0, "fx":[0.00011,0.00011,0.00011,0.00011], "fy":[-0.08182,-0.08182,-0.08182,-0.08182]}, + {"t":2.28457, "x":4.67865, "y":0.69039, "heading":3.14159, "vx":-2.69929, "vy":-0.00388, "omega":0.0, "ax":0.00001, "ay":-0.00389, "alpha":0.0, "fx":[0.00009,0.00009,0.00009,0.00009], "fy":[-0.06437,-0.06437,-0.06437,-0.06437]}, + {"t":2.32301, "x":4.57488, "y":0.69024, "heading":3.14159, "vx":-2.69929, "vy":-0.00403, "omega":0.0, "ax":0.0, "ay":-0.00306, "alpha":0.0, "fx":[0.00008,0.00008,0.00008,0.00008], "fy":[-0.05061,-0.05061,-0.05061,-0.05061]}, + {"t":2.36146, "x":4.47111, "y":0.69008, "heading":3.14159, "vx":-2.69929, "vy":-0.00414, "omega":0.0, "ax":0.0, "ay":-0.0024, "alpha":0.0, "fx":[0.00006,0.00006,0.00006,0.00006], "fy":[-0.03975,-0.03975,-0.03975,-0.03975]}, + {"t":2.3999, "x":4.36734, "y":0.68992, "heading":3.14159, "vx":-2.69929, "vy":-0.00424, "omega":0.0, "ax":0.0, "ay":-0.00189, "alpha":0.0, "fx":[0.00005,0.00005,0.00005,0.00005], "fy":[-0.03118,-0.03118,-0.03118,-0.03118]}, + {"t":2.43834, "x":4.26358, "y":0.68975, "heading":3.14159, "vx":-2.69929, "vy":-0.00431, "omega":0.0, "ax":0.0, "ay":-0.00148, "alpha":0.0, "fx":[0.00004,0.00004,0.00004,0.00004], "fy":[-0.02439,-0.02439,-0.02439,-0.02439]}, + {"t":2.47678, "x":4.15981, "y":0.68959, "heading":3.14159, "vx":-2.69929, "vy":-0.00437, "omega":0.0, "ax":0.0, "ay":-0.00115, "alpha":0.0, "fx":[0.00003,0.00003,0.00003,0.00003], "fy":[-0.019,-0.019,-0.019,-0.019]}, + {"t":2.51523, "x":4.05604, "y":0.68942, "heading":3.14159, "vx":-2.69929, "vy":-0.00441, "omega":0.0, "ax":0.0, "ay":-0.00089, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[-0.0147,-0.0147,-0.0147,-0.0147]}, + {"t":2.55367, "x":3.95227, "y":0.68925, "heading":3.14159, "vx":-2.69929, "vy":-0.00444, "omega":0.0, "ax":0.0, "ay":-0.00068, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[-0.01124,-0.01124,-0.01124,-0.01124]}, + {"t":2.59211, "x":3.8485, "y":0.68908, "heading":3.14159, "vx":-2.69929, "vy":-0.00447, "omega":0.0, "ax":0.0, "ay":-0.00051, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[-0.00843,-0.00843,-0.00843,-0.00843]}, + {"t":2.63056, "x":3.74473, "y":0.6889, "heading":3.14159, "vx":-2.69929, "vy":-0.00449, "omega":0.0, "ax":0.0, "ay":-0.00037, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[-0.0061,-0.0061,-0.0061,-0.0061]}, + {"t":2.669, "x":3.64096, "y":0.68873, "heading":3.14159, "vx":-2.69929, "vy":-0.0045, "omega":0.0, "ax":0.0, "ay":-0.00025, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[-0.00413,-0.00413,-0.00413,-0.00413]}, + {"t":2.70744, "x":3.53719, "y":0.68856, "heading":3.14159, "vx":-2.69929, "vy":-0.00451, "omega":0.0, "ax":0.0, "ay":-0.00014, "alpha":0.0, "fx":[0.00002,0.00002,0.00002,0.00002], "fy":[-0.00238,-0.00238,-0.00238,-0.00238]}, + {"t":2.74588, "x":3.43342, "y":0.68838, "heading":3.14159, "vx":-2.69929, "vy":-0.00452, "omega":0.0, "ax":0.01839, "ay":-0.00002, "alpha":0.0, "fx":[0.30406,0.30406,0.30406,0.30406], "fy":[-0.00027,-0.00027,-0.00027,-0.00027]}, + {"t":2.78433, "x":3.32967, "y":0.68821, "heading":3.14159, "vx":-2.69859, "vy":-0.00452, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/RPreTrenchReversedtoRCleanup.traj b/src/main/deploy/choreo/RPreTrenchReversedtoRCleanup.traj new file mode 100644 index 00000000..fb3a0240 --- /dev/null +++ b/src/main/deploy/choreo/RPreTrenchReversedtoRCleanup.traj @@ -0,0 +1,163 @@ +{ + "name":"RPreTrenchReversedtoRCleanup", + "version":3, + "snapshot":{ + "waypoints":[ + {"x":3.32966947555542, "y":0.688210129737854, "heading":3.141592653589793, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.34099006652832, "y":0.7324375510215759, "heading":0.0, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":6.224050045013428, "y":1.9603075981140137, "heading":0.0, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.884834289550781, "y":2.913686037063598, "heading":1.5707963267948966, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.824512958526611, "y":4.3513383865356445, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"RPreTrenchReversed.x", "val":3.32966947555542}, "y":{"exp":"RPreTrenchReversed.y", "val":0.688210129737854}, "heading":{"exp":"RPreTrenchReversed.heading", "val":3.141592653589793}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.34099006652832 m", "val":6.34099006652832}, "y":{"exp":"0.7324375510215759 m", "val":0.7324375510215759}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"6.224050045013428 m", "val":6.224050045013428}, "y":{"exp":"1.9603075981140137 m", "val":1.9603075981140137}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.884834289550781 m", "val":5.884834289550781}, "y":{"exp":"2.9136860370635986 m", "val":2.913686037063598}, "heading":{"exp":"1.5707963267948966 rad", "val":1.5707963267948966}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"RCleanup.x", "val":5.824512958526611}, "y":{"exp":"RCleanup.y", "val":4.3513383865356445}, "heading":{"exp":"RCleanup.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, + {"from":3, "to":4, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "config":{ + "frontLeft":{ + "x":0.2761996, + "y":0.2761996 + }, + "backLeft":{ + "x":-0.2761996, + "y":0.2761996 + }, + "mass":66.133767546, + "inertia":10.42384825260472, + "gearing":7.03, + "radius":0.0508, + "vmax":485.8996637552213, + "tmax":0.9, + "cof":2.25, + "bumper":{ + "front":0.44196, + "side":0.44196, + "back":0.44196 + }, + "differentialTrackWidth":0.5427218 + }, + "sampleType":"Swerve", + "waypoints":[0.0,1.18546,1.87005,2.19955,2.84416], + "samples":[ + {"t":0.0, "x":3.32967, "y":0.68821, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":7.52503, "ay":0.11043, "alpha":0.0, "fx":[124.41461,124.41461,124.41461,124.41461], "fy":[1.82571,1.82571,1.82571,1.82571]}, + {"t":0.04391, "x":3.33692, "y":0.68832, "heading":3.14159, "vx":0.33039, "vy":0.00485, "omega":0.0, "ax":7.52423, "ay":0.11041, "alpha":0.0, "fx":[124.40136,124.40136,124.40136,124.40136], "fy":[1.82541,1.82541,1.82541,1.82541]}, + {"t":0.08781, "x":3.35868, "y":0.68864, "heading":3.14159, "vx":0.66075, "vy":0.0097, "omega":0.0, "ax":7.52322, "ay":0.11038, "alpha":0.0, "fx":[124.3848,124.3848,124.3848,124.3848], "fy":[1.82503,1.82503,1.82503,1.82503]}, + {"t":0.13172, "x":3.39494, "y":0.68917, "heading":3.14159, "vx":0.99106, "vy":0.01454, "omega":0.0, "ax":7.52194, "ay":0.11035, "alpha":0.0, "fx":[124.36352,124.36352,124.36352,124.36352], "fy":[1.82455,1.82455,1.82455,1.82455]}, + {"t":0.17562, "x":3.44571, "y":0.68991, "heading":3.14159, "vx":1.32132, "vy":0.01939, "omega":0.0, "ax":7.52022, "ay":0.11032, "alpha":0.0, "fx":[124.33514,124.33514,124.33514,124.33514], "fy":[1.82392,1.82392,1.82392,1.82392]}, + {"t":0.21953, "x":3.51097, "y":0.69087, "heading":3.14159, "vx":1.6515, "vy":0.02423, "omega":0.0, "ax":7.51782, "ay":0.11027, "alpha":0.0, "fx":[124.29544,124.29544,124.29544,124.29544], "fy":[1.82309,1.82309,1.82309,1.82309]}, + {"t":0.26344, "x":3.59073, "y":0.69204, "heading":3.14159, "vx":1.98158, "vy":0.02907, "omega":0.0, "ax":7.51422, "ay":0.11019, "alpha":0.0, "fx":[124.23593,124.23593,124.23593,124.23593], "fy":[1.8219,1.8219,1.8219,1.8219]}, + {"t":0.30734, "x":3.68497, "y":0.69342, "heading":3.14159, "vx":2.3115, "vy":0.03391, "omega":0.0, "ax":7.50823, "ay":0.11008, "alpha":0.0, "fx":[124.13691,124.13691,124.13691,124.13691], "fy":[1.82006,1.82006,1.82006,1.82006]}, + {"t":0.35125, "x":3.7937, "y":0.69502, "heading":3.14159, "vx":2.64115, "vy":0.03874, "omega":0.0, "ax":7.4963, "ay":0.10988, "alpha":0.0, "fx":[123.93958,123.93958,123.93958,123.93958], "fy":[1.81665,1.81665,1.81665,1.81665]}, + {"t":0.39515, "x":3.91688, "y":0.69683, "heading":3.14159, "vx":2.97029, "vy":0.04357, "omega":0.0, "ax":7.46093, "ay":0.10931, "alpha":0.0, "fx":[123.35478,123.35478,123.35478,123.35478], "fy":[1.80727,1.80727,1.80727,1.80727]}, + {"t":0.43906, "x":4.05449, "y":0.69884, "heading":3.14159, "vx":3.29786, "vy":0.04837, "omega":0.0, "ax":4.77669, "ay":0.06867, "alpha":0.0, "fx":[78.97516,78.97516,78.97516,78.97516], "fy":[1.13527,1.13527,1.13527,1.13527]}, + {"t":0.48296, "x":4.20389, "y":0.70103, "heading":3.14159, "vx":3.50759, "vy":0.05138, "omega":0.0, "ax":0.00151, "ay":-0.00133, "alpha":0.0, "fx":[0.02499,0.02499,0.02499,0.02499], "fy":[-0.022,-0.022,-0.022,-0.022]}, + {"t":0.52687, "x":4.35789, "y":0.70329, "heading":3.14159, "vx":3.50766, "vy":0.05132, "omega":0.0, "ax":0.00002, "ay":-0.00167, "alpha":0.0, "fx":[0.00041,0.00041,0.00041,0.00041], "fy":[-0.02757,-0.02757,-0.02757,-0.02757]}, + {"t":0.57078, "x":4.5119, "y":0.70554, "heading":3.14159, "vx":3.50766, "vy":0.05125, "omega":0.0, "ax":0.00003, "ay":-0.00233, "alpha":0.0, "fx":[0.00056,0.00056,0.00056,0.00056], "fy":[-0.03845,-0.03845,-0.03845,-0.03845]}, + {"t":0.61468, "x":4.66591, "y":0.70779, "heading":3.14159, "vx":3.50766, "vy":0.05115, "omega":0.0, "ax":0.00005, "ay":-0.00328, "alpha":0.0, "fx":[0.00079,0.00079,0.00079,0.00079], "fy":[-0.05429,-0.05429,-0.05429,-0.05429]}, + {"t":0.65859, "x":4.81991, "y":0.71003, "heading":3.14159, "vx":3.50766, "vy":0.051, "omega":0.0, "ax":0.00007, "ay":-0.00471, "alpha":0.0, "fx":[0.00113,0.00113,0.00113,0.00113], "fy":[-0.07789,-0.07789,-0.07789,-0.07789]}, + {"t":0.70249, "x":4.97392, "y":0.71227, "heading":3.14159, "vx":3.50766, "vy":0.0508, "omega":0.0, "ax":0.00011, "ay":-0.0073, "alpha":0.0, "fx":[0.00174,0.00174,0.00174,0.00174], "fy":[-0.12063,-0.12063,-0.12063,-0.12063]}, + {"t":0.7464, "x":5.12793, "y":0.71449, "heading":3.14159, "vx":3.50767, "vy":0.05048, "omega":0.0, "ax":0.00021, "ay":-0.01479, "alpha":0.0, "fx":[0.0035,0.0035,0.0035,0.0035], "fy":[-0.24451,-0.24451,-0.24451,-0.24451]}, + {"t":0.79031, "x":5.28194, "y":0.71669, "heading":3.14159, "vx":3.50768, "vy":0.04983, "omega":0.0, "ax":0.0007, "ay":-0.05062, "alpha":0.0, "fx":[0.01161,0.01161,0.01161,0.01161], "fy":[-0.83687,-0.83687,-0.83687,-0.83687]}, + {"t":0.83421, "x":5.43595, "y":0.71883, "heading":3.14159, "vx":3.50771, "vy":0.04761, "omega":0.0, "ax":0.00067, "ay":-0.26595, "alpha":0.0, "fx":[0.011,0.011,0.011,0.011], "fy":[-4.39701,-4.39701,-4.39701,-4.39701]}, + {"t":0.87812, "x":5.58995, "y":0.72066, "heading":3.14159, "vx":3.50774, "vy":0.03593, "omega":0.0, "ax":-5.64628, "ay":-0.76249, "alpha":0.0, "fx":[-93.35239,-93.35239,-93.35239,-93.35239], "fy":[-12.60666,-12.60666,-12.60666,-12.60666]}, + {"t":0.92202, "x":5.73852, "y":0.72151, "heading":3.14159, "vx":3.25983, "vy":0.00245, "omega":0.0, "ax":-7.46174, "ay":-0.12603, "alpha":0.0, "fx":[-123.36829,-123.36829,-123.36829,-123.36829], "fy":[-2.08371,-2.08371,-2.08371,-2.08371]}, + {"t":0.96593, "x":5.87446, "y":0.72149, "heading":3.14159, "vx":2.93222, "vy":-0.00308, "omega":0.0, "ax":-7.49669, "ay":-0.10233, "alpha":0.0, "fx":[-123.94604,-123.94604,-123.94604,-123.94604], "fy":[-1.69193,-1.69193,-1.69193,-1.69193]}, + {"t":1.00984, "x":5.99597, "y":0.72126, "heading":3.14159, "vx":2.60307, "vy":-0.00758, "omega":0.0, "ax":-7.50872, "ay":-0.08149, "alpha":0.0, "fx":[-124.14506,-124.14506,-124.14506,-124.14506], "fy":[-1.34732,-1.34732,-1.34732,-1.34732]}, + {"t":1.05374, "x":6.10303, "y":0.72085, "heading":3.14159, "vx":2.27339, "vy":-0.01115, "omega":0.0, "ax":-7.51494, "ay":-0.04934, "alpha":0.0, "fx":[-124.24782,-124.24782,-124.24782,-124.24782], "fy":[-0.81577,-0.81577,-0.81577,-0.81577]}, + {"t":1.09765, "x":6.1956, "y":0.72031, "heading":3.14159, "vx":1.94344, "vy":-0.01332, "omega":0.0, "ax":-6.97257, "ay":2.81614, "alpha":0.0, "fx":[-115.28059,-115.28059,-115.28059,-115.28059], "fy":[46.56053,46.56053,46.56053,46.56053]}, + {"t":1.14155, "x":6.27421, "y":0.72244, "heading":3.14159, "vx":1.63731, "vy":0.11033, "omega":0.0, "ax":-5.29372, "ay":5.34774, "alpha":0.0, "fx":[-87.52333,-87.52333,-87.52333,-87.52333], "fy":[88.41652,88.41652,88.41652,88.41652]}, + {"t":1.18546, "x":6.34099, "y":0.73244, "heading":3.14159, "vx":1.40488, "vy":0.34512, "omega":0.0, "ax":-3.66686, "ay":5.40562, "alpha":-6.49952, "fx":[-82.72057,-122.64591,-32.09928,-5.03783], "fy":[92.97155,20.78509,119.42963,124.30764]}, + {"t":1.21284, "x":6.37809, "y":0.74392, "heading":3.14159, "vx":1.30447, "vy":0.49315, "omega":-0.17798, "ax":-3.71859, "ay":5.38755, "alpha":-6.44053, "fx":[-82.92568,-122.66075,-34.84628,-5.49176], "fy":[92.7797,20.64005,118.59988,124.27921]}, + {"t":1.24023, "x":6.41241, "y":0.75944, "heading":3.13672, "vx":1.20264, "vy":0.64068, "omega":-0.35435, "ax":-3.80311, "ay":5.36231, "alpha":-6.32609, "fx":[-82.95474,-122.6271,-40.10544,-5.82699], "fy":[92.74347,20.7769,116.85622,124.25301]}, + {"t":1.26761, "x":6.44392, "y":0.77899, "heading":3.12702, "vx":1.0985, "vy":0.78752, "omega":-0.52758, "ax":-3.92052, "ay":5.32389, "alpha":-6.17059, "fx":[-82.81838,-122.54568,-47.84741,-6.06735], "fy":[92.85343,21.1845,113.82268,124.22845]}, + {"t":1.29499, "x":6.47253, "y":0.80256, "heading":3.11257, "vx":0.99114, "vy":0.93331, "omega":-0.69655, "ax":-4.06827, "ay":5.26395, "alpha":-6.00014, "fx":[-82.53151,-122.41683,-57.85615,-6.24538], "fy":[93.09473,21.84361,108.98277,124.20389]}, + {"t":1.32238, "x":6.49815, "y":0.83009, "heading":3.09349, "vx":0.87973, "vy":1.07745, "omega":-0.86086, "ax":-4.23878, "ay":5.17373, "alpha":-5.85324, "fx":[-82.11406,-122.24097,-69.56668,-6.40463], "fy":[93.44695,22.72718,101.80762,124.17662]}, + {"t":1.34976, "x":6.52065, "y":0.86153, "heading":3.06992, "vx":0.76366, "vy":1.21913, "omega":-1.02114, "ax":-4.41822, "ay":5.04794, "alpha":-5.77435, "fx":[-81.59204,-122.019,-81.97802,-6.60432], "fy":[93.88377,23.80168,92.01112,124.14253]}, + {"t":1.37714, "x":6.5399, "y":0.89681, "heading":3.04196, "vx":0.64267, "vy":1.35736, "omega":-1.17926, "ax":-4.58886, "ay":4.88918, "alpha":-5.79848, "fx":[-80.99946,-121.75261,-93.79805,-6.92845], "fy":[94.37207,25.02935,79.84349,124.09523]}, + {"t":1.40453, "x":6.55578, "y":0.93581, "heading":3.00967, "vx":0.51701, "vy":1.49125, "omega":-1.33805, "ax":-4.73553, "ay":4.70939, "alpha":-5.93345, "fx":[-80.38249,-121.44441,-103.8478,-7.50345], "fy":[94.86914,26.37006,66.18676,124.02379]}, + {"t":1.43191, "x":6.56816, "y":0.97841, "heading":2.97302, "vx":0.38734, "vy":1.62021, "omega":-1.50053, "ax":-4.85251, "ay":4.52566, "alpha":-6.15299, "fx":[-79.80869,-121.0986,-111.47508,-8.53212], "fy":[95.31546,27.78012,52.29618,123.9072]}, + {"t":1.4593, "x":6.57695, "y":1.02448, "heading":2.93193, "vx":0.25446, "vy":1.74413, "omega":-1.66902, "ax":-4.94662, "ay":4.35302, "alpha":-6.40438, "fx":[-79.38697,-120.72291,-116.66697,-10.36187], "fy":[95.61838,29.20459,39.35941,123.69915]}, + {"t":1.48668, "x":6.58206, "y":1.07387, "heading":2.88623, "vx":0.119, "vy":1.86334, "omega":-1.8444, "ax":-5.03709, "ay":4.19836, "alpha":-6.6181, "fx":[-79.31063,-120.33351,-119.84645,-13.6313], "fy":[95.61447,30.55768,28.19771,123.28342]}, + {"t":1.51406, "x":6.58343, "y":1.12647, "heading":2.83572, "vx":-0.01893, "vy":1.9783, "omega":-2.02562, "ax":-5.15802, "ay":4.05552, "alpha":-6.70197, "fx":[-79.95602,-119.96653,-121.5819,-19.61504], "fy":[94.97587,31.6773,19.23028,122.32331]}, + {"t":1.54145, "x":6.58098, "y":1.18216, "heading":2.78026, "vx":-0.16018, "vy":2.08936, "omega":-2.20915, "ax":-5.37304, "ay":3.89218, "alpha":-6.49723, "fx":[-82.13754,-119.70588,-122.38631,-31.10987], "fy":[92.93212,32.20818,12.60133,119.66274]}, + {"t":1.56883, "x":6.57458, "y":1.24084, "heading":2.71976, "vx":-0.30731, "vy":2.19594, "omega":-2.38707, "ax":-5.81919, "ay":3.58709, "alpha":-5.63713, "fx":[-87.87909,-119.75736,-122.62925,-54.57916], "fy":[87.20141,31.22725,8.34609,110.45288]}, + {"t":1.59621, "x":6.56398, "y":1.30231, "heading":2.65439, "vx":-0.46666, "vy":2.29417, "omega":-2.54143, "ax":-6.71484, "ay":2.6237, "alpha":-3.31829, "fx":[-102.86539,-120.64478,-122.50431,-98.06323], "fy":[67.94593,25.56072,6.56634,73.44222]}, + {"t":1.6236, "x":6.54869, "y":1.36612, "heading":2.5848, "vx":-0.65054, "vy":2.36602, "omega":-2.6323, "ax":-7.35474, "ay":-0.3406, "alpha":1.07294, "fx":[-120.51303,-121.8064,-121.95124,-122.12591], "fy":[-19.66549,-2.76645,7.8691,-7.96224]}, + {"t":1.65098, "x":6.52812, "y":1.43078, "heading":2.51272, "vx":-0.85194, "vy":2.35669, "omega":-2.60292, "ax":-7.17507, "ay":-0.89563, "alpha":2.18612, "fx":[-113.49677,-119.4845,-120.66477,-120.86855], "fy":[-42.93478,-13.69636,13.26879,-15.86922]}, + {"t":1.67837, "x":6.5021, "y":1.49498, "heading":2.44144, "vx":-1.04842, "vy":2.33216, "omega":-2.54305, "ax":-7.06149, "ay":-0.74095, "alpha":2.62222, "fx":[-111.26684,-117.29905,-118.11297,-120.32384], "fy":[-45.20942,-13.22144,21.51074,-12.08188]}, + {"t":1.70575, "x":6.47074, "y":1.55857, "heading":2.3718, "vx":-1.24179, "vy":2.31187, "omega":-2.47125, "ax":-6.81217, "ay":-0.44626, "alpha":3.41676, "fx":[-107.25083,-112.29101,-111.94782,-119.02497], "fy":[-48.62835,-10.67099,35.82877,-6.04236]}, + {"t":1.73313, "x":6.43418, "y":1.62171, "heading":2.30413, "vx":-1.42833, "vy":2.29965, "omega":-2.37768, "ax":-5.99613, "ay":0.43921, "alpha":5.31438, "fx":[-99.14709,-90.59295,-91.62594,-115.18098], "fy":[-52.25271,9.09608,64.87348,7.32991]}, + {"t":1.76052, "x":6.39282, "y":1.68485, "heading":2.23902, "vx":-1.59253, "vy":2.31168, "omega":-2.23216, "ax":-2.25136, "ay":3.38874, "alpha":9.3088, "fx":[-78.81447,42.63086,-24.44739,-88.26016], "fy":[-12.23417,76.49204,102.90991,56.94243]}, + {"t":1.7879, "x":6.34836, "y":1.74942, "heading":2.1779, "vx":-1.65418, "vy":2.40448, "omega":-1.97725, "ax":3.12842, "ay":4.20355, "alpha":5.62741, "fx":[71.11522,88.67859,46.96116,0.13929], "fy":[34.11269,53.77169,95.08604,95.02647]}, + {"t":1.81528, "x":6.30424, "y":1.81684, "heading":2.12375, "vx":-1.56851, "vy":2.51958, "omega":-1.82315, "ax":3.64941, "ay":3.70207, "alpha":4.37124, "fx":[72.22074,87.48993,57.04013,24.59815], "fy":[31.51436,45.9282,82.9845,84.40456]}, + {"t":1.84267, "x":6.26266, "y":1.88722, "heading":2.07383, "vx":-1.46858, "vy":2.62096, "omega":-1.70345, "ax":4.29061, "ay":3.50674, "alpha":3.50318, "fx":[80.34099,91.33383,67.5036,44.57603], "fy":[32.94456,42.70228,76.50019,79.7666]}, + {"t":1.87005, "x":6.22405, "y":1.96031, "heading":2.02718, "vx":-1.35109, "vy":2.71699, "omega":-1.60752, "ax":4.84281, "ay":0.40846, "alpha":-5.24308, "fx":[85.94545,58.69376,75.62064,100.01329], "fy":[44.88385,33.62767,-44.12043,-7.37815]}, + {"t":1.88438, "x":6.20519, "y":1.99927, "heading":2.00415, "vx":-1.28171, "vy":2.72284, "omega":-1.68263, "ax":4.62349, "ay":0.21485, "alpha":-5.23033, "fx":[84.14237,54.42622,70.30445,96.89572], "fy":[41.13698,29.01342,-45.52346,-10.4184]}, + {"t":1.8987, "x":6.1873, "y":2.0383, "heading":1.98004, "vx":-1.21547, "vy":2.72592, "omega":-1.75756, "ax":4.3675, "ay":-0.027, "alpha":-5.05974, "fx":[81.64455,50.25045,64.35623,92.58822], "fy":[35.72339,22.30872,-46.0307,-13.78679]}, + {"t":1.91303, "x":6.17034, "y":2.07735, "heading":1.95487, "vx":-1.1529, "vy":2.72553, "omega":-1.83005, "ax":4.00206, "ay":-0.27632, "alpha":-4.77374, "fx":[77.15188,44.33802,56.89879,86.28267], "fy":[29.14079,14.79857,-45.2175,-16.99608]}, + {"t":1.92736, "x":6.15423, "y":2.11637, "heading":1.92865, "vx":-1.09557, "vy":2.72157, "omega":-1.89844, "ax":3.50322, "ay":-0.48208, "alpha":-4.23346, "fx":[69.44187,37.21491,48.11534,76.90913], "fy":[21.22844,7.40286,-41.45303,-19.0598]}, + {"t":1.94168, "x":6.1389, "y":2.15531, "heading":1.90145, "vx":-1.04538, "vy":2.71467, "omega":-1.95909, "ax":2.8397, "ay":-0.56384, "alpha":-3.2061, "fx":[56.66251,30.18462,38.40077,62.55222], "fy":[11.95165,1.35792,-32.45623,-18.14199]}, + {"t":1.95601, "x":6.12421, "y":2.19414, "heading":1.87338, "vx":-1.0047, "vy":2.70659, "omega":-2.00502, "ax":1.94585, "ay":-0.41306, "alpha":-1.34823, "fx":[36.3984,24.73045,28.13491,39.42261], "fy":[1.63926,-2.48965,-15.53346,-10.93358]}, + {"t":1.97034, "x":6.11002, "y":2.23288, "heading":1.84466, "vx":-0.97682, "vy":2.70067, "omega":-2.02433, "ax":0.8052, "ay":0.0595, "alpha":1.53692, "fx":[8.40668,22.12099,18.29904,4.42403], "fy":[-8.13159,-4.02726,9.93606,6.15792]}, + {"t":1.98466, "x":6.09611, "y":2.27157, "heading":1.81566, "vx":-0.96528, "vy":2.70152, "omega":-2.00232, "ax":-0.22709, "ay":0.7138, "alpha":4.53466, "fx":[-20.33188,23.04433,11.37479,-29.10566], "fy":[-14.14042,-3.34495,37.47115,27.22073]}, + {"t":1.99899, "x":6.08225, "y":2.31035, "heading":1.78697, "vx":-0.96854, "vy":2.71175, "omega":-1.93735, "ax":-0.80832, "ay":1.33249, "alpha":6.65776, "fx":[-42.32033,27.54306,9.14386,-47.82388], "fy":[-15.00683,-0.19656,59.3837,43.94214]}, + {"t":2.01331, "x":6.0683, "y":2.34934, "heading":1.75922, "vx":-0.98012, "vy":2.73084, "omega":-1.84197, "ax":-1.00826, "ay":1.88796, "alpha":8.03756, "fx":[-57.19247,35.43519,10.94326,-55.86569], "fy":[-12.0755,5.38089,74.62812,56.92429]}, + {"t":2.02764, "x":6.05415, "y":2.38865, "heading":1.73283, "vx":-0.99456, "vy":2.75789, "omega":-1.72682, "ax":-0.95971, "ay":2.39483, "alpha":8.96641, "fx":[-66.98619,46.05538,15.4343,-57.9725], "fy":[-6.80571,12.52424,84.90592,67.75486]}, + {"t":2.04197, "x":6.0398, "y":2.42841, "heading":1.70809, "vx":-1.00831, "vy":2.7922, "omega":-1.59837, "ax":-0.75905, "ay":2.85006, "alpha":9.60724, "fx":[-73.21813,57.97615,21.58,-56.53718], "fy":[-0.05548,19.66925,91.69537,77.17595]}, + {"t":2.05629, "x":6.02528, "y":2.4687, "heading":1.68519, "vx":-1.01919, "vy":2.83303, "omega":-1.46073, "ax":-0.47284, "ay":3.24191, "alpha":10.03961, "fx":[-76.73264,69.60767,28.6536,-52.79933], "fy":[7.69079,25.30168,95.94183,85.46515]}, + {"t":2.07062, "x":6.01063, "y":2.50962, "heading":1.66427, "vx":-1.02596, "vy":2.87947, "omega":-1.3169, "ax":-0.14184, "ay":3.56348, "alpha":10.30917, "fx":[-77.95832,79.96809,36.13458,-47.52496], "fy":[16.07568,28.63963,98.25692,92.69381]}, + {"t":2.08494, "x":5.99592, "y":2.55124, "heading":1.6454, "vx":-1.02799, "vy":2.93052, "omega":-1.16921, "ax":0.21219, "ay":3.81517, "alpha":10.44876, "fx":[-77.09891,88.76251,43.64955,-41.28023], "fy":[24.73421,29.63604,99.07575,98.86558]}, + {"t":2.09927, "x":5.98121, "y":2.59361, "heading":1.62865, "vx":-1.02495, "vy":2.98518, "omega":-1.01952, "ax":0.57802, "ay":4.00133, "alpha":10.48428, "fx":[-74.24158,96.05919,50.93469,-34.52551], "fy":[33.25479,28.63674,98.73952,103.99224]}, + {"t":2.1136, "x":5.96659, "y":2.63679, "heading":1.61404, "vx":-1.01667, "vy":3.0425, "omega":-0.86932, "ax":0.94963, "ay":4.1274, "alpha":10.43616, "fx":[-69.3934,102.03191,57.80567,-27.64177], "fy":[41.18521,26.11371,97.53581,108.12569]}, + {"t":2.12792, "x":5.95212, "y":2.6808, "heading":1.60159, "vx":-1.00307, "vy":3.10163, "omega":-0.71981, "ax":1.32455, "ay":4.19855, "alpha":10.31945, "fx":[-62.44847,106.85424,64.13211,-20.94031], "fy":[48.03808,22.54603,95.72097,111.36101]}, + {"t":2.14225, "x":5.93789, "y":2.72567, "heading":1.59128, "vx":-0.98409, "vy":3.16178, "omega":-0.57197, "ax":1.70442, "ay":4.21907, "alpha":10.14318, "fx":[-53.09161,110.67748,69.81251,-14.67835], "fy":[53.26534,18.39303,93.53887,113.82606]}, + {"t":2.15658, "x":5.92396, "y":2.7714, "heading":1.58308, "vx":-0.95967, "vy":3.22222, "omega":-0.42666, "ax":2.09486, "ay":4.19192, "alpha":9.91179, "fx":[-40.74169,113.6382,74.74242,-9.09783], "fy":[56.1939,14.11497,91.24903,115.66965]}, + {"t":2.1709, "x":5.91043, "y":2.81799, "heading":1.57697, "vx":-0.92966, "vy":3.28228, "omega":-0.28466, "ax":2.48922, "ay":4.11774, "alpha":9.65712, "fx":[-25.47389,115.87921,78.76813,-4.55217], "fy":[55.90967,10.17592,89.17795,117.05824]}, + {"t":2.18523, "x":5.89737, "y":2.86543, "heading":1.57289, "vx":-0.894, "vy":3.34127, "omega":-0.14631, "ax":2.67304, "ay":3.75761, "alpha":10.21287, "fx":[-20.53228,117.74585,82.12173,-2.55695], "fy":[38.16329,4.8002,87.36322,118.17817]}, + {"t":2.19955, "x":5.88483, "y":2.91369, "heading":1.5708, "vx":-0.85571, "vy":3.3951, "omega":0.0, "ax":7.00621, "ay":1.65929, "alpha":0.0, "fx":[115.83685,115.83685,115.83685,115.83685], "fy":[27.43382,27.43382,27.43382,27.43382]}, + {"t":2.23537, "x":5.85868, "y":3.03633, "heading":1.5708, "vx":-0.6048, "vy":3.45452, "omega":0.0, "ax":6.78747, "ay":0.94806, "alpha":0.0, "fx":[112.22023,112.22023,112.22023,112.22023], "fy":[15.67473,15.67473,15.67473,15.67473]}, + {"t":2.27118, "x":5.84138, "y":3.16065, "heading":1.5708, "vx":-0.36174, "vy":3.48847, "omega":0.0, "ax":5.77207, "ay":0.42765, "alpha":0.0, "fx":[95.43223,95.43223,95.43223,95.43223], "fy":[7.07058,7.07058,7.07058,7.07058]}, + {"t":2.30699, "x":5.83212, "y":3.28585, "heading":1.5708, "vx":-0.15503, "vy":3.50379, "omega":0.0, "ax":3.01358, "ay":0.08698, "alpha":0.0, "fx":[49.82481,49.82481,49.82481,49.82481], "fy":[1.43811,1.43811,1.43811,1.43811]}, + {"t":2.3428, "x":5.8285, "y":3.41138, "heading":1.5708, "vx":-0.04711, "vy":3.5069, "omega":0.0, "ax":0.92385, "ay":-0.39529, "alpha":0.0, "fx":[15.27442,15.27442,15.27442,15.27442], "fy":[-6.53547,-6.53547,-6.53547,-6.53547]}, + {"t":2.37861, "x":5.82741, "y":3.53672, "heading":1.5708, "vx":-0.01403, "vy":3.49275, "omega":0.0, "ax":0.05522, "ay":-7.39328, "alpha":0.0, "fx":[0.91305,0.91305,0.91305,0.91305], "fy":[-122.23636,-122.23636,-122.23636,-122.23636]}, + {"t":2.41442, "x":5.82694, "y":3.65706, "heading":1.5708, "vx":-0.01205, "vy":3.22799, "omega":0.0, "ax":0.03746, "ay":-7.47072, "alpha":0.0, "fx":[0.61931,0.61931,0.61931,0.61931], "fy":[-123.51664,-123.51664,-123.51664,-123.51664]}, + {"t":2.45023, "x":5.82653, "y":3.76786, "heading":1.5708, "vx":-0.01071, "vy":2.96045, "omega":0.0, "ax":0.03232, "ay":-7.49303, "alpha":0.0, "fx":[0.5343,0.5343,0.5343,0.5343], "fy":[-123.88555,-123.88555,-123.88555,-123.88555]}, + {"t":2.48604, "x":5.82617, "y":3.86908, "heading":1.5708, "vx":-0.00955, "vy":2.69212, "omega":0.0, "ax":0.02988, "ay":-7.50359, "alpha":0.0, "fx":[0.49402,0.49402,0.49402,0.49402], "fy":[-124.0602,-124.0602,-124.0602,-124.0602]}, + {"t":2.52186, "x":5.82585, "y":3.96067, "heading":1.5708, "vx":-0.00848, "vy":2.4234, "omega":0.0, "ax":0.02846, "ay":-7.50975, "alpha":0.0, "fx":[0.47054,0.47054,0.47054,0.47054], "fy":[-124.16196,-124.16196,-124.16196,-124.16196]}, + {"t":2.55767, "x":5.82556, "y":4.04264, "heading":1.5708, "vx":-0.00746, "vy":2.15447, "omega":0.0, "ax":0.02753, "ay":-7.51377, "alpha":0.0, "fx":[0.45517,0.45517,0.45517,0.45517], "fy":[-124.22855,-124.22855,-124.22855,-124.22855]}, + {"t":2.59348, "x":5.82531, "y":4.11498, "heading":1.5708, "vx":-0.00648, "vy":1.88539, "omega":0.0, "ax":0.02687, "ay":-7.51661, "alpha":0.0, "fx":[0.44433,0.44433,0.44433,0.44433], "fy":[-124.27551,-124.27551,-124.27551,-124.27551]}, + {"t":2.62929, "x":5.8251, "y":4.17768, "heading":1.5708, "vx":-0.00551, "vy":1.61621, "omega":0.0, "ax":0.02639, "ay":-7.51872, "alpha":0.0, "fx":[0.43628,0.43628,0.43628,0.43628], "fy":[-124.3104,-124.3104,-124.3104,-124.3104]}, + {"t":2.6651, "x":5.82492, "y":4.23074, "heading":1.5708, "vx":-0.00457, "vy":1.34696, "omega":0.0, "ax":0.02601, "ay":-7.52035, "alpha":0.0, "fx":[0.43006,0.43006,0.43006,0.43006], "fy":[-124.33734,-124.33734,-124.33734,-124.33734]}, + {"t":2.70091, "x":5.82477, "y":4.27415, "heading":1.5708, "vx":-0.00364, "vy":1.07764, "omega":0.0, "ax":0.02571, "ay":-7.52165, "alpha":0.0, "fx":[0.42512,0.42512,0.42512,0.42512], "fy":[-124.35877,-124.35877,-124.35877,-124.35877]}, + {"t":2.73672, "x":5.82466, "y":4.30792, "heading":1.5708, "vx":-0.00272, "vy":0.80828, "omega":0.0, "ax":0.02547, "ay":-7.52271, "alpha":0.0, "fx":[0.42109,0.42109,0.42109,0.42109], "fy":[-124.37623,-124.37623,-124.37623,-124.37623]}, + {"t":2.77253, "x":5.82458, "y":4.33204, "heading":1.5708, "vx":-0.0018, "vy":0.53888, "omega":0.0, "ax":0.02527, "ay":-7.52358, "alpha":0.0, "fx":[0.41775,0.41775,0.41775,0.41775], "fy":[-124.39072,-124.39072,-124.39072,-124.39072]}, + {"t":2.80835, "x":5.82453, "y":4.34651, "heading":1.5708, "vx":-0.0009, "vy":0.26946, "omega":0.0, "ax":0.0251, "ay":-7.52432, "alpha":0.0, "fx":[0.41493,0.41493,0.41493,0.41493], "fy":[-124.40294,-124.40294,-124.40294,-124.40294]}, + {"t":2.84416, "x":5.82451, "y":4.35134, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 06ef6666..073a90a0 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -111,6 +111,9 @@ public enum Path { RPreTrenchtoRNeutral("RPreTrenchtoRNeutral", Action.INTAKE), RPreTrenchReversedtoRNeutral("RPreTrenchReversedtoRNeutral", Action.INTAKE), + RPreTrenchReversedtoRCleanup("RPreTrenchReversedtoRCleanup", Action.INTAKE), + EndWScoreRCleanuptoRPreTrenchReversed("RCleanuptoRPreTrenchReversed", Action.SCORE_AT_END), + StartingRTrenchtoRNeutral("StartingRTrenchtoRNeutral", Action.INTAKE), StartingLTrenchtoLNeutral("StartingLTrenchtoLNeutral", Action.INTAKE), StartingRTrenchtoDisrupt("StartingRTrenchtoDisruptR", Action.INTAKE), @@ -559,8 +562,10 @@ public Command getDoubleDipRightTrench() { new Path[] { Path.StartingRTrenchtoRNeutral, Path.RNeutraltoRPreTrenchReversed, - Path.RPreTrenchReversedtoRNeutral, - Path.RNeutraltoRPreTrenchReversed, + Path.RPreTrenchReversedtoRCleanup, + Path.EndWScoreRCleanuptoRPreTrenchReversed + // Path.RPreTrenchReversedtoRNeutral, + // Path.RNeutraltoRPreTrenchReversed, }, setRightClimb()); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f4e521a8..c24a0ef1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -72,7 +72,6 @@ import frc.robot.utils.FieldUtils.ClimbTargets; import frc.robot.utils.FieldUtils.FeedTargets; import frc.robot.utils.FieldUtils.TrenchPoses; -import frc.robot.utils.FuelSim; import frc.robot.utils.autoaim.AutoAim; import java.io.File; import java.util.Arrays; @@ -152,7 +151,7 @@ public enum RobotEdition { * This is for when we're testing shot and extension numbers and should be FALSE once bring up is * complete */ - public static final boolean TUNING_MODE = true; + public static final boolean TUNING_MODE = false; public boolean hasZeroedSinceStartup = false; @@ -185,7 +184,7 @@ public enum RobotEdition { private final SlewRateLimiter xAccelLimiter = new SlewRateLimiter(1); private final SlewRateLimiter yAccelLimiter = new SlewRateLimiter(1); - private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(0.5); + // private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(0.5); private static int lowBatteryCycleCount = 0; private static final double lowBatteryVoltage = @@ -216,8 +215,6 @@ public enum RobotEdition { private final CANdleSubsystem candle = new CANdleSubsystem(new CANdleIOReal(0, CANdleSubsystem.getCandleConfig(), canivore)); - private FuelSim fuelSim = new FuelSim(); - // climber only exists for the comp bot - this is accounted for later private final Superstructure superstructure; @@ -442,8 +439,7 @@ public Robot() { : new CANcoderIOSim(5, TurretSubsystem.getCancoder24tConfigs(), canivore), ROBOT_MODE == RobotMode.REAL ? new CANcoderIO(4, TurretSubsystem.getCancoder26tConfigs(), canivore) - : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore), - fuelSim); + : new CANcoderIOSim(4, TurretSubsystem.getCancoder26tConfigs(), canivore)); break; } climber = @@ -604,7 +600,9 @@ public Robot() { * (SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed()), xAccelLimiter.calculate(modifyJoystick(driver.getLeftX())) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - rAccelLimiter.calculate(modifyJoystick(driver.getRightX())) + // rAccelLimiter.calculate( + modifyJoystick(driver.getRightX()) + // ) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()) .times(-1)) .withName("default")); @@ -627,31 +625,6 @@ public Robot() { + (interrupting.isPresent() ? interrupting.get().getName() : "none")); }); - // fuelSim.spawnStartingFuel(); - - fuelSim.registerRobot( - Units.inchesToMeters(28), // from left to right in meters - Units.inchesToMeters(28), // from front to back in meters - Units.inchesToMeters(4), // from floor to top of bumpers in meters - swerve::getPose, // Supplier of robot pose - swerve - ::getVelocityFieldRelative); // Supplier of field-centric chassis speeds - - fuelSim.registerIntake( - Units.inchesToMeters(-14), - Units.inchesToMeters(14), - Units.inchesToMeters(14), - Units.inchesToMeters(20), // robot-centric coordinates for bounding box in meters - () -> - Superstructure.getState() - .isAnIntakeState() // (optional) BooleanSupplier for whether the intake should be - // active at a given moment - ); // (optional) Runnable called whenever a fuel is intaked - - fuelSim.setSubticks(5); - - // fuelSim.start(); - // Log climb poses Logger.recordOutput( "AutoAlign/Climb Targets", @@ -742,12 +715,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta .rightBumper() .or(Autos.autoLeftClimbReq.negate()) .onTrue(Commands.runOnce(() -> leftClimbTarget = false)); - operator - .rightStick() - .onTrue( - shooter - .resetTurretToPosition(shooter::getCalculatedTurretRotations) - .ignoringDisable(true)); + operator.rightStick().onTrue(shooter.resetTurretToCalculatedPosition().ignoringDisable(true)); driver .rightBumper() @@ -822,6 +790,16 @@ private void addAutos() { System.out.println("------- Regenerating Autos"); System.out.println( "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); + + autoChooser.addOption("Right Trench Double Dip", autos.getDoubleDipRightTrench()); + autoChooser.addOption("Left Trench Double Dip", autos.getLeftNeutralScoreTwice()); + + autoChooser.addOption("Left Bump Double Dip", autos.getLeftBumpDoubleDipAuto()); + autoChooser.addOption("Right Bump Double Dip", autos.getRightBumpDoubleDipAuto()); + + autoChooser.addOption("Hub Depot Outpost", autos.getHubDepotOutpostAuto()); + autoChooser.addOption("Hub Outpost Depot", autos.getHubOutpostDepotAuto()); + autoChooser.addOption("Depot Feed Climb", autos.getDepotFeedClimbAuto()); autoChooser.addOption("Depot Score Climb", autos.getDepotScoreClimbAuto()); autoChooser.addOption("Outpost Feed Climb", autos.getOutpostFeedClimbAuto()); @@ -838,27 +816,13 @@ private void addAutos() { "Left Bump Depot Outpost Climb", autos.getLeftBumpDepotOutpostClimbAuto()); autoChooser.addOption("Right Bump Outpost Climb", autos.getRightBumpOutpostClimbAuto()); autoChooser.addOption("Right Bump Outpost Center", autos.getRightBumpOutpostCenterAuto()); - autoChooser.addOption("Right Trench Double Dip Auto", autos.getDoubleDipRightTrench()); + autoChooser.addOption( "Right Trench Disrupt Outpost Auto", autos.getDisruptOutpostRightTrench()); autoChooser.addOption("Left Trench Disrupt Depot Auto", autos.getDisruptDepotLeftTrench()); - autoChooser.addOption("Left Neutral Score Twice", autos.getLeftNeutralScoreTwice()); // autoChooser.addOption("Left Neutral Outpost Score", autos.getLeftNeutralOutpostScore()); - autoChooser.addOption("Hub Depot Outpost", autos.getHubDepotOutpostAuto()); - autoChooser.addOption("Hub Outpost Depot", autos.getHubOutpostDepotAuto()); - - autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); - autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); - autoChooser.addOption("X60 Sysid", indexer.runX60Sysid()); - - autoChooser.addOption("X44 Sysid", indexer.runX44Sysid()); autoChooser.addOption("Right Neutral Outpost Score", autos.getRightNeutralOutpostScore()); - autoChooser.addOption("Left Double Dip Bump", autos.getLeftBumpDoubleDipAuto()); - autoChooser.addOption("Right Double Dip Bump", autos.getRightBumpDoubleDipAuto()); - - autoChooser.addOption("spin", spinTest()); - haveAutosGenerated = true; System.out.println("Done generating autos"); } @@ -869,96 +833,96 @@ public void robotPeriodic() { superstructure.periodic(); - Pose3d turretPose = - new Pose3d( - new Translation3d(-0.177413, -0.111702, 0.350341), - new Rotation3d(0, 0, shooter.getTurretPosition().getRadians())); - Logger.recordOutput( - "Robot/Mechanism Poses", - new Pose3d[] { - // Turret - turretPose, - // Hood - turretPose - // First transform the hood out to the hood pivot, and rotate by the amount needed - .transformBy( - new Transform3d( - new Translation3d(-0.095638, 0, 0.095123), - new Rotation3d(0, shooter.getHoodPosition().getRadians() * 1, 0))) - // Then, transform the hood back to the correct location relative to the turret - .transformBy( - new Transform3d( - new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), - // Intake - // new Pose3d( - // intake.getPosition() * LintakeSubsystem.INTAKE_ROTATION.getCos(), - // 0, - // -(intake.getPosition() * LintakeSubsystem.INTAKE_ROTATION.getSin()), - // Rotation3d.kZero), - // Climber - new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) - }); - - Pose3d turretSetpoint = - new Pose3d( - new Translation3d(-0.177413, -0.111702, 0.350341), - new Rotation3d(0, 0, shooter.getTurretSetpoint().getRadians())); - - Logger.recordOutput( - "Robot/Mechanism Setpoints", - new Pose3d[] { - // Turret - turretSetpoint, - // Hood - turretSetpoint - // First transform the hood out to the hood pivot, and rotate by the amount needed - .transformBy( - new Transform3d( - new Translation3d(-0.095638, 0, 0.095123), - new Rotation3d(0, shooter.getHoodSetpoint().getRadians() * -1, 0))) - // Then, transform the hood back to the correct location relative to the turret - .transformBy( - new Transform3d( - new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), - // Intake - // new Pose3d( - // intake.getPositionSetpoint() * LintakeSubsystem.INTAKE_ROTATION.getCos(), - // 0, - // -(intake.getPositionSetpoint() * LintakeSubsystem.INTAKE_ROTATION.getSin()), - // Rotation3d.kZero), - // Climber - new Pose3d(0, 0, climber.getClimberSetpointMeters(), Rotation3d.kZero) - }); + // Pose3d turretPose = + // new Pose3d( + // new Translation3d(-0.177413, -0.111702, 0.350341), + // new Rotation3d(0, 0, shooter.getTurretPosition().getRadians())); + // Logger.recordOutput( + // "Robot/Mechanism Poses", + // new Pose3d[] { + // // Turret + // turretPose, + // // Hood + // turretPose + // // First transform the hood out to the hood pivot, and rotate by the amount needed + // .transformBy( + // new Transform3d( + // new Translation3d(-0.095638, 0, 0.095123), + // new Rotation3d(0, shooter.getHoodPosition().getRadians() * 1, 0))) + // // Then, transform the hood back to the correct location relative to the turret + // .transformBy( + // new Transform3d( + // new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), + // // Intake + // // new Pose3d( + // // intake.getPosition() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + // // 0, + // // -(intake.getPosition() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + // // Rotation3d.kZero), + // // Climber + // new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) + // }); + + // Pose3d turretSetpoint = + // new Pose3d( + // new Translation3d(-0.177413, -0.111702, 0.350341), + // new Rotation3d(0, 0, shooter.getTurretSetpoint().getRadians())); + + // Logger.recordOutput( + // "Robot/Mechanism Setpoints", + // new Pose3d[] { + // // Turret + // turretSetpoint, + // // Hood + // turretSetpoint + // // First transform the hood out to the hood pivot, and rotate by the amount needed + // .transformBy( + // new Transform3d( + // new Translation3d(-0.095638, 0, 0.095123), + // new Rotation3d(0, shooter.getHoodSetpoint().getRadians() * -1, 0))) + // // Then, transform the hood back to the correct location relative to the turret + // .transformBy( + // new Transform3d( + // new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), + // // Intake + // // new Pose3d( + // // intake.getPositionSetpoint() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + // // 0, + // // -(intake.getPositionSetpoint() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + // // Rotation3d.kZero), + // // Climber + // new Pose3d(0, 0, climber.getClimberSetpointMeters(), Rotation3d.kZero) + // }); updateAlerts(); - Logger.recordOutput("AutoAim/Flywheel Fudge Factor", AutoAim.getFudgeFactor()); + // Logger.recordOutput("AutoAim/Flywheel Fudge Factor", AutoAim.getFudgeFactor()); - Logger.recordOutput( - "trench poses", - Arrays.stream(TrenchPoses.values()).map(target -> target.getPose()).toArray(Pose2d[]::new)); + // Logger.recordOutput( + // "trench poses", + // Arrays.stream(TrenchPoses.values()).map(target -> target.getPose()).toArray(Pose2d[]::new)); Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone()); noLogStickAlert.set(!directory.exists()); - Logger.recordOutput( - "AutoAim/Distance to hub", - shooter - .getTurretPose(swerve.getPose()) - .getTranslation() - .getDistance(FieldUtils.getCurrentHubTranslation())); - - Logger.recordOutput( - "AutoAim/Distance to feed target", - shooter - .getTurretPose(swerve.getPose()) - .getTranslation() - .getDistance( - FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getTranslation())); + // Logger.recordOutput( + // "AutoAim/Distance to hub", + // shooter + // .getTurretPose(swerve.getPose()) + // .getTranslation() + // .getDistance(FieldUtils.getCurrentHubTranslation())); + + // Logger.recordOutput( + // "AutoAim/Distance to feed target", + // shooter + // .getTurretPose(swerve.getPose()) + // .getTranslation() + // .getDistance( + // FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getTranslation())); Logger.recordOutput( "AutoAim/Feed Target", FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose()); - Logger.recordOutput("Wrapped gyro yaw", swerve.getRotation()); + // Logger.recordOutput("Wrapped gyro yaw", swerve.getRotation()); } public void updateAlerts() { @@ -978,28 +942,28 @@ public void updateAlerts() { && !canInitialErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD)); // Log CANivore status - if (Robot.isReal()) { - var canivoreStatus = - Optional.of(canivore.getStatus()); // TODO i don't know if i'm doing the optionaling right - if (canivoreStatus.isPresent()) { - Logger.recordOutput("CANivoreStatus/Status", canivoreStatus.get().Status.getName()); - Logger.recordOutput("CANivoreStatus/Utilization", canivoreStatus.get().BusUtilization); - Logger.recordOutput("CANivoreStatus/OffCount", canivoreStatus.get().BusOffCount); - Logger.recordOutput("CANivoreStatus/TxFullCount", canivoreStatus.get().TxFullCount); - Logger.recordOutput("CANivoreStatus/ReceiveErrorCount", canivoreStatus.get().REC); - Logger.recordOutput("CANivoreStatus/TransmitErrorCount", canivoreStatus.get().TEC); - - if (!canivoreStatus.get().Status.isOK() - || canStatus.transmitErrorCount > 0 - || canStatus.receiveErrorCount > 0) { - canivoreErrorTimer.restart(); - } - } - // TODO i don't really like how this doesn't seem to be sticky - canivoreErrorAlert.set( - !canivoreErrorTimer.hasElapsed(CANIVORE_ERROR_TIME_THRESHOLD) - && !canInitialErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD)); - } + // if (Robot.isReal()) { + // var canivoreStatus = + // Optional.of(canivore.getStatus()); // TODO i don't know if i'm doing the optionaling right + // if (canivoreStatus.isPresent()) { + // Logger.recordOutput("CANivoreStatus/Status", canivoreStatus.get().Status.getName()); + // Logger.recordOutput("CANivoreStatus/Utilization", canivoreStatus.get().BusUtilization); + // Logger.recordOutput("CANivoreStatus/OffCount", canivoreStatus.get().BusOffCount); + // Logger.recordOutput("CANivoreStatus/TxFullCount", canivoreStatus.get().TxFullCount); + // Logger.recordOutput("CANivoreStatus/ReceiveErrorCount", canivoreStatus.get().REC); + // Logger.recordOutput("CANivoreStatus/TransmitErrorCount", canivoreStatus.get().TEC); + + // if (!canivoreStatus.get().Status.isOK() + // || canStatus.transmitErrorCount > 0 + // || canStatus.receiveErrorCount > 0) { + // canivoreErrorTimer.restart(); + // } + // } + // // TODO i don't really like how this doesn't seem to be sticky + // canivoreErrorAlert.set( + // !canivoreErrorTimer.hasElapsed(CANIVORE_ERROR_TIME_THRESHOLD) + // && !canInitialErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD)); + // } // Low battery alert lowBatteryCycleCount += 1; @@ -1011,7 +975,7 @@ public void updateAlerts() { && lowBatteryCycleCount >= lowBatteryMinCycleCount) { lowBatteryAlert.set(true); } - logBFG(); + // logBFG(); } private void logBFG() { @@ -1029,7 +993,6 @@ public void simulationInit() { @Override public void simulationPeriodic() { - // fuelSim.updateSim(); // Log zeroed poses for mechs and robot for debugging in sim Logger.recordOutput( "Robot/Zeroed Mechanism Poses", @@ -1098,23 +1061,23 @@ public void testPeriodic() {} @Override public void testExit() {} - public Command spinTest() { - return Commands.sequence( - Commands.runOnce(() -> swerve.setGyroYaw(Rotation2d.kZero)), - swerve - .driveOpenLoopFieldRelative( - () -> - new ChassisSpeeds( - 0, 0, SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed() / 4.0)) - .withTimeout(20) - // .until() - // , - // // swerve - // // .driveOpenLoopFieldRelative( - // // () -> - // // new ChassisSpeeds(0, 0, - // -SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed())) - // // .withTimeout(10) - ); - } +// public Command spinTest() { +// return Commands.sequence( +// Commands.runOnce(() -> swerve.setGyroYaw(Rotation2d.kZero)), +// swerve +// .driveOpenLoopFieldRelative( +// () -> +// new ChassisSpeeds( +// 0, 0, SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed() / 4.0)) +// .withTimeout(20) +// // .until() +// // , +// // // swerve +// // // .driveOpenLoopFieldRelative( +// // // () -> +// // // new ChassisSpeeds(0, 0, +// // -SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed())) +// // // .withTimeout(10) +// ); +// } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 0eeefcb4..ff5d901c 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -171,9 +171,7 @@ public boolean isScoringActive() { @AutoLogOutput(key = "Superstructure/Score Request") private Trigger scoreReq = - new Trigger(() -> shotTarget == ShotTarget.SCORE) - // .and(() -> canScore()) - .or(Autos.autoScoreReq); + new Trigger(() -> shotTarget == ShotTarget.SCORE).or(Autos.autoScoreReq); @AutoLogOutput(key = "Superstructure/Feed Request") private Trigger feedReq = new Trigger(() -> shotTarget == ShotTarget.FEED); @@ -193,9 +191,6 @@ public boolean isScoringActive() { @AutoLogOutput(key = "Superstructure/Ready?") private Trigger readyTrigger; - @AutoLogOutput(key = "Superstructure/Operator Pose Override?") - private static boolean poseOverride = false; - @AutoLogOutput(key = "Superstructure/Defense?") private boolean defense = false; @@ -290,13 +285,8 @@ private void addTriggers() { operator.povUp().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret())); operator.povDown().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret())); shootReq = - driver - .rightTrigger() - .and(DriverStation::isTeleop) - .and(() -> canShoot()) - .or(Autos.autoScoreReq) - .and(() -> canShoot()); // Maybe should include if its our turn? //TODO fix auto - // bindings + new Trigger(() -> awayFromTrench()) + .and((driver.rightTrigger().and(DriverStation::isTeleop)).or(Autos.autoScoreReq)); intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); @@ -337,9 +327,7 @@ private void addTransitions() { // SCORE_FLOW transitions { bindTransition( - SuperState.IDLE, - SuperState.SPIN_UP_SCORE_FLOW, - scoreReq.and(flowReq).and(shootReq.or(intakeReq))); + SuperState.IDLE, SuperState.SPIN_UP_SCORE_FLOW, scoreReq.and(flowReq).and(shootReq)); bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.SCORE_FLOW, readyTrigger); @@ -348,17 +336,13 @@ private void addTransitions() { SuperState.SPIN_UP_SCORE_FLOW, new Trigger(AutoAim::targetInTurretDeadzone)); - bindTransition( - SuperState.SPIN_UP_SCORE_FLOW, - SuperState.IDLE, - intakeReq.negate().and(shootReq.negate())); + bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.IDLE, shootReq.negate()); bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate()); - bindTransition( - SuperState.SCORE_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate())); + bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, shootReq.negate()); } // -------------------------------------------------------------------------- @@ -377,9 +361,7 @@ private void addTransitions() { // FEED_FLOW transitions { bindTransition( - SuperState.IDLE, - SuperState.SPIN_UP_FEED_FLOW, - feedReq.and(flowReq).and(shootReq.or(intakeReq))); + SuperState.IDLE, SuperState.SPIN_UP_FEED_FLOW, feedReq.and(flowReq).and(shootReq)); bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.FEED_FLOW, readyTrigger); @@ -388,15 +370,13 @@ private void addTransitions() { SuperState.SPIN_UP_FEED_FLOW, new Trigger(AutoAim::targetInTurretDeadzone)); - bindTransition( - SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate())); + bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, shootReq.negate()); bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate()); - bindTransition( - SuperState.FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate())); + bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, shootReq.negate()); } bindTransition(SuperState.SCORE, SuperState.FEED, feedReq); @@ -487,13 +467,16 @@ private void addCommands() { SuperState.SPIN_UP_FEED, intake.restExtended(), indexer.rest(), - shooter.feed( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FeedTargets.getFeedTarget(feedTarget).getTranslation(), - ShotTrees.FEED_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.feed( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), + ShotTrees.FEED_SHOT_TREE))), climber.retract()); bindCommands( @@ -521,13 +504,16 @@ private void addCommands() { SuperState.SPIN_UP_FEED_FLOW, intake.intake(), indexer.index(), - shooter.feed( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FeedTargets.getFeedTarget(feedTarget).getTranslation(), - ShotTrees.FEED_SHOT_TREE)), + shooter + .resetTurretToCalculatedPosition() + .andThen( + shooter.feed( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), + ShotTrees.FEED_SHOT_TREE))), climber.retract()); bindCommands( @@ -587,18 +573,15 @@ private void addCommands() { .flywheelVelocityRotPerSec() // shooter.getTestVel() ), - shooter - .resetTurretToCalculatedPosition() - .andThen( - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE))), + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), // shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())), climber.retract()); @@ -634,18 +617,15 @@ private void addCommands() { : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), - shooter - .resetTurretToCalculatedPosition() - .andThen( - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE))), + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.retract()); bindCommands( @@ -685,18 +665,15 @@ private void addCommands() { SuperState.SPIN_UP_SCORE_PRE_CLIMB, intake.restRetracted(), indexer.rest(), - shooter - .resetTurretToCalculatedPosition() - .andThen( - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE))), + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.extend()); bindCommands( @@ -713,18 +690,15 @@ private void addCommands() { : ShotTrees.COMP_HUB_SHOT_TREE) .shotData() .flywheelVelocityRotPerSec()), - shooter - .resetTurretToCalculatedPosition() - .andThen( - shooter.score( - () -> - AutoAim.getShotParameters( - swerve.getPose(), - swerve.getVelocityRobotRelative(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE))), + shooter.score( + () -> + AutoAim.getShotParameters( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? ShotTrees.ALPHA_HUB_SHOT_TREE + : ShotTrees.COMP_HUB_SHOT_TREE)), climber.extend()); bindCommands( @@ -740,14 +714,14 @@ public void periodic() { Logger.recordOutput("Superstructure/State Timer", stateTimer.get()); // this really should be in robot.java but i cooked myself with the robot selecting thing - Logger.recordOutput( - "shooter sotm viz", - new Pose3d(swerve.getPose()) - .transformBy( - new Transform3d( - new Translation3d(0, 0, 0.5), - new Rotation3d( - 0, ((Math.PI / 2) - shooter.getHoodSetpoint().getRadians()) * -1, 0)))); + // Logger.recordOutput( + // "shooter sotm viz", + // new Pose3d(swerve.getPose()) + // .transformBy( + // new Transform3d( + // new Translation3d(0, 0, 0.5), + // new Rotation3d( + // 0, ((Math.PI / 2) - shooter.getHoodSetpoint().getRadians()) * -1, 0)))); } /** @@ -918,16 +892,10 @@ public boolean inScoringArea() { } public boolean canScore() { - return - // (isOurShift() || !DriverStation.isFMSAttached()) - // && - (inScoringArea() || poseOverride) - && (!swerve.isNearTrench() || poseOverride - // || fixedShotTarget != FixedShotTarget.NONE - ); + return inScoringArea() && !swerve.isNearTrench(); } - public boolean canShoot() { + public boolean awayFromTrench() { return !swerve.isNearTrenchForHood(); } @@ -938,12 +906,4 @@ public static ShotTarget getShotTarget() { public static FeedTarget getFeedTarget() { return feedTarget; } - - // public static FixedShotTarget getFixedShotTarget() { - // return fixedShotTarget; - // } - - public static boolean getPoseOverride() { - return poseOverride; - } } diff --git a/src/main/java/frc/robot/components/camera/Camera.java b/src/main/java/frc/robot/components/camera/Camera.java index dc58d345..14dd413f 100644 --- a/src/main/java/frc/robot/components/camera/Camera.java +++ b/src/main/java/frc/robot/components/camera/Camera.java @@ -106,10 +106,10 @@ public Optional update(PhotonPipelineResult result) { if (result.getTargets().size() < 1) { return Optional.empty(); } - if (Robot.ROBOT_MODE != RobotMode.REAL) - Logger.recordOutput( - "Vision/" + io.getName() + "/Best Distance", - result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); + // if (Robot.ROBOT_MODE != RobotMode.REAL) + // Logger.recordOutput( + // "Vision/" + io.getName() + "/Best Distance", + // result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); Optional estPose = estimator.update(result); return estPose; } @@ -163,12 +163,12 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) { setSimPose(estPose, !inputs.stale); // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + getName() + "/Pose3d", visionPose); + // Logger.recordOutput("Vision/" + getName() + "/Pose3d", visionPose); // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + getName() + "/Pose2d", visionPose.toPose2d()); + // Logger.recordOutput("Vision/" + getName() + "/Pose2d", visionPose.toPose2d()); final Matrix deviations = findVisionMeasurementStdDevs(estPose.get()); // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData()); + // Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData()); Tracer.trace( "Add Measurement From " + getName(), @@ -182,7 +182,7 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) { hasFutureData |= inputs.result.metadata.captureTimestampMicros > RobotController.getTime(); // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update"); + // Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update"); Tracer.trace( "Log Tag Poses", @@ -196,16 +196,16 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) { .get(); } // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds); + // Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds); }); } else { // if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Stale"); + // 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"); + // Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Bad Estimate"); } futureVisionData.set(hasFutureData); } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index a42e8ac9..ba2ed188 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; -import frc.robot.utils.LoggedTunableNumber; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -65,12 +64,6 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO - private LoggedTunableNumber x44KickerSpeedLTN = new LoggedTunableNumber("Kicker Speed", 49); - - private LoggedTunableNumber spinnerSpeedLTN = new LoggedTunableNumber("Spinner Speed", 12); - - private LoggedTunableNumber x60KickSpeedLTN = new LoggedTunableNumber("X60 Kick Speed", 30); - private SysIdRoutine x60Sysid = new SysIdRoutine( new Config( diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 9a018535..a525a481 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -39,12 +39,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Robot; import frc.robot.Robot.RobotEdition; -import frc.robot.Superstructure; import frc.robot.components.cancoder.CANcoderIO; import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged; import frc.robot.utils.FieldUtils; -import frc.robot.utils.FuelSim; -import frc.robot.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.AutoAim.ShotParams; import frc.robot.utils.autoaim.ShotTrees; @@ -79,9 +76,6 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { public static final double CANCODER_24T_TO_TURRET_GEAR_RATIO = (24.0 / 32.0) * (10.0 / 85.0); public static final double CANCODER_26T_TO_TURRET_GEAR_RATIO = (26.0 / 32.0) * (10.0 / 85.0); - private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Hood Degrees", 40); - private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Flywheel RPS", 40); - // TODO: REDO THIS HARDSTOP WHEN FIXED?? // logged for ease of graph viewing @AutoLogOutput(key = "Shooter/Turret/Left Hardstop") @@ -156,21 +150,21 @@ public class TurretSubsystem extends SubsystemBase implements Shooter { "Turret may have gone past hardstop!! Reoffset cancoders + min/max position", AlertType.kError); - private FuelSim fuelSim; + private final Alert turretDriftAlert = + new Alert("Turret drifting detected (>10 deg)", AlertType.kWarning); + public TurretSubsystem( FlywheelIO flywheelIO, HoodIO hoodIO, TurretIO turretIO, CANcoderIO cancoder24t, - CANcoderIO cancoder26t, - FuelSim fuelSim) { + CANcoderIO cancoder26t) { this.flywheelIO = flywheelIO; this.hoodIO = hoodIO; this.turretIO = turretIO; this.cancoder24t = cancoder24t; this.cancoder26t = cancoder26t; - this.fuelSim = fuelSim; // assume we start up at min angle and not 0 hoodIO.resetEncoder(HOOD_MIN_ANGLE); @@ -223,20 +217,13 @@ public void periodic() { // < TurretSubsystem.TURRET_REAR_HARDSTOP_ANGLE.getDegrees())); // if (pastHardstop) turretPastHardstopAlert.set(pastHardstop); // sticky alert - if (Superstructure.getState().isAScoreState() && Robot.isSimulation()) { - System.out.println("launching fuel"); - fuelSim.launchFuel( - // there are few things i despise more than the units library\ - // InchesPerSecond.of( - // flywheelIO.getSetpointRotPerSec() * FLYWHEEL_DIAMETER_INCHES * Math.PI), - // InchesPerSecond.of(200), - angularToLinearVelocity( - RotationsPerSecond.of(flywheelIO.getSetpointRotPerSec()), - Inches.of(FLYWHEEL_DIAMETER_INCHES / 2)), - Rotation2d.fromDegrees(90).minus(hoodIO.getHoodSetpoint()).getMeasure(), - turretIO.getTurretSetpoint().getMeasure(), - Inches.of(13.75)); - } + turretDriftAlert.set( + Math.abs( + getCalculatedTurretRotations().getDegrees() + - turretInputs.positionRotations.getDegrees()) + > 10); + + } public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distance radius) { @@ -247,19 +234,18 @@ public static LinearVelocity angularToLinearVelocity(AngularVelocity vel, Distan public void simulationPeriodic() {} @Override + /** no longer zeros by default at the start of feed to accomodate only zeroing during spinup */ public Command feed(Supplier shotParamsSupplier) { - return resetTurretToCalculatedPosition() - .andThen( - this.run( - () -> { - hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); - // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); - // flywheelIO.setMotionProfiledFlywheelVelocity( - flywheelIO.setFlywheelVelocity( - shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() - + AutoAim.getFudgeFactor()); - turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); - })); + return this.run( + () -> { + hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); + // flywheelIO.setTorqueCurrentVel(shotDataSupplier.get().flywheelVelocityRotPerSec()); + // flywheelIO.setMotionProfiledFlywheelVelocity( + flywheelIO.setFlywheelVelocity( + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + + AutoAim.getFudgeFactor()); + turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); + }); } @Override @@ -332,39 +318,38 @@ public Command runHoodCurrentZeroing() { .andThen(Commands.parallel(Commands.print("Hood Zeroed"), zeroHood())); } + /** no longer zeros by default at the start of score to accomodate only zeroing during spinup */ public Command score(Supplier shotParamsSupplier) { - return resetTurretToCalculatedPosition() - .andThen( - this.run( - () -> { - // switch (Superstructure.getFixedShotTarget()) { - // // in front of left trench with intake facing trench - // case LEFT: - // hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); - // flywheelIO.setMotionProfiledFlywheelVelocity( - // AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); - // turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); - // // in front of tower with intake facing left (to avoid deadzone) - // case MID: - // hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); - // flywheelIO.setMotionProfiledFlywheelVelocity( - // AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); - // turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); - // // in front of right trench with intake facing alliance wall - // case RIGHT: - // hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); - // flywheelIO.setMotionProfiledFlywheelVelocity( - // AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); - // turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); - // case NONE: - hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); - // flywheelIO.setMotionProfiledFlywheelVelocity( - flywheelIO.setFlywheelVelocity( - shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() - + AutoAim.getFudgeFactor()); - turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); - // } - })); + return this.run( + () -> { + // switch (Superstructure.getFixedShotTarget()) { + // // in front of left trench with intake facing trench + // case LEFT: + // hoodIO.setHoodPosition(AutoAim.getLeftFixedShotData().hoodAngle()); + // flywheelIO.setMotionProfiledFlywheelVelocity( + // AutoAim.getLeftFixedShotData().flywheelVelocityRotPerSec()); + // turretIO.setTurretPosition(AutoAim.LEFT_FIXED_SHOT_TURRET_ANGLE); + // // in front of tower with intake facing left (to avoid deadzone) + // case MID: + // hoodIO.setHoodPosition(AutoAim.getMidFixedShotData().hoodAngle()); + // flywheelIO.setMotionProfiledFlywheelVelocity( + // AutoAim.getMidFixedShotData().flywheelVelocityRotPerSec()); + // turretIO.setTurretPosition(AutoAim.MID_FIXED_SHOT_TURRET_ANGLE); + // // in front of right trench with intake facing alliance wall + // case RIGHT: + // hoodIO.setHoodPosition(AutoAim.getRightFixedShotData().hoodAngle()); + // flywheelIO.setMotionProfiledFlywheelVelocity( + // AutoAim.getRightFixedShotData().flywheelVelocityRotPerSec()); + // turretIO.setTurretPosition(AutoAim.RIGHT_FIXED_SHOT_TURRET_ANGLE); + // case NONE: + hoodIO.setHoodPosition(shotParamsSupplier.get().shotData().hoodAngle()); + // flywheelIO.setMotionProfiledFlywheelVelocity( + flywheelIO.setFlywheelVelocity( + shotParamsSupplier.get().shotData().flywheelVelocityRotPerSec() + + AutoAim.getFudgeFactor()); + turretIO.setTurretPosition(shotParamsSupplier.get().turretAngle()); + // } + }); } @Override @@ -645,29 +630,4 @@ public Command runFlywheelSysid() { flywheelSysid.dynamic(Direction.kForward), flywheelSysid.dynamic(Direction.kReverse)); } - - @Override - public Command testShot( - Supplier robotPoseSupplier, Supplier chassisSpeedsSupplier) { - return this.run( - () -> { - hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); - flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); - // turretIO.setTurretPosition(Rotation2d.fromRotations(-0.5)); - turretIO.setTurretPosition( - AutoAim.getShotParameters( - robotPoseSupplier.get(), - chassisSpeedsSupplier.get(), - FieldUtils.getCurrentHubTranslation(), - Robot.ROBOT_EDITION == RobotEdition.ALPHA - ? ShotTrees.ALPHA_HUB_SHOT_TREE - : ShotTrees.COMP_HUB_SHOT_TREE) - .turretAngle()); - }); - } - - @Override - public double getTestVel() { - return testVelocity.get(); - } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 03dc8f33..a401163f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -319,7 +319,7 @@ public void periodic() { Tracer.trace("Update odometry", this::updateOdometry); Tracer.trace("Update vision", this::updateVision); - Logger.recordOutput("Current Hub Pose", FieldUtils.getCurrentHubPose()); + // Logger.recordOutput("Current Hub Pose", FieldUtils.getCurrentHubPose()); }); } @@ -406,7 +406,7 @@ private void updateVision() { } // only do all this logging stuff if we're not irl for performance - Logger.recordOutput("Vision/Camera Poses", cameraPoses); + // Logger.recordOutput("Vision/Camera Poses", cameraPoses); Pose3d[] arr = new Pose3d[cameras.length]; for (int k = 0; k < cameras.length; k++) { // honetsly not sure if this distinction is the way to go but @@ -420,7 +420,7 @@ private void updateVision() { arr[k] = getPose3d().transformBy(cameras[k].getCameraConstants().robotToCamera()); } } - Logger.recordOutput("Vision/Camera Poses on Robot", arr); + // Logger.recordOutput("Vision/Camera Poses on Robot", arr); } /** @@ -756,7 +756,7 @@ public Command faceFeedComp( turretTargetRotation = turretTargetRotation.minus(getRotation()); Logger.recordOutput("Turret/Unclamped target", turretTargetRotation); Rotation2d diff = turretTargetRotation.minus(turretRotation.get()); - Logger.recordOutput("Turret/diff", diff); + // Logger.recordOutput("Turret/diff", diff); // if (diff.getDegrees() > 0) { // diff = Rotation2d.fromDegrees(-diff.getDegrees()); // } @@ -983,7 +983,7 @@ public void simulationPeriodic() { // Update maple simulation SimulatedArena.getInstance().simulationPeriodic(); // Log simulated pose - Logger.recordOutput("MapleSim/Pose", swerveSimulation.getSimulatedDriveTrainPose()); + // Logger.recordOutput("MapleSim/Pose", swerveSimulation.getSimulatedDriveTrainPose()); } public Command runTurnSysid() { diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 73a29fa9..3cadd6cf 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -204,12 +204,16 @@ public static double distanceToHub(Pose2d pose) { /** Increase the flywheel fudge factor by 1 */ public static void incrementFudgeFactor() { + fudgeFactor++; + Logger.recordOutput("AutoAim/Flywheel Fudge Factor", fudgeFactor); + } /** Decrease the flywheel fudge factor by 1 */ public static void decrementFudgeFactor() { fudgeFactor--; + Logger.recordOutput("AutoAim/Flywheel Fudge Factor", fudgeFactor); } /** Get the current flywheel fudge factor */