diff --git a/src/main/deploy/choreo/CTofetchPSlow.traj b/src/main/deploy/choreo/CTofetchPSlow.traj index 757fbd44..8baeb58f 100644 --- a/src/main/deploy/choreo/CTofetchPSlow.traj +++ b/src/main/deploy/choreo/CTofetchPSlow.traj @@ -16,7 +16,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"C.x", "val":3.6132737518457456}, "y":{"exp":"C.y", "val":3.1185381922643174}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9328655004501343 m", "val":1.9328655004501345}, "y":{"exp":"1.1461 m", "val":1.1461}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/DTofetchPSlow.traj b/src/main/deploy/choreo/DTofetchPSlow.traj index ef9c9f7b..e8455ad4 100644 --- a/src/main/deploy/choreo/DTofetchPSlow.traj +++ b/src/main/deploy/choreo/DTofetchPSlow.traj @@ -16,7 +16,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"D.x", "val":3.89906213509461}, "y":{"exp":"D.y", "val":2.953538192264317}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9099534749984741 m", "val":1.909953474998474}, "y":{"exp":"1.1158 m", "val":1.1158}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/ETofetchPSlow.traj b/src/main/deploy/choreo/ETofetchPSlow.traj index 8e58f64f..5ebe854c 100644 --- a/src/main/deploy/choreo/ETofetchPSlow.traj +++ b/src/main/deploy/choreo/ETofetchPSlow.traj @@ -3,30 +3,30 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":4.837096751845746, "y":2.813538192264317, "heading":2.0943951023931953, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.975547790527344, "y":2.0208094120025635, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":1.949252247810364, "y":1.1668, "heading":0.942477796076938, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":4.894185251845, "y":2.7146580097364, "heading":2.0943951023931953, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.480600357055664, "y":2.3546712398529053, "heading":0.0, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.1380813121795654, "y":1.227155, "heading":0.942477796076938, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.69577419757843, "y":0.8236, "heading":0.942477796076938, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"E.x", "val":4.837096751845746}, "y":{"exp":"E.y", "val":2.813538192264317}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.9755477905273438 m", "val":3.975547790527344}, "y":{"exp":"2.0208094120025635 m", "val":2.0208094120025635}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"1.9492522478103638 m", "val":1.949252247810364}, "y":{"exp":"1.1668 m", "val":1.1668}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.480600357055664 m", "val":4.480600357055664}, "y":{"exp":"2.3546712398529053 m", "val":2.3546712398529053}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.1380813121795654 m", "val":2.1380813121795654}, "y":{"exp":"1.227155 m", "val":1.227155}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"5 m / s ^ 2", "val":5.0}}}, "enabled":true}], "targetDt":{ @@ -36,80 +36,79 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.69196,1.4763,1.8936], + "waypoints":[0.0,0.4692,1.36996,1.86027], "samples":[ - {"t":0.0, "x":4.8371, "y":2.81354, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.43996, "ay":-3.6233, "alpha":-12.83605, "fx":[-53.55017,-101.07973,-63.98684,-10.75311], "fy":[-17.48223,-45.37801,-97.93404,-80.8001]}, - {"t":0.02661, "x":4.83588, "y":2.81226, "heading":2.0944, "vx":-0.09155, "vy":-0.09643, "omega":-0.34161, "ax":-3.44852, "ay":-3.61768, "alpha":-10.35324, "fx":[-54.20968,-92.98991,-62.67352,-20.06733], "fy":[-26.073,-47.97851,-90.98046,-76.18822]}, - {"t":0.05323, "x":4.83222, "y":2.80841, "heading":2.0853, "vx":-0.18333, "vy":-0.19271, "omega":-0.61715, "ax":-3.45646, "ay":-3.60996, "alpha":-8.20179, "fx":[-54.51862,-85.53015,-61.7123,-28.70928], "fy":[-32.75617,-50.20749,-84.92099,-72.82032]}, - {"t":0.07984, "x":4.82612, "y":2.802, "heading":2.06888, "vx":-0.27532, "vy":-0.28878, "omega":-0.83543, "ax":-3.46515, "ay":-3.60147, "alpha":-6.44537, "fx":[-54.85593,-80.10451,-61.27947,-34.80954], "fy":[-39.57206,-51.07179,-79.32043,-70.17495]}, - {"t":0.10645, "x":4.81756, "y":2.79304, "heading":2.04664, "vx":-0.36754, "vy":-0.38463, "omega":-1.00697, "ax":-3.47479, "ay":-3.59201, "alpha":-4.91087, "fx":[-55.45428,-74.56579,-60.86625,-40.80581], "fy":[-43.79876,-53.22038,-74.66467,-67.8244]}, - {"t":0.13307, "x":4.80655, "y":2.78153, "heading":2.01985, "vx":-0.46001, "vy":-0.48023, "omega":-1.13766, "ax":-3.48544, "ay":-3.58149, "alpha":-3.60631, "fx":[-55.84393,-70.55761,-60.4338,-45.56684], "fy":[-48.69245,-53.81316,-70.55341,-65.74797]}, - {"t":0.15968, "x":4.79307, "y":2.76748, "heading":1.98957, "vx":-0.55277, "vy":-0.57554, "omega":-1.23364, "ax":-3.49736, "ay":-3.56964, "alpha":-2.54403, "fx":[-56.71809,-66.77518,-60.20761,-49.49654], "fy":[-51.32303,-55.63942,-67.02376,-64.03049]}, - {"t":0.1863, "x":4.77712, "y":2.7509, "heading":1.95674, "vx":-0.64585, "vy":-0.67055, "omega":-1.30135, "ax":-3.51074, "ay":-3.55625, "alpha":-1.59072, "fx":[-57.23085,-63.94265,-59.74218,-53.17339], "fy":[-54.70851,-56.09498,-64.0642,-62.2565]}, - {"t":0.21291, "x":4.75869, "y":2.7318, "heading":1.9221, "vx":-0.73929, "vy":-0.76519, "omega":-1.34368, "ax":-3.52591, "ay":-3.54094, "alpha":-0.87391, "fx":[-58.35138,-61.54306,-59.57535,-55.63092], "fy":[-56.13964,-57.66423,-61.45586,-60.84368]}, - {"t":0.23952, "x":4.73777, "y":2.71018, "heading":1.88634, "vx":-0.83312, "vy":-0.85943, "omega":-1.36694, "ax":-3.54315, "ay":-3.52338, "alpha":-0.16044, "fx":[-58.97395,-59.59274,-59.03486,-58.64914], "fy":[-58.49045,-57.90901,-59.34587,-59.18687]}, - {"t":0.26614, "x":4.71434, "y":2.68606, "heading":1.84996, "vx":-0.92742, "vy":-0.9532, "omega":-1.37121, "ax":-3.56305, "ay":-3.5029, "alpha":0.30315, "fx":[-60.40272,-58.23892,-59.0116,-59.92418], "fy":[-58.88531,-59.36352,-57.32894,-57.98881]}, - {"t":0.29275, "x":4.6884, "y":2.65945, "heading":1.81347, "vx":-1.02224, "vy":-1.04642, "omega":-1.36314, "ax":-3.58618, "ay":-3.4788, "alpha":0.89906, "fx":[-61.05381,-56.86034,-58.40418,-62.80108], "fy":[-60.57414,-59.53769,-55.68442,-56.16329]}, - {"t":0.31936, "x":4.65992, "y":2.63037, "heading":1.77719, "vx":-1.11769, "vy":-1.13901, "omega":-1.33921, "ax":-3.61341, "ay":-3.45, "alpha":1.25062, "fx":[-62.46593,-56.27185,-58.46786,-63.72972], "fy":[-60.7229,-60.54048,-53.94143,-54.83434]}, - {"t":0.34598, "x":4.6289, "y":2.59883, "heading":1.74155, "vx":-1.21385, "vy":-1.23082, "omega":-1.30593, "ax":-3.64595, "ay":-3.41498, "alpha":1.72744, "fx":[-63.49165,-55.40126,-58.00386,-66.20798], "fy":[-61.30815,-60.94587,-52.51312,-52.93731]}, - {"t":0.37259, "x":4.5953, "y":2.56487, "heading":1.7068, "vx":-1.31088, "vy":-1.32171, "omega":-1.25996, "ax":-3.68543, "ay":-3.37157, "alpha":1.99312, "fx":[-64.89703,-55.54309,-58.25166,-67.04559], "fy":[-61.40449,-61.13504,-50.86654,-51.4039]}, - {"t":0.3992, "x":4.55911, "y":2.5285, "heading":1.67326, "vx":-1.40897, "vy":-1.41144, "omega":-1.20691, "ax":-3.73439, "ay":-3.31629, "alpha":2.39063, "fx":[-66.38029,-55.2,-58.10723,-69.31453], "fy":[-60.77936,-61.75214,-49.33237,-49.25977]}, - {"t":0.42582, "x":4.52029, "y":2.48976, "heading":1.64114, "vx":-1.50835, "vy":-1.4997, "omega":-1.14329, "ax":-3.79656, "ay":-3.24365, "alpha":2.56494, "fx":[-67.89418,-56.20873,-58.77445,-70.26998], "fy":[-60.73339,-60.69336,-47.50548,-47.34778]}, - {"t":0.45243, "x":4.4788, "y":2.4487, "heading":1.61072, "vx":-1.60939, "vy":-1.58602, "omega":-1.07503, "ax":-3.87788, "ay":-3.1442, "alpha":2.86972, "fx":[-69.9781,-56.73133,-59.31045,-72.54955], "fy":[-58.53631,-60.9832,-45.45122,-44.67853]}, - {"t":0.47905, "x":4.43459, "y":2.40537, "heading":1.58211, "vx":-1.7126, "vy":-1.6697, "omega":-0.99865, "ax":-3.98839, "ay":-3.00016, "alpha":2.94042, "fx":[-71.95191,-59.02538,-60.93163,-74.02924], "fy":[-57.79342,-57.84484,-42.6737,-41.73323]}, - {"t":0.50566, "x":4.3876, "y":2.35987, "heading":1.55553, "vx":-1.81874, "vy":-1.74955, "omega":-0.9204, "ax":-4.14576, "ay":-2.77441, "alpha":3.09057, "fx":[-75.14728,-61.39572,-63.08581,-76.80269], "fy":[-52.49785,-56.35921,-38.7132,-37.42206]}, - {"t":0.53227, "x":4.33773, "y":2.31233, "heading":1.53103, "vx":-1.92908, "vy":-1.82338, "omega":-0.83815, "ax":-4.38056, "ay":-2.37839, "alpha":2.97386, "fx":[-78.81573,-66.39129,-67.09659,-79.78388], "fy":[-48.1648,-47.58485,-32.017,-30.8195]}, - {"t":0.55889, "x":4.28484, "y":2.26296, "heading":1.50873, "vx":-2.04566, "vy":-1.88668, "omega":-0.759, "ax":-4.73054, "ay":-1.54807, "alpha":2.80248, "fx":[-84.74703,-73.07779,-72.95565,-84.64268], "fy":[-31.66719,-35.46581,-18.47323,-17.61594]}, - {"t":0.5855, "x":4.22872, "y":2.2122, "heading":1.48853, "vx":-2.17156, "vy":-1.92788, "omega":-0.68442, "ax":-4.92919, "ay":0.58264, "alpha":2.31709, "fx":[-87.43669,-77.75761,-76.50906,-86.96558], "fy":[3.18696,3.71387,15.89532,16.05342]}, - {"t":0.61211, "x":4.16918, "y":2.1611, "heading":1.47031, "vx":-2.30274, "vy":-1.91238, "omega":-0.62275, "ax":-3.23882, "ay":3.75322, "alpha":1.57084, "fx":[-58.81284,-50.89926,-49.2118,-57.03455], "fy":[57.86324,59.81874,66.40595,66.16974]}, - {"t":0.63873, "x":4.10675, "y":2.11153, "heading":1.45374, "vx":-2.38894, "vy":-1.81249, "omega":-0.58094, "ax":-2.91309, "ay":4.01998, "alpha":1.05458, "fx":[-52.01865,-46.65554,-45.29513,-50.26979], "fy":[65.04826,64.04985,69.39659,69.54965]}, - {"t":0.66534, "x":4.04214, "y":2.06472, "heading":1.43828, "vx":-2.46647, "vy":-1.7055, "omega":-0.55288, "ax":-2.69333, "ay":4.17751, "alpha":0.69829, "fx":[-47.30168,-43.49164,-42.53902,-46.25407], "fy":[67.95255,68.34049,71.04432,71.21099]}, - {"t":0.69196, "x":3.97555, "y":2.02081, "heading":1.42356, "vx":-2.53815, "vy":-1.59432, "omega":-0.53429, "ax":-2.58519, "ay":4.20152, "alpha":0.10895, "fx":[-43.49584,-43.06733,-42.73583,-43.07626], "fy":[69.86182,69.62165,70.17383,70.49222]}, - {"t":0.71572, "x":3.91449, "y":1.9841, "heading":1.41086, "vx":-2.59959, "vy":-1.49446, "omega":-0.5317, "ax":-2.38833, "ay":4.34463, "alpha":0.05544, "fx":[-40.27189,-39.6395,-39.39035,-39.94744], "fy":[72.04773,72.84708,72.27679,72.51985]}, - {"t":0.73949, "x":3.85203, "y":1.94981, "heading":1.39823, "vx":-2.65636, "vy":-1.3912, "omega":-0.53039, "ax":-2.20931, "ay":4.42448, "alpha":0.09808, "fx":[-37.23774,-36.59682,-36.55637,-36.9219], "fy":[73.56984,73.51986,73.88864,74.03739]}, - {"t":0.76326, "x":3.78827, "y":1.91799, "heading":1.38562, "vx":-2.70887, "vy":-1.28604, "omega":-0.52805, "ax":-2.0266, "ay":4.49054, "alpha":-0.0096, "fx":[-34.09757,-33.64128,-33.50003,-33.89112], "fy":[74.36347,75.77837,74.57187,74.70685]}, - {"t":0.78703, "x":3.72331, "y":1.88869, "heading":1.37307, "vx":-2.75704, "vy":-1.17931, "omega":-0.52828, "ax":-1.83884, "ay":4.53834, "alpha":0.0719, "fx":[-30.9725,-30.40604,-30.42938,-30.80219], "fy":[75.53869,75.54154,75.73029,75.79703]}, - {"t":0.8108, "x":3.65726, "y":1.86195, "heading":1.36051, "vx":-2.80074, "vy":-1.07144, "omega":-0.52657, "ax":-1.64445, "ay":4.55687, "alpha":-0.05475, "fx":[-27.61441,-27.30786,-27.24109,-27.48528], "fy":[75.31701,77.2492,75.59556,75.68149]}, - {"t":0.83456, "x":3.59023, "y":1.83777, "heading":1.348, "vx":-2.83983, "vy":-0.96313, "omega":-0.52788, "ax":-1.44022, "ay":4.52215, "alpha":0.04569, "fx":[-24.20469,-23.78846,-23.92117,-24.11684], "fy":[75.31691,75.29935,75.4315,75.48055]}, - {"t":0.85833, "x":3.52233, "y":1.81615, "heading":1.33545, "vx":-2.87406, "vy":-0.85565, "omega":-0.52679, "ax":-1.19871, "ay":4.30535, "alpha":-0.05949, "fx":[-20.10949,-19.90161,-19.86995,-20.04657], "fy":[71.28662,72.86691,71.42906,71.48987]}, - {"t":0.8821, "x":3.45368, "y":1.79703, "heading":1.32293, "vx":-2.90255, "vy":-0.75332, "omega":-0.5282, "ax":-0.783, "ay":3.1826, "alpha":0.03926, "fx":[-13.2304,-12.76896,-13.0258,-13.18354], "fy":[53.01062,52.9893,53.08968,53.12017]}, - {"t":0.90587, "x":3.38447, "y":1.78003, "heading":1.31038, "vx":-2.92116, "vy":-0.67767, "omega":-0.52727, "ax":-0.49846, "ay":2.24198, "alpha":-0.01254, "fx":[-8.36659,-8.27491,-8.25331,-8.34125], "fy":[37.17666,37.72329,37.27714,37.31376]}, - {"t":0.92964, "x":3.3149, "y":1.76455, "heading":1.29784, "vx":-2.93301, "vy":-0.62439, "omega":-0.52757, "ax":-0.2925, "ay":1.41407, "alpha":0.01858, "fx":[-4.97494,-4.69647,-4.87568,-4.9562], "fy":[23.56036,23.5543,23.58299,23.58983]}, - {"t":0.9534, "x":3.2451, "y":1.75011, "heading":1.2853, "vx":-2.93996, "vy":-0.59078, "omega":-0.52713, "ax":-0.11324, "ay":0.56925, "alpha":0.02776, "fx":[-1.85905,-1.90165,-1.91597,-1.87392], "fy":[9.60712,9.11495,9.61754,9.61707]}, - {"t":0.97717, "x":3.1752, "y":1.73623, "heading":1.27278, "vx":-2.94265, "vy":-0.57725, "omega":-0.52647, "ax":0.01217, "ay":-0.06166, "alpha":-0.02359, "fx":[0.26871,0.13117,0.16886,0.24252], "fy":[-0.986,-0.94551,-1.06958,-1.1101]}, - {"t":1.00094, "x":3.10526, "y":1.72249, "heading":1.26026, "vx":-2.94236, "vy":-0.57871, "omega":-0.52703, "ax":0.15223, "ay":-0.76111, "alpha":0.0328, "fx":[2.72615,2.4418,2.34914,2.63327], "fy":[-12.26588,-13.5628,-12.42179,-12.49912]}, - {"t":1.02471, "x":3.03537, "y":1.70852, "heading":1.24774, "vx":-2.93874, "vy":-0.5968, "omega":-0.52625, "ax":0.32372, "ay":-1.54366, "alpha":-0.10241, "fx":[5.74995,5.0139,5.20996,5.61094], "fy":[-25.58291,-25.43713,-25.8805,-26.02753]}, - {"t":1.04848, "x":2.96561, "y":1.6939, "heading":1.23523, "vx":-2.93105, "vy":-0.63349, "omega":-0.52868, "ax":0.55308, "ay":-2.4438, "alpha":-0.00497, "fx":[9.76701,8.95321,8.67666,9.48112], "fy":[-39.87617,-42.1082,-40.35688,-40.60688]}, - {"t":1.07224, "x":2.8961, "y":1.67816, "heading":1.22266, "vx":-2.9179, "vy":-0.69158, "omega":-0.5288, "ax":0.84792, "ay":-3.36595, "alpha":-0.25259, "fx":[15.00652,13.47366,13.47927,14.57843], "fy":[-55.74188,-55.38293,-56.47063,-56.83963]}, - {"t":1.09601, "x":2.82699, "y":1.66077, "heading":1.21009, "vx":-2.89775, "vy":-0.77158, "omega":-0.5348, "ax":1.25968, "ay":-4.40759, "alpha":-0.16324, "fx":[22.33633,20.39211,19.6878,21.57672], "fy":[-72.03395,-74.96354,-73.14885,-73.74318]}, - {"t":1.11978, "x":2.75847, "y":1.64118, "heading":1.19738, "vx":-2.86781, "vy":-0.87634, "omega":-0.53868, "ax":1.48219, "ay":-4.53921, "alpha":-0.54791, "fx":[26.59528,23.80983,22.95338,25.47103], "fy":[-74.8628,-74.08539,-76.46091,-77.25638]}, - {"t":1.14355, "x":2.69073, "y":1.61907, "heading":1.18458, "vx":-2.83258, "vy":-0.98423, "omega":-0.55171, "ax":1.68343, "ay":-4.55763, "alpha":-0.57903, "fx":[31.0252,26.88719,25.17185,29.16341], "fy":[-73.8177,-76.52562,-76.17571,-77.37476]}, - {"t":1.16732, "x":2.62388, "y":1.59439, "heading":1.17147, "vx":-2.79257, "vy":-1.09255, "omega":-0.56547, "ax":1.87659, "ay":-4.53123, "alpha":-1.14438, "fx":[35.23278,29.89965,27.43241,32.56248], "fy":[-73.8158,-72.15901,-77.26859,-78.89022]}, - {"t":1.19108, "x":2.55803, "y":1.56714, "heading":1.15803, "vx":-2.74797, "vy":-1.20025, "omega":-0.59267, "ax":2.10278, "ay":-4.46053, "alpha":-1.33813, "fx":[40.52857,33.38963,29.65476,36.63634], "fy":[-70.99983,-72.3912,-75.90571,-78.12294]}, - {"t":1.21485, "x":2.49331, "y":1.53736, "heading":1.14394, "vx":-2.69799, "vy":-1.30627, "omega":-0.62447, "ax":3.73617, "ay":-3.25049, "alpha":-1.92119, "fx":[68.15949,61.09334,56.15748,63.71013], "fy":[-51.11624,-47.64477,-57.40708,-60.56815]}, - {"t":1.23862, "x":2.43024, "y":1.50539, "heading":1.1291, "vx":-2.60919, "vy":-1.38353, "omega":-0.67014, "ax":4.73453, "ay":-1.51697, "alpha":-2.05633, "fx":[85.37184,77.1881,72.38589,80.74369], "fy":[-22.15492,-18.52453,-28.32323,-32.14588]}, - {"t":1.26239, "x":2.36957, "y":1.47208, "heading":1.11317, "vx":-2.49665, "vy":-1.41958, "omega":-0.71901, "ax":4.93111, "ay":-0.70244, "alpha":-2.08325, "fx":[88.04534,81.09113,75.95234,83.70822], "fy":[-8.78466,-3.9925,-14.77512,-19.28513]}, - {"t":1.28616, "x":2.31162, "y":1.43814, "heading":1.09608, "vx":-2.37945, "vy":-1.43628, "omega":-0.76853, "ax":4.9783, "ay":-0.27276, "alpha":-1.87948, "fx":[88.44524,81.504,77.42936,84.56464], "fy":[-2.59441,2.63442,-7.11986,-11.10744]}, - {"t":1.30992, "x":2.25647, "y":1.40393, "heading":1.07781, "vx":-2.26113, "vy":-1.44276, "omega":-0.8132, "ax":4.98866, "ay":-0.02065, "alpha":-1.39763, "fx":[87.06189,82.27318,79.10922,84.18982], "fy":[1.32827,4.95513,-2.0574,-5.60319]}, - {"t":1.33369, "x":2.20414, "y":1.36963, "heading":1.05849, "vx":-2.14256, "vy":-1.44325, "omega":-0.84642, "ax":4.98844, "ay":0.1486, "alpha":-0.81419, "fx":[85.4486,82.55286,80.83822,83.7796], "fy":[2.79972,6.00249,1.4072,-0.30133]}, - {"t":1.35746, "x":2.15462, "y":1.33537, "heading":1.03837, "vx":-2.02399, "vy":-1.43972, "omega":-0.86577, "ax":4.98492, "ay":0.26629, "alpha":0.1151, "fx":[82.78552,83.06379,83.4688,83.06693], "fy":[4.30474,3.99355,4.57934,4.87784]}, - {"t":1.38123, "x":2.10792, "y":1.30122, "heading":1.01779, "vx":-1.90551, "vy":-1.43339, "omega":-0.86303, "ax":4.9804, "ay":0.35535, "alpha":1.07093, "fx":[79.72692,83.7924,86.25402,82.30999], "fy":[4.31289,2.49905,6.65488,10.2274]}, - {"t":1.405, "x":2.06404, "y":1.26726, "heading":0.99728, "vx":-1.78714, "vy":-1.42494, "omega":-0.83758, "ax":4.97592, "ay":0.42285, "alpha":2.45711, "fx":[75.93232,84.15608,90.02062,81.67575], "fy":[4.92936,-2.4578,9.07006,16.65293]}, - {"t":1.42876, "x":2.02297, "y":1.23351, "heading":0.97737, "vx":-1.66887, "vy":-1.41489, "omega":-0.77918, "ax":4.9716, "ay":0.47762, "alpha":3.79837, "fx":[71.26822,85.36463,93.96412,80.89959], "fy":[4.40557,-5.76859,10.40219,22.80747]}, - {"t":1.45253, "x":1.98471, "y":1.20001, "heading":0.95885, "vx":-1.5507, "vy":-1.40354, "omega":-0.6889, "ax":4.96774, "ay":0.5212, "alpha":5.66195, "fx":[66.1227,85.97245,98.749,80.39492], "fy":[4.65598,-13.21212,12.27193,31.03667]}, - {"t":1.4763, "x":1.94925, "y":1.1668, "heading":0.94248, "vx":-1.43263, "vy":-1.39115, "omega":-0.55432, "ax":4.90479, "ay":0.93728, "alpha":6.0572, "fx":[62.95801,85.93427,99.30579,78.84387], "fy":[11.90059,-6.86111,18.32196,39.13472]}, - {"t":1.51107, "x":1.9024, "y":1.11899, "heading":0.9232, "vx":-1.26207, "vy":-1.35856, "omega":-0.34369, "ax":4.70639, "ay":1.67894, "alpha":4.74496, "fx":[64.16866,82.14361,92.33426,75.16623], "fy":[27.48808,9.37579,29.06948,46.01511]}, - {"t":1.54585, "x":1.86136, "y":1.07276, "heading":0.91125, "vx":-1.0984, "vy":-1.30018, "omega":-0.17868, "ax":4.42661, "ay":2.31856, "alpha":3.63282, "fx":[62.26049,76.88732,84.84762,71.16267], "fy":[38.11458,25.23796,38.98359,52.26098]}, - {"t":1.58062, "x":1.82584, "y":1.02895, "heading":0.90504, "vx":-0.94447, "vy":-1.21955, "omega":-0.05235, "ax":4.10878, "ay":2.84439, "alpha":2.80434, "fx":[59.89831,71.18861,77.35871,65.52001], "fy":[47.39572,37.04291,47.40983,57.80983]}, - {"t":1.6154, "x":1.79548, "y":0.98826, "heading":0.90322, "vx":-0.80159, "vy":-1.12063, "omega":0.04517, "ax":3.78491, "ay":3.26331, "alpha":2.06642, "fx":[56.26409,65.10411,69.70593,61.2961], "fy":[54.63659,46.88233,54.38715,61.68492]}, - {"t":1.65017, "x":1.76989, "y":0.95126, "heading":0.90479, "vx":-0.66997, "vy":-1.00715, "omega":0.11703, "ax":3.47505, "ay":3.59175, "alpha":1.37628, "fx":[53.61476,59.33112,62.60264,56.16096], "fy":[59.61465,55.16503,59.84709,64.86451]}, - {"t":1.68495, "x":1.74869, "y":0.91841, "heading":0.90886, "vx":-0.54912, "vy":-0.88225, "omega":0.16489, "ax":3.18948, "ay":3.84781, "alpha":0.79312, "fx":[50.48896,54.02316,55.73517,52.42131], "fy":[64.32943,61.21797,64.22128,66.79623]}, - {"t":1.71972, "x":1.73153, "y":0.89006, "heading":0.91459, "vx":-0.43821, "vy":-0.74844, "omega":0.19247, "ax":2.9317, "ay":4.04787, "alpha":0.15633, "fx":[48.57981,49.05054,49.44812,48.40172], "fy":[67.03147,67.07195,67.58134,68.21955]}, - {"t":1.7545, "x":1.71806, "y":0.86648, "heading":0.92128, "vx":-0.33626, "vy":-0.60768, "omega":0.19791, "ax":2.70159, "ay":4.20512, "alpha":-0.46299, "fx":[46.63981,44.53766,43.4636,45.49587], "fy":[70.02772,71.72359,70.04731,68.59061]}, - {"t":1.78927, "x":1.708, "y":0.84789, "heading":0.92817, "vx":-0.24231, "vy":-0.46145, "omega":0.18181, "ax":2.49721, "ay":4.32976, "alpha":-1.05036, "fx":[45.38109,40.68117,37.84249,42.60473], "fy":[72.38334,75.4942,72.08968,68.73312]}, - {"t":1.82405, "x":1.70108, "y":0.83446, "heading":0.93449, "vx":-0.15547, "vy":-0.31088, "omega":0.14528, "ax":2.31588, "ay":4.42954, "alpha":-1.74855, "fx":[44.92725,36.90255,32.20382,40.38467], "fy":[73.93411,79.38014,73.74153,68.29739]}, - {"t":1.85882, "x":1.69708, "y":0.82633, "heading":0.93954, "vx":-0.07493, "vy":-0.15684, "omega":0.08448, "ax":2.15486, "ay":4.51018, "alpha":-2.42922, "fx":[44.60597,34.00225,26.80461,38.26882], "fy":[75.51494,82.73737,75.07414,67.40386]}, - {"t":1.8936, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":4.89419, "y":2.71466, "heading":2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.52379, "ay":-3.54111, "alpha":-13.92501, "fx":[-54.15698,-105.72362,-66.33557,-8.7434], "fy":[-11.86818,-43.36601,-100.23193,-80.64808]}, + {"t":0.02346, "x":4.89322, "y":2.71368, "heading":2.0944, "vx":-0.08267, "vy":-0.08307, "omega":-0.32668, "ax":-3.54952, "ay":-3.51827, "alpha":-12.09304, "fx":[-55.07348,-100.06682,-65.6263,-15.90879], "fy":[-17.88896,-44.9954,-94.83012,-76.87725]}, + {"t":0.04692, "x":4.8903, "y":2.71077, "heading":2.08673, "vx":-0.16594, "vy":-0.16561, "omega":-0.61039, "ax":-3.57561, "ay":-3.49162, "alpha":-10.39574, "fx":[-55.60408,-94.47187,-65.15763,-23.1812], "fy":[-22.91693,-46.03226,-89.82476,-74.04058]}, + {"t":0.07038, "x":4.88542, "y":2.70592, "heading":2.07241, "vx":-0.24982, "vy":-0.24753, "omega":-0.85427, "ax":-3.60379, "ay":-3.46238, "alpha":-8.91669, "fx":[-56.07858,-90.37084,-65.2082,-28.63609], "fy":[-28.46961,-46.24128,-84.66075,-71.49296]}, + {"t":0.09384, "x":4.87857, "y":2.69916, "heading":2.05237, "vx":-0.33437, "vy":-0.32876, "omega":-1.06346, "ax":-3.63434, "ay":-3.43013, "alpha":-7.47146, "fx":[-56.80699,-85.51426,-65.27101,-34.7383], "fy":[-32.24052,-47.52817,-79.94507,-69.00073]}, + {"t":0.1173, "x":4.86973, "y":2.6905, "heading":2.02742, "vx":-0.41963, "vy":-0.40923, "omega":-1.23874, "ax":-3.66753, "ay":-3.39443, "alpha":-6.12403, "fx":[-57.44358,-81.80921,-65.2108,-40.08007], "fy":[-37.00611,-47.51298,-75.27424,-66.54094]}, + {"t":0.14076, "x":4.85887, "y":2.67997, "heading":1.99836, "vx":-0.50567, "vy":-0.48886, "omega":-1.38241, "ax":-3.70372, "ay":-3.35469, "alpha":-4.88531, "fx":[-58.55904,-77.85774,-65.45869,-45.0817], "fy":[-40.001,-48.74554,-70.70437,-64.23368]}, + {"t":0.16422, "x":4.84599, "y":2.66758, "heading":1.96593, "vx":-0.59256, "vy":-0.56756, "omega":-1.49702, "ax":-3.74332, "ay":-3.31023, "alpha":-3.69179, "fx":[-59.52363,-74.64658,-65.36157,-50.06531], "fy":[-43.65207,-48.94668,-66.41226,-61.70855]}, + {"t":0.18768, "x":4.83106, "y":2.65335, "heading":1.93081, "vx":-0.68038, "vy":-0.64522, "omega":-1.58363, "ax":-3.78678, "ay":-3.26015, "alpha":-2.65714, "fx":[-60.98546,-71.7249,-65.52035,-54.2645], "fy":[-45.89807,-49.87602,-62.29372,-59.31304]}, + {"t":0.21114, "x":4.81405, "y":2.63732, "heading":1.89366, "vx":-0.76922, "vy":-0.7217, "omega":-1.64596, "ax":-3.83466, "ay":-3.2034, "alpha":-1.63714, "fx":[-62.45379,-69.17701,-65.37573,-58.6814], "fy":[-48.37955,-50.17716,-58.41156,-56.62834]}, + {"t":0.2346, "x":4.79495, "y":2.6195, "heading":1.85504, "vx":-0.85918, "vy":-0.79686, "omega":-1.68437, "ax":-3.88761, "ay":-3.13859, "alpha":-0.79088, "fx":[-64.22568,-67.35467,-65.54497,-62.0932], "fy":[-49.93318,-50.76023,-54.61527,-53.96654]}, + {"t":0.25806, "x":4.77373, "y":2.59995, "heading":1.81553, "vx":-0.95038, "vy":-0.87049, "omega":-1.70293, "ax":-3.94638, "ay":-3.06397, "alpha":0.07273, "fx":[-66.19826,-65.4893,-65.36586,-66.08383], "fy":[-51.02097,-51.20911,-51.07586,-50.99395]}, + {"t":0.28152, "x":4.75034, "y":2.57868, "heading":1.77558, "vx":-1.04297, "vy":-0.94237, "omega":-1.70122, "ax":-4.01185, "ay":-2.97728, "alpha":0.74016, "fx":[-68.17195,-64.75772,-65.61808,-68.95458], "fy":[-51.80566,-51.15664,-47.54824,-48.00899]}, + {"t":0.30498, "x":4.72477, "y":2.55575, "heading":1.73567, "vx":-1.13708, "vy":-1.01222, "omega":-1.68386, "ax":-4.08502, "ay":-2.87552, "alpha":1.44864, "fx":[-70.62469,-63.73299,-65.58289,-72.44073], "fy":[-51.47666,-51.47185,-44.12985,-44.65588]}, + {"t":0.32844, "x":4.69697, "y":2.53122, "heading":1.69616, "vx":-1.23292, "vy":-1.07968, "omega":-1.64987, "ax":-4.167, "ay":-2.75472, "alpha":1.93899, "fx":[-72.76729,-64.08414,-66.11319,-74.88315], "fy":[-51.34107,-50.45476,-40.64425,-41.23939]}, + {"t":0.3519, "x":4.6669, "y":2.50513, "heading":1.65746, "vx":-1.33068, "vy":-1.1443, "omega":-1.60438, "ax":-4.25895, "ay":-2.60951, "alpha":2.48909, "fx":[-75.59207,-64.03608,-66.39817,-77.95223], "fy":[-49.46791,-50.12356,-37.05098,-37.35482]}, + {"t":0.37536, "x":4.63451, "y":2.47757, "heading":1.61982, "vx":-1.43059, "vy":-1.20552, "omega":-1.54599, "ax":-4.36191, "ay":-2.43249, "alpha":2.77688, "fx":[-78.00484,-65.53797,-67.53098,-79.77027], "fy":[-48.00969,-47.71303,-33.16036,-33.31038]}, + {"t":0.39882, "x":4.59975, "y":2.44861, "heading":1.58355, "vx":-1.53292, "vy":-1.26259, "omega":-1.48084, "ax":-4.4764, "ay":-2.21344, "alpha":3.18829, "fx":[-80.97435,-66.46537,-68.23771,-82.8003], "fy":[-44.22754,-46.08138,-28.80246,-28.47652]}, + {"t":0.42228, "x":4.56255, "y":2.41839, "heading":1.54881, "vx":-1.63794, "vy":-1.31451, "omega":-1.40604, "ax":-4.60153, "ay":-1.93816, "alpha":3.28027, "fx":[-83.53615,-69.03697,-70.09434,-84.15401], "fy":[-40.98737,-41.31862,-23.76472,-23.16182]}, + {"t":0.44574, "x":4.52286, "y":2.38701, "heading":1.51582, "vx":-1.74589, "vy":-1.35998, "omega":-1.32909, "ax":-4.7334, "ay":-1.58659, "alpha":3.5184, "fx":[-86.44655,-70.90645,-71.30613,-86.95499], "fy":[-34.23577,-37.30035,-17.67483,-16.57976]}, + {"t":0.4692, "x":4.4806, "y":2.35467, "heading":1.48464, "vx":-1.85694, "vy":-1.39721, "omega":-1.24655, "ax":-4.79848, "ay":-1.34433, "alpha":4.015, "fx":[-88.60079,-71.41929,-71.37598,-88.55711], "fy":[-32.01687,-34.68838,-12.26254,-10.66931]}, + {"t":0.49422, "x":4.43264, "y":2.31929, "heading":1.45345, "vx":-1.977, "vy":-1.43084, "omega":-1.14609, "ax":-4.82474, "ay":-1.27295, "alpha":4.09641, "fx":[-89.65534,-71.99548,-71.08227,-88.97154], "fy":[-29.9186,-34.45042,-11.21847,-9.29018]}, + {"t":0.51924, "x":4.38166, "y":2.28309, "heading":1.42477, "vx":-2.09772, "vy":-1.46269, "omega":-1.04359, "ax":-4.85036, "ay":-1.16003, "alpha":4.09381, "fx":[-90.13035,-72.60331,-71.67754,-89.00151], "fy":[-28.56191,-32.37199,-9.5025,-6.91203]}, + {"t":0.54426, "x":4.32765, "y":2.24613, "heading":1.39866, "vx":-2.21908, "vy":-1.49172, "omega":-0.94116, "ax":-4.88791, "ay":-0.96645, "alpha":3.95816, "fx":[-90.84639,-74.10336,-71.9749,-88.99195], "fy":[-24.19431,-29.27526,-6.73898,-4.23234]}, + {"t":0.56929, "x":4.2706, "y":2.2085, "heading":1.37511, "vx":-2.34138, "vy":-1.5159, "omega":-0.84212, "ax":-4.94181, "ay":-0.55563, "alpha":3.58412, "fx":[-90.89736,-76.81887,-73.19811,-88.5962], "fy":[-17.29221,-20.87982,-0.76798,1.89187]}, + {"t":0.59431, "x":4.21047, "y":2.1704, "heading":1.35404, "vx":-2.46503, "vy":-1.5298, "omega":-0.75244, "ax":-4.88572, "ay":0.75212, "alpha":3.06185, "fx":[-89.67858,-76.24165,-73.14619,-86.70406], "fy":[5.5346,3.58842,19.56713,21.45959]}, + {"t":0.61933, "x":4.14726, "y":2.13236, "heading":1.33522, "vx":-2.58728, "vy":-1.51098, "omega":-0.67583, "ax":-2.4559, "ay":4.19641, "alpha":2.47, "fx":[-48.1598,-36.44081,-34.01461,-45.13935], "fy":[64.62668,62.96133,75.4758,76.74456]}, + {"t":0.64435, "x":4.08176, "y":2.09587, "heading":1.31831, "vx":-2.64873, "vy":-1.40598, "omega":-0.61403, "ax":-2.15242, "ay":4.25604, "alpha":1.58346, "fx":[-41.35752,-32.55004,-30.488,-39.12366], "fy":[66.36894,68.71398,73.78118,74.92062]}, + {"t":0.66937, "x":4.01481, "y":2.06202, "heading":1.30294, "vx":-2.70258, "vy":-1.29949, "omega":-0.57441, "ax":-1.83893, "ay":4.00854, "alpha":1.15375, "fx":[-34.10834,-28.84527,-26.8808,-32.78181], "fy":[64.56719,63.68686,69.10148,69.92597]}, + {"t":0.69439, "x":3.94661, "y":2.03076, "heading":1.28857, "vx":-2.74859, "vy":-1.1992, "omega":-0.54554, "ax":-1.28679, "ay":3.06721, "alpha":0.67421, "fx":[-23.72333,-19.97303,-19.19574,-22.90878], "fy":[48.97193,50.30681,52.30754,52.92916]}, + {"t":0.71941, "x":3.87744, "y":2.00171, "heading":1.27492, "vx":-2.78079, "vy":-1.12245, "omega":-0.52868, "ax":-0.7925, "ay":2.01505, "alpha":0.47512, "fx":[-14.59325,-12.62685,-11.49139,-14.1306], "fy":[32.66824,32.32236,34.52066,34.84857]}, + {"t":0.74443, "x":3.80761, "y":1.97426, "heading":1.26169, "vx":-2.80062, "vy":-1.07203, "omega":-0.51679, "ax":-0.43559, "ay":1.15365, "alpha":0.29867, "fx":[-8.03519,-6.68047,-6.48895,-7.83944], "fy":[18.42408,18.43083,19.92778,20.14068]}, + {"t":0.76945, "x":3.7374, "y":1.9478, "heading":1.24876, "vx":-2.81152, "vy":-1.04317, "omega":-0.50931, "ax":-0.28431, "ay":0.77715, "alpha":0.16284, "fx":[-5.17657,-4.75352,-3.95492,-5.07203], "fy":[12.60161,12.56706,13.31075,13.33959]}, + {"t":0.79447, "x":3.66696, "y":1.92194, "heading":1.23602, "vx":-2.81863, "vy":-1.02372, "omega":-0.50524, "ax":-0.1075, "ay":0.29443, "alpha":0.12501, "fx":[-1.96891,-1.57499,-1.61523,-2.00864], "fy":[4.62771,4.33307,5.32723,5.34418]}, + {"t":0.8195, "x":3.59641, "y":1.89642, "heading":1.22338, "vx":-2.82132, "vy":-1.01635, "omega":-0.50211, "ax":-0.08439, "ay":0.23736, "alpha":0.05516, "fx":[-1.59561,-1.25942,-1.1519,-1.6199], "fy":[3.82208,3.90636,4.09161,4.0067]}, + {"t":0.84452, "x":3.52579, "y":1.87106, "heading":1.21081, "vx":-2.82343, "vy":-1.01042, "omega":-0.50073, "ax":-0.00366, "ay":0.00853, "alpha":0.04195, "fx":[-0.05407,0.02863,-0.06804,-0.15062], "fy":[-0.00407,-0.09367,0.3558,0.3105]}, + {"t":0.86954, "x":3.45514, "y":1.84578, "heading":1.19828, "vx":-2.82353, "vy":-1.0102, "omega":-0.49968, "ax":-0.01752, "ay":0.05007, "alpha":0.01971, "fx":[-0.3829,-0.13623,-0.21473,-0.43424], "fy":[0.77008,0.86409,0.89921,0.80517]}, + {"t":0.89456, "x":3.38449, "y":1.82052, "heading":1.18578, "vx":-2.82396, "vy":-1.00895, "omega":-0.49919, "ax":0.00416, "ay":-0.01205, "alpha":0.01221, "fx":[0.09675,0.12243,0.04182,0.01613], "fy":[-0.31062,-0.24876,-0.09787,-0.14624]}, + {"t":0.91958, "x":3.31383, "y":1.79527, "heading":1.17329, "vx":-2.82386, "vy":-1.00925, "omega":-0.49888, "ax":0.00273, "ay":-0.00786, "alpha":0.00917, "fx":[0.01238,0.10908,0.09207,-0.03153], "fy":[-0.17706,-0.11187,-0.08495,-0.15014]}, + {"t":0.9446, "x":3.24318, "y":1.77002, "heading":1.16081, "vx":-2.82379, "vy":-1.00945, "omega":-0.49865, "ax":-0.00226, "ay":0.00719, "alpha":-0.01473, "fx":[-0.02917,0.00383,-0.04626,-0.07923], "fy":[-0.03084,0.34739,0.10119,0.06159]}, + {"t":0.96962, "x":3.17252, "y":1.74476, "heading":1.14833, "vx":-2.82385, "vy":-1.00927, "omega":-0.49902, "ax":0.03354, "ay":-0.09523, "alpha":0.0023, "fx":[0.55996,0.59764,0.56366,0.51522], "fy":[-1.61976,-1.57465,-1.55525,-1.60022]}, + {"t":0.99464, "x":3.10188, "y":1.71948, "heading":1.13585, "vx":-2.82301, "vy":-1.01165, "omega":-0.49897, "ax":0.0304, "ay":-0.08265, "alpha":-0.03686, "fx":[0.53718,0.52711,0.47641,0.48664], "fy":[-1.51692,-1.00783,-1.46461,-1.52167]}, + {"t":1.01966, "x":3.03125, "y":1.69414, "heading":1.12336, "vx":-2.82225, "vy":-1.01372, "omega":-0.49989, "ax":0.14006, "ay":-0.3904, "alpha":-0.01776, "fx":[2.3741,2.48376,2.19686,2.28446], "fy":[-6.5068,-6.43186,-6.50964,-6.58303]}, + {"t":1.04468, "x":2.96068, "y":1.66866, "heading":1.11085, "vx":-2.81874, "vy":-1.02349, "omega":-0.50033, "ax":0.19965, "ay":-0.54305, "alpha":-0.07552, "fx":[3.48507,3.29907,3.1707,3.3575], "fy":[-9.12165,-8.57996,-9.18581,-9.32218]}, + {"t":1.06971, "x":2.89021, "y":1.64288, "heading":1.09834, "vx":-2.81375, "vy":-1.03707, "omega":-0.50222, "ax":0.43498, "ay":-1.16351, "alpha":-0.07145, "fx":[7.39195,7.63993,6.80531,7.16673], "fy":[-19.32336,-19.11627,-19.47123,-19.66947]}, + {"t":1.09473, "x":2.81995, "y":1.61656, "heading":1.08577, "vx":-2.80286, "vy":-1.06619, "omega":-0.50401, "ax":0.68669, "ay":-1.76281, "alpha":-0.10661, "fx":[11.92707,11.3208,10.96736,11.572], "fy":[-29.13413,-29.29799,-29.39033,-29.71858]}, + {"t":1.11975, "x":2.75003, "y":1.58934, "heading":1.07316, "vx":-2.78568, "vy":-1.11029, "omega":-0.50668, "ax":1.1397, "ay":-2.75657, "alpha":-0.19176, "fx":[19.52195,19.30737,18.15843,19.00492], "fy":[-45.75167,-45.26632,-46.15738,-46.62717]}, + {"t":1.14477, "x":2.68069, "y":1.56069, "heading":1.06048, "vx":-2.75717, "vy":-1.17927, "omega":-0.51147, "ax":1.77753, "ay":-3.95841, "alpha":-0.1307, "fx":[30.73147,29.34151,28.54718,29.90206], "fy":[-65.07462,-66.92989,-65.63431,-66.30031]}, + {"t":1.16979, "x":2.61226, "y":1.52995, "heading":1.04768, "vx":-2.71269, "vy":-1.27831, "omega":-0.51474, "ax":2.10628, "ay":-4.24747, "alpha":-0.38537, "fx":[36.27841,35.27878,33.65267,35.23261], "fy":[-70.39768,-69.46934,-71.21217,-72.13413]}, + {"t":1.19481, "x":2.54504, "y":1.49663, "heading":1.0348, "vx":-2.65999, "vy":-1.38459, "omega":-0.52439, "ax":2.32984, "ay":-4.25226, "alpha":-0.27149, "fx":[40.82205,38.36407,36.91097,39.25192], "fy":[-69.44048,-72.09195,-70.44101,-71.5593]}, + {"t":1.21983, "x":2.47922, "y":1.46066, "heading":1.02168, "vx":-2.60169, "vy":-1.49098, "omega":-0.53118, "ax":3.17309, "ay":-3.73835, "alpha":-0.56757, "fx":[54.49961,53.09471,50.91601,53.06492], "fy":[-61.74485,-60.20318,-62.93569,-64.38233]}, + {"t":1.24485, "x":2.41511, "y":1.42218, "heading":1.00839, "vx":-2.5223, "vy":-1.58452, "omega":-0.54538, "ax":4.94896, "ay":-0.33541, "alpha":-0.33502, "fx":[83.76132,82.23747,81.22219,82.76624], "fy":[-5.36858,-4.61016,-5.80006,-6.58564]}, + {"t":1.26987, "x":2.35355, "y":1.38243, "heading":0.99475, "vx":-2.39847, "vy":-1.59291, "omega":-0.55376, "ax":4.94436, "ay":0.57507, "alpha":0.21313, "fx":[81.64804,82.93292,82.93838,82.16076], "fy":[9.39217,8.85595,9.77454,10.32197]}, + {"t":1.29489, "x":2.29509, "y":1.34275, "heading":0.98089, "vx":-2.27476, "vy":-1.57852, "omega":-0.54843, "ax":4.89699, "ay":0.9307, "alpha":1.00421, "fx":[78.45227,82.36093,84.76224,80.94632], "fy":[14.17933,12.38591,15.94268,19.5492]}, + {"t":1.31992, "x":2.2397, "y":1.30355, "heading":0.96717, "vx":-2.15223, "vy":-1.55524, "omega":-0.5233, "ax":4.86192, "ay":1.11584, "alpha":2.39031, "fx":[74.01309,82.32147,88.14359,79.70552], "fy":[17.13494,9.46202,20.00094,27.80421]}, + {"t":1.34494, "x":2.18737, "y":1.26499, "heading":0.95407, "vx":-2.03058, "vy":-1.52732, "omega":-0.4635, "ax":4.83683, "ay":1.22927, "alpha":3.83532, "fx":[68.67853,83.42752,92.05539,78.34894], "fy":[17.81678,6.69404,22.14327,35.31128]}, + {"t":1.36996, "x":2.13808, "y":1.22715, "heading":0.94248, "vx":-1.90956, "vy":-1.49656, "omega":-0.36753, "ax":4.71783, "ay":1.62191, "alpha":3.76436, "fx":[66.73874,81.56204,90.06939,76.20589], "fy":[24.95929,13.58029,28.21701,41.38923]}, + {"t":1.40498, "x":2.0741, "y":1.17574, "heading":0.92961, "vx":-1.74433, "vy":-1.43975, "omega":-0.2357, "ax":4.50992, "ay":2.14795, "alpha":2.87802, "fx":[66.2051,77.52701,83.88609,73.09491], "fy":[36.04537,24.61173,36.24196,46.32188]}, + {"t":1.44, "x":2.01577, "y":1.12663, "heading":0.92135, "vx":-1.58638, "vy":-1.36453, "omega":-0.1349, "ax":4.32069, "ay":2.50828, "alpha":2.12628, "fx":[65.15064,73.85079,78.76796,70.32615], "fy":[41.25476,34.34501,41.93016,49.71738]}, + {"t":1.47502, "x":1.96286, "y":1.08038, "heading":0.91663, "vx":-1.43506, "vy":-1.27668, "omega":-0.06043, "ax":4.16112, "ay":2.76589, "alpha":1.688, "fx":[63.99562,70.87842,74.713,67.86862], "fy":[46.47634,39.73226,46.09167,52.12382]}, + {"t":1.51005, "x":1.91516, "y":1.03736, "heading":0.91451, "vx":-1.28933, "vy":-1.17981, "omega":-0.00132, "ax":4.02812, "ay":2.95693, "alpha":1.2603, "fx":[63.03009,68.2874,71.21085,66.05895], "fy":[49.1746,44.83043,49.30644,53.85081]}, + {"t":1.54507, "x":1.87247, "y":0.99786, "heading":0.91446, "vx":-1.14825, "vy":-1.07626, "omega":0.04282, "ax":3.917, "ay":3.10316, "alpha":0.95517, "fx":[62.40676,66.25923,68.46436,64.04739], "fy":[51.62309,48.32118,51.73807,55.23033]}, + {"t":1.58009, "x":1.83466, "y":0.96207, "heading":0.91596, "vx":-1.01107, "vy":-0.96757, "omega":0.07628, "ax":3.82344, "ay":3.21814, "alpha":0.67746, "fx":[61.57523,64.41396,65.82517,63.12546], "fy":[53.72795,51.05115,53.7595,56.04061]}, + {"t":1.61511, "x":1.80159, "y":0.93015, "heading":0.91864, "vx":-0.87716, "vy":-0.85487, "omega":0.1, "ax":3.74395, "ay":3.31061, "alpha":0.39571, "fx":[61.4356,62.88859,63.79943,61.51607], "fy":[54.7407,53.92394,55.27609,56.80443]}, + {"t":1.65014, "x":1.77317, "y":0.90224, "heading":0.92214, "vx":-0.74604, "vy":-0.73892, "omega":0.11386, "ax":3.67578, "ay":3.38642, "alpha":0.1796, "fx":[60.80088,61.53079,61.64645,61.11557], "fy":[56.62372,55.47038,56.64386,57.06233]}, + {"t":1.68516, "x":1.74929, "y":0.87844, "heading":0.92613, "vx":-0.61731, "vy":-0.62032, "omega":0.12015, "ax":3.61677, "ay":3.4496, "alpha":-0.11621, "fx":[61.05146,60.27521,59.97689,59.8557], "fy":[56.88729,58.0954,57.64373,57.3865]}, + {"t":1.72018, "x":1.72989, "y":0.85883, "heading":0.93033, "vx":-0.49064, "vy":-0.49951, "omega":0.11608, "ax":3.56527, "ay":3.503, "alpha":-0.3341, "fx":[60.674,59.19442,58.07688,59.78013], "fy":[58.56038,59.19511,58.59659,57.22107]}, + {"t":1.7552, "x":1.7149, "y":0.84349, "heading":0.9344, "vx":-0.36577, "vy":-0.37682, "omega":0.10438, "ax":3.51998, "ay":3.54868, "alpha":-0.67841, "fx":[60.6876,57.92615,56.33525,59.75636], "fy":[59.51543,61.39634,59.06675,56.64033]}, + {"t":1.79023, "x":1.70425, "y":0.83247, "heading":0.93805, "vx":-0.24249, "vy":-0.25254, "omega":0.08062, "ax":3.47986, "ay":3.58817, "alpha":-0.9976, "fx":[61.28225,56.93201,54.75641,59.05983], "fy":[59.75315,63.41312,59.71884,56.367]}, + {"t":1.82525, "x":1.69789, "y":0.82582, "heading":0.94088, "vx":-0.12062, "vy":-0.12687, "omega":0.04568, "ax":3.4441, "ay":3.62263, "alpha":-1.30436, "fx":[61.70191,56.41958,52.83887,58.68581], "fy":[60.45793,64.82164,60.41152,55.859]}, + {"t":1.86027, "x":1.69577, "y":0.8236, "heading":0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/JTofetchSlow.traj b/src/main/deploy/choreo/JTofetchSlow.traj index b9504a8f..9d7c402e 100644 --- a/src/main/deploy/choreo/JTofetchSlow.traj +++ b/src/main/deploy/choreo/JTofetchSlow.traj @@ -3,30 +3,30 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":5.079583864905389, "y":5.098261807735684, "heading":4.1887902047863905, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.077451705932617, "y":6.090557098388672, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":1.949252247810364, "y":6.8849358558654785, "heading":-0.942477796076938, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.1366723649053885, "y":5.197141990263579, "heading":4.1887902047863905, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.532416343688965, "y":5.864877223968506, "heading":0.0, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.1380813121795654, "y":6.824644565582275, "heading":-0.942477796076938, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":1.69577419757843, "y":7.228199481964111, "heading":-0.942477796076938, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":3.0}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.077451705932617 m", "val":4.077451705932617}, "y":{"exp":"6.090557098388672 m", "val":6.090557098388672}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"1.9492522478103638 m", "val":1.949252247810364}, "y":{"exp":"6.8849358558654785 m", "val":6.8849358558654785}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.532416343688965 m", "val":4.532416343688965}, "y":{"exp":"5.864877223968506 m", "val":5.864877223968506}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.1380813121795654 m", "val":2.1380813121795654}, "y":{"exp":"6.824644565582275 m", "val":6.824644565582275}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}, {"from":0, "to":3, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"5 m / s ^ 2", "val":5.0}}}, "enabled":true}], "targetDt":{ @@ -36,82 +36,81 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.77299,1.58511,2.0032], + "waypoints":[0.0,0.60323,1.48769,1.97879], "samples":[ - {"t":0.0, "x":5.07958, "y":5.09826, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.25598, "ay":3.78965, "alpha":12.29419, "fx":[-9.26965,-60.16634,-96.6264,-51.04003], "fy":[82.78376,98.73816,48.87787,22.28701]}, - {"t":0.02761, "x":5.07834, "y":5.09971, "heading":-2.0944, "vx":-0.08989, "vy":0.10462, "omega":0.3394, "ax":-3.26019, "ay":3.78836, "alpha":9.79726, "fx":[-18.61416,-58.8375,-88.40782,-51.5237], "fy":[78.35796,91.85094,51.25166,31.14007]}, - {"t":0.05521, "x":5.07462, "y":5.10404, "heading":-2.08503, "vx":-0.17989, "vy":0.2092, "omega":0.60987, "ax":-3.26351, "ay":3.78537, "alpha":7.65105, "fx":[-26.75697,-58.03927,-81.05303,-51.75549], "fy":[74.94357,85.72955,53.34964,38.37815]}, - {"t":0.08282, "x":5.06841, "y":5.11126, "heading":-2.06819, "vx":-0.26999, "vy":0.31371, "omega":0.8211, "ax":-3.26717, "ay":3.78206, "alpha":5.91496, "fx":[-33.28327,-57.42579,-75.17808,-51.96132], "fy":[72.17868,80.69871,54.98073,44.32264]}, - {"t":0.11043, "x":5.05971, "y":5.12136, "heading":-2.04552, "vx":-0.36018, "vy":0.41812, "omega":0.98439, "ax":-3.27126, "ay":3.77836, "alpha":4.38692, "fx":[-38.83279,-57.06681,-69.80227,-52.41973], "fy":[70.08207,75.95351,56.63926,49.25869]}, - {"t":0.13803, "x":5.04852, "y":5.13434, "heading":-2.01834, "vx":-0.45049, "vy":0.52243, "omega":1.1055, "ax":-3.27583, "ay":3.77421, "alpha":3.16201, "fx":[-43.46885,-56.65967,-65.65671,-52.64112], "fy":[68.11903,72.27602,58.00152,53.26047]}, - {"t":0.16564, "x":5.03484, "y":5.1502, "heading":-1.98783, "vx":-0.54093, "vy":0.62662, "omega":1.19279, "ax":-3.28101, "ay":3.7695, "alpha":2.12736, "fx":[-47.13514,-56.25622,-61.98698,-53.39337], "fy":[66.64879,69.00408,59.31021,56.37982]}, - {"t":0.19325, "x":5.01865, "y":5.16894, "heading":-1.9549, "vx":-0.6315, "vy":0.73068, "omega":1.25152, "ax":-3.28689, "ay":3.76414, "alpha":1.27668, "fx":[-50.45435,-55.79998,-59.16509,-53.74397], "fy":[64.96825,66.5826,60.46204,58.97261]}, - {"t":0.22085, "x":4.99997, "y":5.19054, "heading":-1.92035, "vx":-0.72225, "vy":0.8346, "omega":1.28677, "ax":-3.29365, "ay":3.75794, "alpha":0.56041, "fx":[-52.85873,-55.39211,-56.71687,-54.64678], "fy":[63.94984,64.13085,61.55835,60.93347]}, - {"t":0.24846, "x":4.97877, "y":5.21502, "heading":-1.88482, "vx":-0.81317, "vy":0.93834, "omega":1.30224, "ax":-3.30147, "ay":3.75076, "alpha":-0.05426, "fx":[-55.3241,-54.87249,-54.79362,-55.14558], "fy":[62.29925,62.5927,62.58505,62.61631]}, - {"t":0.27607, "x":4.95506, "y":5.24235, "heading":-1.84887, "vx":-0.90432, "vy":1.04189, "omega":1.30074, "ax":-3.31067, "ay":3.74227, "alpha":-0.57299, "fx":[-56.96571,-54.41871,-53.16515,-56.19922], "fy":[61.52835,60.67088,63.52806,63.79995]}, - {"t":0.30368, "x":4.92884, "y":5.27254, "heading":-1.81296, "vx":-0.99571, "vy":1.1452, "omega":1.28492, "ax":-3.32159, "ay":3.73212, "alpha":-1.03267, "fx":[-58.91771,-53.85046,-51.8995,-56.80982], "fy":[59.7843,59.82235,64.38858,64.85536]}, - {"t":0.33128, "x":4.90008, "y":5.30558, "heading":-1.77749, "vx":-1.08741, "vy":1.24824, "omega":1.25641, "ax":-3.33482, "ay":3.71976, "alpha":-1.31969, "fx":[-59.89028,-53.25963,-51.0442,-58.16532], "fy":[59.73738,58.58512,64.6882,65.01553]}, - {"t":0.35889, "x":4.86879, "y":5.34145, "heading":-1.7428, "vx":-1.17948, "vy":1.35093, "omega":1.21998, "ax":-3.35114, "ay":3.70438, "alpha":-1.8084, "fx":[-61.70124,-53.01311,-50.12154,-58.61161], "fy":[57.26018,57.47391,66.00007,66.26673]}, - {"t":0.3865, "x":4.83495, "y":5.38016, "heading":-1.70912, "vx":-1.27199, "vy":1.45319, "omega":1.17006, "ax":-3.37177, "ay":3.68474, "alpha":-2.13436, "fx":[-62.76952,-52.64372,-49.46972,-59.94019], "fy":[56.753,55.86261,66.65708,66.41857]}, - {"t":0.4141, "x":4.79855, "y":5.42168, "heading":-1.67682, "vx":-1.36507, "vy":1.55492, "omega":1.11113, "ax":-3.39871, "ay":3.65875, "alpha":-2.37452, "fx":[-63.98811,-52.4321,-49.45586,-60.74353], "fy":[54.75879,55.64559,66.96086,66.59313]}, - {"t":0.44171, "x":4.75957, "y":5.466, "heading":-1.64615, "vx":-1.4589, "vy":1.65592, "omega":1.04558, "ax":-3.43528, "ay":3.62284, "alpha":-2.62099, "fx":[-65.15271,-52.48335,-49.54781,-61.87378], "fy":[53.97723,53.90508,67.25194,66.4301]}, - {"t":0.46932, "x":4.71799, "y":5.5131, "heading":-1.61728, "vx":-1.55374, "vy":1.75594, "omega":0.97322, "ax":-3.48786, "ay":3.5699, "alpha":-2.7654, "fx":[-66.226,-52.79494,-50.20287,-63.33981], "fy":[51.88498,53.51203,66.89093,65.74617]}, - {"t":0.49692, "x":4.67377, "y":5.56293, "heading":-1.59042, "vx":-1.65003, "vy":1.85449, "omega":0.89688, "ax":-3.56934, "ay":3.48461, "alpha":-2.83034, "fx":[-67.56223,-53.87148,-51.62997,-64.93318], "fy":[51.17629,50.9539,65.85817,64.35907]}, - {"t":0.52453, "x":4.62685, "y":5.61546, "heading":-1.56566, "vx":-1.74856, "vy":1.95069, "omega":0.81874, "ax":-3.71344, "ay":3.32336, "alpha":-2.7951, "fx":[-69.59464,-56.00338,-54.39231,-67.61481], "fy":[46.92203,50.0582,63.14578,61.46946]}, - {"t":0.55214, "x":4.57717, "y":5.67058, "heading":-1.54305, "vx":-1.85108, "vy":2.04244, "omega":0.74158, "ax":-4.02669, "ay":2.91659, "alpha":-2.61889, "fx":[-73.58693,-61.44434,-60.45332,-73.00716], "fy":[42.47725,41.57487,56.01648,54.40418]}, - {"t":0.57974, "x":4.52453, "y":5.72807, "heading":-1.52258, "vx":-1.96225, "vy":2.12296, "omega":0.66928, "ax":-4.87024, "ay":0.72415, "alpha":-2.24076, "fx":[-86.02958,-76.14356,-76.35915,-86.20613], "fy":[5.53338,6.67534,18.36057,17.71552]}, - {"t":0.60735, "x":4.4685, "y":5.78696, "heading":-1.5041, "vx":-2.0967, "vy":2.14295, "omega":0.60742, "ax":-3.44425, "ay":-3.48367, "alpha":-1.7309, "fx":[-60.60535,-53.25511,-54.30851,-61.48727], "fy":[-63.7882,-61.87081,-53.48678,-53.13864]}, - {"t":0.63496, "x":4.40931, "y":5.84479, "heading":-1.48733, "vx":-2.19178, "vy":2.04677, "omega":0.55963, "ax":-3.27755, "ay":-3.67394, "alpha":-1.09372, "fx":[-56.58755,-51.67142,-52.7453,-57.53677], "fy":[-63.12905,-65.20614,-58.43539,-58.2009]}, - {"t":0.66256, "x":4.34755, "y":5.8999, "heading":-1.47188, "vx":-2.28226, "vy":1.94535, "omega":0.52944, "ax":-3.11913, "ay":-3.83388, "alpha":-0.72556, "fx":[-52.86524,-50.02178,-50.58819,-54.50266], "fy":[-66.25018,-65.13882,-62.24617,-62.00041]}, - {"t":0.69017, "x":4.28335, "y":5.95214, "heading":-1.45727, "vx":-2.36837, "vy":1.83951, "omega":0.50941, "ax":-2.94924, "ay":-3.98314, "alpha":-0.39849, "fx":[-49.83327,-48.04976,-48.54604,-50.22034], "fy":[-66.97603,-68.00821,-65.33394,-65.27015]}, - {"t":0.71778, "x":4.21685, "y":6.0014, "heading":-1.44321, "vx":-2.44979, "vy":1.72954, "omega":0.49841, "ax":-2.76898, "ay":-4.1223, "alpha":-0.30731, "fx":[-46.30509,-45.27123,-45.47688,-47.57698], "fy":[-69.64097,-69.12551,-68.12642,-67.97401]}, - {"t":0.74538, "x":4.14816, "y":6.04758, "heading":-1.42945, "vx":-2.52624, "vy":1.61574, "omega":0.48992, "ax":-2.54139, "ay":-4.27458, "alpha":-0.17089, "fx":[-42.6558,-41.89033,-42.11024,-42.79849], "fy":[-71.49907,-71.97363,-70.77179,-70.77653]}, - {"t":0.77299, "x":4.07745, "y":6.09056, "heading":-1.41592, "vx":-2.5964, "vy":1.49773, "omega":0.48521, "ax":-2.42267, "ay":-4.30492, "alpha":0.24844, "fx":[-39.88544,-41.60976,-40.49968,-39.54403], "fy":[-71.30991,-71.20879,-72.26783,-72.25724]}, - {"t":0.7976, "x":4.01282, "y":6.12611, "heading":-1.40398, "vx":-2.65602, "vy":1.39179, "omega":0.49132, "ax":-2.21546, "ay":-4.43998, "alpha":0.05782, "fx":[-36.75631,-37.23996,-37.13875,-36.58755], "fy":[-73.57434,-74.49848,-73.99688,-73.97937]}, - {"t":0.82221, "x":3.94679, "y":6.15902, "heading":-1.39189, "vx":-2.71054, "vy":1.28253, "omega":0.49274, "ax":-2.02737, "ay":-4.51667, "alpha":0.08945, "fx":[-33.60036,-34.43497,-33.66558,-33.48006], "fy":[-75.16736,-75.11769,-75.4551,-75.42304]}, - {"t":0.84682, "x":3.87947, "y":6.18921, "heading":-1.37976, "vx":-2.76043, "vy":1.17137, "omega":0.49495, "ax":-1.83599, "ay":-4.57953, "alpha":-0.04016, "fx":[-30.56678,-30.65149,-30.68914,-30.51257], "fy":[-75.91283,-77.44572,-76.01094,-75.98472]}, - {"t":0.87143, "x":3.81098, "y":6.21665, "heading":-1.36758, "vx":-2.80561, "vy":1.05867, "omega":0.49396, "ax":-1.64029, "ay":-4.6243, "alpha":0.05051, "fx":[-27.2041,-27.8573,-27.14307,-27.16711], "fy":[-77.047,-77.01196,-77.15616,-77.12414]}, - {"t":0.89604, "x":3.74144, "y":6.24131, "heading":-1.35543, "vx":-2.84598, "vy":0.94487, "omega":0.4952, "ax":-1.4394, "ay":-4.64073, "alpha":-0.05463, "fx":[-23.99399,-23.97944,-24.02919,-23.974], "fy":[-77.02165,-78.41175,-77.00994,-76.9919]}, - {"t":0.92065, "x":3.67096, "y":6.26315, "heading":-1.34324, "vx":-2.8814, "vy":0.83066, "omega":0.49386, "ax":-1.23101, "ay":-4.60485, "alpha":0.02994, "fx":[-20.43022,-20.87635,-20.35408,-20.42066], "fy":[-76.74435,-76.72974,-76.79569,-76.77245]}, - {"t":0.94526, "x":3.59968, "y":6.2822, "heading":-1.33109, "vx":-2.9117, "vy":0.71734, "omega":0.49459, "ax":-1.00539, "ay":-4.43495, "alpha":-0.05949, "fx":[-16.76663,-16.74197,-16.77573,-16.75346], "fy":[-73.58974,-75.03791,-73.54696,-73.53911]}, - {"t":0.96987, "x":3.52772, "y":6.29851, "heading":-1.31891, "vx":-2.93644, "vy":0.6082, "omega":0.49313, "ax":-0.64373, "ay":-3.33987, "alpha":0.00645, "fx":[-10.71796,-10.82297,-10.66404,-10.71758], "fy":[-55.66465,-55.66684,-55.68875,-55.67614]}, - {"t":0.99448, "x":3.45526, "y":6.31247, "heading":-1.30678, "vx":-2.95228, "vy":0.52601, "omega":0.49329, "ax":-0.3925, "ay":-2.33416, "alpha":-0.02927, "fx":[-6.54793,-6.53703,-6.54332,-6.54302], "fy":[-38.76476,-39.43387,-38.71806,-38.72078]}, - {"t":1.01909, "x":3.38249, "y":6.32471, "heading":-1.29464, "vx":-2.96194, "vy":0.46856, "omega":0.49257, "ax":-0.2262, "ay":-1.49004, "alpha":-0.01203, "fx":[-3.81533,-3.64906,-3.79824,-3.81972], "fy":[-24.84191,-24.84035,-24.83475,-24.83601]}, - {"t":1.0437, "x":3.30953, "y":6.33579, "heading":-1.28252, "vx":-2.96751, "vy":0.43189, "omega":0.49227, "ax":-0.0718, "ay":-0.49984, "alpha":0.01272, "fx":[-1.19777,-1.18918,-1.19647,-1.20438], "fy":[-8.42513,-8.04199,-8.4283,-8.43288]}, - {"t":1.06831, "x":3.23648, "y":6.34626, "heading":-1.2704, "vx":-2.96928, "vy":0.41959, "omega":0.49258, "ax":0.00177, "ay":0.01271, "alpha":-0.01443, "fx":[-0.02841,0.26091,-0.09354,-0.02089], "fy":[0.21748,0.22268,0.2062,0.20088]}, - {"t":1.09292, "x":3.1634, "y":6.35659, "heading":-1.25828, "vx":-2.96923, "vy":0.41991, "omega":0.49223, "ax":0.13066, "ay":0.89862, "alpha":0.06098, "fx":[2.20693,2.14528,2.14914,2.21058], "fy":[14.78093,15.9191,14.59536,14.6228]}, - {"t":1.11752, "x":3.09037, "y":6.3672, "heading":-1.24617, "vx":-2.96602, "vy":0.44202, "omega":0.49373, "ax":0.24208, "ay":1.55565, "alpha":0.01248, "fx":[4.00568,4.29469,3.75889,4.08232], "fy":[26.03907,26.0005,25.8258,25.86259]}, - {"t":1.14213, "x":3.01745, "y":6.37855, "heading":-1.23402, "vx":-2.96006, "vy":0.4803, "omega":0.49404, "ax":0.47969, "ay":2.75652, "alpha":0.16758, "fx":[8.1278,7.75623,7.86803,8.23255], "fy":[45.72455,47.84436,45.04249,45.18786]}, - {"t":1.16674, "x":2.94475, "y":6.3912, "heading":-1.22186, "vx":-2.94825, "vy":0.54814, "omega":0.49816, "ax":0.71994, "ay":3.57139, "alpha":0.12056, "fx":[12.13079,11.93413,11.50477,12.43463], "fy":[59.95053,59.75621,59.1204,59.30597]}, - {"t":1.19135, "x":2.87242, "y":6.40577, "heading":-1.2096, "vx":-2.93054, "vy":0.63603, "omega":0.50113, "ax":1.07074, "ay":4.51543, "alpha":0.37133, "fx":[18.22856,16.9776,17.49142,18.6974], "fy":[75.40711,77.9933,73.62614,74.05371]}, - {"t":1.21596, "x":2.80062, "y":6.42279, "heading":-1.19727, "vx":-2.90419, "vy":0.74715, "omega":0.51027, "ax":1.28767, "ay":4.62381, "alpha":0.40109, "fx":[21.99447,20.30066,20.64503,22.91927], "fy":[78.22447,77.67932,75.92969,76.47354]}, - {"t":1.24057, "x":2.72954, "y":6.44258, "heading":-1.18471, "vx":-2.8725, "vy":0.86094, "omega":0.52014, "ax":1.49351, "ay":4.64077, "alpha":0.77195, "fx":[25.7493,22.52528,24.08892,27.22082], "fy":[78.67481,80.5392,74.60487,75.61906]}, - {"t":1.26518, "x":2.6593, "y":6.46517, "heading":-1.17191, "vx":-2.83574, "vy":0.97515, "omega":0.53913, "ax":1.69319, "ay":4.61471, "alpha":1.01325, "fx":[29.43349,24.63273,26.95939,31.87318], "fy":[79.71566,78.43471,74.11294,75.43667]}, - {"t":1.28979, "x":2.59003, "y":6.49057, "heading":-1.15864, "vx":-2.79407, "vy":1.08872, "omega":0.56407, "ax":1.92741, "ay":4.54769, "alpha":1.54043, "fx":[33.46416,27.1038,30.89144,37.05639], "fy":[79.38347,80.51426,70.6006,72.73311]}, - {"t":1.3144, "x":2.52185, "y":6.51874, "heading":-1.14476, "vx":-2.74664, "vy":1.20063, "omega":0.60198, "ax":3.55495, "ay":3.45546, "alpha":1.7811, "fx":[60.6246,53.78695,57.63959,64.98591], "fy":[63.39443,60.61342,51.74388,54.65161]}, - {"t":1.33901, "x":2.45533, "y":6.54933, "heading":-1.12994, "vx":-2.65916, "vy":1.28567, "omega":0.64581, "ax":4.63657, "ay":1.80154, "alpha":2.0382, "fx":[78.85193,71.04542,75.82011,83.44021], "fy":[36.74526,33.72272,22.89543,26.7599]}, - {"t":1.36362, "x":2.3913, "y":6.58152, "heading":-1.11405, "vx":-2.54505, "vy":1.33001, "omega":0.69597, "ax":4.88149, "ay":0.99815, "alpha":1.9757, "fx":[82.74815,75.96959,79.59655,87.17389], "fy":[23.78947,19.7169,9.37265,13.67545]}, - {"t":1.38823, "x":2.33014, "y":6.61455, "heading":-1.09692, "vx":-2.42492, "vy":1.35457, "omega":0.74459, "ax":4.9542, "ay":0.57008, "alpha":1.79546, "fx":[83.9808,77.20227,81.24948,87.90385], "fy":[16.18776,11.43281,3.12614,7.26493]}, - {"t":1.41284, "x":2.27197, "y":6.64806, "heading":-1.0786, "vx":-2.303, "vy":1.3686, "omega":0.78878, "ax":4.9798, "ay":0.31204, "alpha":1.37371, "fx":[83.92849,79.31941,81.82485,86.97054], "fy":[10.3866,7.00109,-0.01924,3.43802]}, - {"t":1.43745, "x":2.2168, "y":6.68183, "heading":-1.05919, "vx":-2.18045, "vy":1.37628, "omega":0.82258, "ax":4.9894, "ay":0.13953, "alpha":0.75869, "fx":[83.76608,80.81058,82.58334,85.52337], "fy":[5.36878,2.44768,-0.23991,1.7271]}, - {"t":1.46206, "x":2.16465, "y":6.71575, "heading":-1.03894, "vx":-2.05766, "vy":1.37971, "omega":0.84125, "ax":4.99258, "ay":0.01786, "alpha":0.0233, "fx":[82.70749,83.42244,82.67242,84.09355], "fy":[0.19765,0.21526,0.54043,0.23772]}, - {"t":1.48667, "x":2.11552, "y":6.74971, "heading":-1.01824, "vx":-1.9348, "vy":1.38015, "omega":0.84183, "ax":4.99302, "ay":-0.07362, "alpha":-1.09232, "fx":[82.68885,86.14777,83.80401,80.28438], "fy":[-5.21051,-3.28011,3.27658,0.30492]}, - {"t":1.51128, "x":2.06942, "y":6.78365, "heading":-0.99752, "vx":-1.81192, "vy":1.37834, "omega":0.81495, "ax":4.99222, "ay":-0.14388, "alpha":-2.33653, "fx":[82.01254,89.98078,84.43711,76.44146], "fy":[-11.43168,-4.5381,6.47719,-0.10123]}, - {"t":1.53589, "x":2.02634, "y":6.81753, "heading":-0.97747, "vx":-1.68906, "vy":1.3748, "omega":0.75745, "ax":4.99087, "ay":-0.2002, "alpha":-3.7797, "fx":[81.58279,93.71185,85.21617,72.27063], "fy":[-17.98186,-7.31461,11.38227,0.56504]}, - {"t":1.5605, "x":1.98629, "y":6.8513, "heading":-0.95883, "vx":-1.56624, "vy":1.36987, "omega":0.66443, "ax":4.9893, "ay":-0.24591, "alpha":-5.49739, "fx":[81.10589,98.67055,85.97874,66.92182], "fy":[-25.84,-7.98086,16.96387,0.45998]}, - {"t":1.58511, "x":1.94925, "y":6.88494, "heading":-0.94248, "vx":-1.44346, "vy":1.36382, "omega":0.52914, "ax":4.94872, "ay":-0.66909, "alpha":-5.87898, "fx":[80.06385,98.73716,85.86874,65.30165], "fy":[-34.26798,-15.41566,12.03692,-6.96711]}, - {"t":1.61995, "x":1.90196, "y":6.93205, "heading":-0.92404, "vx":-1.27104, "vy":1.34051, "omega":0.32431, "ax":4.78383, "ay":-1.44378, "alpha":-4.54682, "fx":[77.05466,92.31939,82.7566,66.84569], "fy":[-42.5949,-25.45428,-6.08382,-22.1354]}, - {"t":1.65479, "x":1.86058, "y":6.97788, "heading":-0.91274, "vx":-1.10436, "vy":1.29021, "omega":0.16589, "ax":4.5189, "ay":-2.13317, "alpha":-3.45021, "fx":[72.86149,85.21467,78.18515,65.05014], "fy":[-48.67229,-36.83722,-21.85193,-34.87441]}, - {"t":1.68963, "x":1.82485, "y":7.02153, "heading":-0.90696, "vx":-0.94691, "vy":1.21588, "omega":0.04568, "ax":4.19743, "ay":-2.71187, "alpha":-2.64754, "fx":[67.66828,77.82474,72.31418,62.06934], "fy":[-55.53431,-45.12448,-35.12429,-45.03905]}, - {"t":1.72447, "x":1.7944, "y":7.06225, "heading":-0.90537, "vx":-0.80067, "vy":1.1214, "omega":-0.04657, "ax":3.85683, "ay":-3.17797, "alpha":-1.87733, "fx":[62.6223,70.20436,66.15312,58.18599], "fy":[-59.57537,-53.28979,-46.00096,-53.03462]}, - {"t":1.75931, "x":1.76885, "y":7.09939, "heading":-0.90699, "vx":-0.66629, "vy":1.01067, "omega":-0.11197, "ax":3.52346, "ay":-3.54425, "alpha":-1.27448, "fx":[57.42082,62.8839,59.95288,54.68012], "fy":[-63.77658,-58.87754,-54.57561,-59.09428]}, - {"t":1.79416, "x":1.74777, "y":7.13246, "heading":-0.91089, "vx":-0.54353, "vy":0.88718, "omega":-0.15638, "ax":3.21223, "ay":-3.82883, "alpha":-0.68827, "fx":[52.89034,55.85546,54.2677,51.17155], "fy":[-66.08306,-63.98001,-61.40244,-63.83369]}, - {"t":1.829, "x":1.73078, "y":7.16104, "heading":-0.91634, "vx":-0.43161, "vy":0.75378, "omega":-0.18036, "ax":2.92952, "ay":-4.04944, "alpha":-0.13368, "fx":[48.58839,49.26543,48.91996,48.5612], "fy":[-68.11929,-67.45984,-67.01587,-67.41356]}, - {"t":1.86384, "x":1.71752, "y":7.18485, "heading":-0.92263, "vx":-0.32954, "vy":0.61269, "omega":-0.18502, "ax":2.67659, "ay":-4.22107, "alpha":0.42289, "fx":[45.06735,43.06276,44.20382,46.13573], "fy":[-68.96339,-70.51016,-71.68142,-70.29784]}, - {"t":1.89868, "x":1.70767, "y":7.20363, "heading":-0.92907, "vx":-0.23628, "vy":0.46562, "omega":-0.17028, "ax":2.45196, "ay":-4.35555, "alpha":0.99832, "fx":[41.79024,37.20779,39.84972,44.64428], "fy":[-69.54135,-72.60449,-75.72864,-72.545]}, - {"t":1.93352, "x":1.70092, "y":7.21721, "heading":-0.93501, "vx":-0.15085, "vy":0.31387, "omega":-0.1355, "ax":2.253, "ay":-4.46185, "alpha":1.62348, "fx":[39.26474,31.48881,35.97302,43.49926], "fy":[-69.25925,-74.45092,-79.37447,-74.42277]}, - {"t":1.96836, "x":1.69703, "y":7.22544, "heading":-0.93973, "vx":-0.07236, "vy":0.15841, "omega":-0.07894, "ax":2.07673, "ay":-4.54669, "alpha":2.26557, "fx":[36.91177,26.25391,32.38634,42.92038], "fy":[-68.63227,-75.71395,-82.80016,-76.01793]}, - {"t":2.0032, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":5.13667, "y":5.19714, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.97063, "ay":4.01702, "alpha":13.86886, "fx":[1.83289,-57.13977,-98.34625,-44.42277], "fy":[88.26359,106.05218,51.96135,21.5698]}, + {"t":0.02623, "x":5.13565, "y":5.19852, "heading":-2.0944, "vx":-0.07791, "vy":0.10536, "omega":0.36375, "ax":-2.99691, "ay":3.99978, "alpha":11.8486, "fx":[-6.13368,-56.30172,-91.89129,-45.50121], "fy":[84.52782,100.38958,53.37494,28.40512]}, + {"t":0.05246, "x":5.13258, "y":5.20266, "heading":-2.08485, "vx":-0.15651, "vy":0.21026, "omega":0.67451, "ax":-3.02421, "ay":3.97905, "alpha":9.89124, "fx":[-14.62388,-55.81388,-85.29285,-45.91809], "fy":[81.26542,95.00383,54.65254,34.39377]}, + {"t":0.07868, "x":5.12743, "y":5.20955, "heading":-2.06716, "vx":-0.23583, "vy":0.31462, "omega":0.93393, "ax":-3.05401, "ay":3.9561, "alpha":8.28141, "fx":[-20.72028,-55.80944,-80.32049,-46.78517], "fy":[78.56073,89.81633,55.45997,39.94809]}, + {"t":0.10491, "x":5.1202, "y":5.21916, "heading":-2.04267, "vx":-0.31593, "vy":0.41838, "omega":1.15113, "ax":-3.08663, "ay":3.93056, "alpha":6.5763, "fx":[-27.99529,-55.84941,-74.69067,-47.27513], "fy":[75.59543,84.70216,56.7265,45.05786]}, + {"t":0.13114, "x":5.11085, "y":5.23148, "heading":-2.01248, "vx":-0.39689, "vy":0.52147, "omega":1.32361, "ax":-3.1225, "ay":3.90196, "alpha":5.17586, "fx":[-33.42328,-56.04013,-70.31817,-48.42052], "fy":[73.71803,79.62732,57.51194,49.31807]}, + {"t":0.15737, "x":5.09936, "y":5.2465, "heading":-1.97776, "vx":-0.47878, "vy":0.62381, "omega":1.45936, "ax":-3.1621, "ay":3.86976, "alpha":3.79116, "fx":[-39.56554,-55.88376,-66.02448,-49.36894], "fy":[70.64148,75.59212,58.5485,53.24598]}, + {"t":0.18359, "x":5.08572, "y":5.26419, "heading":-1.93949, "vx":-0.56172, "vy":0.7253, "omega":1.5588, "ax":-3.20604, "ay":3.83323, "alpha":2.5784, "fx":[-44.33053,-55.99337,-62.39506,-51.05353], "fy":[68.82283,70.8633,59.51684,56.38957]}, + {"t":0.20982, "x":5.06988, "y":5.28454, "heading":-1.8986, "vx":-0.6458, "vy":0.82584, "omega":1.62642, "ax":-3.25503, "ay":3.79149, "alpha":1.47091, "fx":[-49.43145,-55.83181,-59.49471,-52.2814], "fy":[65.53281,67.53207,60.51725,59.2268]}, + {"t":0.23605, "x":5.05183, "y":5.3075, "heading":-1.85595, "vx":-0.73117, "vy":0.92528, "omega":1.665, "ax":-3.30997, "ay":3.74336, "alpha":0.44843, "fx":[-53.5176,-55.74148,-56.70751,-54.73592], "fy":[63.62105,63.30079,61.54065,61.13731]}, + {"t":0.26228, "x":5.03151, "y":5.33305, "heading":-1.81228, "vx":-0.81799, "vy":1.02346, "omega":1.67676, "ax":-3.37194, "ay":3.68732, "alpha":-0.40163, "fx":[-57.79891,-55.73213,-55.27506,-56.02868], "fy":[60.03785,60.55767,62.34012,62.92805]}, + {"t":0.2885, "x":5.0089, "y":5.36117, "heading":-1.7683, "vx":-0.90642, "vy":1.12017, "omega":1.66623, "ax":-3.4423, "ay":3.62138, "alpha":-1.23923, "fx":[-61.25388,-55.50281,-53.42529,-59.34366], "fy":[57.88639,56.90379,63.16037,63.51602]}, + {"t":0.31473, "x":4.98394, "y":5.39179, "heading":-1.7246, "vx":-0.99671, "vy":1.21515, "omega":1.63372, "ax":-3.52272, "ay":3.54277, "alpha":-1.84465, "fx":[-64.67025,-55.83351,-53.33757,-61.04679], "fy":[54.48733,54.25483,63.43374,64.04929]}, + {"t":0.34096, "x":4.95659, "y":5.42488, "heading":-1.68175, "vx":-1.0891, "vy":1.30807, "omega":1.58534, "ax":-3.61529, "ay":3.44774, "alpha":-2.50876, "fx":[-67.88763,-55.77196,-52.68307,-64.71799], "fy":[51.40695,51.35929,63.70195,63.42069]}, + {"t":0.36719, "x":4.92678, "y":5.46037, "heading":-1.64017, "vx":-1.18392, "vy":1.39849, "omega":1.51955, "ax":-3.72263, "ay":3.33093, "alpha":-2.90871, "fx":[-70.69842,-56.79533,-53.88943,-66.83486], "fy":[48.16182,48.12971,63.14597,62.66253]}, + {"t":0.39341, "x":4.89445, "y":5.4982, "heading":-1.60032, "vx":-1.28156, "vy":1.48586, "omega":1.44326, "ax":-3.84786, "ay":3.18467, "alpha":-3.3848, "fx":[-73.82463,-57.32506,-54.64952,-70.7684], "fy":[43.86432,45.66614,62.24319,60.57391]}, + {"t":0.41964, "x":4.85951, "y":5.53826, "heading":-1.56246, "vx":-1.38248, "vy":1.56938, "omega":1.35448, "ax":-3.99465, "ay":2.99747, "alpha":-3.59073, "fx":[-76.36536,-59.38983,-57.21092,-73.3894], "fy":[40.48923,41.03121,60.20038,58.14473]}, + {"t":0.44587, "x":4.82188, "y":5.58046, "heading":-1.52694, "vx":-1.48725, "vy":1.648, "omega":1.26031, "ax":-4.16661, "ay":2.75209, "alpha":-3.86077, "fx":[-79.53732,-61.05852,-59.64687,-77.57865], "fy":[34.36149,38.02605,57.10633,54.01005]}, + {"t":0.4721, "x":4.78144, "y":5.62463, "heading":-1.49388, "vx":-1.59653, "vy":1.72018, "omega":1.15905, "ax":-4.36586, "ay":2.42171, "alpha":-3.84908, "fx":[-82.18004,-64.41904,-63.7146,-80.79328], "fy":[29.94643,30.68081,52.07847,48.76904]}, + {"t":0.49832, "x":4.73807, "y":5.67057, "heading":-1.46349, "vx":-1.71103, "vy":1.7837, "omega":1.0581, "ax":-4.58816, "ay":1.96538, "alpha":-3.8891, "fx":[-85.31526,-67.50257,-67.88163,-85.23007], "fy":[20.38147,25.08981,44.64914,40.9274]}, + {"t":0.52455, "x":4.69161, "y":5.71803, "heading":-1.43573, "vx":-1.83137, "vy":1.83524, "omega":0.95609, "ax":-4.81152, "ay":1.32258, "alpha":-3.66046, "fx":[-87.79082,-71.51088,-72.83393,-88.6875], "fy":[12.1902,12.37867,33.49715,30.12095]}, + {"t":0.55078, "x":4.64192, "y":5.76662, "heading":-1.41066, "vx":-1.95756, "vy":1.86993, "omega":0.86009, "ax":-4.97061, "ay":0.42099, "alpha":-3.43186, "fx":[-89.17349,-74.15291,-76.62598,-91.4783], "fy":[-4.05284,0.11016,17.4088,14.60493]}, + {"t":0.57701, "x":4.58887, "y":5.81581, "heading":-1.3881, "vx":-2.08793, "vy":1.88097, "omega":0.77008, "ax":-4.92672, "ay":-0.77269, "alpha":-3.0306, "fx":[-87.18904,-73.8331,-77.10104,-90.38075], "fy":[-20.6444,-21.07083,-3.86875,-5.9373]}, + {"t":0.60323, "x":4.53242, "y":5.86488, "heading":-1.3679, "vx":-2.21715, "vy":1.86071, "omega":0.69059, "ax":-4.62455, "ay":-1.81483, "alpha":-2.59137, "fx":[-80.8666,-70.31224,-73.01156,-84.16583], "fy":[-37.7657,-36.76897,-22.42315,-24.05163]}, + {"t":0.6285, "x":4.47491, "y":5.91132, "heading":-1.35045, "vx":-2.33401, "vy":1.81485, "omega":0.62511, "ax":-4.04707, "ay":-2.89936, "alpha":-2.04854, "fx":[-70.40196,-61.38918,-64.50426,-73.55577], "fy":[-53.78125,-53.45613,-42.58237,-43.50402]}, + {"t":0.65377, "x":4.41464, "y":5.95625, "heading":-1.33465, "vx":-2.43628, "vy":1.74158, "omega":0.57334, "ax":-2.92885, "ay":-4.01784, "alpha":-1.62666, "fx":[-51.38084,-42.98886,-46.95213,-53.96827], "fy":[-70.98434,-70.65072,-62.79571,-63.47137]}, + {"t":0.67904, "x":4.35214, "y":5.99898, "heading":-1.32017, "vx":-2.51029, "vy":1.64005, "omega":0.53224, "ax":-2.63206, "ay":-4.20967, "alpha":-1.14107, "fx":[-45.52095,-40.22959,-42.25782,-47.49209], "fy":[-72.60695,-73.49404,-67.06964,-67.52229]}, + {"t":0.70432, "x":4.28786, "y":6.03908, "heading":-1.30672, "vx":-2.5768, "vy":1.53367, "omega":0.5034, "ax":-2.44569, "ay":-4.3086, "alpha":-0.73153, "fx":[-41.95046,-38.25242,-39.6559,-43.21531], "fy":[-73.57394,-73.39096,-69.9875,-70.33686]}, + {"t":0.72959, "x":4.22197, "y":6.07646, "heading":-1.294, "vx":-2.63861, "vy":1.42479, "omega":0.48492, "ax":-2.25651, "ay":-4.39282, "alpha":-0.5219, "fx":[-38.36831,-36.00023,-36.88785,-39.20305], "fy":[-74.02779,-75.26464,-71.68483,-71.92764]}, + {"t":0.75486, "x":4.15457, "y":6.11106, "heading":-1.28174, "vx":-2.69563, "vy":1.31378, "omega":0.47173, "ax":-2.06131, "ay":-4.4595, "alpha":-0.30947, "fx":[-34.954,-33.11402,-33.96146,-35.41449], "fy":[-74.99445,-74.95604,-73.6217,-73.77874]}, + {"t":0.78013, "x":4.08579, "y":6.14284, "heading":-1.26982, "vx":-2.74772, "vy":1.20109, "omega":0.46391, "ax":-1.85839, "ay":-4.49896, "alpha":-0.25145, "fx":[-31.2829,-30.36786,-30.71568,-31.54771], "fy":[-74.87741,-77.02898,-73.99039,-74.08551]}, + {"t":0.8054, "x":4.01576, "y":6.17175, "heading":-1.2581, "vx":-2.79468, "vy":1.0874, "omega":0.45755, "ax":-1.64306, "ay":-4.48661, "alpha":-0.12011, "fx":[-27.69971,-26.76894,-27.28766,-27.79947], "fy":[-74.97917,-75.00681,-74.56745,-74.60492]}, + {"t":0.83067, "x":3.94461, "y":6.1978, "heading":-1.24654, "vx":-2.8362, "vy":0.97402, "omega":0.45452, "ax":-1.39847, "ay":-4.34135, "alpha":-0.1092, "fx":[-23.41512,-23.15769,-23.23055,-23.444], "fy":[-72.04807,-73.86265,-71.77991,-71.78233]}, + {"t":0.85594, "x":3.8725, "y":6.22103, "heading":-1.23505, "vx":-2.87154, "vy":0.86431, "omega":0.45176, "ax":-0.93667, "ay":-3.2808, "alpha":-0.05605, "fx":[-15.84287,-15.12937,-15.66551,-15.81734], "fy":[-54.69747,-54.75219,-54.66801,-54.63975]}, + {"t":0.88121, "x":3.79963, "y":6.24182, "heading":-1.22363, "vx":-2.89521, "vy":0.78141, "omega":0.45034, "ax":-0.55241, "ay":-2.12447, "alpha":-0.02631, "fx":[-9.23321,-9.22326,-9.18764,-9.18938], "fy":[-35.2726,-35.96917,-35.22592,-35.18796]}, + {"t":0.90648, "x":3.72629, "y":6.26089, "heading":-1.21225, "vx":-2.90917, "vy":0.72772, "omega":0.44968, "ax":-0.34105, "ay":-1.40021, "alpha":-0.04121, "fx":[-5.89347,-5.20235,-5.79526,-5.84946], "fy":[-23.29897,-23.3558,-23.37899,-23.32945]}, + {"t":0.93175, "x":3.65267, "y":6.27883, "heading":-1.20089, "vx":-2.91779, "vy":0.69234, "omega":0.44864, "ax":-0.13791, "ay":-0.58669, "alpha":0.00786, "fx":[-2.30785,-2.33686,-2.29003,-2.26063], "fy":[-9.85206,-9.63793,-9.83155,-9.79786]}, + {"t":0.95702, "x":3.57889, "y":6.29614, "heading":-1.18955, "vx":-2.92127, "vy":0.67751, "omega":0.44883, "ax":-0.10715, "ay":-0.46732, "alpha":-0.02902, "fx":[-1.93116,-1.4627,-1.84385,-1.90718], "fy":[-7.76157,-7.8012,-7.81789,-7.77933]}, + {"t":0.98229, "x":3.50504, "y":6.31311, "heading":-1.17821, "vx":-2.92398, "vy":0.6657, "omega":0.4481, "ax":-0.01997, "ay":-0.08718, "alpha":0.00553, "fx":[-0.34834,-0.3443,-0.31726,-0.32146], "fy":[-1.60463,-1.16024,-1.52814,-1.51992]}, + {"t":1.00756, "x":3.43114, "y":6.32991, "heading":-1.16689, "vx":-2.92449, "vy":0.6635, "omega":0.44824, "ax":-0.01753, "ay":-0.07765, "alpha":-0.01769, "fx":[-0.36188,-0.17621,-0.26197,-0.36877], "fy":[-1.30097,-1.31511,-1.28773,-1.27361]}, + {"t":1.03283, "x":3.35723, "y":6.34665, "heading":-1.15556, "vx":-2.92493, "vy":0.66154, "omega":0.44779, "ax":0.00026, "ay":0.00097, "alpha":-0.00783, "fx":[-0.02067,0.02734,0.02935,-0.01867], "fy":[-0.1486,0.23405,0.00064,-0.02111]}, + {"t":1.0581, "x":3.28332, "y":6.36337, "heading":-1.14424, "vx":-2.92492, "vy":0.66156, "omega":0.4476, "ax":0.03733, "ay":0.16513, "alpha":-0.00319, "fx":[0.63503,0.50986,0.74451,0.59997], "fy":[2.71513,2.72437,2.78992,2.78083]}, + {"t":1.08337, "x":3.20942, "y":6.38014, "heading":-1.13293, "vx":-2.92398, "vy":0.66574, "omega":0.44752, "ax":0.03201, "ay":0.13924, "alpha":-0.01451, "fx":[0.50713,0.57244,0.56028,0.49463], "fy":[2.15291,2.494,2.33938,2.29813]}, + {"t":1.10864, "x":3.13554, "y":6.397, "heading":-1.12162, "vx":-2.92317, "vy":0.66926, "omega":0.44715, "ax":0.14348, "ay":0.62025, "alpha":0.02015, "fx":[2.49294,2.03487,2.58911,2.44972], "fy":[10.30284,10.31789,10.37461,10.36196]}, + {"t":1.13391, "x":3.06172, "y":6.41411, "heading":-1.11033, "vx":-2.91954, "vy":0.68493, "omega":0.44766, "ax":0.19286, "ay":0.80812, "alpha":0.00497, "fx":[3.20746,3.20943,3.22388,3.21861], "fy":[13.25948,13.89847,13.37561,13.35027]}, + {"t":1.15918, "x":2.988, "y":6.43168, "heading":-1.09901, "vx":-2.91467, "vy":0.70535, "omega":0.44778, "ax":0.42753, "ay":1.71135, "alpha":0.06352, "fx":[7.31858,6.50801,7.35259,7.32747], "fy":[28.57472,28.54649,28.4737,28.51434]}, + {"t":1.18445, "x":2.91448, "y":6.45005, "heading":-1.0877, "vx":-2.90387, "vy":0.7486, "omega":0.44939, "ax":0.71397, "ay":2.64323, "alpha":0.08216, "fx":[11.95556,11.69976,11.85726,12.09379], "fy":[43.7275,45.39165,43.52513,43.6009]}, + {"t":1.20972, "x":2.84133, "y":6.46981, "heading":-1.07634, "vx":-2.88582, "vy":0.81539, "omega":0.45146, "ax":1.14009, "ay":3.79027, "alpha":0.13601, "fx":[19.22848,18.23163,19.11269,19.44632], "fy":[63.48865,63.31366,62.85482,63.07097]}, + {"t":1.23499, "x":2.76877, "y":6.49163, "heading":-1.06493, "vx":-2.85701, "vy":0.91117, "omega":0.4549, "ax":1.5117, "ay":4.43467, "alpha":0.23257, "fx":[25.38232,24.46977,25.05391,25.89143], "fy":[73.77951,76.00424,72.78969,73.12161]}, + {"t":1.26026, "x":2.69706, "y":6.51607, "heading":-1.05344, "vx":-2.81881, "vy":1.02324, "omega":0.46078, "ax":1.73903, "ay":4.50146, "alpha":0.31347, "fx":[29.29465,27.68128,28.92856,30.05062], "fy":[75.92602,75.40141,74.10611,74.71543]}, + {"t":1.28553, "x":2.62638, "y":6.54336, "heading":-1.04179, "vx":-2.77487, "vy":1.13699, "omega":0.4687, "ax":1.94833, "ay":4.48606, "alpha":0.5001, "fx":[32.8321,30.71318,32.19637,34.16905], "fy":[75.37113,77.27978,72.82385,73.64734]}, + {"t":1.3108, "x":2.55688, "y":6.57353, "heading":-1.02995, "vx":-2.72563, "vy":1.25035, "omega":0.48134, "ax":2.19905, "ay":4.4076, "alpha":0.67315, "fx":[37.23357,33.77852,36.73465,38.88186], "fy":[75.42862,74.22409,71.45972,72.77787]}, + {"t":1.33607, "x":2.4887, "y":6.60653, "heading":-1.01779, "vx":-2.67006, "vy":1.36173, "omega":0.49835, "ax":4.18949, "ay":2.64375, "alpha":0.63852, "fx":[70.17697,67.7081,69.54176,71.92017], "fy":[45.72519,45.76624,41.67648,43.11234]}, + {"t":1.36134, "x":2.42257, "y":6.64179, "heading":-1.00519, "vx":-2.56419, "vy":1.42854, "omega":0.51448, "ax":4.8922, "ay":0.90134, "alpha":0.31958, "fx":[81.50824,81.12108,81.15974,82.41339], "fy":[16.51697,15.37842,13.53436,14.66985]}, + {"t":1.38661, "x":2.35933, "y":6.67818, "heading":-0.99219, "vx":-2.44057, "vy":1.45132, "omega":0.52256, "ax":4.97871, "ay":0.2093, "alpha":-0.20079, "fx":[82.96395,83.38852,83.0059,82.61251], "fy":[2.88761,2.45976,4.6054,4.00292]}, + {"t":1.41188, "x":2.29925, "y":6.71492, "heading":-0.97899, "vx":-2.31475, "vy":1.45661, "omega":0.51749, "ax":4.98558, "ay":-0.13711, "alpha":-1.03663, "fx":[82.57816,86.22894,83.59625,80.02551], "fy":[-6.20801,-3.14284,1.59752,-1.38894]}, + {"t":1.43715, "x":2.24235, "y":6.75168, "heading":-0.96591, "vx":-2.18877, "vy":1.45314, "omega":0.49129, "ax":4.97833, "ay":-0.34199, "alpha":-2.18995, "fx":[82.01305,89.1582,84.10146,76.67262], "fy":[-13.72491,-8.69195,3.13076,-3.51699]}, + {"t":1.46242, "x":2.18863, "y":6.78829, "heading":-0.95349, "vx":-2.06296, "vy":1.4445, "omega":0.43595, "ax":4.96895, "ay":-0.47681, "alpha":-3.61992, "fx":[81.2697,93.08606,84.83708,72.12719], "fy":[-22.1559,-10.26671,6.03561,-5.40593]}, + {"t":1.48769, "x":2.13808, "y":6.82464, "heading":-0.94248, "vx":-1.9374, "vy":1.43245, "omega":0.34447, "ax":4.89808, "ay":-0.95235, "alpha":-3.53848, "fx":[79.86983,91.76491,83.85016,71.10955], "fy":[-29.11819,-18.92662,-1.90145,-13.55443]}, + {"t":1.52277, "x":2.07313, "y":6.87431, "heading":-0.93039, "vx":-1.76558, "vy":1.39904, "omega":0.22035, "ax":4.72119, "ay":-1.63277, "alpha":-2.75888, "fx":[77.21026,86.5079,80.84823,70.23346], "fy":[-38.219,-27.7071,-16.85133,-26.09235]}, + {"t":1.55785, "x":2.0141, "y":6.92238, "heading":-0.92266, "vx":-1.59997, "vy":1.34177, "omega":0.12357, "ax":4.51968, "ay":-2.12923, "alpha":-1.96583, "fx":[73.89576,81.11662,76.96186,69.38946], "fy":[-42.73957,-36.546,-27.77808,-34.90924]}, + {"t":1.59293, "x":1.96076, "y":6.96814, "heading":-0.91833, "vx":-1.44142, "vy":1.26708, "omega":0.05461, "ax":4.32887, "ay":-2.49532, "alpha":-1.60955, "fx":[71.2749,77.02363,73.76332,66.57893], "fy":[-47.4056,-41.57951,-35.81165,-41.58643]}, + {"t":1.628, "x":1.91286, "y":7.01105, "heading":-0.91641, "vx":-1.28957, "vy":1.17955, "omega":-0.00185, "ax":4.15853, "ay":-2.77059, "alpha":-1.14081, "fx":[68.31042,72.92435,70.33729,65.71048], "fy":[-50.38784,-46.15771,-42.01453,-46.17775]}, + {"t":1.66308, "x":1.87018, "y":7.05072, "heading":-0.91648, "vx":-1.1437, "vy":1.08236, "omega":-0.04187, "ax":4.00989, "ay":-2.98219, "alpha":-0.89637, "fx":[66.43534,69.73655,67.85356,63.34602], "fy":[-52.54518,-49.68663,-46.62798,-49.98689]}, + {"t":1.69816, "x":1.83253, "y":7.08685, "heading":-0.91795, "vx":-1.00304, "vy":0.97775, "omega":-0.07331, "ax":3.88108, "ay":-3.1484, "alpha":-0.5891, "fx":[64.11326,66.68486,65.21983,62.76518], "fy":[-54.66676,-52.17911,-50.49175,-52.59162]}, + {"t":1.73324, "x":1.79973, "y":7.11921, "heading":-0.92052, "vx":-0.86689, "vy":0.86731, "omega":-0.09397, "ax":3.76946, "ay":-3.28155, "alpha":-0.35761, "fx":[62.75977,64.07584,63.27548,61.22926], "fy":[-55.60015,-54.74069,-53.51667,-54.94986]}, + {"t":1.76832, "x":1.77164, "y":7.14762, "heading":-0.92382, "vx":-0.73467, "vy":0.75219, "omega":-0.10652, "ax":3.67237, "ay":-3.39012, "alpha":-0.1264, "fx":[61.0591,61.74387,61.28874,60.77495], "fy":[-57.05651,-56.12244,-56.22706,-56.64074]}, + {"t":1.8034, "x":1.74813, "y":7.17192, "heading":-0.92755, "vx":-0.60584, "vy":0.63327, "omega":-0.11095, "ax":3.58749, "ay":-3.48004, "alpha":0.12653, "fx":[60.15191,59.48276,59.73499,59.83742], "fy":[-57.17024,-58.11623,-58.49624,-58.25994]}, + {"t":1.83848, "x":1.72909, "y":7.19199, "heading":-0.93144, "vx":-0.48, "vy":0.5112, "omega":-0.10651, "ax":3.51286, "ay":-3.55555, "alpha":0.34195, "fx":[58.87884,57.5448,58.13856,59.66869], "fy":[-58.21125,-58.79499,-60.64737,-59.42396]}, + {"t":1.87355, "x":1.71441, "y":7.20774, "heading":-0.93518, "vx":-0.35677, "vy":0.38647, "omega":-0.09452, "ax":3.44686, "ay":-3.61974, "alpha":0.62996, "fx":[57.93806,55.30393,56.73526,59.85261], "fy":[-58.36282,-60.35129,-62.43006,-60.21322]}, + {"t":1.90863, "x":1.70402, "y":7.21907, "heading":-0.9385, "vx":-0.23586, "vy":0.2595, "omega":-0.07242, "ax":3.38816, "ay":-3.67489, "alpha":0.89358, "fx":[57.46409,53.42361,55.54802,59.48006], "fy":[-58.17125,-61.32726,-64.28683,-61.24906]}, + {"t":1.94371, "x":1.69783, "y":7.22591, "heading":-0.94104, "vx":-0.11701, "vy":0.13059, "omega":-0.04108, "ax":3.33566, "ay":-3.72273, "alpha":1.17096, "fx":[56.77371,51.87261,54.21745,59.55174], "fy":[-57.94744,-62.00717,-66.18322,-62.08636]}, + {"t":1.97879, "x":1.69577, "y":7.2282, "heading":-0.94248, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/deploy/choreo/KTofetchSlow.traj b/src/main/deploy/choreo/KTofetchSlow.traj index 64b2e9c5..47d8f8fb 100644 --- a/src/main/deploy/choreo/KTofetchSlow.traj +++ b/src/main/deploy/choreo/KTofetchSlow.traj @@ -16,7 +16,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9099534749984741 m", "val":1.909953474998474}, "y":{"exp":"6.936121463775635 m", "val":6.936121463775635}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.6957741975784302 m", "val":1.6957741975784302}, "y":{"exp":"7.228199481964111 m", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/LTofetchSlow.traj b/src/main/deploy/choreo/LTofetchSlow.traj index 70162acd..9b15b1a0 100644 --- a/src/main/deploy/choreo/LTofetchSlow.traj +++ b/src/main/deploy/choreo/LTofetchSlow.traj @@ -16,7 +16,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"1.9328655004501343 m", "val":1.9328655004501345}, "y":{"exp":"6.905690670013428 m", "val":6.905690670013428}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/fetchPToCSlow.traj b/src/main/deploy/choreo/fetchPToCSlow.traj index 09ed6aea..3d4c83b4 100644 --- a/src/main/deploy/choreo/fetchPToCSlow.traj +++ b/src/main/deploy/choreo/fetchPToCSlow.traj @@ -19,7 +19,7 @@ "waypoints":[ {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"3.2043471336364746 m", "val":3.2043471336364746}, "y":{"exp":"2.569765567779541 m", "val":2.569765567779541}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"C.x", "val":3.6132737518457456}, "y":{"exp":"C.y", "val":3.1185381922643174}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "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}, diff --git a/src/main/deploy/choreo/fetchPToDSlow.traj b/src/main/deploy/choreo/fetchPToDSlow.traj index b69d327f..a41c8fd0 100644 --- a/src/main/deploy/choreo/fetchPToDSlow.traj +++ b/src/main/deploy/choreo/fetchPToDSlow.traj @@ -19,7 +19,7 @@ "waypoints":[ {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"3.427126884460449 m", "val":3.427126884460449}, "y":{"exp":"2.2866694927215576 m", "val":2.2866694927215576}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"D.x", "val":3.89906213509461}, "y":{"exp":"D.y", "val":2.953538192264317}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":27, "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}, diff --git a/src/main/deploy/choreo/fetchToKSlow.traj b/src/main/deploy/choreo/fetchToKSlow.traj index 9d4d14a7..4f0cdeb9 100644 --- a/src/main/deploy/choreo/fetchToKSlow.traj +++ b/src/main/deploy/choreo/fetchToKSlow.traj @@ -19,7 +19,7 @@ "waypoints":[ {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"3.6935606002807617 m", "val":3.6935606002807617}, "y":{"exp":"5.774015426635742 m", "val":5.774015426635742}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "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}, diff --git a/src/main/deploy/choreo/fetchToLSlow.traj b/src/main/deploy/choreo/fetchToLSlow.traj index 52eb094f..9d7119b4 100644 --- a/src/main/deploy/choreo/fetchToLSlow.traj +++ b/src/main/deploy/choreo/fetchToLSlow.traj @@ -19,7 +19,7 @@ "waypoints":[ {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"3.410693407058716 m", "val":3.410693407058716}, "y":{"exp":"5.76054573059082 m", "val":5.76054573059082}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "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}, diff --git a/src/main/deploy/choreo/startPToESlow.traj b/src/main/deploy/choreo/startPToESlow.traj index 920e52a4..2cbf88a8 100644 --- a/src/main/deploy/choreo/startPToESlow.traj +++ b/src/main/deploy/choreo/startPToESlow.traj @@ -19,7 +19,7 @@ "waypoints":[ {"x":{"exp":"startP.x", "val":7.1}, "y":{"exp":"startP.y", "val":2.97}, "heading":{"exp":"startP.heading", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.436488628387451 m", "val":5.436488628387451}, "y":{"exp":"2.3468501567840576 m", "val":2.3468501567840576}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"E.x", "val":4.837096751845746}, "y":{"exp":"E.y", "val":2.813538192264317}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "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}, diff --git a/src/main/deploy/choreo/startToJSlow.traj b/src/main/deploy/choreo/startToJSlow.traj index 19c4a4be..d70110ac 100644 --- a/src/main/deploy/choreo/startToJSlow.traj +++ b/src/main/deploy/choreo/startToJSlow.traj @@ -19,7 +19,7 @@ "waypoints":[ {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "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}, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d386a4e1..1e52943a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -96,17 +96,16 @@ public void teleopInit() { m_autonomousCommand.cancel(); } if (!m_robotContainer.hasElevatorZeroed()) m_robotContainer.zeroElevator(); + else if (m_robotContainer.wasScoringCoral()) { + m_robotContainer.finishAuto(); + } else { + m_robotContainer.stow(); + } m_robotContainer.setIsAuto(false); m_robotContainer.setIsAutoPlacing(true); m_robotContainer.setScoringSide(ScoreSide.LEFT); m_robotContainer.disableNoMotionCal(); - - if (m_robotContainer.wasScoringCoral()) { - m_robotContainer.finishAuto(); - } else { - m_robotContainer.stow(); - } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de44cd08..23ce1668 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -424,30 +424,6 @@ private void configureOperatorBindings() { .onTrue( new ClimbPrepCommand( robotStateSubsystem, climbSubsystem, elevatorSubsystem, biscuitSubsystem)); - - // Move biscuit - new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband)) - .onTrue( - new JogBiscuitCommand( - biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations))) - .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); - new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband)) - .onTrue( - new JogBiscuitCommand( - biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations))) - .onFalse(new HoldBiscuitCommand(biscuitSubsystem)); - - // Move elevator - new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband)) - .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); - new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband)) - .onTrue( - new JogElevatorCommand( - elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations))) - .onFalse(new HoldElevatorCommand(elevatorSubsystem)); } private void configureTestOperatorBindings() { @@ -621,6 +597,17 @@ private void configureMatchDashboard() { .withSize(1, 1) .withPosition(9, 1); + Shuffleboard.getTab("Match") + .add( + "Clear Coral", + new InstantCommand( + () -> { + funnelSubsystem.clearCoral(); + coralSubsystem.setState(CoralSubsystem.CoralState.EMPTY); + })) + .withSize(1, 1) + .withPosition(9, 0); + // Shuffleboard.getTab("Match") // .addBoolean( // "Cams Connected", diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java index b2ec03c0..3204cf8c 100644 --- a/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java @@ -31,6 +31,7 @@ public class NonProcessorDeepAutonCommand extends SequentialCommandGroup private List NodeNames; private List NodeLevels; private char startNode; + private boolean lastAlgae; public NonProcessorDeepAutonCommand( DriveSubsystem driveSubsystem, @@ -46,7 +47,8 @@ public NonProcessorDeepAutonCommand( List NodeNames, List NodeLevels, char startNode, - Boolean startScoreLeft, + boolean startScoreLeft, + boolean lastAlgae, Pose2d startPose) { addRequirements( driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); @@ -59,6 +61,7 @@ public NonProcessorDeepAutonCommand( this.NodeNames = NodeNames; this.NodeLevels = NodeLevels; this.startNode = startNode; + this.lastAlgae = lastAlgae; startPath = new DriveAutonServoCommand( @@ -102,5 +105,6 @@ public void reassignAlliance() { pathHandler.setNodeNames(NodeNames); pathHandler.setNodeLevels(NodeLevels); pathHandler.setStartNode(startNode); + pathHandler.setGetAlgaeLast(lastAlgae); } } diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java index 05fa0789..5051fe49 100644 --- a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java @@ -31,6 +31,7 @@ public class NonProcessorShallowAutonCommand extends SequentialCommandGroup private List NodeNames; private List NodeLevels; private char startNode; + private boolean lastAlgae; public NonProcessorShallowAutonCommand( DriveSubsystem driveSubsystem, @@ -46,7 +47,8 @@ public NonProcessorShallowAutonCommand( List NodeNames, List NodeLevels, char startNode, - Boolean startScoreLeft, + boolean startScoreLeft, + boolean lastAlgae, Pose2d startPose) { addRequirements( driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); @@ -59,6 +61,7 @@ public NonProcessorShallowAutonCommand( this.NodeNames = NodeNames; this.NodeLevels = NodeLevels; this.startNode = startNode; + this.lastAlgae = lastAlgae; startPath = new DriveAutonServoCommand( @@ -116,6 +119,7 @@ public void reassignAlliance() { pathHandler.setNodeNames(NodeNames); pathHandler.setNodeLevels(NodeLevels); pathHandler.setStartNode(startNode); + pathHandler.setGetAlgaeLast(lastAlgae); // pathHandler.reassignAlliance(); } } diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java index 551dca6f..6f5c4a88 100644 --- a/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java @@ -119,6 +119,7 @@ public void reassignAlliance() { pathHandler.setNodeNames(NodeNames); pathHandler.setNodeLevels(NodeLevels); pathHandler.setStartNode(startNode); + pathHandler.setGetAlgaeLast(false); // pathHandler.reassignAlliance(); } } diff --git a/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java index a46fd807..1115f505 100644 --- a/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java @@ -31,6 +31,7 @@ public class ProcessorShallowAutonCommand extends SequentialCommandGroup private List NodeNames; private List NodeLevels; private char startNode; + private boolean lastAlgae; public ProcessorShallowAutonCommand( DriveSubsystem driveSubsystem, @@ -47,6 +48,7 @@ public ProcessorShallowAutonCommand( List NodeLevels, char startNode, boolean startScoreLeft, + boolean lastAlgae, Pose2d startPose) { addRequirements( driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); @@ -59,6 +61,7 @@ public ProcessorShallowAutonCommand( this.NodeNames = NodeNames; this.NodeLevels = NodeLevels; this.startNode = startNode; + this.lastAlgae = lastAlgae; startPath = new DriveAutonServoCommand( @@ -116,6 +119,7 @@ public void reassignAlliance() { pathHandler.setNodeNames(NodeNames); pathHandler.setNodeLevels(NodeLevels); pathHandler.setStartNode(startNode); + pathHandler.setGetAlgaeLast(lastAlgae); // pathHandler.reassignAlliance(); } } diff --git a/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java b/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java index d0a22d8c..86d86e77 100644 --- a/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java @@ -116,6 +116,7 @@ public void reassignAlliance() { pathHandler.setNodeNames(NodeNames); pathHandler.setNodeLevels(NodeLevels); pathHandler.setStartNode(startNode); + pathHandler.setGetAlgaeLast(false); // pathHandler.reassignAlliance(); } } diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index 6a604b81..9a0c2a03 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -32,7 +32,7 @@ public class AlgaeConstants { public static final double kIntakingSpeed = -1; public static final double kHasAlgaeVelThreshold = 10; - public static final double kSuperCycleHasAlgaeVelThres = 50; + public static final double kSuperCycleHasAlgaeVelThres = 45; public static final double kHasAlgaeCounts = 2; // Example Talon FX Config diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java index 467b5e0e..944f06c4 100644 --- a/src/main/java/frc/robot/constants/BiscuitConstants.java +++ b/src/main/java/frc/robot/constants/BiscuitConstants.java @@ -4,9 +4,10 @@ public class BiscuitConstants { // public static final double kZero = .37; public static final int talonID = 25; - public static final double kCloseEnough = 0.05; - public static final double kRezeroVelocityCloseEnough = 0.1; // FIXME: find a real value - public static final double kRezeroErrorCloseEnough = 0.1; // FIXME: find a real value + public static final double kCloseEnough = 1.3; // was 0.05 + public static final double kInFunnel = 0.94; // TODO Put real numbers in here + public static final double kTooLow = 0.217; + public static final double kZeroVelThresh = 2; // public static final double kSafeToStowUpper = 40; // public static final double kSafeToStowLower = -5; diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java index d1ed5649..befacab7 100644 --- a/src/main/java/frc/robot/constants/ClimbConstants.java +++ b/src/main/java/frc/robot/constants/ClimbConstants.java @@ -44,8 +44,10 @@ public class ClimbConstants { // Climb positions // public static final Angle kClimbCagePos = Rotations.of(); // will test public static final double kClimbRatchedEngage = -0.12; + public static final double kSpeedUpPos = -0.12; public static final double kFullyClimbed = kArmMaxFwd; - public static final double kClimbOpenLoopSpeed = 3.0; + public static final double kClimbOpenLoopFastSpeed = 11.0; + public static final double kClimbOpenLoopSpeed = 6.0; // Cage location info public static final double kCenterX = DriveConstants.kFieldMaxX / 2.0; @@ -61,6 +63,7 @@ public class ClimbConstants { public static final double kCloseEnoughX = 0.1; public static final double kCloseEnoughY = 0.03; public static final double kClimbAngleGood = RobotStateConstants.kClimbAngleSmall; + public static final double kFastClimbAfter = 3; public static TalonFXConfiguration getPivotArmFxConfig() { TalonFXConfiguration armFxConfig = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index d4cdf1be..a67539b3 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -43,9 +43,9 @@ public class DriveConstants { public static final double kWheelDiameterInches = 3.375; public static final double kMaxSpeedMetersPerSecond = 3.782; - public static final double kSpeedStillThreshold = 0.1; // meters per second - public static final double kGyroRateStillThreshold = 10.0; // 25 5 degrees per second - public static final double kGyroDifferentThreshold = 0.5; // 5 degrees + public static final double kSpeedStillThreshold = 0.2; // meters per second + public static final double kGyroRateStillThreshold = 10.0; // was 10 + public static final double kGyroDifferentThreshold = 0.3; // 5 degrees public static final int kGyroDifferentCount = 3; public static final double kRobotLength = 0.61595; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 2156bf15..8c0abd67 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -21,14 +21,13 @@ public class ElevatorConstants { public static final double kCloseEnoughRotations = 0.4; - public static final double kStowThresholdDone = 0.3676757 + 0.5; public static final double kMaxFwd = 53; public static final double kMaxRev = 0.1; public static final double kElevatorLiftHeight = 1; // for pit command public static final int kZeroMultiple = 0; // some constant to multiply, add by to turn the analog input into a position - public static final double kZeroSpeed = -0.05; - public static final double kZeroVolts = -0.5; + // public static final double kZeroSpeed = -0.05; + public static final double kZeroVolts = -0.75; public static final int kZeroCounter = 2; public static final double kZeroedThreshhold = 0.025; @@ -65,7 +64,8 @@ public class ElevatorConstants { public static final Angle kHpAlgaeSetpoint = Rotations.of(14.9063); // Algae scoring - public static final Angle kProcessorSetpoint = Rotations.of(3.348); // was 4.297 -> 5.964 + public static final Angle kProcessorSetpoint = + Rotations.of(1.49365); // was 4.297 -> 5.964 -> 3.348 public static final Angle kBargeSetpoint = Rotations.of(44.785); // 41.936 public static final Angle kBargeHigherThan = Rotations.of(31.0901); diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java index d02f3a7d..26b38164 100644 --- a/src/main/java/frc/robot/constants/RobotConstants.java +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -336,7 +336,7 @@ public static class CompConstants { public static double kTicksPerRot = 160; public static final double kZero = .67; - public static final double kSafeToStowUpper = 40 / 2; + public static final double kSafeToStowUpper = 4.06; // was 20 public static final double kSafeToStowLower = -5 / 2; // Speeds @@ -370,7 +370,7 @@ public static class CompConstants { public static Angle kHpAlgaeSetpoint = kBiscuitStowSetpoint; // Algae scoring - public static Angle kProcessorSetpoint = Rotations.of(15.0139); + public static Angle kProcessorSetpoint = Rotations.of(14.634); // was 15.0139 public static Angle kBargeSetpoint = Rotations.of(2.697); // public static Angle kBargeBackwardSetpoint = Rotations.of(-12.3489 / 2); // 9.089 diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java index d75389ca..ccbd76bf 100644 --- a/src/main/java/frc/robot/constants/RobotStateConstants.java +++ b/src/main/java/frc/robot/constants/RobotStateConstants.java @@ -12,8 +12,10 @@ public class RobotStateConstants { public static final double kProcessorStowRadius = 0.5; + public static final double kElevatorWaitRadius = 1.5; + // Super Cycle Constants - public static final double kBiscuitSuperCycleSafeThres = 6; + public static final double kBiscuitSuperCycleSafeThres = 5; // 6 is max // Elevator good for climb, tolerates stuck coral public static final double kElevatorClimbMax = 11.4; diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index 06c5fce9..d21b502c 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -60,9 +60,11 @@ public class TagServoingConstants { // End conditions public static final double kInitialCloseEnough = 0.1; public static final double kCoralDriveXCloseEnough = 0.03; - public static final double kCoralDriveYCloseEnough = 0.025; + public static final double kCoralDriveYCloseEnough = 0.015; // was 0.025 public static final double kAlgaeDriveXCloseEnough = 0.03; - public static final double kAlgaeDriveYCloseEnough = 0.025; + public static final double kAlgaeDriveYCloseEnough = kCoralDriveYCloseEnough; + + public static final double kSmallYThres = 0.3; // For elevator staging public static final double kEndDriveCurrentThreshold = 25; public static final int kEndCountThreshold = 1; diff --git a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java index 0f508f56..0b74ba64 100644 --- a/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/algae/AlgaeSubsystem.java @@ -104,6 +104,7 @@ public void periodic() { } if (slowCounts >= AlgaeConstants.kHasAlgaeCounts) { + slowCounts = 0; hold(); setState(AlgaeStates.HAS_ALGAE); } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index 3759fb53..ebee5a4b 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -182,6 +182,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) { new ArrayList(Arrays.asList(4, 4, 4)), 'j', false, + false, AutonConstants.kNonProcessorShallow); } case 0x02 -> { @@ -200,6 +201,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) { new ArrayList(Arrays.asList(4, 4, 4)), 'j', false, + false, AutonConstants.kNonProcessorShallow); } case 0x03 -> { @@ -218,9 +220,30 @@ private AutoCommandInterface getAutoCommand(int switchPos) { new ArrayList(Arrays.asList(4, 4, 4)), 'k', true, + false, AutonConstants.kNonProcessorDeep); } + case 0x04 -> { + return new NonProcessorShallowAutonCommand( + driveSubsystem, + pathHandler, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + "startToJ", + new ArrayList(Arrays.asList('k', 'l', 'k')), + new ArrayList(Arrays.asList(4, 4, 3)), + 'j', + false, + true, + AutonConstants.kNonProcessorShallow); + } + case 0x20 -> { return new ProcessorShallowSlowAutonCommand( driveSubsystem, diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java index 7511ae6c..62654a30 100644 --- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java +++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitSubsystem.java @@ -5,11 +5,13 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.constants.BiscuitConstants; import frc.robot.constants.RobotConstants; import java.util.Set; +import net.jafama.FastMath; import org.littletonrobotics.junction.Logger; import org.slf4j.LoggerFactory; import org.strykeforce.telemetry.TelemetryService; @@ -22,7 +24,11 @@ public class BiscuitSubsystem extends MeasurableSubsystem { private BiscuitIO io; private BiscuitIOInputsAutoLogged inputs = new BiscuitIOInputsAutoLogged(); private Angle setPoint = Rotations.of(0); + private Angle prevSetPoint = Rotations.of(0); private boolean hasZeroed = false; + private int zeroCounter = 0; + + public BiscuitState curState = BiscuitState.NORMAL; public BiscuitSubsystem(BiscuitIO io) { this.logger = LoggerFactory.getLogger(this.getClass()); @@ -30,6 +36,10 @@ public BiscuitSubsystem(BiscuitIO io) { hasZeroed = io.zero(); } + public BiscuitState getState() { + return curState; + } + public boolean hasZeroed() { return hasZeroed; } @@ -51,8 +61,18 @@ public AngularVelocity getVelocity(AngularVelocity velocity) { return RotationsPerSecond.of(inputs.velocity); } + public void zeroCheck() { + zeroCounter++; + if (!getIsEncoderBroken() && curState != BiscuitState.BROKEN) { + hasZeroed = io.zero(); + curState = BiscuitState.NORMAL; + } else { + curState = BiscuitState.BROKEN; + } + } + public void zero() { - hasZeroed = io.zero(); + io.zero(); } public boolean isFinished() { @@ -64,24 +84,38 @@ public boolean isSafeToStow() { && getPosition().in(Rotations) > RobotConstants.kSafeToStowLower; } + private boolean getIsEncoderBroken() { + double curPos = MathUtil.inputModulus(inputs.rawPulseWidth, 0, 1); + return curPos >= BiscuitConstants.kInFunnel || curPos <= BiscuitConstants.kTooLow; + } + @Override public void periodic() { io.updateInputs(inputs); + switch (curState) { + case NORMAL: + if (isFinished() + && FastMath.abs(inputs.velocity) <= BiscuitConstants.kZeroVelThresh + && setPoint != prevSetPoint) curState = BiscuitState.CHECK_ZERO; + break; + case CHECK_ZERO: + prevSetPoint = setPoint; + zeroCheck(); + break; + case BROKEN: + break; + } + Logger.processInputs(getName(), inputs); - Logger.recordOutput("Biscuit setPoint", setPoint.in(Rotations)); - Logger.recordOutput("Is Biscuit Finished", isFinished() ? 1.0 : 0.0); - - // double pos = MathUtil.inputModulus(inputs.rawPulseWidth, 0, 1); - // double error = - // Math.abs(BiscuitConstants.kTicksPerRot * (RobotConstants.kZero - pos) - inputs.position); - - // if (setPoint == BiscuitConstants.kStowSetpoint - // && isFinished() - // && inputs.velocity < BiscuitConstants.kRezeroVelocityCloseEnough - // && error >= BiscuitConstants.kRezeroErrorCloseEnough) { - // logger.info("Rezeroing Biscuit"); - // zero(); + Logger.recordOutput("Biscuit/setPoint", setPoint.in(Rotations)); + Logger.recordOutput("Biscuit/IsFinished", isFinished() ? 1.0 : 0.0); + Logger.recordOutput("Biscuit/curState", curState); + Logger.recordOutput("Biscuit/zeroCounts", zeroCounter); + + // if (setPoint != prevSetPoint && isFinished()) { + // zeroCheck(); + // prevSetPoint = setPoint; // } } @@ -97,4 +131,10 @@ public Set getMeasures() { new Measure("Is Biscuit Finished", () -> isFinished() ? 1.0 : 0.0), new Measure("Biscuit Set Point", () -> setPoint.in(Rotations))); } + + public enum BiscuitState { + NORMAL, + CHECK_ZERO, + BROKEN + } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java index 571044c1..407d9857 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -12,6 +13,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Servo; import frc.robot.constants.ClimbConstants; import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs; @@ -36,7 +38,8 @@ public class ClimbIOServoFX implements ClimbIO { new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0); private VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(false); - StatusSignal currPosition; + private StatusSignal currPosition; + private StatusSignal currVelocity; public ClimbIOServoFX() { logger = LoggerFactory.getLogger(this.getClass()); @@ -52,6 +55,7 @@ public ClimbIOServoFX() { configuratorFront.apply(ClimbConstants.getPivotArmFxConfig()); currPosition = talonFxPivotArmFront.getPosition(); + currVelocity = talonFxPivotArmFront.getVelocity(); } // @Override @@ -83,7 +87,9 @@ public void setPinServoPosition(double position) { @Override public void updateInputs(ClimbIOInputs inputs) { - inputs.position = currPosition.refresh().getValueAsDouble(); + BaseStatusSignal.refreshAll(currVelocity, currPosition); + inputs.position = currPosition.getValueAsDouble(); + inputs.velocity = currVelocity.getValueAsDouble(); inputs.ratchetServoPosition = ratchetServo.getPosition(); inputs.pinServoPosition = pinServo.getPosition(); } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java index 8c6e4ec4..5bbae3d9 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Rotations; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.ClimbConstants; import frc.robot.standards.ClosedLoopPosSubsystem; @@ -28,6 +29,7 @@ public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPos private boolean isPinDeployed = false; private boolean proceedToClimb = false; private int climbZeroStableCounts; // idk what this means + private boolean spedUp = false; // more private double setpoints = 0.0; @@ -36,6 +38,7 @@ public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPos public ClimbSubsystem(ClimbIO io) { this.io = io; + Logger.recordOutput("Climb/isDesparate", false); enableRatchet(false); deployClimb(false); zero(); @@ -99,7 +102,15 @@ public void prepClimb() { public void climb() { if (curState == ClimbState.PREPPED) { // setPosition(ClimbConstants.kClimbCagePos); - io.setPercent(ClimbConstants.kClimbOpenLoopSpeed); + if (DriverStation.getMatchTime() < ClimbConstants.kFastClimbAfter) { + Logger.recordOutput("Climb/isDesparate", true); + io.setPercent(ClimbConstants.kClimbOpenLoopFastSpeed); + spedUp = true; + } else { + Logger.recordOutput("Climb/isDesparate", false); + io.setPercent(ClimbConstants.kClimbOpenLoopSpeed); + spedUp = false; + } setState(ClimbState.CLIMBING); } } @@ -149,6 +160,10 @@ public void periodic() { if (climbInputs.position >= ClimbConstants.kClimbRatchedEngage && !isRatchetOn) { enableRatchet(true); } + // if (climbInputs.position >= ClimbConstants.kSpeedUpPos && !spedUp) { + // io.setPercent(ClimbConstants.kClimbOpenLoopFastSpeed); + // spedUp = true; + // } if (isFinished()) { io.setCoastMode(true); setState(ClimbState.CLIMBED); diff --git a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java index 9d531991..44dccfcc 100644 --- a/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java +++ b/src/main/java/frc/robot/subsystems/coral/CoralSubsystem.java @@ -33,7 +33,7 @@ public AngularVelocity getSpeed() { return inputs.velocity; } - private void setState(CoralState state) { + public void setState(CoralState state) { logger.info("{} -> {}", curState, state); this.curState = state; } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index ef69849f..d87df58b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -47,6 +47,7 @@ public class DriveSubsystem extends MeasurableSubsystem { private double driveMultiplier = 1.0; private int gyroDifferentCount = 0; + private int gyroCorrectionCount = 0; private boolean ignoreSticks = false; @@ -449,6 +450,14 @@ public void periodic() { Logger.processInputs(getName(), inputs); org.littletonrobotics.junction.Logger.recordOutput( "DriveSubsystem/Swerve Pose", inputs.swervePose); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Gyro Correction Count", gyroCorrectionCount); + + org.littletonrobotics.junction.Logger.recordOutput( + "DriveSubsystem/Gyro Disagreement", + inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees()); + if (Math.abs(inputs.gyroRotation2d.minus(inputs.navxRotation2d).getDegrees()) > DriveConstants.kGyroDifferentThreshold) { gyroDifferentCount++; @@ -463,6 +472,7 @@ public void periodic() { inputs.gyroRotation2d.getDegrees(), inputs.navxRotation2d.getDegrees()); gyroDifferentCount = 0; + gyroCorrectionCount++; } switch (currDriveState) { diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java index 2888cf61..9d5e9281 100644 --- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java +++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java @@ -57,6 +57,7 @@ public class PathHandler extends MeasurableSubsystem { private boolean teleop = false; private boolean isPlacing = false; private boolean hasPreppedCoral = false; + private boolean algaeOnLast = false; public PathHandler( DriveSubsystem driveSubsystem, @@ -127,6 +128,10 @@ public void setProceedToNext(boolean proceed) { this.proceedToNext = proceed; } + public void setGetAlgaeLast(boolean algae) { + this.algaeOnLast = algae; + } + public void startPathHandler() { teleop = false; nodeNames.add(0, startNode); @@ -223,7 +228,11 @@ private void drivePath() { false); driveSubsystem.setAutoDebugMsg("Servo Start"); if (!hasPreppedCoral) { - robotStateSubsystem.toPrepCoral(); + if (algaeOnLast && nodeNames.size() == 1) { + robotStateSubsystem.toReefAlignAlgaeAuto(); + } else { + robotStateSubsystem.toPrepCoral(); + } hasPreppedCoral = true; } curState = PathStates.DRIVE_PLACE_SERVO; @@ -414,7 +423,11 @@ public void periodic() { <= AutonConstants.kElevatorStageRadius*/ shouldStageElevator() && !hasPreppedCoral) { hasPreppedCoral = true; - robotStateSubsystem.toPrepCoral(); + if (algaeOnLast && nodeNames.size() == 1) { + robotStateSubsystem.toReefAlignAlgaeAuto(); + } else { + robotStateSubsystem.toPrepCoral(); + } } if (!runningPath) { if (nodeNames.size() > 0) { @@ -437,7 +450,11 @@ public void periodic() { <= AutonConstants.kElevatorStageRadius*/ shouldStageElevator() && !hasPreppedCoral) { hasPreppedCoral = true; - robotStateSubsystem.toPrepCoral(); + if (algaeOnLast && nodeNames.size() == 1) { + robotStateSubsystem.toReefAlignAlgaeAuto(); + } else { + robotStateSubsystem.toPrepCoral(); + } } if (runningPath && isServoing) { drivePathServo(); diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index a9afc1b7..2f712aaa 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -18,6 +18,7 @@ import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.climb.ClimbSubsystem.ClimbState; import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem.CoralState; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem.ElevatorStates; @@ -74,6 +75,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem { private boolean isBargeSafe = true; private boolean isAutoReadyForEject = false; private boolean prestagingForAlgae = false; + private boolean hasElevatorAutoCoralPrestaged = false; private Timer scoringTimer = new Timer(); @@ -168,6 +170,10 @@ public boolean hasAlgae() { return algaeSubsystem.hasAlgae(); } + public boolean safeMoveElevator() { + return !(coralSubsystem.getState() != CoralState.HAS_CORAL && funnelSubsystem.hasCoral()); + } + public void startupSequence() { if (curState == RobotStates.IDLE) { elevatorSubsystem.zero(); @@ -291,12 +297,12 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) { public void toStow() { visionSubsystem.setYawUpdateCamera(-1); biscuitSubsystem.setIsRemovingAlgae(false); + driveSubsystem.removeDriveMultiplier(); + driveSubsystem.setIgnoreSticks(false); if (biscuitSubsystem.isSafeToStow()) { biscuitSubsystem.setPosition(RobotConstants.kStowSetpoint, hasAlgae()); elevatorSubsystem.setPosition(RobotConstants.kElevatorStowSetpoint); - driveSubsystem.removeDriveMultiplier(); - driveSubsystem.setIgnoreSticks(false); algaeSubsystem.hold(); setState(RobotStates.TO_STOW); @@ -377,7 +383,15 @@ public void toReefAlign() { toReefAlign(getAlgaeOnCycle, true); } + public void toReefAlignAlgaeAuto() { + toReefAlign(true, true); + } + private void toReefAlign(boolean getAlgae, boolean drive) { + if (!safeMoveElevator()) { + logger.info("Elevator movement is dangerous!"); + return; + } boolean wantAlgae = getAlgae || !coralSubsystem.hasCoral(); if ((hasAlgae() && scoreSide == ScoreSide.RIGHT && drive) || (hasAlgae() && !hasCoral())) { setState(RobotStates.STOW); @@ -446,25 +460,34 @@ private void toReefAlign(boolean getAlgae, boolean drive) { return; } - currentLevel = scoringLevel; - switch (scoringLevel) { - case L1 -> { - biscuitSubsystem.setPosition(RobotConstants.kL1CoralSetpoint, hasAlgae()); - elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint); - } - case L2 -> { - biscuitSubsystem.setPosition(RobotConstants.kL2CoralSetpoint, hasAlgae()); - elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint); - } - case L3 -> { - biscuitSubsystem.setPosition(RobotConstants.kL3CoralSetpoint, hasAlgae()); - elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint); - } - case L4 -> { - biscuitSubsystem.setPosition(RobotConstants.kL4CoralSetpoint, hasAlgae()); - elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint); + if (!isAutoPlacing + || (tagAlignSubsystem.yErrorSmall() + && tagAlignSubsystem.getCurRadius(allianceColor) + < RobotStateConstants.kElevatorWaitRadius)) { + currentLevel = scoringLevel; + switch (scoringLevel) { + case L1 -> { + biscuitSubsystem.setPosition(RobotConstants.kL1CoralSetpoint, hasAlgae()); + elevatorSubsystem.setPosition(ElevatorConstants.kL1CoralSetpoint); + } + case L2 -> { + biscuitSubsystem.setPosition(RobotConstants.kL2CoralSetpoint, hasAlgae()); + elevatorSubsystem.setPosition(ElevatorConstants.kL2CoralSetpoint); + } + case L3 -> { + biscuitSubsystem.setPosition(RobotConstants.kL3CoralSetpoint, hasAlgae()); + elevatorSubsystem.setPosition(ElevatorConstants.kL3CoralSetpoint); + } + case L4 -> { + biscuitSubsystem.setPosition(RobotConstants.kL4CoralSetpoint, hasAlgae()); + elevatorSubsystem.setPosition(ElevatorConstants.kL4CoralSetpoint); + } } + hasElevatorAutoCoralPrestaged = true; + } else { + hasElevatorAutoCoralPrestaged = false; } + setState(RobotStates.REEF_ALIGN_CORAL, true); } } @@ -489,6 +512,11 @@ public void toPlaceCoral() { } public void toAlgaeFloorPickup() { + if (!safeMoveElevator()) { + logger.info("Elevator movement is dangerous!"); + return; + } + algaeSubsystem.intake(); currentAlgaeHeight = algaeHeight; @@ -525,6 +553,12 @@ public void toScoreAlgae() { futureState = RobotStates.BARGE_ALGAE; toStowSafe(); } + + if (!safeMoveElevator()) { + logger.info("Elevator movement is dangerous!"); + return; + } + switch (algaeHeight) { case LOW -> { toProcessor(); @@ -578,6 +612,10 @@ public void releaseAlgae() { } public void toHpAlgae() { + if (!safeMoveElevator()) { + logger.info("Elevator movement is dangerous!"); + return; + } currentAlgaeHeight = algaeHeight; if (needSafeAlgaeTransfer(RobotStates.HP_ALGAE)) { return; @@ -592,6 +630,11 @@ public void toHpAlgae() { } private void toProcessor() { + if (!safeMoveElevator()) { + logger.info("Elevator movement is dangerous!"); + return; + } + currentAlgaeHeight = algaeHeight; if (needSafeAlgaeTransfer(RobotStates.PROCESSOR_ALGAE)) { return; @@ -682,10 +725,8 @@ public void periodic() { } case TO_STOW -> { - if (biscuitSubsystem.isFinished() - && (elevatorSubsystem.isFinished() - || elevatorSubsystem.getPosition().in(Rotations) - < ElevatorConstants.kStowThresholdDone)) { + if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) { + driveSubsystem.removeDriveMultiplier(); setState(RobotStates.STOW); } } @@ -750,6 +791,10 @@ public void periodic() { if (currentLevel != scoringLevel) { toReefAlign(false, false); } + if (!hasElevatorAutoCoralPrestaged && tagAlignSubsystem.yErrorSmall()) { + toReefAlign(false, false); + break; + } if ((isAutoPlacing && tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE && elevatorSubsystem.isFinished()) @@ -823,7 +868,7 @@ public void periodic() { } case HP_ALGAE -> { if (algaeSubsystem.hasAlgae()) { - toProcessor(); + toStowSafe(); } } case PROCESSOR_ALGAE -> { diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java index 0231bcec..cdbda6b6 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java @@ -50,6 +50,7 @@ public class TagAlignSubsystem extends MeasurableSubsystem { private boolean scoreLeft = true; private int currentThresCount = 0; private boolean finalDrive = false; + private double yError = 2767; // private long startServoTime; @@ -94,6 +95,12 @@ public void setProceedToAlign(boolean proceed) { this.proceedToAlign = proceed; } + public boolean yErrorSmall() { + return curState == TagAlignStates.DONE + || finalDrive + || yError < TagServoingConstants.kSmallYThres; + } + public int computeHexant(Alliance color) { Translation2d reefT = color == Alliance.Blue @@ -234,6 +241,12 @@ public void setup(Alliance alliance, boolean scoreLeft, boolean algae) { fieldRelHexant = computeFieldRelHexant(alliance); proceedToAlign = false; + this.yError = + targetPose + .getTranslation() + .minus(driveSubsystem.getPoseMeters().getTranslation()) + .rotateBy(Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant])) + .getY(); // Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId); // Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag); @@ -365,12 +378,14 @@ public void periodic() { vY = driveY.calculate(rotated.getY(), rotatedTarget.getY()); Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getError()); Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getError()); + yError = driveY.getError(); } case TAG_ALIGN -> { vX = alignX.calculate(rotated.getX(), rotatedTarget.getX()); vY = alignY.calculate(rotated.getY(), rotatedTarget.getY()); Logger.recordOutput("TagAlignSubsystem/DriveXError", alignX.getError()); Logger.recordOutput("TagAlignSubsystem/DriveYError", alignY.getError()); + yError = alignY.getError(); } default -> {} }