diff --git a/.vscode/settings.json b/.vscode/settings.json index 26bbbfd3..248c951e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.compile.nullAnalysis.mode": "automatic", - "java.debug.settings.onBuildFailureProceed": true + "java.debug.settings.onBuildFailureProceed": true, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/src/main/deploy/choreo/LOtoJ.traj b/src/main/deploy/choreo/LOtoJ.traj index 5e328416..8c038d3c 100644 --- a/src/main/deploy/choreo/LOtoJ.traj +++ b/src/main/deploy/choreo/LOtoJ.traj @@ -3,30 +3,30 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":7.111314296722412, "y":6.2355637550354, "heading":-2.356194490192345, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.7510480880737305, "y":5.597940921783447, "heading":0.0, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.004220962524414, "y":5.243917942047119, "heading":-2.095116885713791, "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":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":4.484625190496445, "y":4.0126113295555115, "r":3.25}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":2.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"7.111314296722412 m", "val":7.111314296722412}, "y":{"exp":"6.2355637550354 m", "val":6.2355637550354}, "heading":{"exp":"-135 deg", "val":-2.356194490192345}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.7510480880737305 m", "val":5.7510480880737305}, "y":{"exp":"5.597940921783447 m", "val":5.597940921783447}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"J.x", "val":5.004220962524414}, "y":{"exp":"J.y", "val":5.243917942047119}, "heading":{"exp":"-2.095116885713791 rad", "val":-2.095116885713791}, "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":2, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"4.484625190496445 m", "val":4.484625190496445}, "y":{"exp":"4.0126113295555115 m", "val":4.0126113295555115}, "r":{"exp":"3.25 m", "val":3.25}}}, "enabled":false}, - {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}, + {"from":1, "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"2 m / s ^ 2", "val":2.0}}}, "enabled":true}], "targetDt":{ @@ -36,66 +36,68 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,0.87859,1.805], + "waypoints":[0.0,0.9336,1.90766], "samples":[ - {"t":0.0, "x":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.49989, "ay":-2.10796, "alpha":0.00896, "fx":[-73.55183,-73.59107,-73.57728,-73.53804], "fy":[-34.48806,-34.40435,-34.4341,-34.51771]}, - {"t":0.03254, "x":7.10893, "y":6.23445, "heading":-2.35619, "vx":-0.14643, "vy":-0.06859, "omega":0.00029, "ax":-4.49955, "ay":-2.10781, "alpha":0.00923, "fx":[-73.54597,-73.58637,-73.57218,-73.53178], "fy":[-34.48644,-34.40026,-34.43089,-34.51697]}, - {"t":0.06508, "x":7.10178, "y":6.2311, "heading":-2.35618, "vx":-0.29284, "vy":-0.13718, "omega":0.00059, "ax":-4.49917, "ay":-2.10764, "alpha":0.00953, "fx":[-73.53938,-73.58108,-73.56644,-73.52474], "fy":[-34.48462,-34.39565,-34.42728,-34.51613]}, - {"t":0.09762, "x":7.08987, "y":6.22552, "heading":-2.35617, "vx":-0.43925, "vy":-0.20576, "omega":0.0009, "ax":-4.49875, "ay":-2.10745, "alpha":0.00987, "fx":[-73.5319,-73.57509,-73.55993,-73.51675], "fy":[-34.48255,-34.39043,-34.42319,-34.51519]}, - {"t":0.13016, "x":7.0732, "y":6.21771, "heading":-2.35614, "vx":-0.58564, "vy":-0.27434, "omega":0.00122, "ax":-4.49826, "ay":-2.10724, "alpha":0.01025, "fx":[-73.52335,-73.56823,-73.55249,-73.50761], "fy":[-34.48019,-34.38446,-34.41851,-34.51411]}, - {"t":0.1627, "x":7.05176, "y":6.20767, "heading":-2.3561, "vx":-0.73201, "vy":-0.34291, "omega":0.00156, "ax":-4.49769, "ay":-2.10699, "alpha":0.0107, "fx":[-73.51347,-73.56031,-73.5439,-73.49707], "fy":[-34.47745,-34.37756,-34.41311,-34.51286]}, - {"t":0.19524, "x":7.02556, "y":6.19539, "heading":-2.35605, "vx":-0.87837, "vy":-0.41147, "omega":0.0019, "ax":-4.49703, "ay":-2.1067, "alpha":0.01122, "fx":[-73.50195,-73.55107,-73.53387,-73.48475], "fy":[-34.47427,-34.36951,-34.4068,-34.5114]}, - {"t":0.22778, "x":6.9946, "y":6.18089, "heading":-2.35598, "vx":-1.0247, "vy":-0.48003, "omega":0.00227, "ax":-4.49625, "ay":-2.10635, "alpha":0.01184, "fx":[-73.48831,-73.54014,-73.52201,-73.47019], "fy":[-34.4705,-34.35999,-34.39934,-34.50968]}, - {"t":0.26032, "x":6.95887, "y":6.16415, "heading":-2.35591, "vx":-1.17101, "vy":-0.54857, "omega":0.00266, "ax":-4.49532, "ay":-2.10594, "alpha":0.01258, "fx":[-73.47193,-73.52701,-73.50777,-73.45269], "fy":[-34.46596,-34.34855,-34.39039,-34.50761]}, - {"t":0.29286, "x":6.91839, "y":6.14519, "heading":-2.35582, "vx":-1.31729, "vy":-0.6171, "omega":0.00306, "ax":-4.49417, "ay":-2.10544, "alpha":0.01349, "fx":[-73.45188,-73.51094,-73.49034,-73.43129], "fy":[-34.46042,-34.33455,-34.37943,-34.50509]}, - {"t":0.3254, "x":6.87314, "y":6.12399, "heading":-2.35572, "vx":-1.46353, "vy":-0.68561, "omega":0.0035, "ax":-4.49273, "ay":-2.1048, "alpha":0.01463, "fx":[-73.42678,-73.49083,-73.46853,-73.40449], "fy":[-34.45348,-34.31701,-34.36572,-34.50193]}, - {"t":0.35794, "x":6.82314, "y":6.10057, "heading":-2.35561, "vx":-1.60973, "vy":-0.7541, "omega":0.00398, "ax":-4.49089, "ay":-2.10399, "alpha":0.0161, "fx":[-73.39445,-73.46492,-73.44044,-73.36998], "fy":[-34.44453,-34.29442,-34.34806,-34.49786]}, - {"t":0.39048, "x":6.76838, "y":6.07491, "heading":-2.35548, "vx":-1.75586, "vy":-0.82256, "omega":0.0045, "ax":-4.48842, "ay":-2.1029, "alpha":0.01807, "fx":[-73.35122,-73.43029,-73.40291,-73.32385], "fy":[-34.43258,-34.26423,-34.32446,-34.49242]}, - {"t":0.42302, "x":6.70887, "y":6.04703, "heading":-2.35533, "vx":-1.90191, "vy":-0.89099, "omega":0.00509, "ax":-4.48495, "ay":-2.10137, "alpha":0.02084, "fx":[-73.29047,-73.38164,-73.35022,-73.25907], "fy":[-34.41578,-34.22179,-34.29133,-34.4848]}, - {"t":0.45556, "x":6.64461, "y":6.01693, "heading":-2.35517, "vx":-2.04786, "vy":-0.95937, "omega":0.00577, "ax":-4.47972, "ay":-2.09907, "alpha":0.02502, "fx":[-73.19887,-73.30831,-73.27086,-73.16146], "fy":[-34.39046,-34.15781,-34.24141,-34.47332]}, - {"t":0.4881, "x":6.5756, "y":5.9846, "heading":-2.35498, "vx":-2.19363, "vy":-1.02767, "omega":0.00658, "ax":-4.47094, "ay":-2.0952, "alpha":0.03206, "fx":[-73.04489,-73.18511,-73.13774,-72.99758], "fy":[-34.3479,-34.05027,-34.15766,-34.45407]}, - {"t":0.52064, "x":6.50185, "y":5.95005, "heading":-2.35477, "vx":-2.33911, "vy":-1.09585, "omega":0.00763, "ax":-4.45313, "ay":-2.08734, "alpha":0.04646, "fx":[-72.73195,-72.93502,-72.86822,-72.66528], "fy":[-34.26147,-33.83177,-33.98799,-34.41514]}, - {"t":0.55318, "x":6.42338, "y":5.91329, "heading":-2.35452, "vx":-2.48402, "vy":-1.16377, "omega":0.00914, "ax":-4.39758, "ay":-2.06285, "alpha":0.09257, "fx":[-71.7508,-72.15502,-72.03306,-71.62963], "fy":[-33.99243,-33.1452,-33.46015,-34.29714]}, - {"t":0.58572, "x":6.34022, "y":5.87432, "heading":-2.35422, "vx":-2.62712, "vy":-1.2309, "omega":0.01215, "ax":3.22933, "ay":1.48263, "alpha":1.33572, "fx":[56.17561,50.92751,49.14119,54.9295], "fy":[22.47913,30.87511,26.46115,17.13753]}, - {"t":0.61826, "x":6.25644, "y":5.83506, "heading":-2.35383, "vx":-2.52203, "vy":-1.18266, "omega":0.05562, "ax":4.41293, "ay":2.06445, "alpha":0.07275, "fx":[72.25229,71.9358,72.03357,72.35044], "fy":[33.54179,34.20252,33.9611,33.29415]}, - {"t":0.6508, "x":6.17671, "y":5.79766, "heading":-2.35202, "vx":-2.37844, "vy":-1.11548, "omega":0.05798, "ax":4.45754, "ay":2.0866, "alpha":0.03527, "fx":[72.92343,72.76966,72.821,72.97483], "fy":[34.00902,34.3335,34.21555,33.88961]}, - {"t":0.68334, "x":6.10167, "y":5.76247, "heading":-2.35013, "vx":-2.23339, "vy":-1.04758, "omega":0.05913, "ax":4.47315, "ay":2.09435, "alpha":0.02237, "fx":[73.15945,73.06187,73.09545,73.19305], "fy":[34.17311,34.3798,34.30452,34.09724]}, - {"t":0.71588, "x":6.03137, "y":5.72949, "heading":-2.3482, "vx":-2.08783, "vy":-0.97943, "omega":0.05986, "ax":4.4811, "ay":2.0983, "alpha":0.01583, "fx":[73.27989,73.21085,73.23505,73.30409], "fy":[34.25684,34.40337,34.34979,34.20297]}, - {"t":0.74843, "x":5.9658, "y":5.69873, "heading":-2.34626, "vx":-1.94201, "vy":-0.91115, "omega":0.06037, "ax":4.48592, "ay":2.1007, "alpha":0.01187, "fx":[73.35295,73.3012,73.31958,73.37134], "fy":[34.30764,34.41761,34.37721,34.26706]}, - {"t":0.78097, "x":5.90498, "y":5.67019, "heading":-2.34429, "vx":-1.79604, "vy":-0.84279, "omega":0.06076, "ax":4.48915, "ay":2.1023, "alpha":0.00922, "fx":[73.402,73.36183,73.37625,73.41642], "fy":[34.34173,34.42714,34.3956,34.31008]}, - {"t":0.81351, "x":5.84892, "y":5.64388, "heading":-2.34231, "vx":-1.64996, "vy":-0.77438, "omega":0.06106, "ax":4.49147, "ay":2.10345, "alpha":0.00731, "fx":[73.4372,73.40535,73.41689,73.44874], "fy":[34.36619,34.43395,34.40879,34.34096]}, - {"t":0.84605, "x":5.7976, "y":5.6198, "heading":-2.34033, "vx":-1.50381, "vy":-0.70594, "omega":0.0613, "ax":4.49322, "ay":2.10432, "alpha":0.00588, "fx":[73.46369,73.4381,73.44746,73.47305], "fy":[34.38459,34.43906,34.41872,34.36421]}, - {"t":0.87859, "x":5.75105, "y":5.59794, "heading":-2.33833, "vx":-1.3576, "vy":-0.63746, "omega":0.06149, "ax":0.05832, "ay":-0.10122, "alpha":1.51111, "fx":[6.73272,0.62347,-4.79461,1.25233], "fy":[-1.50228,4.37411,-1.81317,-7.67764]}, - {"t":0.90947, "x":5.70915, "y":5.57821, "heading":-2.33643, "vx":-1.3558, "vy":-0.64059, "omega":0.10815, "ax":0.02493, "ay":-0.05267, "alpha":1.38357, "fx":[5.68474,0.13097,-4.86759,0.68199], "fy":[-0.70223,4.65891,-1.02368,-6.37725]}, - {"t":0.94035, "x":5.6673, "y":5.5584, "heading":-2.33309, "vx":-1.35503, "vy":-0.64221, "omega":0.15088, "ax":0.00696, "ay":-0.01468, "alpha":1.25968, "fx":[4.91612,-0.16085,-4.6853,0.38505], "fy":[-0.07236,4.78604,-0.40866,-5.26465]}, - {"t":0.97123, "x":5.62546, "y":5.53856, "heading":-2.32844, "vx":-1.35481, "vy":-0.64267, "omega":0.18978, "ax":0.00131, "ay":-0.00276, "alpha":1.13959, "fx":[4.3644,-0.25506,-4.31524,0.29161], "fy":[0.13102,4.5023,-0.22172,-4.59228]}, - {"t":1.00211, "x":5.58362, "y":5.51871, "heading":-2.32257, "vx":-1.35477, "vy":-0.64275, "omega":0.22497, "ax":0.0003, "ay":-0.0003, "alpha":1.02307, "fx":[3.91265,-0.30751,-3.87093,0.28512], "fy":[0.17857,4.07794,-0.1884,-4.08778]}, - {"t":1.03299, "x":5.54178, "y":5.49887, "heading":-2.31563, "vx":-1.35476, "vy":-0.64276, "omega":0.25656, "ax":0.55648, "ay":0.27049, "alpha":0.8834, "fx":[12.47808,8.60369,5.84522,9.46226], "fy":[4.56691,7.97959,4.28226,0.85941]}, - {"t":1.06387, "x":5.50021, "y":5.47915, "heading":-2.3077, "vx":-1.33758, "vy":-0.63441, "omega":0.28384, "ax":1.79229, "ay":0.85037, "alpha":0.66647, "fx":[31.62643,29.04632,26.87604,29.65333], "fy":[13.88336,16.70929,13.94088,11.07409]}, - {"t":1.09475, "x":5.45976, "y":5.45996, "heading":-2.29894, "vx":-1.28223, "vy":-0.60815, "omega":0.30442, "ax":1.79971, "ay":0.85382, "alpha":0.57317, "fx":[31.42802,29.14591,27.35953,29.75361], "fy":[13.95822,16.37976,13.97137,11.52394]}, - {"t":1.12563, "x":5.42102, "y":5.44159, "heading":-2.28954, "vx":-1.22666, "vy":-0.58178, "omega":0.32212, "ax":1.80216, "ay":0.85494, "alpha":0.48421, "fx":[31.15922,29.20603,27.72678,29.75547], "fy":[13.99603,16.02311,13.96581,11.9217]}, - {"t":1.15651, "x":5.384, "y":5.42403, "heading":-2.27959, "vx":-1.171, "vy":-0.55538, "omega":0.33708, "ax":1.80339, "ay":0.85549, "alpha":0.39663, "fx":[30.87415,29.26137,28.06271,29.72942], "fy":[14.0204,15.66051,13.95626,12.30527]}, - {"t":1.18739, "x":5.3487, "y":5.40729, "heading":-2.26918, "vx":-1.11531, "vy":-0.52896, "omega":0.34932, "ax":1.80412, "ay":0.85581, "alpha":0.30958, "fx":[30.58288,29.3186,28.38524,29.68909], "fy":[14.03561,15.29531,13.9494,12.68302]}, - {"t":1.21827, "x":5.31512, "y":5.39136, "heading":-2.2584, "vx":-1.0596, "vy":-0.50253, "omega":0.35888, "ax":1.80461, "ay":0.85602, "alpha":0.22267, "fx":[30.28837,29.38016,28.70077,29.63874], "fy":[14.04279,14.92818,13.94764,13.05826]}, - {"t":1.24915, "x":5.28326, "y":5.37625, "heading":-2.24731, "vx":-1.00387, "vy":-0.4761, "omega":0.36576, "ax":1.80497, "ay":0.85616, "alpha":0.13563, "fx":[29.99167,29.44727,29.01223,29.57999], "fy":[14.04224,14.55911,13.95218,13.43271]}, - {"t":1.28003, "x":5.25312, "y":5.36196, "heading":-2.23602, "vx":-0.94814, "vy":-0.44966, "omega":0.36995, "ax":1.80523, "ay":0.85626, "alpha":0.04824, "fx":[29.69302,29.5207,29.32135,29.51351], "fy":[14.03399,14.18779,13.96369,13.80757]}, - {"t":1.31091, "x":5.2247, "y":5.34848, "heading":-2.22459, "vx":-0.89239, "vy":-0.42322, "omega":0.37144, "ax":1.80544, "ay":0.85634, "alpha":-0.03969, "fx":[29.3924,29.60097,29.62922,29.43961], "fy":[14.01802,13.81389,13.98256,14.18369]}, - {"t":1.34179, "x":5.19801, "y":5.33582, "heading":-2.21312, "vx":-0.83664, "vy":-0.39678, "omega":0.37021, "ax":1.80561, "ay":0.8564, "alpha":-0.12834, "fx":[29.0896,29.68844,29.93668,29.35841], "fy":[13.99424,13.43694,14.00903,14.56188]}, - {"t":1.37268, "x":5.17303, "y":5.32397, "heading":-2.20169, "vx":-0.78088, "vy":-0.37033, "omega":0.36625, "ax":1.80575, "ay":0.85645, "alpha":-0.21785, "fx":[28.78435,29.78337,30.24441,29.27002], "fy":[13.96267,13.05655,14.04321,14.9428]}, - {"t":1.40356, "x":5.14978, "y":5.31295, "heading":-2.19038, "vx":-0.72512, "vy":-0.34388, "omega":0.35952, "ax":1.80586, "ay":0.85649, "alpha":-0.30841, "fx":[28.47625,29.88591,30.55302,29.1745], "fy":[13.92334,12.67221,14.08509,15.3271]}, - {"t":1.43444, "x":5.12825, "y":5.30273, "heading":-2.17928, "vx":-0.66935, "vy":-0.31743, "omega":0.35, "ax":1.80596, "ay":0.85652, "alpha":-0.40014, "fx":[28.16493,29.99615,30.86304,29.07197], "fy":[13.87637,12.28346,14.13457,15.7154]}, - {"t":1.46532, "x":5.10844, "y":5.29334, "heading":-2.16847, "vx":-0.61358, "vy":-0.29098, "omega":0.33764, "ax":1.80605, "ay":0.85655, "alpha":-0.49322, "fx":[27.84989,30.11409,31.17504,28.96258], "fy":[13.82197,11.88977,14.19143,16.10835]}, - {"t":1.4962, "x":5.09035, "y":5.28476, "heading":-2.15804, "vx":-0.55781, "vy":-0.26453, "omega":0.32241, "ax":1.80612, "ay":0.85657, "alpha":-0.58779, "fx":[27.53063,30.23963,31.48956,28.84657], "fy":[13.76043,11.49059,14.25538,16.50657]}, - {"t":1.52708, "x":5.07399, "y":5.277, "heading":-2.14809, "vx":-0.50203, "vy":-0.23808, "omega":0.30426, "ax":1.80618, "ay":0.85659, "alpha":-0.68401, "fx":[27.20657,30.37263,31.80721,28.7242], "fy":[13.69215,11.0853,14.32598,16.91077]}, - {"t":1.55796, "x":5.05935, "y":5.27006, "heading":-2.13869, "vx":-0.44626, "vy":-0.21163, "omega":0.28314, "ax":1.80624, "ay":0.8566, "alpha":-0.78203, "fx":[26.87706,30.51282,32.12859,28.59586], "fy":[13.61765,10.67326,14.40271,17.32165]}, - {"t":1.58884, "x":5.04643, "y":5.26393, "heading":-2.12995, "vx":-0.39048, "vy":-0.18518, "omega":0.25899, "ax":1.80629, "ay":0.85662, "alpha":-0.88203, "fx":[26.54138,30.6599,32.45437,28.462], "fy":[13.53756,10.25373,14.48494,17.73997]}, - {"t":1.61972, "x":5.03523, "y":5.25862, "heading":-2.12195, "vx":-0.3347, "vy":-0.15872, "omega":0.23175, "ax":1.80634, "ay":0.85663, "alpha":-0.98415, "fx":[26.19873,30.81342,32.78527,28.32319], "fy":[13.45269,9.82592,14.57188,18.16654]}, - {"t":1.6506, "x":5.02575, "y":5.25413, "heading":-2.11479, "vx":-0.27892, "vy":-0.13227, "omega":0.20136, "ax":1.80638, "ay":0.85664, "alpha":-1.08858, "fx":[25.84823,30.97287,33.12211,28.18009], "fy":[13.36395,9.38893,14.66262,18.60225]}, - {"t":1.68148, "x":5.018, "y":5.25045, "heading":-2.10858, "vx":-0.22314, "vy":-0.10582, "omega":0.16774, "ax":1.80641, "ay":0.85665, "alpha":-1.19549, "fx":[25.48887,31.13761,33.46573,28.03351], "fy":[13.27249,8.94179,14.75611,19.04802]}, - {"t":1.71236, "x":5.01197, "y":5.24759, "heading":-2.1034, "vx":-0.16736, "vy":-0.07936, "omega":0.13083, "ax":1.80645, "ay":0.85666, "alpha":-1.30507, "fx":[25.11955,31.30687,33.8171,27.8844], "fy":[13.17962,8.48342,14.85111,19.50486]}, - {"t":1.74324, "x":5.00767, "y":5.24555, "heading":-2.09936, "vx":-0.11157, "vy":-0.05291, "omega":0.09052, "ax":1.80648, "ay":0.85667, "alpha":-1.41752, "fx":[24.73903,31.47979,34.1773,27.73383], "fy":[13.0869,8.01262,14.94619,19.97385]}, - {"t":1.77412, "x":5.00508, "y":5.24433, "heading":-2.09656, "vx":-0.05579, "vy":-0.02645, "omega":0.04675, "ax":1.80651, "ay":0.85668, "alpha":-1.51391, "fx":[24.34243,31.64265,34.54806,27.59864], "fy":[13.02489,7.67471,15.00881,20.31166]}, - {"t":1.805, "x":5.00422, "y":5.24392, "heading":-2.09512, "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":7.11131, "y":6.23556, "heading":-2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.1785, "ay":-1.95764, "alpha":0.00825, "fx":[-68.29881,-68.33496,-68.32225,-68.28611], "fy":[-32.02858,-31.95148,-31.97888,-32.05588]}, + {"t":0.03334, "x":7.10899, "y":6.23448, "heading":-2.35619, "vx":-0.13932, "vy":-0.06527, "omega":0.00028, "ax":-4.1782, "ay":-1.95751, "alpha":0.00851, "fx":[-68.29346,-68.33072,-68.31763,-68.28037], "fy":[-32.02716,-31.94768,-31.97592,-32.05531]}, + {"t":0.06669, "x":7.10202, "y":6.23121, "heading":-2.35619, "vx":-0.27864, "vy":-0.13054, "omega":0.00056, "ax":-4.17785, "ay":-1.95736, "alpha":0.0088, "fx":[-68.28742,-68.32595,-68.31242,-68.2739], "fy":[-32.02556,-31.94339,-31.97259,-32.05466]}, + {"t":0.10003, "x":7.09041, "y":6.22577, "heading":-2.35617, "vx":-0.41794, "vy":-0.19581, "omega":0.00085, "ax":-4.17746, "ay":-1.95719, "alpha":0.00913, "fx":[-68.28058,-68.32053,-68.30651,-68.26656], "fy":[-32.02374,-31.93852,-31.96882,-32.05393]}, + {"t":0.13337, "x":7.07415, "y":6.21815, "heading":-2.35614, "vx":-0.55723, "vy":-0.26106, "omega":0.00116, "ax":-4.17702, "ay":-1.95699, "alpha":0.0095, "fx":[-68.27274,-68.31434,-68.29975,-68.25816], "fy":[-32.02166,-31.93295,-31.9645,-32.05309]}, + {"t":0.16671, "x":7.05325, "y":6.20836, "heading":-2.3561, "vx":-0.6965, "vy":-0.32632, "omega":0.00147, "ax":-4.1765, "ay":-1.95676, "alpha":0.00993, "fx":[-68.26369,-68.30718,-68.29194,-68.24845], "fy":[-32.01926,-31.92652,-31.95951,-32.05212]}, + {"t":0.20006, "x":7.02771, "y":6.19639, "heading":-2.35605, "vx":-0.83575, "vy":-0.39156, "omega":0.0018, "ax":-4.1759, "ay":-1.9565, "alpha":0.01044, "fx":[-68.25311,-68.29882,-68.28281,-68.23711], "fy":[-32.01645,-31.91899,-31.95368,-32.05099]}, + {"t":0.2334, "x":6.99752, "y":6.18225, "heading":-2.35599, "vx":-0.97499, "vy":-0.45679, "omega":0.00215, "ax":-4.17518, "ay":-1.95619, "alpha":0.01104, "fx":[-68.24058,-68.28891,-68.27201,-68.22368], "fy":[-32.01312,-31.91009,-31.94677,-32.04965]}, + {"t":0.26674, "x":6.96269, "y":6.16593, "heading":-2.35592, "vx":-1.1142, "vy":-0.52202, "omega":0.00252, "ax":-4.17432, "ay":-1.95581, "alpha":0.01176, "fx":[-68.22551,-68.277,-68.25901,-68.20752], "fy":[-32.00912,-31.89937,-31.93847,-32.04804]}, + {"t":0.30009, "x":6.92322, "y":6.14744, "heading":-2.35583, "vx":-1.25339, "vy":-0.58723, "omega":0.00291, "ax":-4.17327, "ay":-1.95535, "alpha":0.01265, "fx":[-68.20704,-68.2624,-68.24308,-68.18773], "fy":[-32.00422,-31.88624,-31.9283,-32.04607]}, + {"t":0.33343, "x":6.87911, "y":6.12677, "heading":-2.35574, "vx":-1.39254, "vy":-0.65243, "omega":0.00333, "ax":-4.17195, "ay":-1.95477, "alpha":0.01376, "fx":[-68.18386,-68.24408,-68.2231,-68.16289], "fy":[-31.99806,-31.86976,-31.91554,-32.04359]}, + {"t":0.36677, "x":6.83036, "y":6.10393, "heading":-2.35563, "vx":-1.53164, "vy":-0.71761, "omega":0.00379, "ax":-4.17025, "ay":-1.95402, "alpha":0.01519, "fx":[-68.15392,-68.22042,-68.19731,-68.13082], "fy":[-31.99011,-31.84848,-31.89907,-32.0404]}, + {"t":0.40011, "x":6.77697, "y":6.07892, "heading":-2.3555, "vx":-1.67069, "vy":-0.78276, "omega":0.0043, "ax":-4.16796, "ay":-1.95302, "alpha":0.01712, "fx":[-68.11375,-68.18868,-68.16273,-68.08782], "fy":[-31.97943,-31.81993,-31.87698,-32.03611]}, + {"t":0.43346, "x":6.71895, "y":6.05173, "heading":-2.35536, "vx":-1.80966, "vy":-0.84788, "omega":0.00487, "ax":-4.16474, "ay":-1.9516, "alpha":0.01984, "fx":[-68.05705,-68.14388,-68.11395,-68.02714], "fy":[-31.96437,-31.77963,-31.84583,-32.03006]}, + {"t":0.4668, "x":6.65629, "y":6.02238, "heading":-2.35519, "vx":-1.94852, "vy":-0.91295, "omega":0.00553, "ax":-4.15984, "ay":-1.94945, "alpha":0.02398, "fx":[-67.97094,-68.07586,-68.03996,-67.93508], "fy":[-31.94148,-31.71845,-31.79858,-32.02087]}, + {"t":0.50014, "x":6.58901, "y":5.99085, "heading":-2.35501, "vx":-2.08722, "vy":-0.97795, "omega":0.00633, "ax":-4.15152, "ay":-1.9458, "alpha":0.03103, "fx":[-67.82455,-67.96026,-67.91443,-67.77878], "fy":[-31.90255,-31.61452,-31.71843,-32.00523]}, + {"t":0.53348, "x":6.51711, "y":5.95717, "heading":-2.3548, "vx":-2.22565, "vy":-1.04283, "omega":0.00737, "ax":-4.13428, "ay":-1.93824, "alpha":0.0457, "fx":[-67.5205,-67.72031,-67.65469,-67.45502], "fy":[-31.82161,-31.39891,-31.55262,-31.97267]}, + {"t":0.56683, "x":6.4406, "y":5.92132, "heading":-2.35455, "vx":-2.3635, "vy":-1.10745, "omega":0.00889, "ax":-4.07711, "ay":-1.91312, "alpha":0.09507, "fx":[-66.507,-66.9221,-66.79863,-66.38426], "fy":[-31.55122,-30.68244,-31.00625,-31.86384]}, + {"t":0.60017, "x":6.35953, "y":5.88333, "heading":-2.35426, "vx":-2.49944, "vy":-1.17124, "omega":0.01206, "ax":3.61219, "ay":1.67604, "alpha":0.59454, "fx":[60.18933,57.70457,57.87735,60.43836], "fy":[26.12223,30.74728,28.89646,23.83423]}, + {"t":0.63351, "x":6.2782, "y":5.84521, "heading":-2.35385, "vx":-2.379, "vy":-1.11536, "omega":0.03188, "ax":4.10597, "ay":1.92116, "alpha":0.06392, "fx":[67.22021,66.94197,67.02907,67.30759], "fy":[31.22369,31.80561,31.5935,31.00648]}, + {"t":0.66686, "x":6.20116, "y":5.80909, "heading":-2.35279, "vx":-2.24209, "vy":-1.0513, "omega":0.03402, "ax":4.14198, "ay":1.93911, "alpha":0.03232, "fx":[67.76045,67.61947,67.66663,67.80767], "fy":[31.60625,31.90391,31.79608,31.4971]}, + {"t":0.7002, "x":6.12871, "y":5.77511, "heading":-2.35166, "vx":-2.10399, "vy":-0.98665, "omega":0.03509, "ax":4.15518, "ay":1.9457, "alpha":0.02083, "fx":[67.95919,67.86828,67.89949,67.99042], "fy":[31.74716,31.93979,31.87001,31.67683]}, + {"t":0.73354, "x":6.06086, "y":5.74329, "heading":-2.35049, "vx":-1.96544, "vy":-0.92177, "omega":0.03579, "ax":4.16203, "ay":1.94911, "alpha":0.01489, "fx":[68.06248,67.9975,68.02014,68.08513], "fy":[31.82043,31.95835,31.90829,31.77009]}, + {"t":0.76688, "x":5.99764, "y":5.71364, "heading":-2.34929, "vx":-1.82667, "vy":-0.85678, "omega":0.03628, "ax":4.16623, "ay":1.9512, "alpha":0.01125, "fx":[68.12576,68.07665,68.09394,68.14305], "fy":[31.86533,31.96968,31.93171,31.8272]}, + {"t":0.80023, "x":5.93905, "y":5.68616, "heading":-2.34808, "vx":-1.68776, "vy":-0.79172, "omega":0.03666, "ax":4.16906, "ay":1.95262, "alpha":0.0088, "fx":[68.16851,68.13011,68.14374,68.18214], "fy":[31.89566,31.97731,31.94752,31.86576]}, + {"t":0.83357, "x":5.8851, "y":5.66085, "heading":-2.34686, "vx":-1.54875, "vy":-0.72662, "omega":0.03695, "ax":4.1711, "ay":1.95363, "alpha":0.00703, "fx":[68.19932,68.16864,68.17961,68.21029], "fy":[31.91752,31.9828,31.9589,31.89356]}, + {"t":0.86691, "x":5.83577, "y":5.63771, "heading":-2.34563, "vx":-1.40967, "vy":-0.66148, "omega":0.03719, "ax":4.17263, "ay":1.9544, "alpha":0.0057, "fx":[68.22259,68.19774,68.20668,68.23153], "fy":[31.93403,31.98693,31.9675,31.91455]}, + {"t":0.90026, "x":5.79109, "y":5.61674, "heading":-2.34439, "vx":-1.27054, "vy":-0.59631, "omega":0.03738, "ax":4.17384, "ay":1.955, "alpha":0.00466, "fx":[68.24078,68.22048,68.22782,68.24813], "fy":[31.94693,31.99015,31.97422,31.93097]}, + {"t":0.9336, "x":5.75105, "y":5.59794, "heading":-2.34314, "vx":-1.13138, "vy":-0.53113, "omega":0.03753, "ax":0.05238, "ay":-0.09348, "alpha":1.49741, "fx":[6.57224,0.57467,-4.83962,1.1178], "fy":[-1.40773,4.45521,-1.65746,-7.50296]}, + {"t":0.96502, "x":5.71552, "y":5.58121, "heading":-2.34196, "vx":-1.12973, "vy":-0.53407, "omega":0.08458, "ax":0.01787, "ay":-0.03775, "alpha":1.37278, "fx":[5.52189,0.04856,-4.93597,0.53432], "fy":[-0.49098,4.86812,-0.7473,-6.09862]}, + {"t":0.99644, "x":5.68004, "y":5.56441, "heading":-2.33931, "vx":-1.12917, "vy":-0.53525, "omega":0.12772, "ax":0.00401, "ay":-0.00847, "alpha":1.25114, "fx":[4.82917,-0.17508,-4.69552,0.30395], "fy":[-0.00375,4.86175,-0.27407,-5.13758]}, + {"t":1.02786, "x":5.64456, "y":5.54758, "heading":-2.33529, "vx":-1.12904, "vy":-0.53552, "omega":0.16703, "ax":0.00059, "ay":-0.00124, "alpha":1.13272, "fx":[4.31864,-0.22756,-4.29777,0.24515], "fy":[0.12336,4.5082,-0.1641,-4.54854]}, + {"t":1.05928, "x":5.60908, "y":5.53076, "heading":-2.33004, "vx":-1.12902, "vy":-0.53556, "omega":0.20262, "ax":-0.00015, "ay":0.00032, "alpha":1.01731, "fx":[3.86339,-0.23564,-3.86766,0.23007], "fy":[0.15717,4.07396,-0.1468,-4.06359]}, + {"t":1.09071, "x":5.57361, "y":5.51393, "heading":-2.32368, "vx":-1.12903, "vy":-0.53555, "omega":0.23459, "ax":-0.00031, "ay":0.00066, "alpha":0.90457, "fx":[3.42794,-0.23321,-3.43816,0.223], "fy":[0.16923,3.63045,-0.14766,-3.60895]}, + {"t":1.12213, "x":5.53813, "y":5.4971, "heading":-2.31631, "vx":-1.12904, "vy":-0.53553, "omega":0.26301, "ax":-0.00039, "ay":0.00083, "alpha":0.79418, "fx":[3.00298,-0.22702,-3.01615,0.21457], "fy":[0.17535,3.19342,-0.14831,-3.16644]}, + {"t":1.15355, "x":5.50265, "y":5.48027, "heading":-2.30804, "vx":-1.12905, "vy":-0.5355, "omega":0.28796, "ax":-0.00041, "ay":0.00086, "alpha":0.6858, "fx":[2.58689,-0.21619,-2.60083,0.20333], "fy":[0.17516,2.76239,-0.1469,-2.73416]}, + {"t":1.18497, "x":5.46718, "y":5.46345, "heading":-2.29899, "vx":-1.12906, "vy":-0.53547, "omega":0.30951, "ax":-0.00008, "ay":0.00016, "alpha":0.57911, "fx":[2.1833,-0.19517,-2.18627,0.19313], "fy":[0.15774,2.32621,-0.15247,-2.32092]}, + {"t":1.21639, "x":5.4317, "y":5.44662, "heading":-2.28927, "vx":-1.12907, "vy":-0.53547, "omega":0.32771, "ax":0.00078, "ay":-0.00165, "alpha":0.47381, "fx":[1.79501,-0.16358,-1.76737,0.18714], "fy":[0.11628,1.87773,-0.17021,-1.93165]}, + {"t":1.24781, "x":5.39622, "y":5.4298, "heading":-2.27897, "vx":-1.12904, "vy":-0.53552, "omega":0.3426, "ax":-0.00109, "ay":0.0046, "alpha":0.37056, "fx":[1.41513,-0.3206,-1.34619,0.18036], "fy":[0.19979,1.56608,-0.04929,-1.41557]}, + {"t":1.27923, "x":5.36075, "y":5.41297, "heading":-2.26821, "vx":-1.12908, "vy":-0.53538, "omega":0.35424, "ax":1.63703, "ay":0.77583, "alpha":0.2267, "fx":[27.53843,26.66109,25.95436,26.89534], "fy":[12.72101,13.65064,12.65271,11.70923]}, + {"t":1.31066, "x":5.32608, "y":5.39653, "heading":-2.25708, "vx":-1.07764, "vy":-0.511, "omega":0.36136, "ax":1.79641, "ay":0.8519, "alpha":0.14028, "fx":[29.83813,29.31506,28.86792,29.45041], "fy":[13.95483,14.53003,13.90458,13.31815]}, + {"t":1.34208, "x":5.2931, "y":5.3809, "heading":-2.24572, "vx":-1.02119, "vy":-0.48423, "omega":0.36577, "ax":1.8016, "ay":0.85434, "alpha":0.06247, "fx":[29.66372,29.44902,29.22378,29.47448], "fy":[13.99472,14.23186,13.94242,13.69856]}, + {"t":1.3735, "x":5.26191, "y":5.3661, "heading":-2.23423, "vx":-0.96458, "vy":-0.45738, "omega":0.36773, "ax":1.80341, "ay":0.85519, "alpha":-0.0157, "fx":[29.43169,29.54313,29.51962,29.43451], "fy":[14.0026,13.90469,13.96141,14.05401]}, + {"t":1.40492, "x":5.23249, "y":5.35215, "heading":-2.22267, "vx":-0.90792, "vy":-0.43051, "omega":0.36724, "ax":1.80432, "ay":0.85561, "alpha":-0.09445, "fx":[29.18347,29.63266,29.79954,29.37326], "fy":[13.99707,13.56787,13.981,14.40455]}, + {"t":1.43634, "x":5.20485, "y":5.33905, "heading":-2.21113, "vx":-0.85122, "vy":-0.40363, "omega":0.36427, "ax":1.80488, "ay":0.85587, "alpha":-0.17389, "fx":[28.92765,29.72458,30.07323,29.29981], "fy":[13.98195,13.22527,14.00523,14.75469]}, + {"t":1.46776, "x":5.17899, "y":5.32679, "heading":-2.19969, "vx":-0.79451, "vy":-0.37674, "omega":0.35881, "ax":1.80525, "ay":0.85604, "alpha":-0.25415, "fx":[28.66697,29.82122,30.34421,29.21725], "fy":[13.95852,12.87801,14.03541,15.10624]}, + {"t":1.49918, "x":5.15492, "y":5.31537, "heading":-2.18841, "vx":-0.73779, "vy":-0.34984, "omega":0.35082, "ax":1.80552, "ay":0.85616, "alpha":-0.33534, "fx":[28.40242,29.92354,30.61423,29.12698], "fy":[13.9274,12.52633,14.07203,15.46025]}, + {"t":1.53061, "x":5.13263, "y":5.30481, "heading":-2.17739, "vx":-0.68106, "vy":-0.32294, "omega":0.34029, "ax":1.80572, "ay":0.85624, "alpha":-0.41757, "fx":[28.13428,30.03206,30.88432,29.02971], "fy":[13.88904,12.17025,14.11513,15.81742]}, + {"t":1.56203, "x":5.11212, "y":5.29508, "heading":-2.1667, "vx":-0.62432, "vy":-0.29603, "omega":0.32717, "ax":1.80588, "ay":0.85631, "alpha":-0.50095, "fx":[27.86255,30.1469,31.15523,28.92598], "fy":[13.8438,11.80952,14.16464,16.17839]}, + {"t":1.59345, "x":5.0934, "y":5.2862, "heading":-2.15642, "vx":-0.56757, "vy":-0.26913, "omega":0.31143, "ax":1.806, "ay":0.85637, "alpha":-0.58559, "fx":[27.587,30.2681,31.42761,28.81623], "fy":[13.79208,11.44382,14.22026,16.54375]}, + {"t":1.62487, "x":5.07645, "y":5.27817, "heading":-2.14663, "vx":-0.51083, "vy":-0.24222, "omega":0.29303, "ax":1.80611, "ay":0.85641, "alpha":-0.67163, "fx":[27.3073,30.39553,31.70202,28.70087], "fy":[13.73437,11.07275,14.28163,16.91406]}, + {"t":1.65629, "x":5.06129, "y":5.27098, "heading":-2.13743, "vx":-0.45408, "vy":-0.21531, "omega":0.27192, "ax":1.8062, "ay":0.85645, "alpha":-0.75916, "fx":[27.02301,30.52901,31.97904,28.58033], "fy":[13.67123,10.69588,14.34819,17.2899]}, + {"t":1.68771, "x":5.04792, "y":5.26464, "heading":-2.12888, "vx":-0.39732, "vy":-0.1884, "omega":0.24807, "ax":1.80627, "ay":0.85648, "alpha":-0.84832, "fx":[26.73359,30.66819,32.25928,28.45514], "fy":[13.60332,10.31264,14.41936,17.6719]}, + {"t":1.71913, "x":5.03633, "y":5.25914, "heading":-2.12109, "vx":-0.34057, "vy":-0.16149, "omega":0.22141, "ax":1.80633, "ay":0.85651, "alpha":-0.93923, "fx":[26.43843,30.8127,32.54334,28.32585], "fy":[13.53144,9.92242,14.49438,18.06071]}, + {"t":1.75056, "x":5.02652, "y":5.25449, "heading":-2.11413, "vx":-0.28381, "vy":-0.13457, "omega":0.1919, "ax":1.80639, "ay":0.85653, "alpha":-1.03201, "fx":[26.13683,30.96207,32.83191,28.1931], "fy":[13.45654,9.52455,14.57236,18.45699]}, + {"t":1.78198, "x":5.01849, "y":5.25068, "heading":-2.1081, "vx":-0.22705, "vy":-0.10766, "omega":0.15947, "ax":1.80643, "ay":0.85655, "alpha":-1.12681, "fx":[25.82798,31.1157,33.12571,28.05765], "fy":[13.37969,9.11826,14.6523,18.86148]}, + {"t":1.8134, "x":5.01225, "y":5.24772, "heading":-2.10309, "vx":-0.17029, "vy":-0.08075, "omega":0.12407, "ax":1.80648, "ay":0.85657, "alpha":-1.22375, "fx":[25.51101,31.2729,33.42555,27.92036], "fy":[13.30214,8.70268,14.73307,19.27497]}, + {"t":1.84482, "x":5.00779, "y":5.24561, "heading":-2.09919, "vx":-0.11353, "vy":-0.05383, "omega":0.08562, "ax":1.80651, "ay":0.85658, "alpha":-1.32299, "fx":[25.18491,31.43287,33.73227,27.78223], "fy":[13.22537,8.27686,14.81337,19.69828]}, + {"t":1.87624, "x":5.00511, "y":5.24434, "heading":-2.0965, "vx":-0.05676, "vy":-0.02692, "omega":0.04405, "ax":1.80655, "ay":0.8566, "alpha":-1.40176, "fx":[24.83872,31.57646,34.04764,27.67166], "fy":[13.20488,8.00309,14.85831,19.94854]}, + {"t":1.90766, "x":5.00422, "y":5.24392, "heading":-2.09512, "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/LtoAGround.traj b/src/main/deploy/choreo/LtoAGround.traj new file mode 100644 index 00000000..0565bd94 --- /dev/null +++ b/src/main/deploy/choreo/LtoAGround.traj @@ -0,0 +1,144 @@ +{ + "name":"LtoAGround", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0471975511965976, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.2501322031021118, "y":6.730594158172607, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":1.203567624092102, "y":6.226143836975098, "heading":-1.5344487260604618, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.1651506423950195, "y":4.182002544403076, "heading":0.0, "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}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"(-60) deg", "val":-1.0471975511965976}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.2501322031021118 m", "val":1.2501322031021118}, "y":{"exp":"6.730594158172607 m", "val":6.730594158172607}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"1.203567624092102 m", "val":1.203567624092102}, "y":{"exp":"6.226143836975098 m", "val":6.226143836975098}, "heading":{"exp":"-1.5344487260604618 rad", "val":-1.5344487260604618}, "intervals":47, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"A.x", "val":3.1651506423950195}, "y":{"exp":"A.y", "val":4.182002544403076}, "heading":{"exp":"0 deg", "val":0.0}, "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}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.53916,1.97037,3.2716], + "samples":[ + {"t":0.0, "x":3.69166, "y":5.08934, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.21138, "ay":3.29527, "alpha":-1.07639, "fx":[-57.74783,-49.19857,-47.66356,-55.39005], "fy":[48.55353,57.19446,58.4952,51.24257]}, + {"t":0.0405, "x":3.68902, "y":5.09204, "heading":-1.0472, "vx":-0.13007, "vy":0.13347, "omega":-0.0436, "ax":-3.2169, "ay":3.28954, "alpha":-1.07568, "fx":[-57.82933,-49.30624,-47.75891,-55.46659], "fy":[48.44949,57.09549,58.41218,51.15398]}, + {"t":0.08101, "x":3.68112, "y":5.10015, "heading":-1.04896, "vx":-0.26037, "vy":0.26671, "omega":-0.08717, "ax":-3.22306, "ay":3.28313, "alpha":-1.07478, "fx":[-57.92403,-49.43632,-47.8594,-55.54402], "fy":[48.32849,56.97595,58.32403,51.06349]}, + {"t":0.12151, "x":3.66793, "y":5.11364, "heading":-1.05249, "vx":-0.39092, "vy":0.39969, "omega":-0.1307, "ax":-3.22998, "ay":3.27591, "alpha":-1.07365, "fx":[-58.03344,-49.5911,-47.96724,-55.62398], "fy":[48.18835,56.83344,58.22877,50.96918]}, + {"t":0.16202, "x":3.64944, "y":5.13252, "heading":-1.05779, "vx":-0.52175, "vy":0.53238, "omega":-0.17419, "ax":-3.23779, "ay":3.26771, "alpha":-1.07227, "fx":[-58.15945,-49.77351,-48.08523,-55.70852], "fy":[48.02634,56.66484,58.12386,50.8686]}, + {"t":0.20252, "x":3.62565, "y":5.15676, "heading":-1.06484, "vx":-0.65289, "vy":0.66474, "omega":-0.21762, "ax":-3.24669, "ay":3.25833, "alpha":-1.0706, "fx":[-58.30442,-49.98724,-48.217,-55.80026], "fy":[47.83898,56.46613,58.00598,50.75862]}, + {"t":0.24302, "x":3.59655, "y":5.18636, "heading":-1.07366, "vx":-0.7844, "vy":0.79671, "omega":-0.26098, "ax":-3.25693, "ay":3.24747, "alpha":-1.06858, "fx":[-58.47141,-50.23713,-48.36725,-55.90261], "fy":[47.62173,56.23204,57.87076,50.6351]}, + {"t":0.28353, "x":3.5621, "y":5.22129, "heading":-1.08423, "vx":-0.91632, "vy":0.92825, "omega":-0.30426, "ax":-3.26883, "ay":3.23476, "alpha":-1.06616, "fx":[-58.66451,-50.52952,-48.54221,-56.02009], "fy":[47.36847,55.9555,57.71236,50.4925]}, + {"t":0.32403, "x":3.52231, "y":5.26154, "heading":-1.09655, "vx":-1.04872, "vy":1.05927, "omega":-0.34745, "ax":-3.28282, "ay":3.2197, "alpha":-1.06324, "fx":[-58.88922,-50.87293,-48.75031,-56.15883], "fy":[47.07083,55.6269,57.52277,50.32324]}, + {"t":0.36454, "x":3.47714, "y":5.30709, "heading":-1.11063, "vx":-1.18168, "vy":1.18968, "omega":-0.39051, "ax":-3.29951, "ay":3.20155, "alpha":-1.05969, "fx":[-59.15326,-51.27904,-49.00317,-56.32736], "fy":[46.71696,55.23274,57.29072,50.11659]}, + {"t":0.40504, "x":3.42657, "y":5.3579, "heading":-1.12644, "vx":-1.31533, "vy":1.31936, "omega":-0.43344, "ax":-3.31976, "ay":3.17927, "alpha":-1.05536, "fx":[-59.46762,-51.76437,-49.31731,-56.53791], "fy":[46.28957,54.7535,56.99986,49.85696]}, + {"t":0.44555, "x":3.37057, "y":5.41395, "heading":-1.144, "vx":-1.44979, "vy":1.44813, "omega":-0.47618, "ax":-3.34485, "ay":3.15126, "alpha":-1.04997, "fx":[-59.84863,-52.35307,-49.71711,-56.80856], "fy":[45.7625,54.15979,56.62546,49.52074]}, + {"t":0.48605, "x":3.3091, "y":5.47519, "heading":-1.16329, "vx":-1.58527, "vy":1.57577, "omega":-0.51871, "ax":-3.3767, "ay":3.11502, "alpha":-1.04313, "fx":[-60.32136,-53.08193,-50.24001,-57.16723], "fy":[45.0943,53.40523,56.12827,49.07053]}, + {"t":0.52655, "x":3.24212, "y":5.54157, "heading":-1.1843, "vx":-1.72204, "vy":1.70194, "omega":-0.56096, "ax":-3.41847, "ay":3.0663, "alpha":-1.03417, "fx":[-60.92619,-54.0099,-50.94674,-57.65922], "fy":[44.21539,52.41196,55.44201,48.4434]}, + {"t":0.56706, "x":3.16957, "y":5.61302, "heading":-1.20702, "vx":-1.86051, "vy":1.82614, "omega":-0.60285, "ax":-3.47556, "ay":2.99743, "alpha":-1.02186, "fx":[-61.73214,-55.23753,-51.94268,-58.36305], "fy":[43.00003,51.03866,54.44522,47.52491]}, + {"t":0.60756, "x":3.09136, "y":5.68945, "heading":-1.23144, "vx":-2.00128, "vy":1.94755, "omega":-0.64424, "ax":-3.55807, "ay":2.89285, "alpha":-1.00371, "fx":[-62.86618,-56.95045,-53.42742,-59.42653], "fy":[41.1968,49.00013,52.88944,46.08373]}, + {"t":0.64807, "x":3.00738, "y":5.7707, "heading":-1.25753, "vx":-2.1454, "vy":2.06472, "omega":-0.68489, "ax":-3.68694, "ay":2.71601, "alpha":-0.97371, "fx":[-64.58462,-59.52654,-55.82587,-61.16093], "fy":[38.22487,45.62758,50.17879,43.57498]}, + {"t":0.68857, "x":2.91746, "y":5.85656, "heading":-1.28527, "vx":-2.29473, "vy":2.17473, "omega":-0.72433, "ax":-3.91144, "ay":2.35817, "alpha":-0.91314, "fx":[-67.45513,-63.81539,-60.18709,-64.32052], "fy":[32.40732,38.95023,44.48099,38.36752]}, + {"t":0.72907, "x":2.8213, "y":5.94658, "heading":-1.31461, "vx":-2.45316, "vy":2.27025, "omega":-0.76132, "ax":-4.3364, "ay":1.33733, "alpha":-0.72899, "fx":[-72.42017,-71.28298,-69.13358,-70.73068], "fy":[16.69566,20.5892,27.15563,23.01067]}, + {"t":0.76958, "x":2.71838, "y":6.03963, "heading":-1.34545, "vx":-2.6288, "vy":2.32441, "omega":-0.79085, "ax":-3.37255, "ay":-2.93537, "alpha":0.30308, "fx":[-55.52997,-56.57959,-54.78627,-53.64323], "fy":[-47.44414,-46.34201,-48.51952,-49.645]}, + {"t":0.81008, "x":2.60914, "y":6.13137, "heading":-1.37748, "vx":-2.76541, "vy":2.20552, "omega":-0.77857, "ax":0.88654, "ay":-4.44071, "alpha":1.10025, "fx":[20.69077,8.28553,9.16649,19.83028], "fy":[-71.22411,-73.70342,-73.79318,-71.66802]}, + {"t":0.85059, "x":2.49785, "y":6.21706, "heading":-1.40901, "vx":-2.7295, "vy":2.02565, "omega":-0.73401, "ax":1.98131, "ay":-4.10889, "alpha":1.12753, "fx":[39.4398,28.27147,25.95657,35.89475], "fy":[-63.54593,-69.19713,-70.22012,-65.7269]}, + {"t":0.89109, "x":2.38892, "y":6.29574, "heading":-1.43874, "vx":-2.64925, "vy":1.85922, "omega":-0.68834, "ax":2.36764, "ay":-3.91535, "alpha":1.1158, "fx":[45.66192,35.72188,32.12989,41.31172], "fy":[-59.53816,-65.93915,-67.8466,-62.71027]}, + {"t":0.9316, "x":2.28356, "y":6.36784, "heading":-1.46663, "vx":-2.55335, "vy":1.70064, "omega":-0.64314, "ax":2.55917, "ay":-3.80183, "alpha":1.10529, "fx":[48.65879,39.5396,35.24203,43.90993], "fy":[-57.27825,-63.87513,-66.40695,-61.05029]}, + {"t":0.9721, "x":2.18224, "y":6.4336, "heading":-1.49268, "vx":-2.44969, "vy":1.54665, "omega":-0.59837, "ax":2.67271, "ay":-3.72847, "alpha":1.09738, "fx":[50.40413,41.87521,37.10011,45.39539], "fy":[-55.8498,-62.46441,-65.46424,-60.03509]}, + {"t":1.0126, "x":2.08521, "y":6.49319, "heading":-1.51691, "vx":-2.34143, "vy":1.39563, "omega":-0.55392, "ax":2.74763, "ay":-3.67744, "alpha":1.09139, "fx":[51.5403,43.46667,38.33032,46.33648], "fy":[-54.8719,-61.43317,-64.80439,-59.36681]}, + {"t":1.05311, "x":1.99262, "y":6.5467, "heading":-1.53935, "vx":-2.23014, "vy":1.24668, "omega":-0.50972, "ax":2.8007, "ay":-3.63996, "alpha":1.08674, "fx":[52.3355,44.63068,39.2037,46.97429], "fy":[-54.16379,-60.64059,-64.31813,-58.90321]}, + {"t":1.09361, "x":1.90459, "y":6.59421, "heading":-1.55999, "vx":-2.1167, "vy":1.09924, "omega":-0.4657, "ax":2.84024, "ay":-3.61131, "alpha":1.08306, "fx":[52.92108,45.52477,39.85585,47.42806], "fy":[-53.62948,-60.00868,-63.94519,-58.56853]}, + {"t":1.13412, "x":1.82119, "y":6.63577, "heading":-1.57886, "vx":-2.00166, "vy":0.95297, "omega":-0.42183, "ax":2.87082, "ay":-3.5887, "alpha":1.0801, "fx":[53.36884,46.23585,40.36182,47.7634], "fy":[-53.21339,-59.49118,-63.64992,-58.31889]}, + {"t":1.17462, "x":1.74246, "y":6.67143, "heading":-1.59594, "vx":-1.88538, "vy":0.80761, "omega":-0.37808, "ax":2.89518, "ay":-3.57041, "alpha":1.07769, "fx":[53.72142,46.81578,40.76632,48.01938], "fy":[-52.88111,-59.05904,-63.41007,-58.12719]}, + {"t":1.21512, "x":1.66847, "y":6.70121, "heading":-1.61126, "vx":-1.76812, "vy":0.663, "omega":-0.33443, "ax":2.91504, "ay":-3.55531, "alpha":1.07572, "fx":[54.00572,47.29736,41.09761,48.22068], "fy":[-52.61016,-58.69317,-63.21108,-57.97581]}, + {"t":1.25563, "x":1.59925, "y":6.72515, "heading":-1.6248, "vx":-1.65004, "vy":0.51899, "omega":-0.29086, "ax":2.93153, "ay":-3.54264, "alpha":1.07411, "fx":[54.23958,47.70236,41.37432,48.38365], "fy":[-52.38524,-58.3805,-63.04307,-57.85282]}, + {"t":1.29613, "x":1.53482, "y":6.74326, "heading":-1.63658, "vx":-1.53131, "vy":0.3755, "omega":-0.24736, "ax":2.94545, "ay":-3.53185, "alpha":1.0728, "fx":[54.43535,48.04582,41.60925,48.51955], "fy":[-52.19552,-58.1118,-62.89912,-57.74988]}, + {"t":1.33664, "x":1.47521, "y":6.75557, "heading":-1.6466, "vx":-1.412, "vy":0.23244, "omega":-0.2039, "ax":2.95735, "ay":-3.52256, "alpha":1.07174, "fx":[54.60179,48.33845,41.81143,48.63639], "fy":[-52.03315,-57.88036,-62.77428,-57.66093]}, + {"t":1.37714, "x":1.42045, "y":6.7621, "heading":-1.65486, "vx":-1.29222, "vy":0.08977, "omega":-0.16049, "ax":2.96764, "ay":-3.51447, "alpha":1.07089, "fx":[54.74536,48.58808,41.98742,48.74002], "fy":[-51.89228,-57.68118,-62.66487,-57.58155]}, + {"t":1.41765, "x":1.37054, "y":6.76285, "heading":-1.66136, "vx":-1.17202, "vy":-0.05258, "omega":-0.11712, "ax":2.97662, "ay":-3.50737, "alpha":1.07023, "fx":[54.87086,48.80058,42.14211,48.83482], "fy":[-51.76847,-57.51047,-62.56812,-57.50835]}, + {"t":1.45815, "x":1.32551, "y":6.75785, "heading":-1.66611, "vx":-1.05145, "vy":-0.19465, "omega":-0.07377, "ax":2.98453, "ay":-3.50109, "alpha":1.06974, "fx":[54.98196,48.98043,42.27923,48.92409], "fy":[-51.65832,-57.36531,-62.48191,-57.43875]}, + {"t":1.49865, "x":1.28537, "y":6.74709, "heading":-1.66909, "vx":-0.93056, "vy":-0.33646, "omega":-0.03044, "ax":2.99155, "ay":-3.49548, "alpha":1.0694, "fx":[55.0815,49.13111,42.40169,49.01044], "fy":[-51.55915,-57.24338,-62.40457,-57.37073]}, + {"t":1.53916, "x":1.25013, "y":6.73059, "heading":-1.67033, "vx":-0.80939, "vy":-0.47804, "omega":0.01288, "ax":3.009, "ay":-3.47446, "alpha":1.0823, "fx":[55.39658,49.46716,42.63819,49.26385], "fy":[-51.12059,-56.85068,-62.16018,-57.072]}, + {"t":1.56072, "x":1.23338, "y":6.71948, "heading":-1.67005, "vx":-0.74452, "vy":-0.55295, "omega":0.03621, "ax":3.0433, "ay":-3.44219, "alpha":1.1366, "fx":[56.19068,50.12688,42.91052,49.78057], "fy":[-50.24203,-56.2634,-61.96839,-56.61889]}, + {"t":1.58228, "x":1.21804, "y":6.70676, "heading":-1.66927, "vx":-0.6789, "vy":-0.62717, "omega":0.06072, "ax":3.07871, "ay":-3.40805, "alpha":1.1931, "fx":[57.00257,50.81362,43.19472,50.31357], "fy":[-49.31436,-55.6371,-61.76634,-56.14269]}, + {"t":1.60384, "x":1.20411, "y":6.69244, "heading":-1.66796, "vx":-0.61252, "vy":-0.70065, "omega":0.08644, "ax":3.11528, "ay":-3.37191, "alpha":1.25193, "fx":[57.83193,51.52839,43.49151,50.86365], "fy":[-48.33423,-54.96846,-61.55322,-55.64162]}, + {"t":1.6254, "x":1.19163, "y":6.67655, "heading":-1.6661, "vx":-0.54535, "vy":-0.77335, "omega":0.11343, "ax":3.15302, "ay":-3.33363, "alpha":1.31321, "fx":[58.67829,52.2722,43.8017,51.43159], "fy":[-47.29806,-54.2538,-61.32817,-55.11375]}, + {"t":1.64696, "x":1.18061, "y":6.6591, "heading":-1.66365, "vx":-0.47737, "vy":-0.84522, "omega":0.14175, "ax":3.19198, "ay":-3.29302, "alpha":1.37707, "fx":[59.54095,53.04606,44.12613,52.01823], "fy":[-46.20202,-53.48903,-61.09023,-54.55696]}, + {"t":1.66852, "x":1.17106, "y":6.64012, "heading":-1.66059, "vx":-0.40855, "vy":-0.91622, "omega":0.17144, "ax":3.23218, "ay":-3.2499, "alpha":1.44366, "fx":[60.41902,53.85092,44.46571,52.62436], "fy":[-45.04201,-52.66965,-60.83834,-53.96896]}, + {"t":1.69008, "x":1.163, "y":6.61961, "heading":-1.6569, "vx":-0.33886, "vy":-0.98629, "omega":0.20256, "ax":3.27364, "ay":-3.20409, "alpha":1.51311, "fx":[61.31131,54.68767,44.82143,53.25081], "fy":[-43.81369,-51.79062,-60.57133,-53.34722]}, + {"t":1.71164, "x":1.15645, "y":6.5976, "heading":-1.65253, "vx":-0.26828, "vy":-1.05538, "omega":0.23519, "ax":3.31638, "ay":-3.15535, "alpha":1.58559, "fx":[62.21632,55.55709,45.19436,53.89836], "fy":[-42.51242,-50.84639,-60.2879,-52.68903]}, + {"t":1.73321, "x":1.15144, "y":6.57411, "heading":-1.64746, "vx":-0.19678, "vy":-1.12341, "omega":0.26937, "ax":3.36041, "ay":-3.10345, "alpha":1.66127, "fx":[63.13219,56.45985,45.58566,54.56777], "fy":[-41.13331,-49.83075,-59.98661,-51.99141]}, + {"t":1.75477, "x":1.14798, "y":6.54916, "heading":-1.64165, "vx":-0.12432, "vy":-1.19032, "omega":0.30519, "ax":3.40574, "ay":-3.04814, "alpha":1.7403, "fx":[64.05661,57.39638,45.99658,55.25977], "fy":[-39.67122,-48.73681,-59.66584,-51.25112]}, + {"t":1.77633, "x":1.14609, "y":6.52279, "heading":-1.63507, "vx":-0.05089, "vy":-1.25604, "omega":0.34271, "ax":3.45235, "ay":-2.98913, "alpha":1.82288, "fx":[64.98678,58.36687,46.42846,55.97497], "fy":[-38.12074,-47.55689,-59.32381,-50.46466]}, + {"t":1.79789, "x":1.14579, "y":6.49502, "heading":-1.62768, "vx":0.02354, "vy":-1.32049, "omega":0.38202, "ax":3.50021, "ay":-2.92611, "alpha":1.90919, "fx":[65.91933,59.37113,46.88277,56.71395], "fy":[-36.47629,-46.28245,-58.95851,-49.62819]}, + {"t":1.81945, "x":1.14711, "y":6.46586, "heading":-1.61945, "vx":0.09901, "vy":-1.38358, "omega":0.42318, "ax":3.5493, "ay":-2.85876, "alpha":1.99943, "fx":[66.85024,60.40851,47.36109,57.47709], "fy":[-34.73205,-44.904,-58.56768,-48.73758]}, + {"t":1.84101, "x":1.15007, "y":6.43537, "heading":-1.61032, "vx":0.17553, "vy":-1.44522, "omega":0.46629, "ax":3.59954, "ay":-2.78672, "alpha":2.09382, "fx":[67.77474,61.47771,47.86514,58.26467], "fy":[-32.88214,-43.411,-58.14881,-47.78834]}, + {"t":1.86257, "x":1.1547, "y":6.40356, "heading":-1.60027, "vx":0.25314, "vy":-1.5053, "omega":0.51143, "ax":3.65084, "ay":-2.7096, "alpha":2.19256, "fx":[68.68724,62.57667,48.39675,59.0767], "fy":[-30.9206,-41.79176,-57.69904,-46.77562]}, + {"t":1.88413, "x":1.161, "y":6.37048, "heading":-1.58924, "vx":0.33186, "vy":-1.56372, "omega":0.55871, "ax":3.7031, "ay":-2.62698, "alpha":2.2959, "fx":[69.58123,63.70226,48.95791,59.91297], "fy":[-28.84154,-40.03341,-57.21517,-45.69422]}, + {"t":1.90569, "x":1.16902, "y":6.33615, "heading":-1.57719, "vx":0.4117, "vy":-1.62036, "omega":0.60821, "ax":3.75614, "ay":-2.53842, "alpha":2.40407, "fx":[70.44919,64.85004,49.55074,60.77292], "fy":[-26.63922,-38.12177,-56.69358,-44.53851]}, + {"t":1.92725, "x":1.17877, "y":6.30062, "heading":-1.56408, "vx":0.49269, "vy":-1.67509, "omega":0.66004, "ax":3.80976, "ay":-2.44344, "alpha":2.51731, "fx":[71.28249,66.01392,50.17752,61.65561], "fy":[-24.30826,-36.04142,-56.13019,-43.30251]}, + {"t":1.94881, "x":1.19028, "y":6.26394, "heading":-1.54985, "vx":0.57483, "vy":-1.72777, "omega":0.71432, "ax":3.86371, "ay":-2.34155, "alpha":2.6359, "fx":[72.07136,67.18571,50.84063,62.55959], "fy":[-21.84378,-33.77571,-55.52037,-41.97984]}, + {"t":1.97037, "x":1.20357, "y":6.22614, "heading":-1.53445, "vx":0.65813, "vy":-1.77826, "omega":0.77115, "ax":3.90328, "ay":-2.28536, "alpha":2.59842, "fx":[72.36541,67.69406,51.94157,63.24341], "fy":[-20.99481,-32.88926,-54.55521,-41.00617]}, + {"t":1.99806, "x":1.22328, "y":6.17604, "heading":-1.5131, "vx":0.7662, "vy":-1.84153, "omega":0.84309, "ax":3.91306, "ay":-2.26766, "alpha":2.59323, "fx":[72.48701,67.62672,52.20185,63.56843], "fy":[-20.52719,-32.98811,-54.29007,-40.48258]}, + {"t":2.02574, "x":1.246, "y":6.12418, "heading":-1.48976, "vx":0.87453, "vy":-1.90431, "omega":0.91488, "ax":3.92402, "ay":-2.24752, "alpha":2.58797, "fx":[72.61789,67.55903,52.49984,63.92457], "fy":[-20.00793,-33.082,-53.98366,-39.89725]}, + {"t":2.05343, "x":1.27171, "y":6.0706, "heading":-1.46443, "vx":0.98317, "vy":-1.96654, "omega":0.98653, "ax":3.93642, "ay":-2.22439, "alpha":2.58277, "fx":[72.75855,67.49648,52.84263,64.31395], "fy":[-19.42949,-33.15875,-53.62719,-39.24251]}, + {"t":2.08112, "x":1.30044, "y":6.0153, "heading":-1.43712, "vx":1.09215, "vy":-2.02812, "omega":1.05804, "ax":3.95053, "ay":-2.19753, "alpha":2.57779, "fx":[72.90968,67.44644,53.2389,64.73938], "fy":[-18.78163,-33.20214,-53.20949,-38.50841]}, + {"t":2.1088, "x":1.33219, "y":5.95831, "heading":-1.40782, "vx":1.20152, "vy":-2.08896, "omega":1.12941, "ax":3.96675, "ay":-2.16597, "alpha":2.57313, "fx":[73.07222,67.41886,53.69959,65.20456], "fy":[-18.05026,-33.19028,-52.71594,-37.6817]}, + {"t":2.13649, "x":1.36698, "y":5.89965, "heading":-1.37656, "vx":1.31135, "vy":-2.14892, "omega":1.20064, "ax":3.9856, "ay":-2.12838, "alpha":2.56887, "fx":[73.2474,67.42731,54.23878,65.71448], "fy":[-17.21561,-33.09301,-52.12687,-36.74434]}, + {"t":2.16417, "x":1.40481, "y":5.83934, "heading":-1.34331, "vx":1.42169, "vy":-2.20785, "omega":1.27177, "ax":4.00778, "ay":-2.08286, "alpha":2.56498, "fx":[73.43672,67.49034,54.87513,66.27594], "fy":[-16.24932,-32.86791,-51.41499,-35.67111]}, + {"t":2.19186, "x":1.44571, "y":5.77741, "heading":-1.30811, "vx":1.53265, "vy":-2.26552, "omega":1.34278, "ax":4.03423, "ay":-2.02668, "alpha":2.56126, "fx":[73.64177,67.63362,55.6342,66.89839], "fy":[-15.10974,-32.45343,-50.54089,-34.42551]}, + {"t":2.21954, "x":1.48968, "y":5.71391, "heading":-1.27093, "vx":1.64434, "vy":-2.32163, "omega":1.41369, "ax":4.06629, "ay":-1.95571, "alpha":2.55725, "fx":[73.86377,67.89289,56.55232,67.59516], "fy":[-13.73374,-31.75698,-49.44509,-32.95272]}, + {"t":2.24723, "x":1.53677, "y":5.64889, "heading":-1.23179, "vx":1.75691, "vy":-2.37577, "omega":1.48449, "ax":4.10581, "ay":-1.86343, "alpha":2.552, "fx":[74.10187,68.31819,57.68324,68.38527], "fy":[-12.02213,-30.63258,-48.03283,-31.16653]}, + {"t":2.27491, "x":1.58698, "y":5.5824, "heading":-1.19069, "vx":1.87059, "vy":-2.42736, "omega":1.55514, "ax":4.15543, "ay":-1.73896, "alpha":2.54371, "fx":[74.34864,68.97886,59.11001,69.29589], "fy":[-9.81165,-28.83653,-46.14291,-28.92334]}, + {"t":2.3026, "x":1.64036, "y":5.51453, "heading":-1.14764, "vx":1.98563, "vy":-2.4755, "omega":1.62557, "ax":4.21872, "ay":-1.56277, "alpha":2.52891, "fx":[74.57644,69.96461,60.96663,70.36452], "fy":[-6.81772,-25.93208,-43.47742,-25.9663]}, + {"t":2.33029, "x":1.69695, "y":5.4454, "heading":-1.10263, "vx":2.10243, "vy":-2.51877, "omega":1.69558, "ax":4.29957, "ay":-1.29661, "alpha":2.50063, "fx":[74.69362,71.35667,63.47545,71.63331], "fy":[-2.5074,-21.06655,-39.42312,-21.79122]}, + {"t":2.35797, "x":1.75681, "y":5.37517, "heading":-1.05569, "vx":2.22147, "vy":-2.55467, "omega":1.76481, "ax":4.39623, "ay":-0.85649, "alpha":2.44287, "fx":[74.38533,73.02478,66.98071,73.08931], "fy":[4.20723,-12.41172,-32.52322,-15.28036]}, + {"t":2.38566, "x":1.81999, "y":5.30411, "heading":-1.00683, "vx":2.34318, "vy":-2.57838, "omega":1.83244, "ax":4.46045, "ay":-0.03538, "alpha":2.30696, "fx":[72.43726,73.43232,71.59025,74.2198], "fy":[15.73235,4.13886,-18.63577,-3.54902]}, + {"t":2.41334, "x":1.88658, "y":5.23271, "heading":-0.9561, "vx":2.46667, "vy":-2.57936, "omega":1.89631, "ax":4.12316, "ay":1.64783, "alpha":1.84747, "fx":[63.57578,64.04929,71.59135,70.40699], "fy":[36.90381,34.75365,14.64334,21.45452]}, + {"t":2.44103, "x":1.95645, "y":5.16193, "heading":-0.9036, "vx":2.58082, "vy":-2.53374, "omega":1.94746, "ax":2.0449, "ay":3.95487, "alpha":0.56278, "fx":[32.28272,29.7575,34.7593,36.92146], "fy":[65.46187,66.43792,63.84892,62.8694]}, + {"t":2.46871, "x":2.02868, "y":5.0933, "heading":-0.84968, "vx":2.63743, "vy":-2.42425, "omega":1.96304, "ax":-0.68371, "ay":4.43461, "alpha":-0.99068, "fx":[-13.01184,-3.77744,-9.69884,-18.22109], "fy":[72.17315,73.42204,73.08642,71.30828]}, + {"t":2.4964, "x":2.10144, "y":5.02788, "heading":-0.79533, "vx":2.61851, "vy":-2.30147, "omega":1.93561, "ax":-1.97649, "ay":4.04404, "alpha":-1.74231, "fx":[-38.59019,-20.80768,-27.91508,-41.93467], "fy":[63.07849,71.11363,68.93027,61.32727]}, + {"t":2.52409, "x":2.17318, "y":4.96572, "heading":-0.74174, "vx":2.56379, "vy":-2.18951, "omega":1.88738, "ax":-2.55314, "ay":3.71474, "alpha":-2.09165, "fx":[-49.94137,-29.32285,-36.23577,-51.45574], "fy":[55.08007,68.43143,65.31787,54.08652]}, + {"t":2.55177, "x":2.24318, "y":4.90652, "heading":-0.68949, "vx":2.4931, "vy":-2.08667, "omega":1.82947, "ax":-2.85727, "ay":3.49101, "alpha":-2.27758, "fx":[-55.54618,-34.05377,-40.9397,-56.30416], "fy":[49.79725,66.45802,62.70365,49.32673]}, + {"t":2.57946, "x":2.31111, "y":4.85009, "heading":-0.63884, "vx":2.41399, "vy":-1.99001, "omega":1.76641, "ax":-3.04082, "ay":3.33579, "alpha":-2.3879, "fx":[-58.66532,-36.93467,-44.01443,-59.23213], "fy":[46.34607,65.07729,60.73535,45.97679]}, + {"t":2.60714, "x":2.37677, "y":4.79627, "heading":-0.58994, "vx":2.32981, "vy":-1.89766, "omega":1.7003, "ax":-3.16253, "ay":3.22335, "alpha":-2.45802, "fx":[-60.551,-38.81659,-46.22923,-61.20883], "fy":[44.04786,64.10097,59.17392,43.45986]}, + {"t":2.63483, "x":2.44006, "y":4.74497, "heading":-0.54286, "vx":2.24225, "vy":-1.80842, "omega":1.63225, "ax":-3.24883, "ay":3.13865, "alpha":-2.50418, "fx":[-61.75026,-40.11597,-47.93531,-62.64741], "fy":[42.4979,63.39308,57.88038,41.4723]}, + {"t":2.66251, "x":2.5009, "y":4.69611, "heading":-0.49767, "vx":2.15231, "vy":-1.72153, "omega":1.56292, "ax":-3.31312, "ay":3.07276, "alpha":-2.5348, "fx":[-62.53256,-41.05561,-49.31355,-63.75078], "fy":[41.45386,62.86446,56.77264,39.84422]}, + {"t":2.6902, "x":2.55921, "y":4.64962, "heading":-0.4544, "vx":2.06058, "vy":-1.63645, "omega":1.49274, "ax":-3.36282, "ay":3.02017, "alpha":-2.5548, "fx":[-63.04435,-41.76327,-50.46583,-64.62958], "fy":[40.76412,62.45723,55.80028,38.47422]}, + {"t":2.71788, "x":2.61497, "y":4.60547, "heading":-0.41307, "vx":1.96748, "vy":-1.55284, "omega":1.42201, "ax":-3.4024, "ay":2.97729, "alpha":-2.56728, "fx":[-63.37183,-42.31615,-51.45363,-65.34922], "fy":[40.32915,62.13348,54.93113,37.29822]}, + {"t":2.74557, "x":2.66814, "y":4.56362, "heading":-0.37371, "vx":1.87328, "vy":-1.47041, "omega":1.35094, "ax":-3.43464, "ay":2.94172, "alpha":-2.57439, "fx":[-63.56924,-42.76281,-52.31614,-65.95096], "fy":[40.08058,61.86798,54.14394,36.27372]}, + {"t":2.77326, "x":2.71869, "y":4.52404, "heading":-0.3363, "vx":1.77819, "vy":-1.38897, "omega":1.27966, "ax":-3.4614, "ay":2.91179, "alpha":-2.57769, "fx":[-63.67265,-43.13473,-53.07947,-66.46216], "fy":[39.96958,61.6438,53.42426,35.37126]}, + {"t":2.80094, "x":2.76659, "y":4.4867, "heading":-0.30088, "vx":1.68236, "vy":-1.30835, "omega":1.2083, "ax":-3.48395, "ay":2.88629, "alpha":-2.57835, "fx":[-63.70731,-43.45275,-53.76172,-66.90182], "fy":[39.9601,61.44946,52.76201,34.56967]}, + {"t":2.82863, "x":2.81183, "y":4.45159, "heading":-0.26742, "vx":1.58591, "vy":-1.22845, "omega":1.13691, "ax":-3.50319, "ay":2.86433, "alpha":-2.57726, "fx":[-63.69159,-43.73091,-54.37581,-67.28361], "fy":[40.02475,61.27712,52.15003,33.85324]}, + {"t":2.85631, "x":2.8544, "y":4.41867, "heading":-0.23595, "vx":1.48892, "vy":-1.14914, "omega":1.06556, "ax":-3.51978, "ay":2.84524, "alpha":-2.5751, "fx":[-63.6394,-43.97876,-54.93125,-67.61767], "fy":[40.14227,61.12144,51.58312,33.20992]}, + {"t":2.884, "x":2.89427, "y":4.38795, "heading":-0.20645, "vx":1.39147, "vy":-1.07037, "omega":0.99427, "ax":-3.53422, "ay":2.8285, "alpha":-2.5724, "fx":[-63.5616,-44.20283,-55.43522,-67.91172], "fy":[40.29583,60.97879,51.05745,32.63025]}, + {"t":2.91168, "x":2.93144, "y":4.3594, "heading":-0.17892, "vx":1.29362, "vy":-0.99206, "omega":0.92305, "ax":-3.54689, "ay":2.81371, "alpha":-2.56954, "fx":[-63.46685,-44.40759,-55.89324,-68.1718], "fy":[40.47195,60.84676,50.57016,32.10662]}, + {"t":2.93937, "x":2.96589, "y":4.33301, "heading":-0.15336, "vx":1.19543, "vy":-0.91416, "omega":0.85191, "ax":-3.55807, "ay":2.80056, "alpha":-2.56682, "fx":[-63.36226,-44.59609,-56.30965,-68.4027], "fy":[40.65968,60.72373,50.11906,31.63281]}, + {"t":2.96705, "x":2.99763, "y":4.30878, "heading":-0.12978, "vx":1.09692, "vy":-0.83663, "omega":0.78085, "ax":-3.56801, "ay":2.78878, "alpha":-2.56444, "fx":[-63.25372,-44.77041,-56.68791,-68.60831], "fy":[40.85008,60.60868,49.70252,31.20361]}, + {"t":2.99474, "x":3.02663, "y":4.28668, "heading":-0.10816, "vx":0.99814, "vy":-0.75942, "omega":0.70985, "ax":-3.57688, "ay":2.77816, "alpha":-2.56254, "fx":[-63.14619,-44.93193,-57.03085,-68.7918], "fy":[41.03578,60.50099,49.31924,30.81463]}, + {"t":3.02243, "x":3.05289, "y":4.26672, "heading":-0.08851, "vx":0.89911, "vy":-0.68251, "omega":0.6389, "ax":-3.58485, "ay":2.76854, "alpha":-2.56121, "fx":[-63.04386,-45.08156,-57.34075,-68.95581], "fy":[41.21068,60.40033,48.96823,30.46211]}, + {"t":3.05011, "x":3.07641, "y":4.24889, "heading":-0.07082, "vx":0.79986, "vy":-0.60586, "omega":0.56799, "ax":-3.59204, "ay":2.75977, "alpha":-2.56049, "fx":[-62.95035,-45.2199,-57.61953,-69.10256], "fy":[41.36972,60.30654,48.64868,30.14283]}, + {"t":3.0778, "x":3.09718, "y":4.23317, "heading":-0.05509, "vx":0.70041, "vy":-0.52945, "omega":0.49711, "ax":-3.59856, "ay":2.75173, "alpha":-2.5604, "fx":[-62.86872,-45.34733,-57.8688,-69.23388], "fy":[41.50864,60.21958,48.35999,29.85399]}, + {"t":3.10548, "x":3.11519, "y":4.21957, "heading":-0.04133, "vx":0.60078, "vy":-0.45327, "omega":0.42622, "ax":-3.6045, "ay":2.74433, "alpha":-2.56094, "fx":[-62.80161,-45.46409,-58.08987,-69.35135], "fy":[41.62389,60.13948,48.10163,29.59316]}, + {"t":3.13317, "x":3.13044, "y":4.20807, "heading":-0.02953, "vx":0.50099, "vy":-0.37729, "omega":0.35532, "ax":-3.60993, "ay":2.73748, "alpha":-2.5621, "fx":[-62.75128,-45.57033,-58.2839,-69.45629], "fy":[41.71246,60.0663,47.8732,29.35828]}, + {"t":3.16085, "x":3.14293, "y":4.19868, "heading":-0.01969, "vx":0.40105, "vy":-0.3015, "omega":0.28439, "ax":-3.61491, "ay":2.73111, "alpha":-2.56387, "fx":[-62.71968,-45.6662,-58.45182,-69.54982], "fy":[41.77178,60.00011,47.67437,29.14755]}, + {"t":3.18854, "x":3.15265, "y":4.19138, "heading":-0.01182, "vx":0.30097, "vy":-0.22589, "omega":0.2134, "ax":-3.6195, "ay":2.72516, "alpha":-2.56624, "fx":[-62.70843,-45.75181,-58.59445,-69.63286], "fy":[41.79967,59.94096,47.50486,28.95946]}, + {"t":3.21622, "x":3.15959, "y":4.18617, "heading":-0.00591, "vx":0.20076, "vy":-0.15044, "omega":0.14236, "ax":-3.62374, "ay":2.71959, "alpha":-2.56918, "fx":[-62.71891,-45.82732,-58.71245,-69.70621], "fy":[41.79421,59.88886,47.36443,28.79275]}, + {"t":3.24391, "x":3.16376, "y":4.18304, "heading":-0.00197, "vx":0.10043, "vy":-0.07515, "omega":0.07123, "ax":-3.62767, "ay":2.71433, "alpha":-2.57271, "fx":[-62.75226,-45.89292,-58.80637,-69.77048], "fy":[41.75374,59.84375,47.25287,28.6464]}, + {"t":3.2716, "x":3.16515, "y":4.182, "heading":0.0, "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/PLOtoK.traj b/src/main/deploy/choreo/PLOtoK.traj index 340ec034..ec584baa 100644 --- a/src/main/deploy/choreo/PLOtoK.traj +++ b/src/main/deploy/choreo/PLOtoK.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.4653401374816895, "y":5.783571720123291, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.9681568145751953, "y":5.253054141998291, "heading":-1.0455529770312977, "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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.4653401374816895 m", "val":3.4653401374816895}, "y":{"exp":"5.783571720123291 m", "val":5.783571720123291}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"K.x", "val":3.9681568145751953}, "y":{"exp":"K.y", "val":5.253054141998291}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,62 +30,62 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.24249,1.89266], + "waypoints":[0.0,1.27566,1.99673], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42298, "ay":-3.09524, "alpha":-0.01592, "fx":[55.89073,56.00508,56.02772,55.91319], "fy":[-50.67707,-50.55073,-50.52536,-50.65202]}, - {"t":0.03765, "x":1.55258, "y":7.5212, "heading":-0.93501, "vx":0.12888, "vy":-0.11654, "omega":-0.0006, "ax":3.42275, "ay":-3.0951, "alpha":-0.01626, "fx":[55.88554,56.00232,56.02544,55.90847], "fy":[-50.67633,-50.54731,-50.52139,-50.65074]}, - {"t":0.0753, "x":1.55986, "y":7.51462, "heading":-0.93503, "vx":0.25775, "vy":-0.23307, "omega":-0.00121, "ax":3.4225, "ay":-3.09494, "alpha":-0.01664, "fx":[55.87978,55.99925,56.0229,55.90322], "fy":[-50.6755,-50.54351,-50.51699,-50.64932]}, - {"t":0.11295, "x":1.57199, "y":7.50365, "heading":-0.93507, "vx":0.38661, "vy":-0.3496, "omega":-0.00184, "ax":3.42221, "ay":-3.09476, "alpha":-0.01706, "fx":[55.87333,55.99582,56.02006,55.89736], "fy":[-50.67458,-50.53928,-50.51206,-50.64773]}, - {"t":0.1506, "x":1.58897, "y":7.4883, "heading":-0.93514, "vx":0.51546, "vy":-0.46612, "omega":-0.00248, "ax":3.42189, "ay":-3.09456, "alpha":-0.01753, "fx":[55.86609,55.99196,56.01687,55.89077], "fy":[-50.67355,-50.53451,-50.50652,-50.64595]}, - {"t":0.18826, "x":1.6108, "y":7.46855, "heading":-0.93524, "vx":0.6443, "vy":-0.58264, "omega":-0.00314, "ax":3.42153, "ay":-3.09433, "alpha":-0.01807, "fx":[55.85788,55.98758,56.01326,55.88331], "fy":[-50.67238,-50.52911,-50.50024,-50.64392]}, - {"t":0.22591, "x":1.63748, "y":7.44442, "heading":-0.93535, "vx":0.77312, "vy":-0.69914, "omega":-0.00382, "ax":3.42112, "ay":-3.09407, "alpha":-0.01868, "fx":[55.84849,55.98259,56.00913,55.87478], "fy":[-50.67104,-50.52295,-50.49307,-50.6416]}, - {"t":0.26356, "x":1.66902, "y":7.41591, "heading":-0.9355, "vx":0.90193, "vy":-0.81563, "omega":-0.00452, "ax":3.42064, "ay":-3.09377, "alpha":-0.01939, "fx":[55.83768,55.97683,56.00437,55.86495], "fy":[-50.66951,-50.51585,-50.48479,-50.63893]}, - {"t":0.30121, "x":1.7054, "y":7.383, "heading":-0.93567, "vx":1.03072, "vy":-0.93212, "omega":-0.00525, "ax":3.42008, "ay":-3.09342, "alpha":-0.02021, "fx":[55.82506,55.97011,55.99883,55.85348], "fy":[-50.66771,-50.50756,-50.47514,-50.63581]}, - {"t":0.33886, "x":1.74663, "y":7.34572, "heading":-0.93587, "vx":1.15949, "vy":-1.04859, "omega":-0.00601, "ax":3.41943, "ay":-3.09301, "alpha":-0.02118, "fx":[55.81016,55.96217,55.99228,55.83994], "fy":[-50.6656,-50.49778,-50.46374,-50.63212]}, - {"t":0.37651, "x":1.79271, "y":7.30404, "heading":-0.93609, "vx":1.28824, "vy":-1.16504, "omega":-0.00681, "ax":3.41864, "ay":-3.09251, "alpha":-0.02235, "fx":[55.7923,55.95266,55.98442,55.8237], "fy":[-50.66306,-50.48606,-50.45007,-50.6277]}, - {"t":0.41416, "x":1.84364, "y":7.25799, "heading":-0.93635, "vx":1.41695, "vy":-1.28148, "omega":-0.00765, "ax":3.41768, "ay":-3.09191, "alpha":-0.02378, "fx":[55.7705,55.94105,55.97484,55.80387], "fy":[-50.65997,-50.47176,-50.43337,-50.62229]}, - {"t":0.45181, "x":1.89941, "y":7.20755, "heading":-0.93664, "vx":1.54563, "vy":-1.39789, "omega":-0.00855, "ax":3.41648, "ay":-3.09116, "alpha":-0.02556, "fx":[55.74327,55.92656,55.96287,55.77911], "fy":[-50.6561,-50.4539,-50.41253,-50.61555]}, - {"t":0.48946, "x":1.96003, "y":7.15272, "heading":-0.93696, "vx":1.67427, "vy":-1.51428, "omega":-0.00951, "ax":3.41494, "ay":-3.09019, "alpha":-0.02785, "fx":[55.70834,55.90797,55.94752,55.74732], "fy":[-50.65114,-50.43099,-50.38576,-50.60688]}, - {"t":0.52712, "x":2.02549, "y":7.09352, "heading":-0.93732, "vx":1.80284, "vy":-1.63063, "omega":-0.01056, "ax":3.41289, "ay":-3.0889, "alpha":-0.0309, "fx":[55.66188,55.88326,55.92708,55.70501], "fy":[-50.64455,-50.40053,-50.35013,-50.59535]}, - {"t":0.56477, "x":2.09579, "y":7.02993, "heading":-0.93772, "vx":1.93134, "vy":-1.74693, "omega":-0.01172, "ax":3.41004, "ay":-3.0871, "alpha":-0.03516, "fx":[55.59706,55.8488,55.89856,55.64592], "fy":[-50.63533,-50.35803,-50.30039,-50.57924]}, - {"t":0.60242, "x":2.17092, "y":6.96197, "heading":-0.93816, "vx":2.05973, "vy":-1.86316, "omega":-0.01305, "ax":3.40577, "ay":-3.08442, "alpha":-0.04154, "fx":[55.50032,55.7974,55.85596,55.55763], "fy":[-50.62156,-50.29464,-50.22608,-50.55515]}, - {"t":0.64007, "x":2.25088, "y":6.88964, "heading":-0.93865, "vx":2.18796, "vy":-1.97929, "omega":-0.01461, "ax":3.39871, "ay":-3.07997, "alpha":-0.0521, "fx":[55.34046,55.71254,55.78543,55.41138], "fy":[-50.5987,-50.18994,-50.10305,-50.51518]}, - {"t":0.67772, "x":2.33567, "y":6.81293, "heading":-0.9392, "vx":2.31593, "vy":-2.09526, "omega":-0.01657, "ax":3.3848, "ay":-3.0712, "alpha":-0.07301, "fx":[55.02581,55.54567,55.64606,55.12236], "fy":[-50.5533,-49.98407,-49.8601,-50.43589]}, - {"t":0.71537, "x":2.42527, "y":6.73186, "heading":-0.93982, "vx":2.44337, "vy":-2.21089, "omega":-0.01932, "ax":3.34461, "ay":-3.0458, "alpha":-0.13385, "fx":[54.12159,55.06675,55.2407,54.28269], "fy":[-50.41915,-49.3939,-49.156,-50.20279]}, - {"t":0.75302, "x":2.51963, "y":6.64646, "heading":-0.94055, "vx":2.5693, "vy":-2.32557, "omega":-0.02436, "ax":2.00182, "ay":-2.12377, "alpha":-2.4523, "fx":[28.16582,41.09782,38.75506,22.88519], "fy":[-43.783,-32.95583,-24.2922,-37.84709]}, - {"t":0.79067, "x":2.61779, "y":6.5574, "heading":-0.94147, "vx":2.64467, "vy":-2.40553, "omega":-0.11669, "ax":-3.361, "ay":3.01304, "alpha":-0.13185, "fx":[-55.49683,-54.56436,-54.40098,-55.32147], "fy":[48.62263,49.65633,49.88191,48.86911]}, - {"t":0.82832, "x":2.71498, "y":6.46896, "heading":-0.94586, "vx":2.51812, "vy":-2.29209, "omega":-0.12166, "ax":-3.39492, "ay":3.05633, "alpha":-0.06069, "fx":[-55.75946,-55.32726,-55.2429,-55.67252], "fy":[49.67292,50.15149,50.255,49.78096]}, - {"t":0.86597, "x":2.80739, "y":6.38483, "heading":-0.95044, "vx":2.3903, "vy":-2.17701, "omega":-0.12394, "ax":-3.4059, "ay":3.07038, "alpha":-0.03754, "fx":[-55.84157,-55.57385,-55.51899,-55.78573], "fy":[50.01389,50.31023,50.3751,50.08052]}, - {"t":0.90363, "x":2.89497, "y":6.30504, "heading":-0.95511, "vx":2.26206, "vy":-2.06141, "omega":-0.12536, "ax":-3.41133, "ay":3.07735, "alpha":-0.026, "fx":[-55.88133,-55.69583,-55.65647,-55.8415], "fy":[50.18304,50.38832,50.43405,50.22961]}, - {"t":0.94128, "x":2.97772, "y":6.22961, "heading":-0.95983, "vx":2.13362, "vy":-1.94555, "omega":-0.12634, "ax":-3.41457, "ay":3.0815, "alpha":-0.01907, "fx":[-55.90465,-55.76862,-55.73889,-55.87467], "fy":[50.28424,50.43475,50.46892,50.31886]}, - {"t":0.97893, "x":3.05563, "y":6.15854, "heading":-0.96458, "vx":2.00506, "vy":-1.82952, "omega":-0.12705, "ax":-3.41672, "ay":3.08426, "alpha":-0.01444, "fx":[-55.9199,-55.81697,-55.79387,-55.89666], "fy":[50.35165,50.46553,50.4919,50.37828]}, - {"t":1.01658, "x":3.12871, "y":6.09184, "heading":-0.96937, "vx":1.87642, "vy":-1.7134, "omega":-0.1276, "ax":-3.41825, "ay":3.08623, "alpha":-0.01112, "fx":[-55.93062,-55.85141,-55.83318,-55.91231], "fy":[50.39981,50.48745,50.50815,50.42067]}, - {"t":1.05423, "x":3.19693, "y":6.02952, "heading":-0.97417, "vx":1.74772, "vy":-1.5972, "omega":-0.12802, "ax":-3.4194, "ay":3.08771, "alpha":-0.00862, "fx":[-55.93854,-55.87717,-55.86271,-55.92404], "fy":[50.43596,50.50385,50.52022,50.45242]}, - {"t":1.09188, "x":3.26031, "y":5.97157, "heading":-0.97899, "vx":1.61897, "vy":-1.48094, "omega":-0.12834, "ax":-3.42029, "ay":3.08885, "alpha":-0.00667, "fx":[-55.94462,-55.89716,-55.88572,-55.93315], "fy":[50.46411,50.51661,50.52952,50.47707]}, - {"t":1.12953, "x":3.31884, "y":5.918, "heading":-0.98382, "vx":1.4902, "vy":-1.36464, "omega":-0.12859, "ax":-3.421, "ay":3.08977, "alpha":-0.00511, "fx":[-55.94942,-55.91311,-55.90416,-55.94046], "fy":[50.48666,50.52683,50.5369,50.49676]}, - {"t":1.16718, "x":3.37253, "y":5.86881, "heading":-0.98867, "vx":1.36139, "vy":-1.24831, "omega":-0.12878, "ax":-3.42158, "ay":3.09051, "alpha":-0.00383, "fx":[-55.95329,-55.92612,-55.91928,-55.94644], "fy":[50.50514,50.5352,50.54288,50.51284]}, - {"t":1.20483, "x":3.42136, "y":5.824, "heading":-0.99351, "vx":1.23257, "vy":-1.13195, "omega":-0.12893, "ax":-3.42206, "ay":3.09114, "alpha":-0.00276, "fx":[-55.95648,-55.93693,-55.9319,-55.95145], "fy":[50.52056,50.54219,50.54783,50.52621]}, - {"t":1.24249, "x":3.46534, "y":5.78357, "heading":-0.99837, "vx":1.10372, "vy":-1.01556, "omega":-0.12903, "ax":-1.79347, "ay":-1.82922, "alpha":-0.51189, "fx":[-29.18831,-27.19212,-29.49743,-31.40171], "fy":[-31.68933,-30.93732,-28.0663,-28.92436]}, - {"t":1.27499, "x":3.50027, "y":5.74959, "heading":-1.00256, "vx":1.04542, "vy":-1.07503, "omega":-0.14567, "ax":-0.45558, "ay":-0.43702, "alpha":-0.65351, "fx":[-7.98189,-4.93786,-6.923,-9.94863], "fy":[-9.61063,-7.72567,-4.65626,-6.58501]}, - {"t":1.3075, "x":3.53402, "y":5.71441, "heading":-1.0073, "vx":1.03061, "vy":-1.08924, "omega":-0.16692, "ax":-0.08038, "ay":-0.07587, "alpha":-0.38805, "fx":[-1.66253,0.1674,-0.9661,-2.79498], "fy":[-2.71349,-1.55952,0.23414,-0.92254]}, - {"t":1.34001, "x":3.56748, "y":5.67896, "heading":-1.01273, "vx":1.028, "vy":-1.0917, "omega":-0.17953, "ax":-0.01382, "ay":-0.01301, "alpha":-0.09976, "fx":[-0.31774,0.15433,-0.13418,-0.60624], "fy":[-0.59109,-0.29642,0.16574,-0.12897]}, - {"t":1.37252, "x":3.60089, "y":5.64347, "heading":-1.01856, "vx":1.02755, "vy":-1.09213, "omega":-0.18277, "ax":-0.00237, "ay":-0.00223, "alpha":0.18863, "fx":[0.13897,-0.7569,-0.2165,0.67938], "fy":[0.67806,0.12602,-0.75098,-0.19897]}, - {"t":1.40503, "x":3.63429, "y":5.60796, "heading":-1.0245, "vx":1.02747, "vy":-1.0922, "omega":-0.17664, "ax":-0.00041, "ay":-0.00038, "alpha":0.47768, "fx":[0.45419,-1.82277,-0.46749,1.80948], "fy":[1.80049,0.41604,-1.81298,-0.42856]}, - {"t":1.43754, "x":3.66769, "y":5.57246, "heading":-1.03025, "vx":1.02746, "vy":-1.09221, "omega":-0.16111, "ax":-0.00007, "ay":-0.00007, "alpha":0.7684, "fx":[0.75685,-2.91865,-0.75913,2.91637], "fy":[2.90097,0.69501,-2.90311,-0.69716]}, - {"t":1.47004, "x":3.70109, "y":5.53695, "heading":-1.03548, "vx":1.02745, "vy":-1.09221, "omega":-0.13613, "ax":-0.00001, "ay":-0.00001, "alpha":1.06179, "fx":[1.0682,-4.02656,-1.06859,4.02616], "fy":[4.00435,0.98277,-4.00472,-0.98315]}, - {"t":1.50255, "x":3.7345, "y":5.50144, "heading":-1.03991, "vx":1.02745, "vy":-1.09221, "omega":-0.10162, "ax":-0.00001, "ay":0.0, "alpha":1.35879, "fx":[1.38978,-5.14716,-1.39006,5.14688], "fy":[5.11867,1.28077,-5.11855,-1.28065]}, - {"t":1.53506, "x":3.7679, "y":5.46594, "heading":-1.04321, "vx":1.02745, "vy":-1.09221, "omega":-0.05745, "ax":-0.07403, "ay":0.07869, "alpha":1.65856, "fx":[0.49807,-7.47444,-2.94322,5.07885], "fy":[7.52547,2.87082,-4.9602,-0.29016]}, - {"t":1.56757, "x":3.80126, "y":5.43047, "heading":-1.04508, "vx":1.02505, "vy":-1.08966, "omega":-0.00353, "ax":-3.12096, "ay":3.31767, "alpha":0.0287, "fx":[-50.88787,-51.10815,-51.15581,-50.93489], "fy":[54.36555,54.15988,54.10912,54.31575]}, - {"t":1.60008, "x":3.83293, "y":5.3968, "heading":-1.04519, "vx":0.92359, "vy":-0.9818, "omega":-0.00259, "ax":-3.14452, "ay":3.34272, "alpha":0.01562, "fx":[-51.33317,-51.4538,-51.48069,-51.35987], "fy":[54.71694,54.60387,54.57703,54.69039]}, - {"t":1.63259, "x":3.86129, "y":5.36665, "heading":-1.04528, "vx":0.82137, "vy":-0.87314, "omega":-0.00209, "ax":-3.15179, "ay":3.35045, "alpha":0.0116, "fx":[-51.47083,-51.56058,-51.58077,-51.49092], "fy":[54.82539,54.74117,54.72142,54.8058]}, - {"t":1.6651, "x":3.88633, "y":5.34004, "heading":-1.04535, "vx":0.71891, "vy":-0.76422, "omega":-0.00171, "ax":-3.15532, "ay":3.3542, "alpha":0.00965, "fx":[-51.53775,-51.61247,-51.62936,-51.55457], "fy":[54.87808,54.80792,54.79156,54.86184]}, - {"t":1.6976, "x":3.90803, "y":5.31697, "heading":-1.0454, "vx":0.61633, "vy":-0.65518, "omega":-0.0014, "ax":-3.15741, "ay":3.35642, "alpha":0.0085, "fx":[-51.57731,-51.64314,-51.65807,-51.59218], "fy":[54.90922,54.84738,54.83301,54.89494]}, - {"t":1.73011, "x":3.9264, "y":5.29744, "heading":-1.04545, "vx":0.51369, "vy":-0.54607, "omega":-0.00112, "ax":-3.15879, "ay":3.35789, "alpha":0.00774, "fx":[-51.60344,-51.6634,-51.67701,-51.61701], "fy":[54.92977,54.87344,54.86038,54.91679]}, - {"t":1.76262, "x":3.94143, "y":5.28146, "heading":-1.04548, "vx":0.411, "vy":-0.43691, "omega":-0.00087, "ax":-3.15977, "ay":3.35893, "alpha":0.0072, "fx":[-51.62198,-51.67777,-51.69046,-51.63462], "fy":[54.94436,54.89193,54.8798,54.93229]}, - {"t":1.79513, "x":3.95312, "y":5.26904, "heading":-1.04551, "vx":0.30828, "vy":-0.32771, "omega":-0.00063, "ax":-3.1605, "ay":3.35971, "alpha":0.00679, "fx":[-51.63581,-51.6885,-51.70049,-51.64777], "fy":[54.95525,54.90574,54.89429,54.94386]}, - {"t":1.82764, "x":3.96147, "y":5.26016, "heading":-1.04553, "vx":0.20554, "vy":-0.21849, "omega":-0.00041, "ax":-3.16107, "ay":3.36031, "alpha":0.00648, "fx":[-51.64654,-51.69681,-51.70826,-51.65796], "fy":[54.96369,54.91644,54.90552,54.95282]}, - {"t":1.86015, "x":3.96649, "y":5.25483, "heading":-1.04555, "vx":0.10278, "vy":-0.10925, "omega":-0.0002, "ax":-3.16152, "ay":3.36079, "alpha":0.00623, "fx":[-51.6551,-51.70344,-51.71446,-51.66609], "fy":[54.97042,54.92497,54.91448,54.95997]}, - {"t":1.89266, "x":3.96816, "y":5.25305, "heading":-1.04555, "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":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.42195, "ay":-3.09664, "alpha":-0.01591, "fx":[55.87399,55.98829,56.01089,55.8964], "fy":[-50.69989,-50.57369,-50.54841,-50.67492]}, + {"t":0.03866, "x":1.55271, "y":7.52108, "heading":-0.93501, "vx":0.13228, "vy":-0.1197, "omega":-0.00061, "ax":3.4217, "ay":-3.09653, "alpha":-0.01631, "fx":[55.86814,55.98534,56.0085,55.89111], "fy":[-50.69993,-50.57055,-50.54461,-50.67433]}, + {"t":0.07731, "x":1.56038, "y":7.51414, "heading":-0.93503, "vx":0.26455, "vy":-0.2394, "omega":-0.00125, "ax":3.42142, "ay":-3.0964, "alpha":-0.01676, "fx":[55.86162,55.98205,56.00584,55.88521], "fy":[-50.69998,-50.56704,-50.54038,-50.67367]}, + {"t":0.11597, "x":1.57316, "y":7.50258, "heading":-0.93508, "vx":0.39681, "vy":-0.3591, "omega":-0.00189, "ax":3.4211, "ay":-3.09626, "alpha":-0.01727, "fx":[55.85431,55.97836,56.00286,55.8786], "fy":[-50.70004,-50.56311,-50.53563,-50.67293]}, + {"t":0.15462, "x":1.59106, "y":7.48638, "heading":-0.93515, "vx":0.52906, "vy":-0.47879, "omega":-0.00256, "ax":3.42075, "ay":-3.0961, "alpha":-0.01784, "fx":[55.84605,55.97419,55.9995,55.87113], "fy":[-50.70011,-50.55868,-50.53027,-50.6721]}, + {"t":0.19328, "x":1.61406, "y":7.46556, "heading":-0.93525, "vx":0.66129, "vy":-0.59847, "omega":-0.00325, "ax":3.42034, "ay":-3.09591, "alpha":-0.01848, "fx":[55.83666,55.96945,55.99568,55.86264], "fy":[-50.70018,-50.55364,-50.52418,-50.67114]}, + {"t":0.23194, "x":1.64218, "y":7.44011, "heading":-0.93538, "vx":0.79351, "vy":-0.71815, "omega":-0.00396, "ax":3.41988, "ay":-3.0957, "alpha":-0.01923, "fx":[55.82588,55.964,55.99129,55.8529], "fy":[-50.70027,-50.54786,-50.51717,-50.67004]}, + {"t":0.27059, "x":1.67541, "y":7.41004, "heading":-0.93553, "vx":0.92571, "vy":-0.83782, "omega":-0.00471, "ax":3.41934, "ay":-3.09546, "alpha":-0.02009, "fx":[55.81338,55.95769,55.98621,55.8416], "fy":[-50.70037,-50.54116,-50.50906,-50.66877]}, + {"t":0.30925, "x":1.71375, "y":7.37534, "heading":-0.93571, "vx":1.05789, "vy":-0.95748, "omega":-0.00548, "ax":3.41871, "ay":-3.09517, "alpha":-0.02111, "fx":[55.79872,55.95028,55.98024,55.82835], "fy":[-50.70049,-50.53331,-50.49953,-50.66727]}, + {"t":0.34791, "x":1.7572, "y":7.33601, "heading":-0.93592, "vx":1.19004, "vy":-1.07712, "omega":-0.0063, "ax":3.41796, "ay":-3.09483, "alpha":-0.02231, "fx":[55.78127,55.94147,55.97314,55.81257], "fy":[-50.70064,-50.52396,-50.48819,-50.66549]}, + {"t":0.38656, "x":1.80575, "y":7.29206, "heading":-0.93617, "vx":1.32216, "vy":-1.19676, "omega":-0.00716, "ax":3.41705, "ay":-3.09442, "alpha":-0.02377, "fx":[55.76016,55.93082,55.96456,55.79349], "fy":[-50.70081,-50.51266,-50.47446,-50.66333]}, + {"t":0.42522, "x":1.85942, "y":7.24349, "heading":-0.93644, "vx":1.45426, "vy":-1.31638, "omega":-0.00808, "ax":3.41593, "ay":-3.09391, "alpha":-0.02558, "fx":[55.73411,55.91767,55.95397,55.76993], "fy":[-50.70103,-50.49871,-50.45752,-50.66066]}, + {"t":0.46387, "x":1.91819, "y":7.19029, "heading":-0.93676, "vx":1.5863, "vy":-1.43598, "omega":-0.00907, "ax":3.41451, "ay":-3.09326, "alpha":-0.02786, "fx":[55.70115,55.90104,55.94056,55.74011], "fy":[-50.7013,-50.48107,-50.43608,-50.65727]}, + {"t":0.50253, "x":1.98206, "y":7.13247, "heading":-0.93711, "vx":1.71829, "vy":-1.55555, "omega":-0.01015, "ax":3.41266, "ay":-3.09242, "alpha":-0.03084, "fx":[55.65813,55.87934,55.92306,55.70116], "fy":[-50.70165,-50.45806,-50.40806,-50.65285]}, + {"t":0.54119, "x":2.05103, "y":7.07003, "heading":-0.9375, "vx":1.85021, "vy":-1.67509, "omega":-0.01134, "ax":3.41013, "ay":-3.09127, "alpha":-0.03491, "fx":[55.59958,55.84983,55.89925,55.64811], "fy":[-50.70211,-50.42675,-50.36992,-50.64681]}, + {"t":0.57984, "x":2.1251, "y":7.00297, "heading":-0.93794, "vx":1.98204, "vy":-1.79459, "omega":-0.01269, "ax":3.4065, "ay":-3.08962, "alpha":-0.04077, "fx":[55.5153,55.80737,55.86494,55.57166], "fy":[-50.70274,-50.38169,-50.31495,-50.63808]}, + {"t":0.6185, "x":2.20426, "y":6.93129, "heading":-0.93843, "vx":2.11372, "vy":-1.91402, "omega":-0.01427, "ax":3.40082, "ay":-3.08703, "alpha":-0.04995, "fx":[55.38354,55.74106,55.81124,55.4519], "fy":[-50.70365,-50.31129,-50.22888,-50.62435]}, + {"t":0.65716, "x":2.28851, "y":6.85499, "heading":-0.93898, "vx":2.24518, "vy":-2.03335, "omega":-0.0162, "ax":3.39067, "ay":-3.0824, "alpha":-0.0664, "fx":[55.14851,55.62294,55.71521,55.23757], "fy":[-50.70499,-50.18582,-50.07494,-50.59957]}, + {"t":0.69581, "x":2.37784, "y":6.77409, "heading":-0.93961, "vx":2.37625, "vy":-2.15251, "omega":-0.01876, "ax":3.3674, "ay":-3.07173, "alpha":-0.1043, "fx":[54.61085,55.35329,55.49416,54.74381], "fy":[-50.70645,-49.89932,-49.72087,-50.54129]}, + {"t":0.73447, "x":2.47221, "y":6.68858, "heading":-0.94033, "vx":2.50642, "vy":-2.27125, "omega":-0.02279, "ax":3.25888, "ay":-3.02124, "alpha":-0.28362, "fx":[52.13558,54.11858,54.4465,52.40523], "fy":[-50.68105,-48.58911,-48.05582,-50.2401]}, + {"t":0.77312, "x":2.57153, "y":6.59853, "heading":-0.94121, "vx":2.6324, "vy":-2.38804, "omega":-0.03376, "ax":-3.31806, "ay":2.91526, "alpha":-0.31165, "fx":[-55.50223,-53.34113,-53.01702,-55.11565], "fy":[46.13397,48.56598,49.12806,46.80776]}, + {"t":0.81178, "x":2.67081, "y":6.50839, "heading":-0.94252, "vx":2.50414, "vy":-2.27535, "omega":-0.04581, "ax":-3.39222, "ay":3.03976, "alpha":-0.09365, "fx":[-55.85326,-55.1882,-55.06256,-55.72154], "fy":[49.24229,49.98196,50.14076,49.41186]}, + {"t":0.85044, "x":2.76508, "y":6.42271, "heading":-0.94429, "vx":2.37301, "vy":-2.15784, "omega":-0.04943, "ax":-3.4062, "ay":3.06361, "alpha":-0.05249, "fx":[-55.90952,-55.53524,-55.4612,-55.83355], "fy":[49.83143,50.24669,50.33524,49.92339]}, + {"t":0.88909, "x":2.85426, "y":6.34158, "heading":-0.9462, "vx":2.24133, "vy":-2.03941, "omega":-0.05145, "ax":-3.41211, "ay":3.07373, "alpha":-0.03505, "fx":[-55.93219,-55.68186,-55.6311,-55.88057], "fy":[50.08086,50.35831,50.41763,50.14171]}, + {"t":0.92775, "x":2.93836, "y":6.26504, "heading":-0.94819, "vx":2.10944, "vy":-1.92059, "omega":-0.05281, "ax":-3.41537, "ay":3.07933, "alpha":-0.02541, "fx":[-55.94435,-55.76279,-55.7253,-55.90641], "fy":[50.2187,50.41983,50.46309,50.26277]}, + {"t":0.96641, "x":3.01735, "y":6.1931, "heading":-0.95023, "vx":1.97741, "vy":-1.80156, "omega":-0.05379, "ax":-3.41743, "ay":3.08288, "alpha":-0.01928, "fx":[-55.95191,-55.81408,-55.78519,-55.92276], "fy":[50.30619,50.4588,50.49187,50.33973]}, + {"t":1.00506, "x":3.09123, "y":6.12576, "heading":-0.95231, "vx":1.84531, "vy":-1.68239, "omega":-0.05454, "ax":-3.41886, "ay":3.08533, "alpha":-0.01504, "fx":[-55.95703,-55.8495,-55.82665,-55.93403], "fy":[50.36666,50.4857,50.51172,50.39296]}, + {"t":1.04372, "x":3.16001, "y":6.06303, "heading":-0.95442, "vx":1.71315, "vy":-1.56312, "omega":-0.05512, "ax":-3.4199, "ay":3.08713, "alpha":-0.01194, "fx":[-55.96073,-55.87541,-55.85706,-55.94227], "fy":[50.41097,50.50539,50.52621,50.43196]}, + {"t":1.08237, "x":3.22368, "y":6.00492, "heading":-0.95655, "vx":1.58095, "vy":-1.44378, "omega":-0.05558, "ax":-3.4207, "ay":3.0885, "alpha":-0.00956, "fx":[-55.96351,-55.89519,-55.88032,-55.94857], "fy":[50.44483,50.52044,50.53726,50.46177]}, + {"t":1.12103, "x":3.28224, "y":5.95141, "heading":-0.9587, "vx":1.44871, "vy":-1.32439, "omega":-0.05595, "ax":-3.42133, "ay":3.08958, "alpha":-0.00768, "fx":[-55.96568,-55.91078,-55.89869,-55.95354], "fy":[50.47157,50.53231,50.54595,50.48528]}, + {"t":1.15969, "x":3.33568, "y":5.90253, "heading":-0.96086, "vx":1.31646, "vy":-1.20496, "omega":-0.05625, "ax":-3.42183, "ay":3.09046, "alpha":-0.00616, "fx":[-55.96741,-55.92338,-55.91357,-55.95757], "fy":[50.49321,50.54192,50.55296,50.5043]}, + {"t":1.19834, "x":3.38402, "y":5.85826, "heading":-0.96303, "vx":1.18418, "vy":-1.0855, "omega":-0.05648, "ax":-3.42225, "ay":3.09118, "alpha":-0.00491, "fx":[-55.96881,-55.93377,-55.92587,-55.9609], "fy":[50.51109,50.54987,50.55874,50.51999]}, + {"t":1.237, "x":3.42724, "y":5.8186, "heading":-0.96522, "vx":1.05189, "vy":-0.966, "omega":-0.05667, "ax":-3.42261, "ay":3.09179, "alpha":-0.00385, "fx":[-55.96998,-55.94248,-55.93621,-55.96371], "fy":[50.52612,50.55655,50.56358,50.53316]}, + {"t":1.27566, "x":3.46534, "y":5.78357, "heading":-0.96741, "vx":0.91959, "vy":-0.84649, "omega":-0.05682, "ax":-1.5087, "ay":-1.53167, "alpha":-1.13942, "fx":[-24.65478,-20.00114,-24.84409,-29.15736], "fy":[-29.11667,-26.86294,-20.74079,-23.43943]}, + {"t":1.31171, "x":3.49751, "y":5.75206, "heading":-0.96946, "vx":0.86519, "vy":-0.90171, "omega":-0.0979, "ax":-0.20145, "ay":-0.19175, "alpha":-1.43419, "fx":[-4.36719,2.23451,-2.2412,-8.79968], "fy":[-8.60231,-4.14368,2.37787,-2.17111]}, + {"t":1.34776, "x":3.52858, "y":5.71942, "heading":-0.97299, "vx":0.85793, "vy":-0.90862, "omega":-0.14961, "ax":-0.02066, "ay":-0.01949, "alpha":-1.12338, "fx":[-1.20182,3.9795,0.52494,-4.65371], "fy":[-4.61642,-1.09214,3.98192,0.45197]}, + {"t":1.38382, "x":3.5595, "y":5.68665, "heading":-0.97838, "vx":0.85718, "vy":-0.90932, "omega":-0.19012, "ax":-0.0021, "ay":-0.00198, "alpha":-0.80656, "fx":[-0.67094,3.06192,0.60235,-3.13037], "fy":[-3.11552,-0.60327,3.05108,0.53854]}, + {"t":1.41987, "x":3.5904, "y":5.65386, "heading":-0.98523, "vx":0.85711, "vy":-0.9094, "omega":-0.21919, "ax":-0.00021, "ay":-0.0002, "alpha":-0.49278, "fx":[-0.40534,1.88575,0.39841,-1.89268], "fy":[-1.88435,-0.36503,1.87783,0.35849]}, + {"t":1.45593, "x":3.6213, "y":5.62108, "heading":-0.99314, "vx":0.8571, "vy":-0.9094, "omega":-0.23696, "ax":-0.00002, "ay":-0.00002, "alpha":-0.18094, "fx":[-0.15337,0.69228,0.15267,-0.69297], "fy":[-0.68986,-0.13864,0.6892,0.13798]}, + {"t":1.49198, "x":3.6522, "y":5.58829, "heading":-1.00168, "vx":0.8571, "vy":-0.9094, "omega":-0.24349, "ax":0.0, "ay":0.0, "alpha":0.13018, "fx":[0.11428,-0.49747,-0.11436,0.4974], "fy":[0.49508,0.10373,-0.49515,-0.10379]}, + {"t":1.52803, "x":3.6831, "y":5.5555, "heading":-1.01046, "vx":0.8571, "vy":-0.9094, "omega":-0.23879, "ax":0.0, "ay":0.0, "alpha":0.44181, "fx":[0.40273,-1.6851,-0.40273,1.68509], "fy":[1.67691,0.36696,-1.67691,-0.36697]}, + {"t":1.56409, "x":3.71401, "y":5.52271, "heading":-1.01907, "vx":0.8571, "vy":-0.9094, "omega":-0.22286, "ax":0.0, "ay":0.0, "alpha":0.75519, "fx":[0.71304,-2.87483,-0.71304,2.87482], "fy":[2.8603,0.65202,-2.8603,-0.65203]}, + {"t":1.60014, "x":3.74491, "y":5.48993, "heading":-1.0271, "vx":0.8571, "vy":-0.9094, "omega":-0.19563, "ax":0.0, "ay":0.0, "alpha":1.07151, "fx":[1.04427,-4.07141,-1.04428,4.07141], "fy":[4.05011,0.95788,-4.05011,-0.95788]}, + {"t":1.6362, "x":3.77581, "y":5.45714, "heading":-1.03416, "vx":0.8571, "vy":-0.9094, "omega":-0.157, "ax":0.0, "ay":0.0, "alpha":1.39194, "fx":[1.39361,-5.28005,-1.39365,5.28001], "fy":[5.25155,1.28158,-5.25158,-1.28161]}, + {"t":1.67225, "x":3.80671, "y":5.42435, "heading":-1.03982, "vx":0.8571, "vy":-0.9094, "omega":-0.10682, "ax":-0.00009, "ay":0.00007, "alpha":1.71758, "fx":[1.7548,-6.50772,-1.75785,6.50472], "fy":[6.47153,1.6195,-6.46911,-1.61706]}, + {"t":1.7083, "x":3.83761, "y":5.39156, "heading":-1.04367, "vx":0.8571, "vy":-0.9094, "omega":-0.04489, "ax":-1.6595, "ay":1.76081, "alpha":1.17929, "fx":[-24.82136,-31.20566,-29.70566,-22.78626], "fy":[33.35883,28.76988,24.01327,29.00162]}, + {"t":1.74436, "x":3.86744, "y":5.35992, "heading":-1.04529, "vx":0.79726, "vy":-0.84592, "omega":-0.00237, "ax":-3.1427, "ay":3.33448, "alpha":0.02031, "fx":[-51.28155,-51.43802,-51.47288,-51.31608], "fy":[54.60327,54.45646,54.42113,54.56843]}, + {"t":1.78041, "x":3.89414, "y":5.33159, "heading":-1.04537, "vx":0.68396, "vy":-0.7257, "omega":-0.00164, "ax":-3.15561, "ay":3.34818, "alpha":0.01168, "fx":[-51.5329,-51.62314,-51.64356,-51.55321], "fy":[54.78867,54.70382,54.68385,54.76886]}, + {"t":1.81646, "x":3.91675, "y":5.3076, "heading":-1.04543, "vx":0.57019, "vy":-0.60498, "omega":-0.00122, "ax":-3.15995, "ay":3.35279, "alpha":0.00877, "fx":[-51.61756,-51.68546,-51.70091,-51.63295], "fy":[54.85105,54.78716,54.77223,54.83622]}, + {"t":1.85252, "x":3.93525, "y":5.28797, "heading":-1.04548, "vx":0.45626, "vy":-0.4841, "omega":-0.0009, "ax":-3.16213, "ay":3.3551, "alpha":0.00732, "fx":[-51.66005,-51.71673,-51.72966,-51.67294], "fy":[54.88234,54.82898,54.81656,54.86999]}, + {"t":1.88857, "x":3.94965, "y":5.27269, "heading":-1.04551, "vx":0.34225, "vy":-0.36313, "omega":-0.00064, "ax":-3.16344, "ay":3.35649, "alpha":0.00645, "fx":[-51.68559,-51.73553,-51.74694,-51.69697], "fy":[54.90115,54.85413,54.84321,54.89028]}, + {"t":1.92463, "x":3.95993, "y":5.26178, "heading":-1.04553, "vx":0.22819, "vy":-0.24212, "omega":-0.00041, "ax":-3.16432, "ay":3.35742, "alpha":0.00587, "fx":[-51.70264,-51.74808,-51.75847,-51.71301], "fy":[54.9137,54.87091,54.86099,54.90382]}, + {"t":1.96068, "x":3.9661, "y":5.25524, "heading":-1.04555, "vx":0.11411, "vy":-0.12107, "omega":-0.0002, "ax":-3.16494, "ay":3.35808, "alpha":0.00545, "fx":[-51.71482,-51.75704,-51.76671,-51.72447], "fy":[54.92267,54.88291,54.8737,54.91349]}, + {"t":1.99673, "x":3.96816, "y":5.25305, "heading":-1.04555, "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/PLOtoL.traj b/src/main/deploy/choreo/PLOtoL.traj index 25bb77af..ee67414c 100644 --- a/src/main/deploy/choreo/PLOtoL.traj +++ b/src/main/deploy/choreo/PLOtoL.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":1.550151228904724, "y":7.523398399353027, "heading":-0.9350057865774468, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":3.183215618133545, "y":5.706628799438477, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.691657066345215, "y":5.089337348937988, "heading":-1.0455529770312977, "intervals":46, "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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PLO.x", "val":1.550151228904724}, "y":{"exp":"PLO.y", "val":7.523398399353027}, "heading":{"exp":"-0.9350057865774468 rad", "val":-0.9350057865774468}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"3.183215618133545 m", "val":3.183215618133545}, "y":{"exp":"5.706628799438477 m", "val":5.706628799438477}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"L.x", "val":3.691657066345215}, "y":{"exp":"L.y", "val":5.089337348937988}, "heading":{"exp":"-1.0455529770312977 rad", "val":-1.0455529770312977}, "intervals":46, "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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,61 +30,63 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.20207,1.89844], + "waypoints":[0.0,1.235,2.011], "samples":[ - {"t":0.0, "x":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0897, "ay":-3.42807, "alpha":-0.0103, "fx":[50.46517,50.5483,50.55626,50.47302], "fy":[-56.08355,-56.00865,-56.00131,-56.07632]}, - {"t":0.03756, "x":1.55233, "y":7.52098, "heading":-0.93501, "vx":0.11606, "vy":-0.12877, "omega":-0.00039, "ax":3.08951, "ay":-3.42789, "alpha":-0.01052, "fx":[50.4611,50.54593,50.55404,50.46911], "fy":[-56.08142,-56.00499,-55.99749,-56.07404]}, - {"t":0.07513, "x":1.55887, "y":7.51372, "heading":-0.93502, "vx":0.23212, "vy":-0.25754, "omega":-0.00078, "ax":3.08929, "ay":-3.42769, "alpha":-0.01075, "fx":[50.45658,50.54329,50.55159,50.46476], "fy":[-56.07905,-56.00093,-55.99326,-56.07151]}, - {"t":0.11269, "x":1.56977, "y":7.50163, "heading":-0.93505, "vx":0.34817, "vy":-0.3863, "omega":-0.00119, "ax":3.08905, "ay":-3.42746, "alpha":-0.01101, "fx":[50.45153,50.54035,50.54884,50.4599], "fy":[-56.0764,-55.99639,-55.98852,-56.06867]}, - {"t":0.15026, "x":1.58503, "y":7.4847, "heading":-0.93509, "vx":0.46421, "vy":-0.51505, "omega":-0.0016, "ax":3.08879, "ay":-3.42721, "alpha":-0.01131, "fx":[50.44584,50.53704,50.54576,50.45443], "fy":[-56.07343,-55.99128,-55.98318,-56.06548]}, - {"t":0.18782, "x":1.60465, "y":7.46294, "heading":-0.93515, "vx":0.58024, "vy":-0.64379, "omega":-0.00202, "ax":3.08848, "ay":-3.42692, "alpha":-0.01164, "fx":[50.4394,50.53328,50.54226,50.44824], "fy":[-56.07006,-55.98548,-55.97714,-56.06187]}, - {"t":0.22539, "x":1.62862, "y":7.43633, "heading":-0.93523, "vx":0.69625, "vy":-0.77253, "omega":-0.00246, "ax":3.08813, "ay":-3.42659, "alpha":-0.01203, "fx":[50.43203,50.52899,50.53826,50.44115], "fy":[-56.06621,-55.97887,-55.97023,-56.05773]}, - {"t":0.26295, "x":1.65695, "y":7.4049, "heading":-0.93532, "vx":0.81226, "vy":-0.90124, "omega":-0.00291, "ax":3.08773, "ay":-3.42621, "alpha":-0.01247, "fx":[50.42353,50.52403,50.53364,50.43298], "fy":[-56.06176,-55.97123,-55.96225,-56.05296]}, - {"t":0.30052, "x":1.68964, "y":7.36862, "heading":-0.93543, "vx":0.92825, "vy":-1.02995, "omega":-0.00338, "ax":3.08726, "ay":-3.42577, "alpha":-0.01298, "fx":[50.4136,50.51825,50.52826,50.42344], "fy":[-56.05657,-55.96232,-55.95294,-56.04739]}, - {"t":0.33808, "x":1.72669, "y":7.32752, "heading":-0.93556, "vx":1.04422, "vy":-1.15864, "omega":-0.00387, "ax":3.08671, "ay":-3.42525, "alpha":-0.01359, "fx":[50.40188,50.51142,50.5219,50.41216], "fy":[-56.05044,-55.95179,-55.94194,-56.04081]}, - {"t":0.37565, "x":1.7681, "y":7.28158, "heading":-0.9357, "vx":1.16017, "vy":-1.2873, "omega":-0.00438, "ax":3.08605, "ay":-3.42462, "alpha":-0.01432, "fx":[50.3878,50.50322,50.51426,50.39863], "fy":[-56.04309,-55.93915,-55.92873,-56.0329]}, - {"t":0.41321, "x":1.81385, "y":7.2308, "heading":-0.93587, "vx":1.2761, "vy":-1.41595, "omega":-0.00492, "ax":3.08523, "ay":-3.42385, "alpha":-0.01522, "fx":[50.37059,50.49319,50.50492,50.38208], "fy":[-56.0341,-55.92369,-55.91258,-56.02324]}, - {"t":0.45078, "x":1.86397, "y":7.1752, "heading":-0.93605, "vx":1.39199, "vy":-1.54456, "omega":-0.00549, "ax":3.08422, "ay":-3.42289, "alpha":-0.01634, "fx":[50.34907,50.48066,50.49325,50.36138], "fy":[-56.02286,-55.90438,-55.89238,-56.01116]}, - {"t":0.48834, "x":1.91843, "y":7.11476, "heading":-0.93626, "vx":1.50785, "vy":-1.67314, "omega":-0.0061, "ax":3.08291, "ay":-3.42165, "alpha":-0.01778, "fx":[50.3214,50.46455,50.47823,50.33476], "fy":[-56.0084,-55.87954,-55.86639,-55.99561]}, - {"t":0.5259, "x":1.97725, "y":7.0495, "heading":-0.93649, "vx":1.62366, "vy":-1.80168, "omega":-0.00677, "ax":3.08117, "ay":-3.42, "alpha":-0.01971, "fx":[50.2845,50.44307,50.4582,50.29923], "fy":[-55.98912,-55.84641,-55.83172,-55.97487]}, - {"t":0.56347, "x":2.04042, "y":6.97941, "heading":-0.93674, "vx":1.7394, "vy":-1.93015, "omega":-0.00751, "ax":3.07873, "ay":-3.41769, "alpha":-0.02241, "fx":[50.23282,50.41299,50.43013,50.24945], "fy":[-55.96212,-55.80002,-55.78314,-55.94581]}, - {"t":0.60103, "x":2.10793, "y":6.90449, "heading":-0.93703, "vx":1.85505, "vy":-2.05853, "omega":-0.00835, "ax":3.07506, "ay":-3.41423, "alpha":-0.02648, "fx":[50.15528,50.36787,50.38798,50.17468], "fy":[-55.9216,-55.73041,-55.71017,-55.90214]}, - {"t":0.6386, "x":2.17978, "y":6.82475, "heading":-0.93734, "vx":1.97057, "vy":-2.18679, "omega":-0.00935, "ax":3.06895, "ay":-3.40844, "alpha":-0.03327, "fx":[50.02599,50.29264,50.31761,50.04983], "fy":[-55.854,-55.61438,-55.58834,-55.8292]}, - {"t":0.67616, "x":2.25597, "y":6.7402, "heading":-0.93769, "vx":2.08585, "vy":-2.31482, "omega":-0.0106, "ax":3.0567, "ay":-3.39684, "alpha":-0.04694, "fx":[49.76732,50.14211,50.17635,49.79934], "fy":[-55.71854,-55.38225,-55.34394,-55.68266]}, - {"t":0.71373, "x":2.33648, "y":6.65085, "heading":-0.93809, "vx":2.20067, "vy":-2.44242, "omega":-0.01236, "ax":3.01979, "ay":-3.36184, "alpha":-0.08848, "fx":[48.99123,49.68975,49.74824,49.04198], "fy":[-55.30984,-54.68591,-54.6052,-55.23757]}, - {"t":0.75129, "x":2.42128, "y":6.55673, "heading":-0.93855, "vx":2.31411, "vy":-2.56871, "omega":-0.01569, "ax":-1.17911, "ay":1.07722, "alpha":-2.80881, "fx":[-22.81284,-8.23637,-16.70856,-29.34676], "fy":[6.19788,17.67164,28.38594,18.18641]}, - {"t":0.78886, "x":2.50738, "y":6.461, "heading":-0.93914, "vx":2.26982, "vy":-2.52824, "omega":-0.1212, "ax":-3.03383, "ay":3.35394, "alpha":-0.07567, "fx":[-49.92303,-49.32122,-49.27443,-49.87063], "fy":[54.52556,55.06809,55.13255,54.59622]}, - {"t":0.82642, "x":2.5905, "y":6.36839, "heading":-0.9437, "vx":2.15585, "vy":-2.40225, "omega":-0.12404, "ax":-3.06321, "ay":3.39219, "alpha":-0.03596, "fx":[-50.2355,-49.94749,-49.92057,-50.20729], "fy":[55.31079,55.5704,55.60006,55.34188]}, - {"t":0.86399, "x":2.66932, "y":6.28054, "heading":-0.94836, "vx":2.04078, "vy":-2.27483, "omega":-0.12539, "ax":-3.07318, "ay":3.40517, "alpha":-0.02243, "fx":[-50.33995,-50.15993,-50.14158,-50.32111], "fy":[55.57744,55.73971,55.75851,55.5968]}, - {"t":0.90155, "x":2.74382, "y":6.19749, "heading":-0.95307, "vx":1.92534, "vy":-2.14691, "omega":-0.12623, "ax":-3.0782, "ay":3.41171, "alpha":-0.01557, "fx":[-50.39201,-50.26694,-50.25338,-50.3782], "fy":[55.7119,55.82462,55.83803,55.72558]}, - {"t":0.93912, "x":2.81397, "y":6.11925, "heading":-0.95781, "vx":1.80971, "vy":-2.01875, "omega":-0.12682, "ax":-3.08121, "ay":3.41565, "alpha":-0.01141, "fx":[-50.42309,-50.33144,-50.32096,-50.41248], "fy":[55.79301,55.87562,55.88576,55.8033]}, - {"t":0.97668, "x":2.87978, "y":6.04583, "heading":-0.96257, "vx":1.69397, "vy":-1.89045, "omega":-0.12725, "ax":-3.08323, "ay":3.41828, "alpha":-0.00861, "fx":[-50.44369,-50.37456,-50.36627,-50.43533], "fy":[55.84732,55.90963,55.91753,55.85531]}, - {"t":1.01424, "x":2.94123, "y":5.97723, "heading":-0.96735, "vx":1.57815, "vy":-1.76204, "omega":-0.12757, "ax":-3.08467, "ay":3.42017, "alpha":-0.00659, "fx":[-50.45831,-50.40541,-50.39879,-50.45165], "fy":[55.88625,55.93393,55.94018,55.89255]}, - {"t":1.05181, "x":2.99834, "y":5.91345, "heading":-0.97214, "vx":1.46227, "vy":-1.63356, "omega":-0.12782, "ax":-3.08576, "ay":3.42158, "alpha":-0.00506, "fx":[-50.46922,-50.42858,-50.42328,-50.46389], "fy":[55.91553,55.95216,55.95713,55.92053]}, - {"t":1.08937, "x":3.05109, "y":5.8545, "heading":-0.97694, "vx":1.34636, "vy":-1.50503, "omega":-0.12801, "ax":-3.0866, "ay":3.42268, "alpha":-0.00387, "fx":[-50.47765,-50.4466,-50.4424,-50.47343], "fy":[55.93837,55.96635,55.97027,55.94232]}, - {"t":1.12694, "x":3.09949, "y":5.80038, "heading":-0.98175, "vx":1.23041, "vy":-1.37646, "omega":-0.12815, "ax":-3.08727, "ay":3.42356, "alpha":-0.00291, "fx":[-50.48435,-50.46103,-50.45775,-50.48106], "fy":[55.9567,55.97772,55.98076,55.95975]}, - {"t":1.1645, "x":3.14353, "y":5.75109, "heading":-0.98657, "vx":1.11444, "vy":-1.24786, "omega":-0.12826, "ax":-3.08783, "ay":3.42428, "alpha":-0.00212, "fx":[-50.4898,-50.47282,-50.47035,-50.48732], "fy":[55.97173,55.98703,55.98931,55.97402]}, - {"t":1.20207, "x":3.18322, "y":5.70663, "heading":-0.99139, "vx":0.99844, "vy":-1.11922, "omega":-0.12834, "ax":-1.13542, "ay":-0.96733, "alpha":-0.60968, "fx":[-18.83966,-16.19997,-18.31127,-20.89663], "fy":[-18.10667,-16.54806,-13.47617,-15.12505]}, - {"t":1.23689, "x":3.21729, "y":5.66707, "heading":-0.99585, "vx":0.95891, "vy":-1.15291, "omega":-0.14957, "ax":-0.18809, "ay":-0.15555, "alpha":-0.5291, "fx":[-3.52348,-1.04926,-2.62913,-5.09802], "fy":[-4.55516,-2.96129,-0.52525,-2.12982]}, - {"t":1.2717, "x":3.25057, "y":5.62684, "heading":-1.00106, "vx":0.95236, "vy":-1.15832, "omega":-0.16799, "ax":-0.02773, "ay":-0.02278, "alpha":-0.31371, "fx":[-0.72809,0.7456,-0.17873,-1.65216], "fy":[-1.56559,-0.62194,0.82102,-0.12318]}, - {"t":1.30652, "x":3.28371, "y":5.58649, "heading":-1.00691, "vx":0.9514, "vy":-1.15911, "omega":-0.17892, "ax":-0.00406, "ay":-0.00334, "alpha":-0.09691, "fx":[-0.15348,0.30345,0.02058,-0.43634], "fy":[-0.42266,-0.13371,0.3136,0.02465]}, - {"t":1.34134, "x":3.31683, "y":5.54613, "heading":-1.01314, "vx":0.95125, "vy":-1.15923, "omega":-0.18229, "ax":-0.0006, "ay":-0.00049, "alpha":0.11946, "fx":[0.10038,-0.46508,-0.11984,0.44562], "fy":[0.44513,0.09246,-0.4611,-0.10843]}, - {"t":1.37616, "x":3.34995, "y":5.50577, "heading":-1.01949, "vx":0.95123, "vy":-1.15925, "omega":-0.17813, "ax":-0.00009, "ay":-0.00007, "alpha":0.3364, "fx":[0.31673,-1.2819,-0.31958,1.27905], "fy":[1.27283,0.28981,-1.27516,-0.29215]}, - {"t":1.41098, "x":3.38308, "y":5.4654, "heading":-1.02569, "vx":0.95123, "vy":-1.15925, "omega":-0.16642, "ax":-0.00001, "ay":-0.00001, "alpha":0.55496, "fx":[0.53768,-2.10959,-0.5381,2.10918], "fy":[2.09825,0.49296,-2.09859,-0.4933]}, - {"t":1.4458, "x":3.4162, "y":5.42504, "heading":-1.03148, "vx":0.95123, "vy":-1.15925, "omega":-0.14709, "ax":0.0, "ay":0.0, "alpha":0.77618, "fx":[0.76926,-2.9462,-0.76933,2.94614], "fy":[2.93044,0.70675,-2.93049,-0.70681]}, - {"t":1.48062, "x":3.44932, "y":5.38467, "heading":-1.03661, "vx":0.95123, "vy":-1.15925, "omega":-0.12007, "ax":0.0, "ay":0.0, "alpha":1.00108, "fx":[1.01153,-3.79513,-1.01156,3.79509], "fy":[3.77443,0.93101,-3.77446,-0.93103]}, - {"t":1.51544, "x":3.48244, "y":5.34431, "heading":-1.04079, "vx":0.95123, "vy":-1.15925, "omega":-0.08521, "ax":-0.00007, "ay":0.00008, "alpha":1.23068, "fx":[1.26171,-4.66192,-1.26417,4.65949], "fy":[4.6362,1.16537,-4.63357,-1.16273]}, - {"t":1.55026, "x":3.51556, "y":5.30395, "heading":-1.04375, "vx":0.95123, "vy":-1.15925, "omega":-0.04236, "ax":-1.03287, "ay":1.25877, "alpha":1.14645, "fx":[-15.23468,-21.0979,-18.71391,-12.49555], "fy":[24.87235,21.16568,16.18847,20.0872]}, - {"t":1.58507, "x":3.54805, "y":5.26435, "heading":-1.04523, "vx":0.91526, "vy":-1.11542, "omega":-0.00244, "ax":-2.89922, "ay":3.53324, "alpha":0.01888, "fx":[-47.30698,-47.46298,-47.48669,-47.33034], "fy":[57.83656,57.70906,57.6868,57.8147]}, - {"t":1.61989, "x":3.57816, "y":5.22765, "heading":-1.04531, "vx":0.81432, "vy":-0.9924, "omega":-0.00179, "ax":-2.91485, "ay":3.55229, "alpha":0.01081, "fx":[-47.60042,-47.69015,-47.70414,-47.6143], "fy":[58.11603,58.04255,58.03026,58.10386]}, - {"t":1.65471, "x":3.60475, "y":5.19525, "heading":-1.04538, "vx":0.71282, "vy":-0.86871, "omega":-0.00141, "ax":-2.92009, "ay":3.55868, "alpha":0.00811, "fx":[-47.69896,-47.76638,-47.77698,-47.7095], "fy":[58.20978,58.15454,58.14543,58.20074]}, - {"t":1.68953, "x":3.6278, "y":5.16716, "heading":-1.04542, "vx":0.61115, "vy":-0.7448, "omega":-0.00113, "ax":-2.92272, "ay":3.56188, "alpha":0.00676, "fx":[-47.74835,-47.80457,-47.81346,-47.75719], "fy":[58.25675,58.21067,58.20312,58.24926]}, - {"t":1.72435, "x":3.64731, "y":5.14338, "heading":-1.04546, "vx":0.50938, "vy":-0.62078, "omega":-0.00089, "ax":-2.9243, "ay":3.5638, "alpha":0.00594, "fx":[-47.77801,-47.82751,-47.83535,-47.78582], "fy":[58.28496,58.24438,58.23777,58.27839]}, - {"t":1.75917, "x":3.66327, "y":5.12393, "heading":-1.0455, "vx":0.40756, "vy":-0.49669, "omega":-0.00068, "ax":-2.92535, "ay":3.56508, "alpha":0.0054, "fx":[-47.7978,-47.84281,-47.84996,-47.80492], "fy":[58.30378,58.26687,58.26087,58.29781]}, - {"t":1.79399, "x":3.67569, "y":5.1088, "heading":-1.04552, "vx":0.30571, "vy":-0.37256, "omega":-0.0005, "ax":-2.9261, "ay":3.566, "alpha":0.00502, "fx":[-47.81195,-47.85375,-47.86039,-47.81857], "fy":[58.31722,58.28294,58.27738,58.31169]}, - {"t":1.82881, "x":3.68456, "y":5.09799, "heading":-1.04554, "vx":0.20382, "vy":-0.24839, "omega":-0.00032, "ax":-2.92666, "ay":3.56669, "alpha":0.00473, "fx":[-47.82256,-47.86195,-47.86822,-47.8288], "fy":[58.32731,58.295,58.28977,58.3221]}, - {"t":1.86363, "x":3.68988, "y":5.0915, "heading":-1.04555, "vx":0.10192, "vy":-0.12421, "omega":-0.00016, "ax":-2.9271, "ay":3.56722, "alpha":0.0045, "fx":[-47.83081,-47.86833,-47.87431,-47.83676], "fy":[58.33516,58.30438,58.2994,58.3302]}, - {"t":1.89844, "x":3.69166, "y":5.08934, "heading":-1.04555, "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":1.55015, "y":7.5234, "heading":-0.93501, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.08941, "ay":-3.42846, "alpha":-0.00579, "fx":[50.48043,50.52718,50.53164,50.48485], "fy":[-56.07195,-56.02984,-56.02573,-56.06788]}, + {"t":0.03859, "x":1.55245, "y":7.52085, "heading":-0.93501, "vx":0.11923, "vy":-0.13232, "omega":-0.00022, "ax":3.08922, "ay":-3.42827, "alpha":-0.00593, "fx":[50.47666,50.52451,50.52907,50.48118], "fy":[-56.06936,-56.02626,-56.02205,-56.06519]}, + {"t":0.07719, "x":1.55935, "y":7.51319, "heading":-0.93501, "vx":0.23846, "vy":-0.26463, "omega":-0.00045, "ax":3.089, "ay":-3.42806, "alpha":-0.00608, "fx":[50.47244,50.52152,50.52619,50.47708], "fy":[-56.06646,-56.02226,-56.01794,-56.06218]}, + {"t":0.11578, "x":1.57086, "y":7.50042, "heading":-0.93503, "vx":0.35767, "vy":-0.39693, "omega":-0.00069, "ax":3.08876, "ay":-3.42782, "alpha":-0.00625, "fx":[50.4677,50.51815,50.52296,50.47246], "fy":[-56.0632,-56.01776,-56.01331,-56.0588]}, + {"t":0.15437, "x":1.58696, "y":7.48255, "heading":-0.93506, "vx":0.47688, "vy":-0.52922, "omega":-0.00093, "ax":3.08848, "ay":-3.42754, "alpha":-0.00645, "fx":[50.46233,50.51434,50.51929,50.46723], "fy":[-56.05951,-56.01266,-56.00807,-56.05497]}, + {"t":0.19297, "x":1.60767, "y":7.45957, "heading":-0.93509, "vx":0.59608, "vy":-0.6615, "omega":-0.00118, "ax":3.08817, "ay":-3.42723, "alpha":-0.00667, "fx":[50.45618,50.50998,50.5151,50.46126], "fy":[-56.05529,-56.00683,-56.00207,-56.05058]}, + {"t":0.23156, "x":1.63297, "y":7.43149, "heading":-0.93514, "vx":0.71526, "vy":-0.79377, "omega":-0.00143, "ax":3.0878, "ay":-3.42687, "alpha":-0.00693, "fx":[50.4491,50.50496,50.51027,50.45436], "fy":[-56.05042,-56.0001,-55.99516,-56.04553]}, + {"t":0.27016, "x":1.66288, "y":7.3983, "heading":-0.9352, "vx":0.83443, "vy":-0.92603, "omega":-0.0017, "ax":3.08738, "ay":-3.42645, "alpha":-0.00722, "fx":[50.44083,50.49909,50.50463,50.44631], "fy":[-56.04474,-55.99226,-55.98709,-56.03963]}, + {"t":0.30875, "x":1.69738, "y":7.36001, "heading":-0.93526, "vx":0.95358, "vy":-1.05827, "omega":-0.00198, "ax":3.08687, "ay":-3.42595, "alpha":-0.00758, "fx":[50.43106,50.49216,50.49797,50.4368], "fy":[-56.03803,-55.98299,-55.97756,-56.03266]}, + {"t":0.34734, "x":1.73648, "y":7.31662, "heading":-0.93534, "vx":1.07272, "vy":-1.19049, "omega":-0.00227, "ax":3.08627, "ay":-3.42536, "alpha":-0.008, "fx":[50.41933,50.48385,50.48997,50.42539], "fy":[-56.02997,-55.97187,-55.96612,-56.02429]}, + {"t":0.38594, "x":1.78018, "y":7.26812, "heading":-0.93542, "vx":1.19183, "vy":-1.32268, "omega":-0.00258, "ax":3.08553, "ay":-3.42463, "alpha":-0.00852, "fx":[50.40501,50.47369,50.4802,50.41144], "fy":[-56.02013,-55.95828,-55.95213,-56.01406]}, + {"t":0.42453, "x":1.82847, "y":7.21452, "heading":-0.93552, "vx":1.31091, "vy":-1.45485, "omega":-0.00291, "ax":3.08461, "ay":-3.42372, "alpha":-0.00917, "fx":[50.3871,50.46099,50.46799,50.39401], "fy":[-56.00783,-55.9413,-55.93466,-56.00128]}, + {"t":0.46312, "x":1.88136, "y":7.15582, "heading":-0.93564, "vx":1.42996, "vy":-1.58699, "omega":-0.00326, "ax":3.08343, "ay":-3.42255, "alpha":-0.01, "fx":[50.36409,50.44468,50.4523,50.3716], "fy":[-55.99203,-55.91948,-55.91219,-55.98486]}, + {"t":0.50172, "x":1.93885, "y":7.09203, "heading":-0.93576, "vx":1.54896, "vy":-1.71908, "omega":-0.00365, "ax":3.08185, "ay":-3.42099, "alpha":-0.01112, "fx":[50.33342,50.42293,50.43137,50.34173], "fy":[-55.97096,-55.89039,-55.88224,-55.96295]}, + {"t":0.54031, "x":2.00092, "y":7.02313, "heading":-0.9359, "vx":1.6679, "vy":-1.85111, "omega":-0.00408, "ax":3.07964, "ay":-3.41881, "alpha":-0.01268, "fx":[50.29051,50.39252,50.40209,50.29992], "fy":[-55.94149,-55.84969,-55.84032,-55.9323]}, + {"t":0.57891, "x":2.06759, "y":6.94915, "heading":-0.93606, "vx":1.78675, "vy":-1.98305, "omega":-0.00457, "ax":3.07633, "ay":-3.41553, "alpha":-0.01502, "fx":[50.22621,50.34694,50.3582,50.23724], "fy":[-55.89732,-55.78872,-55.77747,-55.88633]}, + {"t":0.6175, "x":2.13883, "y":6.87007, "heading":-0.93624, "vx":1.90548, "vy":-2.11487, "omega":-0.00515, "ax":3.07082, "ay":-3.41009, "alpha":-0.01892, "fx":[50.11924,50.27112,50.2851,50.13286], "fy":[-55.82382,-55.68727,-55.67283,-55.80977]}, + {"t":0.65609, "x":2.21466, "y":6.78591, "heading":-0.93644, "vx":2.02399, "vy":-2.24648, "omega":-0.00588, "ax":3.05983, "ay":-3.39922, "alpha":-0.02674, "fx":[49.90607,50.11998,50.13916,49.92452], "fy":[-55.67721,-55.48511,-55.46394,-55.65684]}, + {"t":0.69469, "x":2.29505, "y":6.69668, "heading":-0.93666, "vx":2.14208, "vy":-2.37766, "omega":-0.00691, "ax":3.02711, "ay":-3.36684, "alpha":-0.05019, "fx":[49.27296,49.67066,49.70323,49.30303], "fy":[-55.24051,-54.8846,-54.84084,-55.19947]}, + {"t":0.73328, "x":2.37998, "y":6.60241, "heading":-0.93693, "vx":2.25891, "vy":-2.5076, "omega":-0.00885, "ax":-0.03913, "ay":-0.15143, "alpha":-2.2462, "fx":[-2.05033,8.04867,0.7743,-9.33143], "fy":[-11.08514,-3.74474,6.20253,-1.27508]}, + {"t":0.77187, "x":2.46713, "y":6.50552, "heading":-0.93727, "vx":2.2574, "vy":-2.51345, "omega":-0.09554, "ax":-3.03493, "ay":3.3596, "alpha":-0.04548, "fx":[-49.81085,-49.44917,-49.42086,-49.78051], "fy":[54.74055,55.06615,55.10447,54.78112]}, + {"t":0.81047, "x":2.55199, "y":6.41102, "heading":-0.94096, "vx":2.14027, "vy":-2.38379, "omega":-0.09729, "ax":-3.06381, "ay":3.39559, "alpha":-0.02173, "fx":[-50.18269,-50.0086,-49.9926,-50.16622], "fy":[55.42409,55.58088,55.59839,55.44212]}, + {"t":0.84906, "x":2.63231, "y":6.32155, "heading":-0.94471, "vx":2.02203, "vy":-2.25274, "omega":-0.09813, "ax":-3.07349, "ay":3.40766, "alpha":-0.01372, "fx":[-50.30635,-50.1962,-50.1853,-50.29526], "fy":[55.6535,55.75272,55.76385,55.66484]}, + {"t":0.88766, "x":2.70806, "y":6.23714, "heading":-0.9485, "vx":1.90341, "vy":-2.12123, "omega":-0.09866, "ax":-3.07834, "ay":3.41371, "alpha":-0.00966, "fx":[-50.36797,-50.2903,-50.28219,-50.35977], "fy":[55.76862,55.83859,55.84659,55.77672]}, + {"t":0.92625, "x":2.77923, "y":6.15782, "heading":-0.95231, "vx":1.7846, "vy":-1.98948, "omega":-0.09903, "ax":-3.08126, "ay":3.41734, "alpha":-0.00721, "fx":[-50.40481,-50.34688,-50.34055,-50.39843], "fy":[55.83788,55.89007,55.89619,55.84405]}, + {"t":0.96484, "x":2.84581, "y":6.08358, "heading":-0.95613, "vx":1.66569, "vy":-1.85759, "omega":-0.09931, "ax":-3.0832, "ay":3.41977, "alpha":-0.00555, "fx":[-50.42928,-50.38466,-50.37958,-50.42418], "fy":[55.88416,55.92436,55.9292,55.88903]}, + {"t":1.00344, "x":2.90779, "y":6.01444, "heading":-0.95996, "vx":1.5467, "vy":-1.72561, "omega":-0.09953, "ax":-3.08459, "ay":3.4215, "alpha":-0.00436, "fx":[-50.44669,-50.41167,-50.40754,-50.44254], "fy":[55.91728,55.94884,55.95274,55.92121]}, + {"t":1.04203, "x":2.96519, "y":5.95039, "heading":-0.9638, "vx":1.42765, "vy":-1.59356, "omega":-0.09969, "ax":-3.08563, "ay":3.4228, "alpha":-0.00345, "fx":[-50.45971,-50.43195,-50.42856,-50.45631], "fy":[55.94217,55.96718,55.97036,55.94536]}, + {"t":1.08062, "x":3.01799, "y":5.89144, "heading":-0.96765, "vx":1.30856, "vy":-1.46146, "omega":-0.09983, "ax":-3.08644, "ay":3.42381, "alpha":-0.00275, "fx":[-50.46981,-50.44773,-50.44494,-50.46701], "fy":[55.96156,55.98145,55.98405,55.96417]}, + {"t":1.11922, "x":3.06619, "y":5.83758, "heading":-0.9715, "vx":1.18945, "vy":-1.32932, "omega":-0.09993, "ax":-3.08709, "ay":3.42462, "alpha":-0.00218, "fx":[-50.47785,-50.46036,-50.45808,-50.47557], "fy":[55.97709,55.99286,55.99497,55.97921]}, + {"t":1.15781, "x":3.1098, "y":5.78883, "heading":-0.97536, "vx":1.0703, "vy":-1.19715, "omega":-0.10002, "ax":-3.08762, "ay":3.42528, "alpha":-0.00171, "fx":[-50.48442,-50.47069,-50.46884,-50.48257], "fy":[55.98982,56.00219,56.0039,55.99153]}, + {"t":1.19641, "x":3.14881, "y":5.74518, "heading":-0.97922, "vx":0.95114, "vy":-1.06496, "omega":-0.10008, "ax":-3.08806, "ay":3.42584, "alpha":-0.00132, "fx":[-50.48987,-50.47929,-50.47783,-50.48841], "fy":[56.00044,56.00997,56.01132,56.0018]}, + {"t":1.235, "x":3.18322, "y":5.70663, "heading":-0.98308, "vx":0.83196, "vy":-0.93274, "omega":-0.10013, "ax":-0.9744, "ay":-0.82976, "alpha":-0.497, "fx":[-16.18986,-14.0089,-15.68417,-17.83521], "fy":[-15.44631,-14.08815,-11.65841,-13.06725]}, + {"t":1.27027, "x":3.21196, "y":5.67321, "heading":-0.98662, "vx":0.79759, "vy":-0.96201, "omega":-0.11766, "ax":-0.11311, "ay":-0.09339, "alpha":-0.4615, "fx":[-2.22683,-0.07972,-1.47285,-3.61746], "fy":[-3.28669,-1.87101,0.23565,-1.18487]}, + {"t":1.30554, "x":3.24002, "y":5.63922, "heading":-0.99077, "vx":0.7936, "vy":-0.96531, "omega":-0.13394, "ax":-0.01201, "ay":-0.00987, "alpha":-0.34675, "fx":[-0.48638,1.13169,0.09378,-1.52415], "fy":[-1.48326,-0.42327,1.16084,0.10057]}, + {"t":1.34082, "x":3.268, "y":5.60517, "heading":-0.99549, "vx":0.79318, "vy":-0.96565, "omega":-0.14617, "ax":-0.00127, "ay":-0.00104, "alpha":-0.23282, "fx":[-0.21974,0.87001,0.1782,-0.91154], "fy":[-0.90379,-0.19712,0.86969,0.163]}, + {"t":1.37609, "x":3.29598, "y":5.5711, "heading":-1.00065, "vx":0.79313, "vy":-0.96569, "omega":-0.15439, "ax":-0.00013, "ay":-0.00011, "alpha":-0.12006, "fx":[-0.10716,0.45668,0.10277,-0.46107], "fy":[-0.45855,-0.09703,0.45494,0.09342]}, + {"t":1.41136, "x":3.32396, "y":5.53704, "heading":-1.00609, "vx":0.79313, "vy":-0.9657, "omega":-0.15862, "ax":-0.00001, "ay":-0.00001, "alpha":-0.00791, "fx":[-0.00731,0.02998,0.00685,-0.03044], "fy":[-0.03026,-0.00663,0.02987,0.00625]}, + {"t":1.44664, "x":3.35193, "y":5.50298, "heading":-1.01169, "vx":0.79313, "vy":-0.9657, "omega":-0.1589, "ax":0.0, "ay":0.0, "alpha":0.10419, "fx":[0.09544,-0.39732,-0.09549,0.39727], "fy":[0.39534,0.08701,-0.39538,-0.08705]}, + {"t":1.48191, "x":3.37991, "y":5.46892, "heading":-1.01729, "vx":0.79313, "vy":-0.9657, "omega":-0.15522, "ax":0.0, "ay":0.0, "alpha":0.21683, "fx":[0.20327,-0.82576,-0.20327,0.82575], "fy":[0.82161,0.18574,-0.82162,-0.18575]}, + {"t":1.51718, "x":3.40788, "y":5.43485, "heading":-1.02277, "vx":0.79313, "vy":-0.9657, "omega":-0.14758, "ax":0.0, "ay":0.0, "alpha":0.33057, "fx":[0.31674,-1.25732,-0.31674,1.25732], "fy":[1.25086,0.29006,-1.25086,-0.29006]}, + {"t":1.55245, "x":3.43586, "y":5.40079, "heading":-1.02797, "vx":0.79313, "vy":-0.9657, "omega":-0.13592, "ax":0.0, "ay":0.0, "alpha":0.44597, "fx":[0.4361,-1.69421,-0.4361,1.69421], "fy":[1.68532,0.40015,-1.68532,-0.40015]}, + {"t":1.58773, "x":3.46384, "y":5.36673, "heading":-1.03277, "vx":0.79313, "vy":-0.9657, "omega":-0.12019, "ax":0.0, "ay":0.0, "alpha":0.56363, "fx":[0.56136,-2.13873,-0.56136,2.13873], "fy":[2.12727,0.51598,-2.12727,-0.51598]}, + {"t":1.623, "x":3.49181, "y":5.33266, "heading":-1.03701, "vx":0.79313, "vy":-0.9657, "omega":-0.10031, "ax":0.0, "ay":0.0, "alpha":0.68412, "fx":[0.69231,-2.59328,-0.69231,2.59327], "fy":[2.57913,0.63728,-2.57913,-0.63729]}, + {"t":1.65827, "x":3.51979, "y":5.2986, "heading":-1.04054, "vx":0.79313, "vy":-0.9657, "omega":-0.07617, "ax":0.0, "ay":0.0, "alpha":0.80805, "fx":[0.82849,-3.06034,-0.82849,3.06033], "fy":[3.0434,0.76355,-3.0434,-0.76356]}, + {"t":1.69355, "x":3.54776, "y":5.26454, "heading":-1.04323, "vx":0.79313, "vy":-0.9657, "omega":-0.04767, "ax":-0.00015, "ay":0.00018, "alpha":0.936, "fx":[0.96671,-3.54498,-0.97159,3.54012], "fy":[3.52564,0.8969,-3.51982,-0.89107]}, + {"t":1.72882, "x":3.57574, "y":5.23048, "heading":-1.04491, "vx":0.79312, "vy":-0.96569, "omega":-0.01466, "ax":-2.02463, "ay":2.46515, "alpha":0.38307, "fx":[-32.021,-34.47573,-34.21918,-31.67969], "fy":[41.76193,39.97843,38.80229,40.65941]}, + {"t":1.76409, "x":3.60246, "y":5.19795, "heading":-1.04543, "vx":0.72171, "vy":-0.87874, "omega":-0.00115, "ax":-2.90753, "ay":3.54014, "alpha":0.01041, "fx":[-47.48284,-47.56901,-47.58228,-47.496], "fy":[57.91577,57.84522,57.83311,57.90379]}, + {"t":1.79936, "x":3.6261, "y":5.16915, "heading":-1.04547, "vx":0.61915, "vy":-0.75387, "omega":-0.00078, "ax":-2.91971, "ay":3.55497, "alpha":0.00585, "fx":[-47.70365,-47.75227,-47.75991,-47.71126], "fy":[58.1403,58.10043,58.09381,58.13371]}, + {"t":1.83464, "x":3.64613, "y":5.14477, "heading":-1.0455, "vx":0.51617, "vy":-0.62847, "omega":-0.00057, "ax":-2.92385, "ay":3.56001, "alpha":0.00431, "fx":[-47.77856,-47.81441,-47.82008,-47.78422], "fy":[58.21642,58.18701,58.18218,58.21161]}, + {"t":1.86991, "x":3.66251, "y":5.12482, "heading":-1.04552, "vx":0.41303, "vy":-0.5029, "omega":-0.00042, "ax":-2.92592, "ay":3.56254, "alpha":0.00353, "fx":[-47.81626,-47.84568,-47.85035,-47.82092], "fy":[58.25472,58.23058,58.22664,58.2508]}, + {"t":1.90518, "x":3.67526, "y":5.1093, "heading":-1.04553, "vx":0.30983, "vy":-0.37724, "omega":-0.00029, "ax":-2.92718, "ay":3.56406, "alpha":0.00307, "fx":[-47.83895,-47.8645,-47.86857,-47.84301], "fy":[58.27778,58.25681,58.2534,58.27438]}, + {"t":1.94045, "x":3.68437, "y":5.09821, "heading":-1.04554, "vx":0.20658, "vy":-0.25153, "omega":-0.00019, "ax":-2.92801, "ay":3.56508, "alpha":0.00276, "fx":[-47.85411,-47.87707,-47.88073,-47.85776], "fy":[58.29318,58.27433,58.27127,58.29013]}, + {"t":1.97573, "x":3.68984, "y":5.09156, "heading":-1.04555, "vx":0.1033, "vy":-0.12578, "omega":-0.00009, "ax":-2.92861, "ay":3.56581, "alpha":0.00253, "fx":[-47.86495,-47.88607,-47.88943,-47.86832], "fy":[58.3042,58.28686,58.28405,58.30139]}, + {"t":2.011, "x":3.69166, "y":5.08934, "heading":-1.04555, "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/PRMtoB.traj b/src/main/deploy/choreo/PRMtoB.traj index c425fb10..a863e89e 100644 --- a/src/main/deploy/choreo/PRMtoB.traj +++ b/src/main/deploy/choreo/PRMtoB.traj @@ -4,27 +4,27 @@ "snapshot":{ "waypoints":[ {"x":1.1213834285736084, "y":0.9940196871757508, "heading":0.9420001549844138, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.9325637817382812, "y":3.2750864028930664, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":2.9325637817382812, "y":3.2750864028930664, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":3.1738216876983643, "y":3.852503538131714, "heading":0.0, "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":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":0.01}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"PRM.x", "val":1.1213834285736084}, "y":{"exp":"PRM.y", "val":0.9940196871757508}, "heading":{"exp":"0.9420001549844138 rad", "val":0.9420001549844138}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.9325637817382812 m", "val":2.9325637817382812}, "y":{"exp":"3.2750864028930664 m", "val":3.2750864028930664}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"2.9325637817382812 m", "val":2.9325637817382812}, "y":{"exp":"3.2750864028930664 m", "val":3.2750864028930664}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"B.x", "val":3.1738216876983643}, "y":{"exp":"B.y", "val":3.852503538131714}, "heading":{"exp":"0 rad", "val":0.0}, "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":1, "to":2, "data":{"type":"KeepInLane", "props":{"tolerance":{"exp":"0.01 m", "val":0.01}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -32,74 +32,74 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.33142,1.91233], + "waypoints":[0.0,1.36526,2.00258], "samples":[ - {"t":0.0, "x":1.12138, "y":0.99402, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.95851, "ay":3.53982, "alpha":-0.25899, "fx":[49.35922,47.233,47.33583,49.53626], "fy":[57.04458,58.81688,58.7297,56.88634]}, - {"t":0.03247, "x":1.12294, "y":0.99589, "heading":0.942, "vx":0.09607, "vy":0.11495, "omega":-0.00841, "ax":2.95764, "ay":3.5402, "alpha":-0.26319, "fx":[49.36116,47.2008,47.30415,49.54116], "fy":[57.03781,58.83778,58.75,56.8767]}, - {"t":0.06495, "x":1.12762, "y":1.00149, "heading":0.94173, "vx":0.19212, "vy":0.22992, "omega":-0.01696, "ax":2.95669, "ay":3.54061, "alpha":-0.26776, "fx":[49.36358,47.16609,47.26935,49.54623], "fy":[57.03019,58.86024,58.77234,56.86645]}, - {"t":0.09742, "x":1.13542, "y":1.01082, "heading":0.94118, "vx":0.28813, "vy":0.34489, "omega":-0.02565, "ax":2.95566, "ay":3.54106, "alpha":-0.27275, "fx":[49.36653,47.12851,47.23102,49.5515], "fy":[57.02161,58.88449,58.79696,56.85548]}, - {"t":0.1299, "x":1.14634, "y":1.02389, "heading":0.94034, "vx":0.38412, "vy":0.45988, "omega":-0.03451, "ax":2.95452, "ay":3.54155, "alpha":-0.27821, "fx":[49.37006,47.08762,47.18866,49.55701], "fy":[57.01196,58.91079,58.82419,56.84368]}, - {"t":0.16237, "x":1.16037, "y":1.04069, "heading":0.93922, "vx":0.48006, "vy":0.57489, "omega":-0.04354, "ax":2.95327, "ay":3.54209, "alpha":-0.28423, "fx":[49.37424,47.04292,47.14169,49.56283], "fy":[57.00109,58.93946,58.85438,56.83091]}, - {"t":0.19484, "x":1.17751, "y":1.06122, "heading":0.93781, "vx":0.57596, "vy":0.68992, "omega":-0.05277, "ax":2.95189, "ay":3.54269, "alpha":-0.29088, "fx":[49.37914,46.99378,47.0894,49.56902], "fy":[56.98884,58.97089,58.88799,56.81699]}, - {"t":0.22732, "x":1.19777, "y":1.0855, "heading":0.93609, "vx":0.67182, "vy":0.80496, "omega":-0.06222, "ax":2.95036, "ay":3.54335, "alpha":-0.29828, "fx":[49.38485,46.93944,47.03091,49.57566], "fy":[56.97499,59.00554,58.92555,56.80172]}, - {"t":0.25979, "x":1.22115, "y":1.1135, "heading":0.93407, "vx":0.76763, "vy":0.92003, "omega":-0.07191, "ax":2.94864, "ay":3.54408, "alpha":-0.30655, "fx":[49.3915,46.87898,46.96513,49.58287], "fy":[56.9593,59.04399,58.96775,56.78481]}, - {"t":0.29226, "x":1.24763, "y":1.14525, "heading":0.93174, "vx":0.86339, "vy":1.03512, "omega":-0.08186, "ax":2.9467, "ay":3.5449, "alpha":-0.31587, "fx":[49.39921,46.81122,46.89071,49.59076], "fy":[56.94144,59.08694,59.01542,56.76596]}, - {"t":0.32474, "x":1.27722, "y":1.18073, "heading":0.92908, "vx":0.95908, "vy":1.15023, "omega":-0.09212, "ax":2.94451, "ay":3.54584, "alpha":-0.32643, "fx":[49.40818,46.73469,46.80591,49.59951], "fy":[56.92101,59.13531,59.06963,56.74474]}, - {"t":0.35721, "x":1.30992, "y":1.21996, "heading":0.92609, "vx":1.0547, "vy":1.26538, "omega":-0.10272, "ax":2.94199, "ay":3.5469, "alpha":-0.33851, "fx":[49.41863,46.64748,46.70851,49.60933], "fy":[56.8975,59.19024,59.13175,56.72061]}, - {"t":0.38969, "x":1.34572, "y":1.26292, "heading":0.92275, "vx":1.15023, "vy":1.38056, "omega":-0.11371, "ax":2.93909, "ay":3.54812, "alpha":-0.35246, "fx":[49.43087,46.54711,46.59556,49.62049], "fy":[56.87023,59.25322,59.20355,56.69285]}, - {"t":0.42216, "x":1.38462, "y":1.30962, "heading":0.91906, "vx":1.24568, "vy":1.49578, "omega":-0.12516, "ax":2.9357, "ay":3.54953, "alpha":-0.36876, "fx":[49.44531,46.4303,46.46312,49.63337], "fy":[56.8383,59.32622,59.28742,56.66052]}, - {"t":0.45463, "x":1.42662, "y":1.36006, "heading":0.915, "vx":1.34101, "vy":1.61105, "omega":-0.13713, "ax":2.93168, "ay":3.5512, "alpha":-0.38804, "fx":[49.46248,46.29253,46.30578,49.64849], "fy":[56.80049,59.41192,59.38659,56.62232]}, - {"t":0.48711, "x":1.47171, "y":1.41425, "heading":0.91054, "vx":1.43621, "vy":1.72637, "omega":-0.14973, "ax":2.92684, "ay":3.55318, "alpha":-0.41121, "fx":[49.48315,46.12755,46.1159,49.66658], "fy":[56.75509,59.514,59.50558,56.57639]}, - {"t":0.51958, "x":1.5199, "y":1.47219, "heading":0.90568, "vx":1.53126, "vy":1.84175, "omega":-0.16309, "ax":2.92092, "ay":3.55559, "alpha":-0.43958, "fx":[49.50843,45.92632,45.88234,49.68868], "fy":[56.69966,59.63771,59.6509,56.52005]}, - {"t":0.55205, "x":1.57116, "y":1.53387, "heading":0.90039, "vx":1.62611, "vy":1.95722, "omega":-0.17736, "ax":2.91349, "ay":3.55856, "alpha":-0.47511, "fx":[49.53995,45.67535,45.5882,49.71641], "fy":[56.63052,59.79081,59.83228,56.44924]}, - {"t":0.58453, "x":1.6255, "y":1.59931, "heading":0.89463, "vx":1.72072, "vy":2.07278, "omega":-0.19279, "ax":2.9039, "ay":3.56233, "alpha":-0.52092, "fx":[49.58029,45.35355,45.20652,49.75228], "fy":[56.54193,59.9852,60.06492,56.35749]}, - {"t":0.617, "x":1.68291, "y":1.6685, "heading":0.88836, "vx":1.81502, "vy":2.18846, "omega":-0.20971, "ax":2.89104, "ay":3.56727, "alpha":-0.58218, "fx":[49.63369,44.926,44.69155,49.8006], "fy":[56.4244,60.24016,60.37397,56.23384]}, - {"t":0.64948, "x":1.74338, "y":1.74144, "heading":0.88155, "vx":1.90891, "vy":2.3043, "omega":-0.22861, "ax":2.87291, "ay":3.574, "alpha":-0.66833, "fx":[49.70769,44.33052,43.95905,49.86917], "fy":[56.26103,60.58908,60.80418,56.05816]}, - {"t":0.68195, "x":1.80688, "y":1.81816, "heading":0.87413, "vx":2.0022, "vy":2.42036, "omega":-0.25032, "ax":2.84545, "ay":3.58369, "alpha":-0.79834, "fx":[49.81706,43.44457,42.8351,49.97398], "fy":[56.01852,61.09504,61.44326,55.78901]}, - {"t":0.71442, "x":1.8734, "y":1.89865, "heading":0.866, "vx":2.0946, "vy":2.53674, "omega":-0.27624, "ax":2.79899, "ay":3.5987, "alpha":-1.01715, "fx":[49.99506,41.98936,40.89501,50.15329], "fy":[55.62105,61.89272,62.48831,55.32547]}, - {"t":0.7469, "x":1.9429, "y":1.98292, "heading":0.85703, "vx":2.1855, "vy":2.6536, "omega":-0.30927, "ax":2.70368, "ay":3.62423, "alpha":-1.46284, "fx":[50.33491,39.16834,36.76957,50.52693], "fy":[54.85139,63.32528,64.47893,54.3412]}, - {"t":0.77937, "x":2.01529, "y":2.071, "heading":0.84699, "vx":2.2733, "vy":2.77129, "omega":-0.35678, "ax":2.40321, "ay":3.65954, "alpha":-2.86568, "fx":[51.22878,31.50188,22.6753,51.7458], "fy":[52.73972,66.51794,69.17468,50.87347]}, - {"t":0.81184, "x":2.09038, "y":2.16293, "heading":0.8354, "vx":2.35134, "vy":2.89013, "omega":-0.44984, "ax":-0.94116, "ay":0.62812, "alpha":-16.05331, "fx":[57.30852,-23.84208,-68.65453,-26.35664], "fy":[26.38333,64.69567,9.81523,-59.82018]}, - {"t":0.84432, "x":2.16624, "y":2.25711, "heading":0.82079, "vx":2.32077, "vy":2.91053, "omega":-0.97115, "ax":-3.21338, "ay":-3.012, "alpha":-2.52922, "fx":[-42.24707,-62.78254,-60.13013,-44.97114], "fy":[-59.27025,-37.00095,-42.45774,-58.23304]}, - {"t":0.87679, "x":2.23991, "y":2.35004, "heading":0.78926, "vx":2.21642, "vy":2.81272, "omega":-1.05328, "ax":-3.12287, "ay":-3.2835, "alpha":-1.19236, "fx":[-45.85938,-55.86328,-55.4771,-47.01186], "fy":[-58.40994,-48.92135,-49.6505,-57.73434]}, - {"t":0.90927, "x":2.31024, "y":2.43965, "heading":0.75505, "vx":2.11501, "vy":2.70609, "omega":-1.092, "ax":-3.08069, "ay":-3.3691, "alpha":-0.75118, "fx":[-46.96409,-53.25645,-53.43235,-47.80061], "fy":[-58.09763,-52.37943,-52.31907,-57.51705]}, - {"t":0.94174, "x":2.3773, "y":2.52575, "heading":0.71959, "vx":2.01497, "vy":2.59668, "omega":-1.11639, "ax":-3.0575, "ay":-3.41074, "alpha":-0.52944, "fx":[-47.51267,-51.92147,-52.28533,-48.21772], "fy":[-57.92649,-54.00176,-53.71125,-57.3971]}, - {"t":0.97421, "x":2.44112, "y":2.60828, "heading":0.68334, "vx":1.91568, "vy":2.48593, "omega":-1.13359, "ax":-3.04292, "ay":-3.43533, "alpha":-0.39547, "fx":[-47.84784,-51.11643,-51.54695,-48.47288], "fy":[-57.81303,-54.93781,-54.57032,-57.32355]}, - {"t":1.00669, "x":2.50173, "y":2.68719, "heading":0.64653, "vx":1.81687, "vy":2.37437, "omega":-1.14643, "ax":-3.03293, "ay":-3.45156, "alpha":-0.30557, "fx":[-48.07824,-50.58188,-51.02842,-48.64215], "fy":[-57.72911,-55.54349,-55.15652,-57.27632]}, - {"t":1.03916, "x":2.55913, "y":2.76248, "heading":0.6093, "vx":1.71838, "vy":2.26228, "omega":-1.15635, "ax":-3.02566, "ay":-3.46305, "alpha":-0.24098, "fx":[-48.24916,-50.20415,-50.64179,-48.75996], "fy":[-57.66257,-55.96471,-55.58432,-57.24577]}, - {"t":1.07163, "x":2.61334, "y":2.83412, "heading":0.57175, "vx":1.62012, "vy":2.14982, "omega":-1.16418, "ax":-3.02013, "ay":-3.47163, "alpha":-0.1923, "fx":[-48.38288,-49.92562,-50.34067,-48.84428], "fy":[-57.60728,-56.27233,-55.91186,-57.22663]}, - {"t":1.10411, "x":2.66436, "y":2.9021, "heading":0.53394, "vx":1.52205, "vy":2.03709, "omega":-1.17042, "ax":-3.01578, "ay":-3.47827, "alpha":-0.15429, "fx":[-48.49161,-49.71397,-50.0983,-48.90541], "fy":[-57.55983,-56.50489,-56.1718,-57.21576]}, - {"t":1.13658, "x":2.71219, "y":2.96642, "heading":0.49593, "vx":1.42411, "vy":1.92413, "omega":-1.17543, "ax":-3.01228, "ay":-3.48356, "alpha":-0.12377, "fx":[-48.5826,-49.54964,-49.89814,-48.94972], "fy":[-57.51817,-56.68518,-56.38389,-57.21113]}, - {"t":1.16906, "x":2.75685, "y":3.02706, "heading":0.45776, "vx":1.32629, "vy":1.81101, "omega":-1.17945, "ax":-3.00939, "ay":-3.48788, "alpha":-0.09875, "fx":[-48.66043,-49.42008,-49.72947,-48.98135], "fy":[-57.48099,-56.82757,-56.56076,-57.2114]}, - {"t":1.20153, "x":2.79833, "y":3.08403, "heading":0.41946, "vx":1.22857, "vy":1.69775, "omega":-1.18266, "ax":-3.00697, "ay":-3.49147, "alpha":-0.07785, "fx":[-48.72812,-49.3169,-49.58499,-49.00316], "fy":[-57.44744,-56.94152,-56.71086,-57.2156]}, - {"t":1.234, "x":2.83664, "y":3.13733, "heading":0.38106, "vx":1.13092, "vy":1.58436, "omega":-1.18519, "ax":-3.00492, "ay":-3.4945, "alpha":-0.06015, "fx":[-48.78776,-49.23421,-49.45962,-49.01715], "fy":[-57.41692,-57.03358,-56.84006,-57.22303]}, - {"t":1.26648, "x":2.87178, "y":3.18693, "heading":0.34257, "vx":1.03334, "vy":1.47088, "omega":-1.18714, "ax":-3.00315, "ay":-3.49709, "alpha":-0.04496, "fx":[-48.84082,-49.16778,-49.34965,-49.0248], "fy":[-57.38901,-57.10841,-56.95257,-57.23316]}, - {"t":1.29895, "x":2.90376, "y":3.23285, "heading":0.30402, "vx":0.93581, "vy":1.35732, "omega":-1.1886, "ax":-3.00161, "ay":-3.49934, "alpha":-0.0318, "fx":[-48.88837,-49.11451,-49.25237,-49.02722], "fy":[-57.36342,-57.1694,-57.05148,-57.24558]}, - {"t":1.33142, "x":2.93256, "y":3.27509, "heading":0.26542, "vx":0.83834, "vy":1.24368, "omega":-1.18963, "ax":-3.81652, "ay":2.35093, "alpha":-0.25802, "fx":[-62.33,-61.35979,-62.47434,-63.40749], "fy":[38.4459,40.09266,38.4359,36.75865]}, - {"t":1.35563, "x":2.95174, "y":3.30588, "heading":0.23663, "vx":0.74596, "vy":1.30059, "omega":-1.19588, "ax":-3.86263, "ay":2.03926, "alpha":-0.33379, "fx":[-63.12796,-61.97421,-63.20101,-64.2831], "fy":[33.11735,35.50522,33.56629,31.16339]}, - {"t":1.37983, "x":2.96866, "y":3.33796, "heading":0.20768, "vx":0.65247, "vy":1.34995, "omega":-1.20396, "ax":-3.57446, "ay":1.59123, "alpha":-0.55012, "fx":[-58.18054,-56.84115,-58.7622,-59.95848], "fy":[25.18977,29.30728,26.80886,22.74815]}, - {"t":1.40404, "x":2.98341, "y":3.3711, "heading":0.17854, "vx":0.56595, "vy":1.38846, "omega":-1.21727, "ax":-1.26851, "ay":0.50096, "alpha":-0.60045, "fx":[-19.0206,-19.40083,-22.4355,-22.09378], "fy":[6.84586,10.34373,9.50364,6.06559]}, - {"t":1.42824, "x":2.99673, "y":3.40485, "heading":0.14908, "vx":0.53525, "vy":1.40059, "omega":-1.23181, "ax":0.00389, "ay":-0.00149, "alpha":1.89466, "fx":[-5.838,-4.26735,5.96427,4.39523], "fy":[4.43397,-6.01954,-4.48171,5.96975]}, - {"t":1.45245, "x":3.00969, "y":3.43875, "heading":0.11926, "vx":0.53534, "vy":1.40055, "omega":-1.18595, "ax":0.19909, "ay":-0.0765, "alpha":4.30276, "fx":[-9.90291,-6.885,16.21779,13.58918], "fy":[9.41072,-14.7391,-11.62899,11.95501]}, - {"t":1.47665, "x":3.02271, "y":3.47263, "heading":0.09055, "vx":0.54016, "vy":1.3987, "omega":-1.0818, "ax":0.18562, "ay":-0.07203, "alpha":6.38358, "fx":[-16.07992,-12.52356,21.77988,18.96166], "fy":[15.28752,-20.82647,-17.04101,17.8698]}, - {"t":1.50086, "x":3.03584, "y":3.50646, "heading":0.06437, "vx":0.54465, "vy":1.39696, "omega":-0.92729, "ax":0.14823, "ay":-0.05801, "alpha":8.10461, "fx":[-21.30461,-17.97742,25.71246,23.26245], "fy":[20.59751,-25.35945,-21.69734,22.66572]}, - {"t":1.52506, "x":3.04906, "y":3.54026, "heading":0.04193, "vx":0.54824, "vy":1.39555, "omega":-0.73112, "ax":0.11835, "ay":-0.04664, "alpha":9.50081, "fx":[-25.29715,-22.64066,28.72142,26.95549], "fy":[25.08686,-28.79521,-25.71787,26.37664]}, - {"t":1.54927, "x":3.06237, "y":3.57402, "heading":0.02423, "vx":0.55111, "vy":1.39442, "omega":-0.50116, "ax":0.09914, "ay":-0.04059, "alpha":10.63316, "fx":[-28.33279,-26.45145,31.12452,30.14247], "fy":[28.77125,-31.50944,-29.14668,29.23052]}, - {"t":1.57347, "x":3.07574, "y":3.60776, "heading":0.0121, "vx":0.55351, "vy":1.39344, "omega":-0.24379, "ax":-0.67302, "ay":-2.11174, "alpha":8.05559, "fx":[-45.34622,-29.85395,11.01706,20.17251], "fy":[-14.03968,-50.03736,-54.96752,-19.04704]}, - {"t":1.59767, "x":3.08894, "y":3.64087, "heading":0.0062, "vx":0.53722, "vy":1.34233, "omega":-0.0488, "ax":-1.66456, "ay":-4.2015, "alpha":0.44006, "fx":[-30.14719,-28.40757,-24.36183,-25.93285], "fy":[-67.46216,-68.31453,-69.82248,-69.14671]}, - {"t":1.62188, "x":3.10145, "y":3.67213, "heading":0.00502, "vx":0.49693, "vy":1.24063, "omega":-0.03815, "ax":-1.6891, "ay":-4.24512, "alpha":0.25384, "fx":[-29.31792,-28.3119,-25.93755,-26.88663], "fy":[-68.70204,-69.15327,-70.06685,-69.67655]}, - {"t":1.64608, "x":3.11298, "y":3.70092, "heading":0.00409, "vx":0.45604, "vy":1.13788, "omega":-0.03201, "ax":-1.69864, "ay":-4.25949, "alpha":0.19016, "fx":[-29.04899,-28.29541,-26.50617,-27.22762], "fy":[-69.11396,-69.44137,-70.13791,-69.84513]}, - {"t":1.67029, "x":3.12352, "y":3.72721, "heading":0.00332, "vx":0.41493, "vy":1.03478, "omega":-0.02741, "ax":-1.70416, "ay":-4.26644, "alpha":0.15803, "fx":[-28.92401,-28.29809,-26.80674,-27.41041], "fy":[-69.31626,-69.58385,-70.16815,-69.92462]}, - {"t":1.69449, "x":3.13307, "y":3.75101, "heading":0.00265, "vx":0.37368, "vy":0.93151, "omega":-0.02358, "ax":-1.70794, "ay":-4.27045, "alpha":0.13866, "fx":[-28.85582,-28.30694,-26.99603,-27.52775], "fy":[-69.43492,-69.66734,-70.18308,-69.96926]}, - {"t":1.7187, "x":3.14161, "y":3.7723, "heading":0.00208, "vx":0.33234, "vy":0.82815, "omega":-0.02022, "ax":-1.71077, "ay":-4.273, "alpha":0.12572, "fx":[-28.815,-28.31765,-27.12774,-27.61096], "fy":[-69.51215,-69.72143,-70.19097,-69.99701]}, - {"t":1.7429, "x":3.14916, "y":3.7911, "heading":0.00159, "vx":0.29093, "vy":0.72472, "omega":-0.01718, "ax":-1.71299, "ay":-4.27475, "alpha":0.11645, "fx":[-28.78895,-28.3285,-27.22536,-27.67367], "fy":[-69.56606,-69.75896,-70.19524,-70.01552]}, - {"t":1.76711, "x":3.1557, "y":3.80739, "heading":0.00118, "vx":0.24947, "vy":0.62126, "omega":-0.01436, "ax":-1.71478, "ay":-4.276, "alpha":0.10949, "fx":[-28.77145,-28.33872,-27.30084,-27.72284], "fy":[-69.60566,-69.78637,-70.19757,-70.02853]}, - {"t":1.79131, "x":3.16123, "y":3.82117, "heading":0.00083, "vx":0.20796, "vy":0.51776, "omega":-0.01171, "ax":-1.71626, "ay":-4.27695, "alpha":0.10407, "fx":[-28.75915,-28.34798,-27.36098,-27.76245], "fy":[-69.63594,-69.80721,-70.19882,-70.0381]}, - {"t":1.81552, "x":3.16576, "y":3.83245, "heading":0.00055, "vx":0.16642, "vy":0.41424, "omega":-0.00919, "ax":-1.7175, "ay":-4.27769, "alpha":0.09973, "fx":[-28.7501,-28.35618,-27.40996,-27.79496], "fy":[-69.65984,-69.8236,-70.19949,-70.04542]}, - {"t":1.83972, "x":3.16929, "y":3.84122, "heading":0.00033, "vx":0.12485, "vy":0.3107, "omega":-0.00678, "ax":-1.71853, "ay":-4.27828, "alpha":0.09618, "fx":[-28.74314,-28.36332,-27.4505,-27.82203], "fy":[-69.67922,-69.83687,-70.19984,-70.05121]}, - {"t":1.86392, "x":3.17181, "y":3.84749, "heading":0.00016, "vx":0.08325, "vy":0.20714, "omega":-0.00445, "ax":-1.71941, "ay":-4.27877, "alpha":0.09322, "fx":[-28.73754,-28.36945,-27.48452,-27.84482], "fy":[-69.69528,-69.84786,-70.20004,-70.05595]}, - {"t":1.88813, "x":3.17332, "y":3.85125, "heading":0.00005, "vx":0.04164, "vy":0.10358, "omega":-0.0022, "ax":-1.72016, "ay":-4.27918, "alpha":0.09072, "fx":[-28.73289,-28.3747,-27.5134,-27.8642], "fy":[-69.70885,-69.85715,-70.20017,-70.0599]}, - {"t":1.91233, "x":3.17382, "y":3.8525, "heading":0.0, "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":1.12138, "y":0.99402, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.91939, "ay":3.57297, "alpha":-0.16326, "fx":[48.37039,47.00801,47.06734,48.45991], "fy":[57.88742,58.99896,58.94891,57.80972]}, + {"t":0.0333, "x":1.123, "y":0.996, "heading":0.942, "vx":0.09721, "vy":0.11898, "omega":-0.00544, "ax":2.91887, "ay":3.57306, "alpha":-0.16632, "fx":[48.37405,46.98637,47.04632,48.46533], "fy":[57.87922,59.01114,58.96045,57.79985]}, + {"t":0.0666, "x":1.12786, "y":1.00194, "heading":0.94182, "vx":0.19441, "vy":0.23796, "omega":-0.01097, "ax":2.91831, "ay":3.57315, "alpha":-0.16966, "fx":[48.37817,46.9629,47.02325,48.47114], "fy":[57.87017,59.02432,58.97315,57.78918]}, + {"t":0.0999, "x":1.13595, "y":1.01185, "heading":0.94145, "vx":0.29159, "vy":0.35694, "omega":-0.01662, "ax":2.9177, "ay":3.57325, "alpha":-0.17332, "fx":[48.38281,46.93731,46.99783,48.47738], "fy":[57.86016,59.03864,58.98717,57.77758]}, + {"t":0.1332, "x":1.14728, "y":1.02571, "heading":0.9409, "vx":0.38874, "vy":0.47592, "omega":-0.0224, "ax":2.91702, "ay":3.57337, "alpha":-0.17734, "fx":[48.38804,46.90929,46.96971,48.48413], "fy":[57.84904,59.05429,59.00269,57.76491]}, + {"t":0.1665, "x":1.16184, "y":1.04354, "heading":0.94015, "vx":0.48588, "vy":0.59491, "omega":-0.0283, "ax":2.91628, "ay":3.57349, "alpha":-0.18179, "fx":[48.39394,46.87846,46.93848,48.49149], "fy":[57.83665,59.07146,59.01995,57.75099]}, + {"t":0.19979, "x":1.17963, "y":1.06533, "heading":0.93921, "vx":0.58299, "vy":0.71391, "omega":-0.03435, "ax":2.91545, "ay":3.57363, "alpha":-0.18674, "fx":[48.40062,46.84433,46.90361,48.49955], "fy":[57.82278,59.09042,59.03922,57.7356]}, + {"t":0.23309, "x":1.20066, "y":1.09109, "heading":0.93807, "vx":0.68007, "vy":0.83291, "omega":-0.04057, "ax":2.91452, "ay":3.57378, "alpha":-0.19226, "fx":[48.40821,46.80632,46.86449,48.50846], "fy":[57.8072,59.11149,59.06085,57.71848]}, + {"t":0.26639, "x":1.22492, "y":1.12081, "heading":0.93672, "vx":0.77712, "vy":0.95191, "omega":-0.04698, "ax":2.91348, "ay":3.57395, "alpha":-0.19848, "fx":[48.41685,46.76369,46.82031,48.51838], "fy":[57.78958,59.13506,59.08528,57.6993]}, + {"t":0.29969, "x":1.25242, "y":1.15448, "heading":0.93515, "vx":0.87413, "vy":1.07092, "omega":-0.05358, "ax":2.91229, "ay":3.57414, "alpha":-0.20553, "fx":[48.42674,46.71553,46.77006,48.52952], "fy":[57.76953,59.16162,59.11304,57.67763]}, + {"t":0.33299, "x":1.28314, "y":1.19213, "heading":0.93337, "vx":0.97111, "vy":1.18993, "omega":-0.06043, "ax":2.91094, "ay":3.57436, "alpha":-0.21358, "fx":[48.43815,46.66063,46.71244,48.54216], "fy":[57.74655,59.19182,59.14484,57.65291]}, + {"t":0.36629, "x":1.31709, "y":1.23373, "heading":0.93136, "vx":1.06804, "vy":1.30896, "omega":-0.06754, "ax":2.90938, "ay":3.57461, "alpha":-0.22288, "fx":[48.45139,46.59746,46.64574,48.55666], "fy":[57.71997,59.22647,59.18161,57.62445]}, + {"t":0.39959, "x":1.35427, "y":1.2793, "heading":0.92911, "vx":1.16492, "vy":1.42799, "omega":-0.07496, "ax":2.90756, "ay":3.5749, "alpha":-0.23372, "fx":[48.46692,46.52394,46.5677,48.57349], "fy":[57.68893,59.26669,59.22454,57.59127]}, + {"t":0.43289, "x":1.39467, "y":1.32883, "heading":0.92661, "vx":1.26174, "vy":1.54703, "omega":-0.08274, "ax":2.9054, "ay":3.57524, "alpha":-0.24654, "fx":[48.48532,46.43728,46.47519,48.59331], "fy":[57.65222,59.31394,59.27532,57.55207]}, + {"t":0.46619, "x":1.4383, "y":1.38233, "heading":0.92386, "vx":1.35849, "vy":1.66608, "omega":-0.09095, "ax":2.90281, "ay":3.57564, "alpha":-0.26192, "fx":[48.50742,46.33356,46.36383,48.61703], "fy":[57.60818,59.3703,59.33625,57.50501]}, + {"t":0.49949, "x":1.48514, "y":1.43979, "heading":0.92083, "vx":1.45515, "vy":1.78515, "omega":-0.09968, "ax":2.89965, "ay":3.57612, "alpha":-0.28073, "fx":[48.53443,46.20715,46.22726,48.64598], "fy":[57.55442,59.43869,59.41068,57.44743]}, + {"t":0.53278, "x":1.5352, "y":1.50122, "heading":0.91751, "vx":1.5517, "vy":1.90423, "omega":-0.10902, "ax":2.89569, "ay":3.57671, "alpha":-0.30423, "fx":[48.56813,46.04964,46.05591,48.68215], "fy":[57.48737,59.52348,59.50359,57.3753]}, + {"t":0.56608, "x":1.58848, "y":1.56661, "heading":0.91388, "vx":1.64813, "vy":2.02333, "omega":-0.11915, "ax":2.89059, "ay":3.57745, "alpha":-0.33445, "fx":[48.61127,45.84793,45.83462,48.72867], "fy":[57.40145,59.63138,59.62279,57.2823]}, + {"t":0.59938, "x":1.64496, "y":1.63597, "heading":0.90991, "vx":1.74438, "vy":2.14245, "omega":-0.13029, "ax":2.88378, "ay":3.5784, "alpha":-0.37474, "fx":[48.66843,45.58035,45.53794,48.79077], "fy":[57.28746,59.77335,59.78119,57.15777]}, + {"t":0.63268, "x":1.70465, "y":1.70929, "heading":0.90557, "vx":1.84041, "vy":2.26161, "omega":-0.14277, "ax":2.87424, "ay":3.57965, "alpha":-0.43113, "fx":[48.74767,45.20835,45.1196,48.87788], "fy":[57.12903,59.96852,60.00179,56.98242]}, + {"t":0.66598, "x":1.76753, "y":1.78659, "heading":0.90082, "vx":1.93612, "vy":2.38081, "omega":-0.15713, "ax":2.8599, "ay":3.58138, "alpha":-0.51565, "fx":[48.86471,44.65614,44.48577,49.00888], "fy":[56.89399,60.25358,60.32991,56.71718]}, + {"t":0.69928, "x":1.83358, "y":1.86785, "heading":0.89558, "vx":2.03135, "vy":2.50007, "omega":-0.1743, "ax":2.83592, "ay":3.58384, "alpha":-0.6563, "fx":[49.05481,43.75159,43.41327,49.22789], "fy":[56.50937,60.70859,60.86854,56.26934]}, + {"t":0.73258, "x":1.9028, "y":1.95309, "heading":0.88978, "vx":2.12578, "vy":2.61941, "omega":-0.19615, "ax":2.78774, "ay":3.58722, "alpha":-0.93678, "fx":[49.41616,42.00279,41.21131,49.667], "fy":[55.76691,61.547,61.91043,55.35276]}, + {"t":0.76588, "x":1.97513, "y":2.0423, "heading":0.88325, "vx":2.21861, "vy":2.73886, "omega":-0.22734, "ax":2.64217, "ay":3.58506, "alpha":-1.77334, "fx":[50.36004,37.23191,34.20914,50.9767], "fy":[53.74271,63.56893,64.68414,52.43971]}, + {"t":0.79918, "x":2.05047, "y":2.13549, "heading":0.87568, "vx":2.30659, "vy":2.85824, "omega":-0.2864, "ax":-0.19101, "ay":0.85926, "alpha":-15.65567, "fx":[57.11884,-8.79183,-62.54868,1.73121], "fy":[29.27025,66.40467,17.93266,-57.41844]}, + {"t":0.83248, "x":2.12717, "y":2.23114, "heading":0.86614, "vx":2.30023, "vy":2.88685, "omega":-0.80771, "ax":-3.00913, "ay":-3.26547, "alpha":-1.80257, "fx":[-41.43195,-56.89172,-55.36033,-43.09038], "fy":[-60.00068,-45.66056,-48.3497,-59.52585]}, + {"t":0.86578, "x":2.2021, "y":2.32546, "heading":0.83925, "vx":2.20003, "vy":2.77811, "omega":-0.86774, "ax":-2.9793, "ay":-3.42861, "alpha":-0.83721, "fx":[-45.0095,-52.18838,-52.0042,-45.62139], "fy":[-59.15017,-52.92053,-53.28736,-58.84676]}, + {"t":0.89907, "x":2.27371, "y":2.41607, "heading":0.81035, "vx":2.10082, "vy":2.66394, "omega":-0.89562, "ax":-2.96513, "ay":-3.47831, "alpha":-0.52854, "fx":[-46.08497,-50.59106,-50.69646,-46.52426], "fy":[-58.85298,-55.02293,-55.00215,-58.57711]}, + {"t":0.93237, "x":2.34202, "y":2.50285, "heading":0.78053, "vx":2.00209, "vy":2.54812, "omega":-0.91322, "ax":-2.95735, "ay":-3.50231, "alpha":-0.3753, "fx":[-46.61042,-49.79256,-49.99694,-46.98811], "fy":[-58.69666,-56.01868,-55.87638,-58.43248]}, + {"t":0.96567, "x":2.40705, "y":2.58576, "heading":0.75012, "vx":1.90361, "vy":2.43149, "omega":-0.92571, "ax":-2.95247, "ay":-3.51643, "alpha":-0.28334, "fx":[-46.92589,-49.31514,-49.55815,-47.26959], "fy":[-58.59725,-56.59804,-56.40927,-58.34311]}, + {"t":0.99897, "x":2.4688, "y":2.66477, "heading":0.71929, "vx":1.8053, "vy":2.3144, "omega":-0.93515, "ax":-2.94912, "ay":-3.52573, "alpha":-0.22191, "fx":[-47.13875,-48.99893,-49.25506,-47.45739], "fy":[-58.52664,-56.97582,-56.76998,-58.28338]}, + {"t":1.03227, "x":2.52728, "y":2.73989, "heading":0.68815, "vx":1.70709, "vy":2.197, "omega":-0.94254, "ax":-2.94669, "ay":-3.53232, "alpha":-0.17791, "fx":[-47.29367,-48.7753,-49.03165,-47.59046], "fy":[-58.47273,-57.24057,-57.03164,-58.24157]}, + {"t":1.06557, "x":2.58249, "y":2.81108, "heading":0.65677, "vx":1.60897, "vy":2.07937, "omega":-0.94846, "ax":-2.94484, "ay":-3.53722, "alpha":-0.14481, "fx":[-47.41258,-48.60988,-48.8591,-47.68866], "fy":[-58.42945,-57.43549,-57.23104,-58.21153]}, + {"t":1.09887, "x":2.63443, "y":2.87836, "heading":0.62519, "vx":1.51091, "vy":1.96159, "omega":-0.95328, "ax":-2.94339, "ay":-3.54102, "alpha":-0.11901, "fx":[-47.50751,-48.48351,-48.72107,-47.76317], "fy":[-58.3934,-57.58418,-57.38868,-58.18969]}, + {"t":1.13217, "x":2.68311, "y":2.94172, "heading":0.59344, "vx":1.4129, "vy":1.84368, "omega":-0.95725, "ax":-2.94222, "ay":-3.54405, "alpha":-0.09831, "fx":[-47.58562,-48.38468,-48.6076,-47.8208], "fy":[-58.36255,-57.70064,-57.51691,-58.17385]}, + {"t":1.16547, "x":2.72853, "y":3.00115, "heading":0.56157, "vx":1.31493, "vy":1.72566, "omega":-0.96052, "ax":-2.94125, "ay":-3.54652, "alpha":-0.08135, "fx":[-47.65142,-48.30604,-48.51226,-47.86594], "fy":[-58.33558,-57.79369,-57.62362,-58.16256]}, + {"t":1.19877, "x":2.77069, "y":3.05664, "heading":0.52958, "vx":1.21699, "vy":1.60757, "omega":-0.96323, "ax":-2.94045, "ay":-3.54857, "alpha":-0.06718, "fx":[-47.70789,-48.24266,-48.43074,-47.90154], "fy":[-58.31164,-57.86917,-57.71405,-58.15481]}, + {"t":1.23206, "x":2.80958, "y":3.10821, "heading":0.49751, "vx":1.11907, "vy":1.4894, "omega":-0.96547, "ax":-2.93976, "ay":-3.55031, "alpha":-0.05519, "fx":[-47.7571,-48.19112,-48.36002,-47.92968], "fy":[-58.29011,-57.93113,-57.79185,-58.1499]}, + {"t":1.26536, "x":2.84521, "y":3.15584, "heading":0.46536, "vx":1.02118, "vy":1.37118, "omega":-0.9673, "ax":-2.93917, "ay":-3.55179, "alpha":-0.04489, "fx":[-47.80051,-48.14899,-48.29795,-47.95185], "fy":[-58.27058,-57.98242,-57.85962,-58.14731]}, + {"t":1.29866, "x":2.87759, "y":3.19953, "heading":0.43315, "vx":0.92331, "vy":1.25291, "omega":-0.9688, "ax":-2.93866, "ay":-3.55307, "alpha":-0.03597, "fx":[-47.83919,-48.11443,-48.24292,-47.96916], "fy":[-58.25274,-58.02516,-57.91928,-58.14665]}, + {"t":1.33196, "x":2.90671, "y":3.23928, "heading":0.40089, "vx":0.82546, "vy":1.1346, "omega":-0.97, "ax":-2.9382, "ay":-3.55419, "alpha":-0.02816, "fx":[-47.87393,-48.0861,-48.19374,-47.98244], "fy":[-58.23634,-58.06092,-57.97226,-58.1476]}, + {"t":1.36526, "x":2.93256, "y":3.27509, "heading":0.36859, "vx":0.72762, "vy":1.01624, "omega":-0.97093, "ax":-3.70783, "ay":2.38395, "alpha":-0.46714, "fx":[-60.22353,-58.75353,-61.04054,-62.44625], "fy":[39.34989,41.85646,38.67566,36.01005]}, + {"t":1.39182, "x":2.95058, "y":3.30291, "heading":0.3428, "vx":0.62915, "vy":1.07955, "omega":-0.98334, "ax":-3.64564, "ay":1.9166, "alpha":-0.92419, "fx":[-58.93726,-56.53613,-60.4259,-62.49761], "fy":[31.23966,37.01893,31.56461,25.508]}, + {"t":1.41837, "x":2.966, "y":3.33226, "heading":0.31669, "vx":0.53234, "vy":1.13045, "omega":-1.00788, "ax":-2.64566, "ay":1.14839, "alpha":-2.67439, "fx":[-37.78713,-37.21506,-48.73048,-49.27361], "fy":[15.09043,32.01479,22.13747,5.85301]}, + {"t":1.44493, "x":2.9792, "y":3.36268, "heading":0.28993, "vx":0.46209, "vy":1.16094, "omega":-1.0789, "ax":-0.54339, "ay":0.21241, "alpha":-4.2358, "fx":[5.70356,-1.48195,-22.76198,-16.99302], "fy":[-4.75829,18.58393,11.18526,-11.12092]}, + {"t":1.47148, "x":2.99128, "y":3.39358, "heading":0.26128, "vx":0.44766, "vy":1.16658, "omega":-1.19138, "ax":0.07099, "ay":-0.02731, "alpha":-2.71268, "fx":[10.2494,6.38144,-7.96615,-4.0223], "fy":[-5.82701,8.76402,4.96478,-9.68773]}, + {"t":1.49804, "x":3.0032, "y":3.42455, "heading":0.22964, "vx":0.44954, "vy":1.16586, "omega":-1.26342, "ax":0.13728, "ay":-0.05318, "alpha":-0.94102, "fx":[5.33597,4.15372,-0.85591,0.34306], "fy":[-2.83596,2.27121,1.10482,-4.01772]}, + {"t":1.52459, "x":3.01518, "y":3.45549, "heading":0.19609, "vx":0.45319, "vy":1.16444, "omega":-1.28841, "ax":0.11671, "ay":-0.0456, "alpha":0.87217, "fx":[-0.90229,0.04857,4.71259,3.77323], "fy":[1.17926,-3.60115,-2.66432,2.10409]}, + {"t":1.55115, "x":3.02726, "y":3.4864, "heading":0.16188, "vx":0.45629, "vy":1.16323, "omega":-1.26525, "ax":0.092, "ay":-0.0362, "alpha":2.64552, "fx":[-6.82958,-4.41718,9.79886,7.46405], "fy":[5.54942,-9.06648,-6.68681,7.83664]}, + {"t":1.5777, "x":3.03941, "y":3.51728, "heading":0.12828, "vx":0.45873, "vy":1.16227, "omega":-1.19499, "ax":0.07318, "ay":-0.02896, "alpha":4.30333, "fx":[-12.02652,-8.88468,14.34405,11.35276], "fy":[9.9795,-13.93708,-10.8238,12.88791]}, + {"t":1.60426, "x":3.05161, "y":3.54813, "heading":0.09654, "vx":0.46067, "vy":1.1615, "omega":-1.08072, "ax":0.05927, "ay":-0.02355, "alpha":5.79995, "fx":[-16.40121,-13.18429,18.23763,15.22342], "fy":[14.27013,-18.09674,-14.883,17.16943]}, + {"t":1.63081, "x":3.06387, "y":3.57897, "heading":0.06785, "vx":0.46225, "vy":1.16088, "omega":-0.9267, "ax":0.04871, "ay":-0.01943, "alpha":7.1206, "fx":[-20.00344,-17.19814,21.48148,18.90534], "fy":[18.28186,-21.55554,-18.71567,20.71894]}, + {"t":1.65737, "x":3.07616, "y":3.60979, "heading":0.04324, "vx":0.46354, "vy":1.16036, "omega":-0.73761, "ax":0.04085, "ay":-0.01634, "alpha":8.27167, "fx":[-22.95211,-20.84037,24.1691,22.29472], "fy":[21.92538,-24.41363,-22.22593,23.64557]}, + {"t":1.68392, "x":3.08848, "y":3.64059, "heading":0.02365, "vx":0.46463, "vy":1.15993, "omega":-0.51796, "ax":0.03643, "ay":-0.01486, "alpha":9.27005, "fx":[-25.38219,-24.03554,26.44848,25.35141], "fy":[25.14297,-26.82686,-25.36231,26.07468]}, + {"t":1.71048, "x":3.10084, "y":3.67139, "heading":0.0099, "vx":0.46559, "vy":1.15953, "omega":-0.27179, "ax":-0.3495, "ay":-1.00775, "alpha":9.30609, "fx":[-36.79367,-28.04746,17.75166,24.23462], "fy":[7.25963,-39.86105,-42.4654,9.16752]}, + {"t":1.73703, "x":3.11308, "y":3.70183, "heading":0.00268, "vx":0.45631, "vy":1.13277, "omega":-0.02467, "ax":-1.69457, "ay":-4.22029, "alpha":0.24467, "fx":[-29.3347,-28.37449,-26.09739,-27.00538], "fy":[-68.31646,-68.76259,-69.64284,-69.25266]}, + {"t":1.76359, "x":3.1246, "y":3.73042, "heading":0.00202, "vx":0.41131, "vy":1.0207, "omega":-0.01817, "ax":-1.71015, "ay":-4.25293, "alpha":0.13735, "fx":[-28.87903,-28.33795,-27.04476,-27.56918], "fy":[-69.15032,-69.38571,-69.89531,-69.67801]}, + {"t":1.79014, "x":3.13492, "y":3.75603, "heading":0.00154, "vx":0.3659, "vy":0.90777, "omega":-0.01452, "ax":-1.7159, "ay":-4.2636, "alpha":0.10155, "fx":[-28.73412,-28.33373,-27.37383,-27.76507], "fy":[-69.42361,-69.59388,-69.97472,-69.81443]}, + {"t":1.8167, "x":3.14403, "y":3.77863, "heading":0.00115, "vx":0.32033, "vy":0.79454, "omega":-0.01182, "ax":-1.71909, "ay":-4.2688, "alpha":0.08367, "fx":[-28.6665,-28.33658,-27.54422,-27.86792], "fy":[-69.55789,-69.69659,-70.0121,-69.88019]}, + {"t":1.84325, "x":3.15193, "y":3.79822, "heading":0.00084, "vx":0.27468, "vy":0.68119, "omega":-0.0096, "ax":-1.72119, "ay":-4.27184, "alpha":0.07294, "fx":[-28.6289,-28.34129,-27.64973,-27.93261], "fy":[-69.6371,-69.7572,-70.0332,-69.91828]}, + {"t":1.86981, "x":3.15861, "y":3.81481, "heading":0.00059, "vx":0.22898, "vy":0.56775, "omega":-0.00767, "ax":-1.7227, "ay":-4.27382, "alpha":0.06579, "fx":[-28.60562,-28.34623,-27.72198,-27.97752], "fy":[-69.68913,-69.79696,-70.04649,-69.94288]}, + {"t":1.89636, "x":3.16409, "y":3.82838, "heading":0.00038, "vx":0.18323, "vy":0.45426, "omega":-0.00592, "ax":-1.72384, "ay":-4.27521, "alpha":0.06068, "fx":[-28.59003,-28.35078,-27.77466,-28.01063], "fy":[-69.72584,-69.82498,-70.05554,-69.96]}, + {"t":1.92292, "x":3.16835, "y":3.83893, "heading":0.00023, "vx":0.13745, "vy":0.34073, "omega":-0.00431, "ax":-1.72473, "ay":-4.27624, "alpha":0.05685, "fx":[-28.57888,-28.35474,-27.81474,-28.036], "fy":[-69.75313,-69.84579,-70.06209,-69.97259]}, + {"t":1.94947, "x":3.17139, "y":3.84647, "heading":0.00011, "vx":0.09165, "vy":0.22717, "omega":-0.0028, "ax":-1.72544, "ay":-4.27703, "alpha":0.05388, "fx":[-28.57046,-28.35806,-27.84619,-28.056], "fy":[-69.77424,-69.86189,-70.06708,-69.98227]}, + {"t":1.97603, "x":3.17321, "y":3.851, "heading":0.00004, "vx":0.04583, "vy":0.11359, "omega":-0.00137, "ax":-1.72601, "ay":-4.27767, "alpha":0.05149, "fx":[-28.56382,-28.3608,-27.87143,-28.07209], "fy":[-69.79109,-69.87474,-70.07103,-69.98997]}, + {"t":2.00258, "x":3.17382, "y":3.8525, "heading":0.0, "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/PROtoC.traj b/src/main/deploy/choreo/PROtoC.traj index fe4a8dcc..b079e800 100644 --- a/src/main/deploy/choreo/PROtoC.traj +++ b/src/main/deploy/choreo/PROtoC.traj @@ -10,7 +10,7 @@ {"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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -22,7 +22,7 @@ {"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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,59 +30,60 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.21555,1.79446], + "waypoints":[0.0,1.2487,1.88374], "samples":[ - {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.04231, "ay":3.47028, "alpha":0.01308, "fx":[49.68719,49.79422,49.78468,49.67782], "fy":[56.77512,56.68129,56.68987,56.78351]}, - {"t":0.03799, "x":1.63553, "y":0.62718, "heading":0.942, "vx":0.11556, "vy":0.13182, "omega":0.0005, "ax":3.04214, "ay":3.47009, "alpha":0.01333, "fx":[49.6834,49.79246,49.78274,49.67386], "fy":[56.77277,56.67716,56.68591,56.78133]}, - {"t":0.07597, "x":1.64211, "y":0.63469, "heading":0.94202, "vx":0.23112, "vy":0.26364, "omega":0.001, "ax":3.04194, "ay":3.46987, "alpha":0.01361, "fx":[49.67918,49.79051,49.78059,49.66946], "fy":[56.77017,56.67257,56.68152,56.7789]}, - {"t":0.11396, "x":1.65309, "y":0.64721, "heading":0.94206, "vx":0.34667, "vy":0.39544, "omega":0.00152, "ax":3.04172, "ay":3.46963, "alpha":0.01391, "fx":[49.67448,49.78833,49.77819,49.66454], "fy":[56.76725,56.66744,56.6766,56.77619]}, - {"t":0.15194, "x":1.66845, "y":0.66473, "heading":0.94211, "vx":0.46222, "vy":0.52724, "omega":0.00205, "ax":3.04148, "ay":3.46936, "alpha":0.01426, "fx":[49.66918,49.78587,49.77548,49.659], "fy":[56.76397,56.66167,56.67108,56.77315]}, - {"t":0.18993, "x":1.6882, "y":0.68726, "heading":0.94219, "vx":0.57775, "vy":0.65903, "omega":0.00259, "ax":3.0412, "ay":3.46906, "alpha":0.01466, "fx":[49.66318,49.78309,49.77241,49.65272], "fy":[56.76024,56.65513,56.66481,56.76969]}, - {"t":0.22792, "x":1.71234, "y":0.7148, "heading":0.94229, "vx":0.69327, "vy":0.7908, "omega":0.00315, "ax":3.04088, "ay":3.46871, "alpha":0.01511, "fx":[49.65632,49.77991,49.76889,49.64554], "fy":[56.75599,56.64765,56.65765,56.76574]}, - {"t":0.2659, "x":1.74087, "y":0.74734, "heading":0.94241, "vx":0.80878, "vy":0.92256, "omega":0.00372, "ax":3.04052, "ay":3.4683, "alpha":0.01563, "fx":[49.6484,49.77624,49.76484,49.63725], "fy":[56.75108,56.63901,56.64939,56.76119]}, - {"t":0.30389, "x":1.77379, "y":0.78489, "heading":0.94255, "vx":0.92428, "vy":1.05431, "omega":0.00431, "ax":3.04009, "ay":3.46783, "alpha":0.01624, "fx":[49.63916,49.77197,49.76011,49.62758], "fy":[56.74534,56.62893,56.63975,56.75588]}, - {"t":0.34187, "x":1.81109, "y":0.82744, "heading":0.94272, "vx":1.03976, "vy":1.18604, "omega":0.00493, "ax":3.03959, "ay":3.46728, "alpha":0.01696, "fx":[49.62823,49.76691,49.75452,49.61615], "fy":[56.73856,56.61701,56.62836,56.7496]}, - {"t":0.37986, "x":1.85278, "y":0.87499, "heading":0.9429, "vx":1.15522, "vy":1.31774, "omega":0.00558, "ax":3.03898, "ay":3.46661, "alpha":0.01782, "fx":[49.61511,49.76083,49.74781,49.60242], "fy":[56.73042,56.6027,56.61469,56.74206]}, - {"t":0.41784, "x":1.89885, "y":0.92755, "heading":0.94311, "vx":1.27066, "vy":1.44943, "omega":0.00625, "ax":3.03824, "ay":3.46579, "alpha":0.01888, "fx":[49.59907,49.75341,49.73961,49.58564], "fy":[56.72047,56.5852,56.59797,56.73285]}, - {"t":0.45583, "x":1.94931, "y":0.98511, "heading":0.94335, "vx":1.38607, "vy":1.58108, "omega":0.00697, "ax":3.03731, "ay":3.46477, "alpha":0.0202, "fx":[49.57901,49.74413,49.72935,49.56466], "fy":[56.70802,56.56331,56.57706,56.72133]}, - {"t":0.49382, "x":2.00416, "y":1.04767, "heading":0.94362, "vx":1.50144, "vy":1.71269, "omega":0.00774, "ax":3.03612, "ay":3.46346, "alpha":0.0219, "fx":[49.55318,49.73218,49.71616,49.53767], "fy":[56.692,56.53514,56.55017,56.70651]}, - {"t":0.5318, "x":2.06338, "y":1.11522, "heading":0.94391, "vx":1.61677, "vy":1.84425, "omega":0.00857, "ax":3.03453, "ay":3.46171, "alpha":0.02418, "fx":[49.51872,49.71624,49.69857,49.50166], "fy":[56.67062,56.49754,56.5143,56.68675]}, - {"t":0.56979, "x":2.12698, "y":1.18778, "heading":0.94424, "vx":1.73204, "vy":1.97575, "omega":0.00949, "ax":3.0323, "ay":3.45925, "alpha":0.02737, "fx":[49.47039,49.69389,49.67393,49.45121], "fy":[56.64065,56.44482,56.46406,56.65906]}, - {"t":0.60777, "x":2.19496, "y":1.26532, "heading":0.9446, "vx":1.84723, "vy":2.10715, "omega":0.01053, "ax":3.02895, "ay":3.45556, "alpha":0.03217, "fx":[49.39776,49.66031,49.63694,49.37546], "fy":[56.5956,56.3656,56.38862,56.61749]}, - {"t":0.64576, "x":2.26732, "y":1.34786, "heading":0.945, "vx":1.96228, "vy":2.23841, "omega":0.01175, "ax":3.02336, "ay":3.4494, "alpha":0.04021, "fx":[49.27633,49.60416,49.57523,49.24907], "fy":[56.52028,56.23318,56.26276,56.5481]}, - {"t":0.68375, "x":2.34404, "y":1.43537, "heading":0.94544, "vx":2.07713, "vy":2.36944, "omega":0.01328, "ax":3.01213, "ay":3.43702, "alpha":0.05642, "fx":[49.03217,49.49115,49.45148,48.99578], "fy":[56.36868,55.96697,56.01061,56.40886]}, - {"t":0.72173, "x":2.42511, "y":1.52786, "heading":0.94595, "vx":2.19155, "vy":2.5, "omega":0.01542, "ax":2.97811, "ay":3.39952, "alpha":0.10592, "fx":[48.28977,49.14591,49.07746,48.23264], "fy":[55.90589,55.158,55.25129,55.98727]}, - {"t":0.75972, "x":2.51051, "y":1.62528, "heading":0.94653, "vx":2.30467, "vy":2.62913, "omega":0.01944, "ax":-1.36098, "ay":-1.50717, "alpha":2.87407, "fx":[-32.5474,-18.85456,-10.60506,-26.99077], "fy":[-24.62425,-35.2734,-25.65908,-13.00052]}, - {"t":0.7977, "x":2.59707, "y":1.72406, "heading":0.94727, "vx":2.25298, "vy":2.57188, "omega":0.12862, "ax":-2.98382, "ay":-3.40096, "alpha":0.08748, "fx":[-49.10221,-48.40352,-48.45361,-49.16], "fy":[-55.33068,-55.94047,-55.87179,-55.25386]}, - {"t":0.83569, "x":2.6805, "y":1.8193, "heading":0.95216, "vx":2.13963, "vy":2.44269, "omega":0.13194, "ax":-3.01422, "ay":-3.43684, "alpha":0.04114, "fx":[-49.42791,-49.09554,-49.12479,-49.45889], "fy":[-56.05668,-56.34738,-56.31579,-56.02324]}, - {"t":0.87367, "x":2.7596, "y":1.90961, "heading":0.95717, "vx":2.02514, "vy":2.31214, "omega":0.1335, "ax":-3.02458, "ay":-3.44907, "alpha":0.02529, "fx":[-49.53856,-49.33352,-49.35338,-49.55908], "fy":[-56.30611,-56.48557,-56.4657,-56.28553]}, - {"t":0.91166, "x":2.83435, "y":1.99495, "heading":0.96224, "vx":1.91024, "vy":2.18113, "omega":0.13446, "ax":-3.0298, "ay":-3.45524, "alpha":0.01724, "fx":[-49.59417,-49.45417,-49.46871,-49.60901], "fy":[-56.43234,-56.55492,-56.54092,-56.41801]}, - {"t":0.94965, "x":2.90472, "y":2.07531, "heading":0.96735, "vx":1.79515, "vy":2.04988, "omega":0.13512, "ax":-3.03295, "ay":-3.45895, "alpha":0.01235, "fx":[-49.6276,-49.5272,-49.53826,-49.6388], "fy":[-56.5086,-56.59652,-56.5861,-56.49801]}, - {"t":0.98763, "x":2.97073, "y":2.15068, "heading":0.97248, "vx":1.67995, "vy":1.91849, "omega":0.13559, "ax":-3.03505, "ay":-3.46144, "alpha":0.00906, "fx":[-49.6499,-49.57622,-49.58477,-49.65853], "fy":[-56.55965,-56.62419,-56.61625,-56.55162]}, - {"t":1.02562, "x":3.03235, "y":2.22106, "heading":0.97763, "vx":1.56466, "vy":1.787, "omega":0.13593, "ax":-3.03656, "ay":-3.46322, "alpha":0.00669, "fx":[-49.66586,-49.61143,-49.61805,-49.67252], "fy":[-56.59621,-56.64389,-56.6378,-56.59007]}, - {"t":1.0636, "x":3.08959, "y":2.28644, "heading":0.98279, "vx":1.44931, "vy":1.65545, "omega":0.13619, "ax":-3.03769, "ay":-3.46455, "alpha":0.00491, "fx":[-49.67784,-49.63796,-49.64304,-49.68294], "fy":[-56.62367,-56.65861,-56.65398,-56.61902]}, - {"t":1.10159, "x":3.14246, "y":2.34682, "heading":0.98797, "vx":1.33392, "vy":1.52384, "omega":0.13637, "ax":-3.03857, "ay":-3.46559, "alpha":0.00351, "fx":[-49.68718,-49.6587,-49.66248,-49.69098], "fy":[-56.64505,-56.67001,-56.66658,-56.64161]}, - {"t":1.13958, "x":3.19093, "y":2.40221, "heading":0.99315, "vx":1.2185, "vy":1.3922, "omega":0.13651, "ax":-3.03927, "ay":-3.46642, "alpha":0.00238, "fx":[-49.69468,-49.67535,-49.67802,-49.69736], "fy":[-56.66216,-56.6791,-56.67669,-56.65974]}, - {"t":1.17756, "x":3.23503, "y":2.45259, "heading":0.99833, "vx":1.10305, "vy":1.26052, "omega":0.1366, "ax":-3.03985, "ay":-3.4671, "alpha":0.00145, "fx":[-49.70084,-49.68903,-49.69072,-49.70253], "fy":[-56.67615,-56.6865,-56.68497,-56.67462]}, - {"t":1.21555, "x":3.27473, "y":2.49797, "heading":1.00352, "vx":0.98758, "vy":1.12882, "omega":0.13665, "ax":-0.26089, "ay":0.21318, "alpha":0.3632, "fx":[-5.65174,-3.95191,-2.87666,-4.57991], "fy":[3.18727,2.10265,3.78654,4.86415]}, - {"t":1.24771, "x":3.30636, "y":2.53439, "heading":1.00792, "vx":0.97919, "vy":1.13568, "omega":0.14833, "ax":-0.04507, "ay":0.03881, "alpha":0.1137, "fx":[-1.17068,-0.63434,-0.30289,-0.83928], "fy":[0.54106,0.20261,0.72787,1.0662]}, - {"t":1.27987, "x":3.33783, "y":2.57093, "heading":1.01269, "vx":0.97774, "vy":1.13693, "omega":0.15199, "ax":-0.00792, "ay":0.00681, "alpha":-0.13923, "fx":[0.40133,-0.25754,-0.66021,-0.00136], "fy":[0.22813,0.63945,-0.00552,-0.41688]}, - {"t":1.31203, "x":3.36927, "y":2.6075, "heading":1.01758, "vx":0.97748, "vy":1.13715, "omega":0.14751, "ax":-0.00139, "ay":0.00119, "alpha":-0.39272, "fx":[1.47276,-0.3913,-1.5182,0.34585], "fy":[0.35638,1.5075,-0.31729,-1.46846]}, - {"t":1.34419, "x":3.40071, "y":2.64407, "heading":1.02232, "vx":0.97744, "vy":1.13719, "omega":0.13488, "ax":-0.00024, "ay":0.00021, "alpha":-0.64766, "fx":[2.45966,-0.62348,-2.46764,0.61549], "fy":[0.57063,2.45445,-0.56376,-2.4476]}, - {"t":1.37636, "x":3.43214, "y":2.68065, "heading":1.02666, "vx":0.97743, "vy":1.13719, "omega":0.11405, "ax":-0.00004, "ay":0.00004, "alpha":-0.90499, "fx":[3.43834,-0.88117,-3.43975,0.87976], "fy":[0.8081,3.42169,-0.80687,-3.42048]}, - {"t":1.40852, "x":3.46358, "y":2.71722, "heading":1.03033, "vx":0.97743, "vy":1.13719, "omega":0.08494, "ax":-0.00004, "ay":-0.00003, "alpha":-1.16562, "fx":[4.42491,-1.15089,-4.42631,1.14948], "fy":[1.05579,4.40165,-1.05676,-4.40262]}, - {"t":1.44068, "x":3.49502, "y":2.7538, "heading":1.03306, "vx":0.97743, "vy":1.13719, "omega":0.04746, "ax":-0.39078, "ay":-0.45465, "alpha":-1.38378, "fx":[-1.1002,-7.90515,-11.58302,-4.96534], "fy":[-6.2748,-2.18391,-8.62876,-12.64344]}, - {"t":1.47284, "x":3.52625, "y":2.79013, "heading":1.03458, "vx":0.96486, "vy":1.12257, "omega":0.00295, "ax":-2.97232, "ay":-3.45815, "alpha":-0.02282, "fx":[-48.51414,-48.69889,-48.66903,-48.48475], "fy":[-56.59853,-56.44038,-56.47023,-56.6278]}, - {"t":1.505, "x":3.55574, "y":2.82445, "heading":1.03468, "vx":0.86926, "vy":1.01135, "omega":0.00222, "ax":-2.99198, "ay":-3.48103, "alpha":-0.01313, "fx":[-48.8686,-48.97538,-48.95755,-48.85093], "fy":[-56.94578,-56.85418,-56.87071,-56.96211]}, - {"t":1.53716, "x":3.58215, "y":2.85518, "heading":1.03475, "vx":0.77304, "vy":0.89939, "omega":0.0018, "ax":-2.99849, "ay":-3.4886, "alpha":-0.00993, "fx":[-48.98589,-49.06678,-49.05313,-48.97233], "fy":[-57.06058,-56.99114,-57.00348,-57.07281]}, - {"t":1.56933, "x":3.60546, "y":2.8823, "heading":1.03481, "vx":0.6766, "vy":0.78719, "omega":0.00148, "ax":-3.00173, "ay":-3.49238, "alpha":-0.00834, "fx":[-49.04433,-49.11231,-49.10077,-49.03286], "fy":[-57.11777,-57.0594,-57.06969,-57.12798]}, - {"t":1.60149, "x":3.62567, "y":2.90581, "heading":1.03486, "vx":0.58006, "vy":0.67487, "omega":0.00121, "ax":-3.00367, "ay":-3.49464, "alpha":-0.00738, "fx":[-49.07933,-49.13956,-49.12931,-49.06913], "fy":[-57.152,-57.10027,-57.10936,-57.16103]}, - {"t":1.63365, "x":3.64277, "y":2.92571, "heading":1.03489, "vx":0.48346, "vy":0.56248, "omega":0.00097, "ax":-3.00497, "ay":-3.49614, "alpha":-0.00675, "fx":[-49.10263,-49.15771,-49.14831,-49.09327], "fy":[-57.1748,-57.12748,-57.13577,-57.18303]}, - {"t":1.66581, "x":3.65677, "y":2.94199, "heading":1.03493, "vx":0.38681, "vy":0.45004, "omega":0.00075, "ax":-3.00589, "ay":-3.49722, "alpha":-0.0063, "fx":[-49.11926,-49.17066,-49.16187,-49.11051], "fy":[-57.19106,-57.1469,-57.15462,-57.19873]}, - {"t":1.69797, "x":3.66766, "y":2.95465, "heading":1.03495, "vx":0.29014, "vy":0.33756, "omega":0.00055, "ax":-3.00658, "ay":-3.49802, "alpha":-0.00596, "fx":[-49.13172,-49.18036,-49.17203,-49.12343], "fy":[-57.20325,-57.16146,-57.16875,-57.2105]}, - {"t":1.73013, "x":3.67543, "y":2.9637, "heading":1.03497, "vx":0.19344, "vy":0.22506, "omega":0.00036, "ax":-3.00712, "ay":-3.49865, "alpha":-0.0057, "fx":[-49.1414,-49.1879,-49.17994,-49.13347], "fy":[-57.21272,-57.17277,-57.17974,-57.21965]}, - {"t":1.7623, "x":3.6801, "y":2.96913, "heading":1.03498, "vx":0.09673, "vy":0.11254, "omega":0.00018, "ax":-3.00755, "ay":-3.49915, "alpha":-0.00548, "fx":[-49.14915,-49.19393,-49.18625,-49.1415], "fy":[-57.2203,-57.18182,-57.18852,-57.22696]}, - {"t":1.79446, "x":3.68165, "y":2.97094, "heading":1.03499, "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":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.0423, "ay":3.47042, "alpha":0.0068, "fx":[49.7105,49.76616,49.76122,49.70561], "fy":[56.75687,56.70808,56.71252,56.76125]}, + {"t":0.03902, "x":1.63565, "y":0.62732, "heading":0.942, "vx":0.11872, "vy":0.13542, "omega":0.00027, "ax":3.04212, "ay":3.47021, "alpha":0.00695, "fx":[49.70701,49.76388,49.75883,49.70201], "fy":[56.75407,56.70422,56.70876,56.75856]}, + {"t":0.07804, "x":1.6426, "y":0.63525, "heading":0.94201, "vx":0.23743, "vy":0.27084, "omega":0.00054, "ax":3.04192, "ay":3.46999, "alpha":0.00712, "fx":[49.70311,49.76133,49.75617,49.698], "fy":[56.75095,56.69991,56.70457,56.75555]}, + {"t":0.11707, "x":1.65418, "y":0.64846, "heading":0.94203, "vx":0.35613, "vy":0.40624, "omega":0.00081, "ax":3.0417, "ay":3.46974, "alpha":0.00731, "fx":[49.69872,49.75847,49.75317,49.69347], "fy":[56.74744,56.69507,56.69984,56.75216]}, + {"t":0.15609, "x":1.67039, "y":0.66695, "heading":0.94206, "vx":0.47482, "vy":0.54164, "omega":0.0011, "ax":3.04144, "ay":3.46945, "alpha":0.00752, "fx":[49.69374,49.75522,49.74977,49.68835], "fy":[56.74346,56.68957,56.6945,56.74832]}, + {"t":0.19511, "x":1.69124, "y":0.69073, "heading":0.94211, "vx":0.5935, "vy":0.67702, "omega":0.00139, "ax":3.04115, "ay":3.46913, "alpha":0.00776, "fx":[49.68805,49.75151,49.74588,49.68249], "fy":[56.73891,56.68329,56.68838,56.74393]}, + {"t":0.23413, "x":1.71671, "y":0.71979, "heading":0.94216, "vx":0.71218, "vy":0.8124, "omega":0.0017, "ax":3.04081, "ay":3.46875, "alpha":0.00804, "fx":[49.68149,49.74722,49.7414,49.67574], "fy":[56.73365,56.67604,56.68133,56.73887]}, + {"t":0.27315, "x":1.74682, "y":0.75413, "heading":0.94223, "vx":0.83083, "vy":0.94775, "omega":0.00201, "ax":3.04042, "ay":3.46831, "alpha":0.00836, "fx":[49.67384,49.74223,49.73617,49.66785], "fy":[56.72753,56.66759,56.6731,56.73296]}, + {"t":0.31218, "x":1.78155, "y":0.79375, "heading":0.94231, "vx":0.94948, "vy":1.08309, "omega":0.00234, "ax":3.03995, "ay":3.46779, "alpha":0.00875, "fx":[49.66479,49.73632,49.72999,49.65854], "fy":[56.72029,56.65759,56.66338,56.72599]}, + {"t":0.3512, "x":1.82092, "y":0.83866, "heading":0.9424, "vx":1.0681, "vy":1.21841, "omega":0.00268, "ax":3.03939, "ay":3.46717, "alpha":0.00921, "fx":[49.65393,49.72924,49.72257,49.64736], "fy":[56.7116,56.6456,56.65171,56.71761]}, + {"t":0.39022, "x":1.86491, "y":0.88884, "heading":0.9425, "vx":1.18671, "vy":1.35371, "omega":0.00304, "ax":3.03871, "ay":3.46641, "alpha":0.00978, "fx":[49.64066,49.72058,49.71351,49.63369], "fy":[56.70098,56.63094,56.63745,56.70738]}, + {"t":0.42924, "x":1.91353, "y":0.94431, "heading":0.94262, "vx":1.30528, "vy":1.48898, "omega":0.00342, "ax":3.03786, "ay":3.46546, "alpha":0.01049, "fx":[49.62407,49.70975,49.70218,49.61662], "fy":[56.6877,56.61261,56.61963,56.69459]}, + {"t":0.46826, "x":1.96678, "y":1.00505, "heading":0.94275, "vx":1.42382, "vy":1.62421, "omega":0.00383, "ax":3.03676, "ay":3.46423, "alpha":0.0114, "fx":[49.60274,49.69584,49.68762,49.59466], "fy":[56.67063,56.58905,56.59672,56.67816]}, + {"t":0.50729, "x":2.02465, "y":1.07107, "heading":0.9429, "vx":1.54233, "vy":1.75939, "omega":0.00427, "ax":3.0353, "ay":3.4626, "alpha":0.01261, "fx":[49.5743,49.67729,49.66822,49.5654], "fy":[56.64787,56.55764,56.56619,56.65625]}, + {"t":0.54631, "x":2.08715, "y":1.14236, "heading":0.94307, "vx":1.66077, "vy":1.8945, "omega":0.00476, "ax":3.03325, "ay":3.46032, "alpha":0.01431, "fx":[49.53449,49.65132,49.64107,49.52446], "fy":[56.61602,56.51367,56.52347,56.6256]}, + {"t":0.58533, "x":2.15427, "y":1.21892, "heading":0.94325, "vx":1.77913, "vy":2.02953, "omega":0.00532, "ax":3.03018, "ay":3.4569, "alpha":0.01687, "fx":[49.47478,49.61237,49.60038,49.46309], "fy":[56.56824,56.44772,56.45944,56.57965]}, + {"t":0.62435, "x":2.226, "y":1.30075, "heading":0.94346, "vx":1.89738, "vy":2.16443, "omega":0.00598, "ax":3.02507, "ay":3.4512, "alpha":0.02113, "fx":[49.37531,49.54749,49.53265,49.36094], "fy":[56.48862,56.33785,56.35287,56.50316]}, + {"t":0.66337, "x":2.30234, "y":1.38783, "heading":0.9437, "vx":2.01542, "vy":2.2991, "omega":0.00681, "ax":3.01486, "ay":3.43983, "alpha":0.02968, "fx":[49.17655,49.41778,49.39753,49.15721], "fy":[56.32947,56.11836,56.14035,56.35051]}, + {"t":0.7024, "x":2.38328, "y":1.48017, "heading":0.94396, "vx":2.13307, "vy":2.43333, "omega":0.00796, "ax":2.9844, "ay":3.40587, "alpha":0.05538, "fx":[48.58215,49.02911,48.99472,48.55088], "fy":[55.85258,55.46212,55.50793,55.89512]}, + {"t":0.74142, "x":2.46879, "y":1.57772, "heading":0.94427, "vx":2.24952, "vy":2.56623, "omega":0.01012, "ax":-0.19624, "ay":-0.18478, "alpha":2.46195, "fx":[-12.64153,-1.61938,6.35428,-4.92602], "fy":[-4.43821,-12.46876,-1.66449,6.48822]}, + {"t":0.78044, "x":2.55642, "y":1.67771, "heading":0.94467, "vx":2.24186, "vy":2.55902, "omega":0.10619, "ax":-2.98627, "ay":-3.40482, "alpha":0.04907, "fx":[-49.00166,-48.60835,-48.6367,-49.03245], "fy":[-55.51037,-55.85381,-55.81557,-55.46956]}, + {"t":0.81946, "x":2.64163, "y":1.77498, "heading":0.94881, "vx":2.12533, "vy":2.42616, "omega":0.10811, "ax":-3.01574, "ay":-3.43922, "alpha":0.02327, "fx":[-49.38747,-49.19912,-49.21535,-49.40425], "fy":[-56.15114,-56.31592,-56.29848,-56.13311]}, + {"t":0.85848, "x":2.72227, "y":1.86704, "heading":0.95303, "vx":2.00765, "vy":2.29195, "omega":0.10902, "ax":-3.02564, "ay":-3.45078, "alpha":0.01453, "fx":[-49.51683,-49.3988,-49.40986,-49.5281], "fy":[-56.36753,-56.47086,-56.45982,-56.35626]}, + {"t":0.89751, "x":2.79831, "y":1.95384, "heading":0.95728, "vx":1.88959, "vy":2.1573, "omega":0.10958, "ax":-3.0306, "ay":-3.45657, "alpha":0.01011, "fx":[-49.58154,-49.49933,-49.50752,-49.58983], "fy":[-56.47634,-56.54834,-56.54046,-56.46836]}, + {"t":0.93653, "x":2.86974, "y":2.03539, "heading":0.96156, "vx":1.77133, "vy":2.02242, "omega":0.10998, "ax":-3.03358, "ay":-3.46006, "alpha":0.00742, "fx":[-49.62034,-49.55994,-49.56627,-49.62672], "fy":[-56.54186,-56.59476,-56.5888,-56.53584]}, + {"t":0.97555, "x":2.93655, "y":2.11168, "heading":0.96585, "vx":1.65295, "vy":1.8874, "omega":0.11027, "ax":-3.03557, "ay":-3.46238, "alpha":0.00561, "fx":[-49.64618,-49.6005,-49.60552,-49.65122], "fy":[-56.58564,-56.62565,-56.62099,-56.58095]}, + {"t":1.01457, "x":2.99874, "y":2.18269, "heading":0.97015, "vx":1.5345, "vy":1.75229, "omega":0.11049, "ax":-3.037, "ay":-3.46404, "alpha":0.00431, "fx":[-49.66462,-49.62957,-49.63359,-49.66865], "fy":[-56.61696,-56.64766,-56.64397,-56.61325]}, + {"t":1.05359, "x":3.0563, "y":2.24843, "heading":0.97447, "vx":1.41599, "vy":1.61712, "omega":0.11066, "ax":-3.03806, "ay":-3.46529, "alpha":0.00332, "fx":[-49.67845,-49.65144,-49.65466,-49.68167], "fy":[-56.64048,-56.66414,-56.6612,-56.63753]}, + {"t":1.09262, "x":3.10925, "y":2.3089, "heading":0.97878, "vx":1.29744, "vy":1.48189, "omega":0.11079, "ax":-3.03889, "ay":-3.46626, "alpha":0.00254, "fx":[-49.6892,-49.6685,-49.67106,-49.69176], "fy":[-56.65878,-56.67692,-56.67459,-56.65645]}, + {"t":1.13164, "x":3.15756, "y":2.36409, "heading":0.98311, "vx":1.17885, "vy":1.34663, "omega":0.11088, "ax":-3.03956, "ay":-3.46703, "alpha":0.00192, "fx":[-49.6978,-49.68217,-49.68418,-49.69981], "fy":[-56.67343,-56.68712,-56.68531,-56.67162]}, + {"t":1.17066, "x":3.20125, "y":2.41399, "heading":0.98743, "vx":1.06024, "vy":1.21134, "omega":0.11096, "ax":-3.0401, "ay":-3.46767, "alpha":0.00141, "fx":[-49.70484,-49.69339,-49.69491,-49.70636], "fy":[-56.68542,-56.69545,-56.69408,-56.68405]}, + {"t":1.20968, "x":3.24031, "y":2.45862, "heading":0.99176, "vx":0.94161, "vy":1.07603, "omega":0.11101, "ax":-3.04055, "ay":-3.4682, "alpha":0.00098, "fx":[-49.71071,-49.70276,-49.70385,-49.71181], "fy":[-56.6954,-56.70237,-56.70139,-56.69442]}, + {"t":1.2487, "x":3.27473, "y":2.49797, "heading":0.9961, "vx":0.82296, "vy":0.94069, "omega":0.11105, "ax":-0.21883, "ay":0.18009, "alpha":0.35826, "fx":[-4.94745,-3.27644,-2.2061,-3.87997], "fy":[2.66244,1.5785,3.22883,4.30706]}, + {"t":1.28213, "x":3.30212, "y":2.52951, "heading":0.99981, "vx":0.81565, "vy":0.94671, "omega":0.12303, "ax":-0.0265, "ay":0.02281, "alpha":0.22906, "fx":[-1.30883,-0.23381,0.4424,-0.63275], "fy":[0.192,-0.49872,0.55397,1.2444]}, + {"t":1.31555, "x":3.32936, "y":2.56117, "heading":1.00392, "vx":0.81476, "vy":0.94747, "omega":0.13068, "ax":-0.00326, "ay":0.0028, "alpha":0.0982, "fx":[-0.42832,0.0338,0.32177,-0.14034], "fy":[-0.0333,-0.32747,0.12492,0.41908]}, + {"t":1.34897, "x":3.35659, "y":2.59284, "heading":1.00829, "vx":0.81466, "vy":0.94757, "omega":0.13396, "ax":-0.0004, "ay":0.00034, "alpha":-0.03225, "fx":[0.11653,-0.03568,-0.12962,0.02259], "fy":[0.03215,0.12811,-0.02089,-0.11685]}, + {"t":1.38239, "x":3.38382, "y":2.62451, "heading":1.01276, "vx":0.81464, "vy":0.94758, "omega":0.13289, "ax":-0.00005, "ay":0.00004, "alpha":-0.16285, "fx":[0.61999,-0.15067,-0.62159,0.14906], "fy":[0.13738,0.61843,-0.136,-0.61705]}, + {"t":1.41582, "x":3.41105, "y":2.65618, "heading":1.01721, "vx":0.81464, "vy":0.94758, "omega":0.12744, "ax":-0.00001, "ay":0.00001, "alpha":-0.29417, "fx":[1.12019,-0.27577,-1.12038,0.27557], "fy":[0.25198,1.11476,-0.25181,-1.11459]}, + {"t":1.44924, "x":3.43828, "y":2.68785, "heading":1.02147, "vx":0.81464, "vy":0.94758, "omega":0.11761, "ax":0.0, "ay":0.0, "alpha":-0.42679, "fx":[1.6238,-0.40686,-1.62382,0.40683], "fy":[0.37239,1.61553,-0.37237,-1.61551]}, + {"t":1.48266, "x":3.4655, "y":2.71952, "heading":1.0254, "vx":0.81464, "vy":0.94758, "omega":0.10335, "ax":0.0, "ay":0.0, "alpha":-0.56132, "fx":[2.13368,-0.54343,-2.13369,0.54343], "fy":[0.49815,2.12261,-0.49815,-2.1226]}, + {"t":1.51609, "x":3.49273, "y":2.75119, "heading":1.02885, "vx":0.81464, "vy":0.94758, "omega":0.08459, "ax":0.0, "ay":0.0, "alpha":-0.69832, "fx":[2.65232,-0.68519,-2.65232,0.68518], "fy":[0.6289,2.63834,-0.6289,-2.63834]}, + {"t":1.54951, "x":3.51996, "y":2.78286, "heading":1.03168, "vx":0.81464, "vy":0.94758, "omega":0.06125, "ax":-0.00002, "ay":-0.00002, "alpha":-0.83841, "fx":[3.18193,-0.83189,-3.18254,0.83128], "fy":[0.76373,3.16493,-0.76439,-3.16559]}, + {"t":1.58293, "x":3.54719, "y":2.81453, "heading":1.03372, "vx":0.81464, "vy":0.94758, "omega":0.03322, "ax":-0.36075, "ay":-0.41962, "alpha":-0.95474, "fx":[-2.26053,-6.92545,-9.49346,-4.91087], "fy":[-6.0444,-3.24466,-7.69221,-10.45887]}, + {"t":1.61635, "x":3.57421, "y":2.84597, "heading":1.03484, "vx":0.80258, "vy":0.93355, "omega":0.00131, "ax":-2.9817, "ay":-3.46828, "alpha":-0.01161, "fx":[-48.70576,-48.79994,-48.78449,-48.69044], "fy":[-56.73271,-56.65201,-56.66692,-56.74747]}, + {"t":1.64978, "x":3.59937, "y":2.87523, "heading":1.03488, "vx":0.70293, "vy":0.81764, "omega":0.00093, "ax":-2.99684, "ay":-3.48588, "alpha":-0.00652, "fx":[-48.97048,-49.02356,-49.01463,-48.96159], "fy":[-57.00629,-56.96073,-56.96886,-57.01438]}, + {"t":1.6832, "x":3.62119, "y":2.90061, "heading":1.03491, "vx":0.60276, "vy":0.70113, "omega":0.00071, "ax":-3.00185, "ay":-3.49171, "alpha":-0.00484, "fx":[-49.05813,-49.09757,-49.09088,-49.05146], "fy":[-57.09683,-57.06296,-57.06894,-57.10278]}, + {"t":1.71662, "x":3.63966, "y":2.9221, "heading":1.03493, "vx":0.50243, "vy":0.58442, "omega":0.00055, "ax":-3.00435, "ay":-3.49462, "alpha":-0.004, "fx":[-49.10182,-49.13445,-49.12889,-49.09627], "fy":[-57.14195,-57.11392,-57.11884,-57.14686]}, + {"t":1.75004, "x":3.65478, "y":2.93968, "heading":1.03495, "vx":0.40202, "vy":0.46762, "omega":0.00041, "ax":-3.00584, "ay":-3.49636, "alpha":-0.0035, "fx":[-49.12798,-49.15654,-49.15166,-49.12312], "fy":[-57.16898,-57.14445,-57.14874,-57.17326]}, + {"t":1.78347, "x":3.66653, "y":2.95335, "heading":1.03497, "vx":0.30156, "vy":0.35077, "omega":0.0003, "ax":-3.00684, "ay":-3.49752, "alpha":-0.00317, "fx":[-49.14541,-49.17125,-49.16683,-49.141], "fy":[-57.18698,-57.16477,-57.16865,-57.19084]}, + {"t":1.81689, "x":3.67493, "y":2.96312, "heading":1.03498, "vx":0.20106, "vy":0.23387, "omega":0.00019, "ax":-3.00755, "ay":-3.49835, "alpha":-0.00293, "fx":[-49.15785,-49.18174,-49.17765,-49.15376], "fy":[-57.19982,-57.17928,-57.18286,-57.20339]}, + {"t":1.85031, "x":3.67997, "y":2.96899, "heading":1.03498, "vx":0.10054, "vy":0.11695, "omega":0.00009, "ax":-3.00809, "ay":-3.49897, "alpha":-0.00275, "fx":[-49.16717,-49.18961,-49.18576,-49.16333], "fy":[-57.20944,-57.19016,-57.19351,-57.21279]}, + {"t":1.88374, "x":3.68165, "y":2.97094, "heading":1.03499, "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/PROtoD.traj b/src/main/deploy/choreo/PROtoD.traj index 8d42bbb6..67573fa4 100644 --- a/src/main/deploy/choreo/PROtoD.traj +++ b/src/main/deploy/choreo/PROtoD.traj @@ -10,7 +10,7 @@ {"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":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.5}}, "enabled":true}], + {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -22,7 +22,7 @@ {"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":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1.5 m / s", "val":1.5}}}, "enabled":true}], + {"from":2, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,60 +30,60 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.26596,1.77624], + "waypoints":[0.0,1.29973,1.85258], "samples":[ - {"t":0.0, "x":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.37088, "ay":3.15193, "alpha":0.01343, "fx":[55.06742,55.16592,55.14742,55.04906], "fy":[51.57075,51.46541,51.48546,51.59058]}, - {"t":0.03723, "x":1.63567, "y":0.62686, "heading":0.942, "vx":0.12551, "vy":0.11736, "omega":0.0005, "ax":3.3707, "ay":3.15176, "alpha":0.01364, "fx":[55.06377,55.16386,55.14507,55.04512], "fy":[51.56859,51.46156,51.48193,51.58874]}, - {"t":0.07447, "x":1.64268, "y":0.63342, "heading":0.94202, "vx":0.25102, "vy":0.23471, "omega":0.00101, "ax":3.37049, "ay":3.15156, "alpha":0.01388, "fx":[55.05975,55.16159,55.14247,55.04078], "fy":[51.5662,51.4573,51.47804,51.58671]}, - {"t":0.1117, "x":1.65436, "y":0.64434, "heading":0.94206, "vx":0.37651, "vy":0.35206, "omega":0.00152, "ax":3.37027, "ay":3.15135, "alpha":0.01415, "fx":[55.05527,55.15906,55.13958,55.03595], "fy":[51.56354,51.45256,51.47371,51.58445]}, - {"t":0.14894, "x":1.67072, "y":0.65963, "heading":0.94211, "vx":0.502, "vy":0.4694, "omega":0.00205, "ax":3.37002, "ay":3.15111, "alpha":0.01444, "fx":[55.05027,55.15624,55.13635,55.03055], "fy":[51.56057,51.44727,51.46888,51.58193]}, - {"t":0.18617, "x":1.69175, "y":0.6793, "heading":0.94219, "vx":0.62748, "vy":0.58672, "omega":0.00259, "ax":3.36973, "ay":3.15084, "alpha":0.01478, "fx":[55.04465,55.15306,55.13271,55.02447], "fy":[51.55723,51.44131,51.46344,51.57909]}, - {"t":0.2234, "x":1.71745, "y":0.70333, "heading":0.94229, "vx":0.75295, "vy":0.70404, "omega":0.00314, "ax":3.36941, "ay":3.15054, "alpha":0.01516, "fx":[55.03827,55.14946,55.12859,55.01758], "fy":[51.55344,51.43455,51.45728,51.57588]}, - {"t":0.26064, "x":1.74782, "y":0.73172, "heading":0.9424, "vx":0.87841, "vy":0.82135, "omega":0.0037, "ax":3.36905, "ay":3.15019, "alpha":0.01559, "fx":[55.03099,55.14534,55.12388,55.00971], "fy":[51.54911,51.42683,51.45023,51.57221]}, - {"t":0.29787, "x":1.78286, "y":0.76449, "heading":0.94254, "vx":1.00385, "vy":0.93865, "omega":0.00428, "ax":3.36862, "ay":3.14979, "alpha":0.01609, "fx":[55.02258,55.1406,55.11844,55.00062], "fy":[51.5441,51.41792,51.44211,51.56797]}, - {"t":0.33511, "x":1.82257, "y":0.80162, "heading":0.9427, "vx":1.12928, "vy":1.05592, "omega":0.00488, "ax":3.36813, "ay":3.14932, "alpha":0.01668, "fx":[55.01277,55.13506,55.11209,54.99002], "fy":[51.53827,51.40752,51.43263,51.56303]}, - {"t":0.37234, "x":1.86696, "y":0.84312, "heading":0.94288, "vx":1.25469, "vy":1.17319, "omega":0.0055, "ax":3.36754, "ay":3.14876, "alpha":0.01737, "fx":[55.00117,55.12851,55.1046,54.97748], "fy":[51.53137,51.39522,51.42142,51.55719]}, - {"t":0.40958, "x":1.91601, "y":0.88899, "heading":0.94309, "vx":1.38008, "vy":1.29043, "omega":0.00615, "ax":3.36684, "ay":3.1481, "alpha":0.0182, "fx":[54.98724,55.12065,55.0956,54.96244], "fy":[51.52309,51.38046,51.40797,51.55018]}, - {"t":0.44681, "x":1.96973, "y":0.93922, "heading":0.94332, "vx":1.50544, "vy":1.40765, "omega":0.00683, "ax":3.36599, "ay":3.14729, "alpha":0.01921, "fx":[54.97022,55.11105,55.0846,54.94405], "fy":[51.51296,51.36241,51.39153,51.54162]}, - {"t":0.48404, "x":2.02811, "y":0.99381, "heading":0.94357, "vx":1.63077, "vy":1.52483, "omega":0.00754, "ax":3.36492, "ay":3.14627, "alpha":0.02049, "fx":[54.94893,55.09904,55.07085,54.92106], "fy":[51.5003,51.33984,51.37099,51.53092]}, - {"t":0.52128, "x":2.09117, "y":1.05277, "heading":0.94385, "vx":1.75606, "vy":1.64198, "omega":0.00831, "ax":3.36354, "ay":3.14496, "alpha":0.02212, "fx":[54.92154,55.0836,55.05318,54.89149], "fy":[51.48401,51.31081,51.34456,51.51716]}, - {"t":0.55851, "x":2.15888, "y":1.11609, "heading":0.94416, "vx":1.8813, "vy":1.75908, "omega":0.00913, "ax":3.3617, "ay":3.14322, "alpha":0.02431, "fx":[54.88499,55.06299,55.02962,54.85206], "fy":[51.46229,51.27207,51.30933,51.49881]}, - {"t":0.59575, "x":2.23126, "y":1.18376, "heading":0.9445, "vx":2.00647, "vy":1.87611, "omega":0.01004, "ax":3.35913, "ay":3.14078, "alpha":0.02737, "fx":[54.83378,55.03414,54.99664,54.79685], "fy":[51.43186,51.2178,51.26,51.47313]}, - {"t":0.63298, "x":2.3083, "y":1.2558, "heading":0.94487, "vx":2.13154, "vy":1.99306, "omega":0.01106, "ax":3.35527, "ay":3.13711, "alpha":0.03198, "fx":[54.75687,54.99081,54.94719,54.71402], "fy":[51.38617,51.13631,51.186,51.43461]}, - {"t":0.67021, "x":2.38999, "y":1.33218, "heading":0.94529, "vx":2.25647, "vy":2.10987, "omega":0.01225, "ax":3.34883, "ay":3.131, "alpha":0.0397, "fx":[54.62846,54.9185,54.8648,54.57596], "fy":[51.3099,51.0003,51.06273,51.3704]}, - {"t":0.70745, "x":2.47633, "y":1.41291, "heading":0.94574, "vx":2.38116, "vy":2.22645, "omega":0.01372, "ax":3.33593, "ay":3.11876, "alpha":0.05521, "fx":[54.37094,54.77342,54.70014,54.29996], "fy":[51.15689,50.72771,50.81646,51.24193]}, - {"t":0.74468, "x":2.5673, "y":1.49797, "heading":0.94625, "vx":2.50537, "vy":2.34257, "omega":0.01578, "ax":3.29715, "ay":3.08195, "alpha":0.10228, "fx":[53.59342,54.33423,54.20686,53.4738], "fy":[50.69367,49.90629,50.08061,50.85558]}, - {"t":0.78192, "x":2.66287, "y":1.58733, "heading":0.94684, "vx":2.62814, "vy":2.45732, "omega":0.01959, "ax":-0.89184, "ay":-0.8555, "alpha":3.30422, "fx":[-26.63892,-11.99065,-1.46793,-18.22182], "fy":[-15.29979,-26.53795,-13.28319,-0.82216]}, - {"t":0.81915, "x":2.76011, "y":1.67823, "heading":0.94757, "vx":2.59493, "vy":2.42547, "omega":0.14262, "ax":-3.29826, "ay":-3.08515, "alpha":0.08608, "fx":[-54.17675,-53.55883,-53.66117,-54.28457], "fy":[-50.1806,-50.8338,-50.69643,-50.03443]}, - {"t":0.85638, "x":2.85445, "y":1.76641, "heading":0.95288, "vx":2.47212, "vy":2.3106, "omega":0.14582, "ax":-3.33595, "ay":-3.11987, "alpha":0.04029, "fx":[-54.65507,-54.36309,-54.41719,-54.71038], "fy":[-50.88141,-51.19182,-51.12732,-50.81493]}, - {"t":0.89362, "x":2.94418, "y":1.85028, "heading":0.95831, "vx":2.34791, "vy":2.19443, "omega":0.14732, "ax":-3.34872, "ay":-3.13163, "alpha":0.02463, "fx":[-54.81701,-54.63803,-54.67305,-54.85248], "fy":[-51.12107,-51.31171,-51.27153,-51.08015]}, - {"t":0.93085, "x":3.02928, "y":1.92981, "heading":0.96379, "vx":2.22323, "vy":2.07783, "omega":0.14824, "ax":-3.35514, "ay":-3.13754, "alpha":0.01666, "fx":[-54.89831,-54.77717,-54.80184,-54.92318], "fy":[-51.2422,-51.37136,-51.34358,-51.21408]}, - {"t":0.96809, "x":3.10974, "y":2.005, "heading":0.96931, "vx":2.0983, "vy":1.96101, "omega":0.14886, "ax":-3.359, "ay":-3.1411, "alpha":0.0118, "fx":[-54.94715,-54.86132,-54.87941,-54.96534], "fy":[-51.31533,-51.40689,-51.38676,-51.29501]}, - {"t":1.00532, "x":3.18554, "y":2.07584, "heading":0.97486, "vx":1.97323, "vy":1.84405, "omega":0.1493, "ax":-3.36158, "ay":-3.14348, "alpha":0.00853, "fx":[-54.97974,-54.91777,-54.93125,-54.99327], "fy":[-51.36426,-51.4304,-51.41551,-51.34928]}, - {"t":1.04256, "x":3.25668, "y":2.14233, "heading":0.98042, "vx":1.84806, "vy":1.727, "omega":0.14962, "ax":-3.36343, "ay":-3.14518, "alpha":0.00616, "fx":[-55.00304,-54.95831,-54.96832,-55.01308], "fy":[-51.39929,-51.44705,-51.43605,-51.38824]}, - {"t":1.07979, "x":3.32316, "y":2.20445, "heading":0.98599, "vx":1.72283, "vy":1.6099, "omega":0.14985, "ax":-3.36482, "ay":-3.14645, "alpha":0.00437, "fx":[-55.02054,-54.98884,-54.99614,-55.02785], "fy":[-51.4256,-51.45944,-51.45146,-51.41759]}, - {"t":1.11702, "x":3.38497, "y":2.26221, "heading":0.99157, "vx":1.59754, "vy":1.49274, "omega":0.15001, "ax":-3.3659, "ay":-3.14745, "alpha":0.00296, "fx":[-55.03417,-55.0127,-55.01777,-55.03926], "fy":[-51.44606,-51.469,-51.46346,-51.44051]}, - {"t":1.15426, "x":3.44212, "y":2.31561, "heading":0.99715, "vx":1.47222, "vy":1.37555, "omega":0.15012, "ax":-3.36676, "ay":-3.14824, "alpha":0.00183, "fx":[-55.04511,-55.03185,-55.03506,-55.04833], "fy":[-51.46243,-51.47659,-51.47309,-51.45892]}, - {"t":1.19149, "x":3.49461, "y":2.36465, "heading":1.00274, "vx":1.34686, "vy":1.25833, "omega":0.15019, "ax":-3.36746, "ay":-3.14889, "alpha":0.0009, "fx":[-55.05408,-55.04758,-55.04919,-55.0557], "fy":[-51.4758,-51.48274,-51.48099,-51.47404]}, - {"t":1.22873, "x":3.54242, "y":2.40932, "heading":1.00833, "vx":1.22148, "vy":1.14108, "omega":0.15022, "ax":-3.36805, "ay":-3.14943, "alpha":0.00012, "fx":[-55.06159,-55.06073,-55.06095,-55.0618], "fy":[-51.48692,-51.48783,-51.4876,-51.48668]}, - {"t":1.26596, "x":3.58557, "y":2.44962, "heading":1.01393, "vx":1.09607, "vy":1.02381, "omega":0.15023, "ax":0.1146, "ay":-0.14004, "alpha":-0.18325, "fx":[2.57237,1.70516,1.17465,2.04209], "fy":[-2.13401,-1.59463,-2.44529,-2.98364]}, - {"t":1.29598, "x":3.61852, "y":2.48029, "heading":1.01844, "vx":1.09951, "vy":1.01961, "omega":0.14472, "ax":0.02428, "ay":-0.0262, "alpha":-0.41718, "fx":[1.98518,0.00416,-1.19155,0.78977], "fy":[-0.06931,1.15224,-0.78787,-2.00837]}, - {"t":1.32599, "x":3.65153, "y":2.51088, "heading":1.02278, "vx":1.10024, "vy":1.01882, "omega":0.1322, "ax":0.00479, "ay":-0.00518, "alpha":-0.652, "fx":[2.55824,-0.54636,-2.40157,0.70317], "fy":[0.48738,2.38265,-0.65692,-2.55169]}, - {"t":1.35601, "x":3.68456, "y":2.54146, "heading":1.02675, "vx":1.10038, "vy":1.01867, "omega":0.11263, "ax":0.00095, "ay":-0.00102, "alpha":-0.88889, "fx":[3.39325,-0.8496,-3.36229,0.8806], "fy":[0.77664,3.34344,-0.8102,-3.37681]}, - {"t":1.38603, "x":3.71759, "y":2.57204, "heading":1.03013, "vx":1.10041, "vy":1.01864, "omega":0.08595, "ax":0.00019, "ay":-0.0002, "alpha":-1.12861, "fx":[4.28836,-1.10977,-4.28225,1.11589], "fy":[1.01857,4.25931,-1.02522,-4.26589]}, - {"t":1.41604, "x":3.75062, "y":2.60261, "heading":1.03271, "vx":1.10042, "vy":1.01863, "omega":0.05207, "ax":-0.00113, "ay":-0.00112, "alpha":-1.37191, "fx":[5.18748,-1.38473,-5.22426,1.34745], "fy":[1.23727,5.1597,-1.27397,-5.19625]}, - {"t":1.44606, "x":3.78365, "y":2.63319, "heading":1.03427, "vx":1.10038, "vy":1.0186, "omega":0.01089, "ax":-2.89025, "ay":-2.67546, "alpha":-0.26115, "fx":[-46.49023,-48.20702,-47.99268,-46.31051], "fy":[-44.21123,-42.50995,-43.29525,-44.93834]}, - {"t":1.47607, "x":3.81538, "y":2.66256, "heading":1.0346, "vx":1.01363, "vy":0.93829, "omega":0.00306, "ax":-3.34632, "ay":-3.09761, "alpha":-0.02506, "fx":[-54.64151,-54.81916,-54.77021,-54.59292], "fy":[-50.70657,-50.51595,-50.57392,-50.76377]}, - {"t":1.50609, "x":3.84429, "y":2.68932, "heading":1.03469, "vx":0.91318, "vy":0.84531, "omega":0.0023, "ax":-3.36736, "ay":-3.11709, "alpha":-0.01481, "fx":[-55.01211,-55.11745,-55.0877,-54.98248], "fy":[-50.99838,-50.88497,-50.9187,-51.03184]}, - {"t":1.53611, "x":3.87019, "y":2.71329, "heading":1.03476, "vx":0.81211, "vy":0.75175, "omega":0.00186, "ax":-3.3748, "ay":-3.12398, "alpha":-0.0112, "fx":[-55.14306,-55.22281,-55.20008,-55.12041], "fy":[-51.10142,-51.01547,-51.04083,-51.12662]}, - {"t":1.56612, "x":3.89304, "y":2.73445, "heading":1.03482, "vx":0.71081, "vy":0.65798, "omega":0.00152, "ax":-3.37861, "ay":-3.1275, "alpha":-0.00935, "fx":[-55.21001,-55.27665,-55.25758,-55.19098], "fy":[-51.15407,-51.0822,-51.10333,-51.17509]}, - {"t":1.59614, "x":3.91286, "y":2.75279, "heading":1.03486, "vx":0.60939, "vy":0.5641, "omega":0.00124, "ax":-3.38092, "ay":-3.12964, "alpha":-0.00823, "fx":[-55.25066,-55.30934,-55.2925,-55.23385], "fy":[-51.18604,-51.12273,-51.14129,-51.20453]}, - {"t":1.62616, "x":3.92963, "y":2.76831, "heading":1.0349, "vx":0.50791, "vy":0.47016, "omega":0.00099, "ax":-3.38247, "ay":-3.13108, "alpha":-0.00748, "fx":[-55.27795,-55.3313,-55.31595,-55.26264], "fy":[-51.20751,-51.14995,-51.1668,-51.22429]}, - {"t":1.65617, "x":3.94335, "y":2.78102, "heading":1.03493, "vx":0.40638, "vy":0.37618, "omega":0.00077, "ax":-3.38359, "ay":-3.13211, "alpha":-0.00694, "fx":[-55.29755,-55.34706,-55.3328,-55.28332], "fy":[-51.22292,-51.16949,-51.18512,-51.23849]}, - {"t":1.68619, "x":3.95402, "y":2.7909, "heading":1.03495, "vx":0.30482, "vy":0.28216, "omega":0.00056, "ax":-3.38442, "ay":-3.13288, "alpha":-0.00653, "fx":[-55.3123,-55.35892,-55.34548,-55.29889], "fy":[-51.23452,-51.1842,-51.1989,-51.24917]}, - {"t":1.7162, "x":3.96165, "y":2.79796, "heading":1.03497, "vx":0.20323, "vy":0.18813, "omega":0.00037, "ax":-3.38508, "ay":-3.13349, "alpha":-0.00622, "fx":[-55.32381,-55.36817,-55.35537,-55.31103], "fy":[-51.24356,-51.19567,-51.20966,-51.2575]}, - {"t":1.74622, "x":3.96622, "y":2.80219, "heading":1.03498, "vx":0.10162, "vy":0.09407, "omega":0.00018, "ax":-3.3856, "ay":-3.13398, "alpha":-0.00596, "fx":[-55.33303,-55.37559,-55.3633,-55.32077], "fy":[-51.25082,-51.20488,-51.21829,-51.26418]}, - {"t":1.77624, "x":3.96775, "y":2.8036, "heading":1.03499, "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":1.63334, "y":0.62468, "heading":0.942, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.37099, "ay":3.15195, "alpha":0.00824, "fx":[55.08474,55.14519,55.13385,55.07345], "fy":[51.55463,51.49,51.50228,51.56683]}, + {"t":0.03823, "x":1.6358, "y":0.62698, "heading":0.942, "vx":0.12886, "vy":0.12049, "omega":0.00032, "ax":3.3708, "ay":3.15177, "alpha":0.0084, "fx":[55.08117,55.14279,55.13123,55.06966], "fy":[51.55221,51.48632,51.49884,51.56465]}, + {"t":0.07645, "x":1.64319, "y":0.63389, "heading":0.94201, "vx":0.25772, "vy":0.24097, "omega":0.00064, "ax":3.37059, "ay":3.15158, "alpha":0.00858, "fx":[55.0772,55.14013,55.12833,55.06545], "fy":[51.54952,51.48223,51.49503,51.56223]}, + {"t":0.11468, "x":1.6555, "y":0.6454, "heading":0.94204, "vx":0.38657, "vy":0.36145, "omega":0.00096, "ax":3.37036, "ay":3.15135, "alpha":0.00878, "fx":[55.07276,55.13715,55.12508,55.06075], "fy":[51.54652,51.47767,51.49077,51.55952]}, + {"t":0.15291, "x":1.67274, "y":0.66152, "heading":0.94207, "vx":0.51541, "vy":0.48192, "omega":0.0013, "ax":3.37009, "ay":3.1511, "alpha":0.009, "fx":[55.06777,55.1338,55.12142,55.05545], "fy":[51.54314,51.47253,51.48597,51.55648]}, + {"t":0.19114, "x":1.69491, "y":0.68225, "heading":0.94212, "vx":0.64424, "vy":0.60238, "omega":0.00164, "ax":3.36979, "ay":3.15082, "alpha":0.00926, "fx":[55.06211,55.13001,55.11728,55.04945], "fy":[51.5393,51.46671,51.48054,51.55303]}, + {"t":0.22936, "x":1.722, "y":0.70758, "heading":0.94219, "vx":0.77306, "vy":0.72282, "omega":0.002, "ax":3.36945, "ay":3.1505, "alpha":0.00955, "fx":[55.05564,55.12567,55.11255,55.04259], "fy":[51.53492,51.46006,51.47433,51.54908]}, + {"t":0.26759, "x":1.75401, "y":0.73751, "heading":0.94226, "vx":0.90186, "vy":0.84326, "omega":0.00236, "ax":3.36906, "ay":3.15012, "alpha":0.00988, "fx":[55.04819,55.12066,55.10709,55.03468], "fy":[51.52986,51.45238,51.46717,51.54454]}, + {"t":0.30582, "x":1.79095, "y":0.77205, "heading":0.94235, "vx":1.03065, "vy":0.96368, "omega":0.00274, "ax":3.3686, "ay":3.14969, "alpha":0.01028, "fx":[55.03948,55.11483,55.10071,55.02545], "fy":[51.52397,51.44342,51.45882,51.53923]}, + {"t":0.34405, "x":1.83281, "y":0.81119, "heading":0.94246, "vx":1.15942, "vy":1.08408, "omega":0.00313, "ax":3.36805, "ay":3.14917, "alpha":0.01074, "fx":[55.0292,55.10793,55.09318,55.01454], "fy":[51.51699,51.43283,51.44895,51.53296]}, + {"t":0.38227, "x":1.87959, "y":0.85493, "heading":0.94258, "vx":1.28817, "vy":1.20447, "omega":0.00354, "ax":3.3674, "ay":3.14855, "alpha":0.0113, "fx":[55.01686,55.09965,55.08415,55.00146], "fy":[51.50863,51.42013,51.4371,51.52545]}, + {"t":0.4205, "x":1.93129, "y":0.90328, "heading":0.94271, "vx":1.4169, "vy":1.32483, "omega":0.00398, "ax":3.3666, "ay":3.1478, "alpha":0.01198, "fx":[55.00177,55.08954,55.07312,54.98546], "fy":[51.49841,51.4046,51.42263,51.51626]}, + {"t":0.45873, "x":1.98792, "y":0.95622, "heading":0.94286, "vx":1.5456, "vy":1.44516, "omega":0.00443, "ax":3.3656, "ay":3.14685, "alpha":0.01283, "fx":[54.98292,55.0769,55.05933,54.96548], "fy":[51.48563,51.3852,51.40455,51.50478]}, + {"t":0.49695, "x":2.04946, "y":1.01376, "heading":0.94303, "vx":1.67425, "vy":1.56545, "omega":0.00492, "ax":3.36432, "ay":3.14564, "alpha":0.01392, "fx":[54.95869,55.06065,55.04161,54.93979], "fy":[51.46921,51.36025,51.38131,51.49003]}, + {"t":0.53518, "x":2.11592, "y":1.0759, "heading":0.94322, "vx":1.80286, "vy":1.6857, "omega":0.00546, "ax":3.36261, "ay":3.14402, "alpha":0.01538, "fx":[54.92638,55.03899,55.01799,54.90556], "fy":[51.44732,51.32699,51.35035,51.47038]}, + {"t":0.57341, "x":2.1873, "y":1.14264, "heading":0.94343, "vx":1.93141, "vy":1.80589, "omega":0.00604, "ax":3.36022, "ay":3.14176, "alpha":0.01742, "fx":[54.88117,55.00869,54.98497,54.85768], "fy":[51.41668,51.28045,51.30703,51.44289]}, + {"t":0.61164, "x":2.26359, "y":1.21397, "heading":0.94366, "vx":2.05986, "vy":1.92599, "omega":0.00671, "ax":3.35663, "ay":3.13837, "alpha":0.02049, "fx":[54.81339,54.96327,54.93551,54.78595], "fy":[51.37076,51.2107,51.24216,51.4017]}, + {"t":0.64986, "x":2.34478, "y":1.28989, "heading":0.94392, "vx":2.18817, "vy":2.04596, "omega":0.00749, "ax":3.35067, "ay":3.13273, "alpha":0.02561, "fx":[54.70054,54.88766,54.85328,54.66665], "fy":[51.2943,51.09458,51.1343,51.33321]}, + {"t":0.68809, "x":2.43088, "y":1.37039, "heading":0.9442, "vx":2.31626, "vy":2.16572, "omega":0.00847, "ax":3.33878, "ay":3.12148, "alpha":0.03586, "fx":[54.47535,54.73675,54.68954,54.42911], "fy":[51.14169,50.86299,50.91962,51.19675]}, + {"t":0.72632, "x":2.52186, "y":1.45546, "heading":0.94453, "vx":2.44389, "vy":2.28504, "omega":0.00984, "ax":3.30345, "ay":3.08806, "alpha":0.06656, "fx":[53.80435,54.2864,54.20408,53.72533], "fy":[50.68611,50.17385,50.2843,50.79129]}, + {"t":0.76455, "x":2.6177, "y":1.54507, "heading":0.94491, "vx":2.57017, "vy":2.40309, "omega":0.01239, "ax":0.29815, "ay":0.26122, "alpha":2.73059, "fx":[-5.76398,6.87286,15.27546,3.1122], "fy":[2.8179,-6.30561,5.82634,14.74326]}, + {"t":0.80277, "x":2.71617, "y":1.63712, "heading":0.94538, "vx":2.58157, "vy":2.41308, "omega":0.11677, "ax":-3.30212, "ay":-3.08838, "alpha":0.06006, "fx":[-54.16293,-53.73074,-53.80239,-54.23727], "fy":[-50.30918,-50.76667,-50.67122,-50.20944]}, + {"t":0.841, "x":2.81244, "y":1.72711, "heading":0.94984, "vx":2.45534, "vy":2.29502, "omega":0.11907, "ax":-3.33825, "ay":-3.12177, "alpha":0.02849, "fx":[-54.65826,-54.45149,-54.48942,-54.6968], "fy":[-50.94784,-51.16783,-51.12277,-50.90179]}, + {"t":0.87923, "x":2.90386, "y":1.81256, "heading":0.95439, "vx":2.32773, "vy":2.17568, "omega":0.12016, "ax":-3.35034, "ay":-3.13295, "alpha":0.0178, "fx":[-54.82396,-54.69444,-54.71936,-54.84912], "fy":[-51.16315,-51.30118,-51.27264,-51.13422]}, + {"t":0.91745, "x":2.9904, "y":1.89344, "heading":0.95899, "vx":2.19965, "vy":2.05592, "omega":0.12084, "ax":-3.3564, "ay":-3.13855, "alpha":0.01237, "fx":[-54.90677,-54.81666,-54.83463,-54.92485], "fy":[-51.27138,-51.36749,-51.3473,-51.251]}, + {"t":0.95568, "x":3.07203, "y":1.96974, "heading":0.96361, "vx":2.07135, "vy":1.93594, "omega":0.12131, "ax":-3.36004, "ay":-3.14191, "alpha":0.00907, "fx":[-54.95641,-54.89034,-54.90391,-54.97004], "fy":[-51.33654,-51.40706,-51.39197,-51.32135]}, + {"t":0.99391, "x":3.14876, "y":2.04145, "heading":0.96824, "vx":1.9429, "vy":1.81583, "omega":0.12166, "ax":-3.36246, "ay":-3.14415, "alpha":0.00685, "fx":[-54.98947,-54.93964,-54.95015,-55.00002], "fy":[-51.38009,-51.43329,-51.42168,-51.36842]}, + {"t":1.03214, "x":3.22057, "y":2.10857, "heading":0.97289, "vx":1.81437, "vy":1.69564, "omega":0.12192, "ax":-3.36419, "ay":-3.14575, "alpha":0.00524, "fx":[-55.01307,-54.97497,-54.98321,-55.02133], "fy":[-51.41124,-51.45192,-51.44286,-51.40214]}, + {"t":1.07036, "x":3.28747, "y":2.17109, "heading":0.97756, "vx":1.68576, "vy":1.57539, "omega":0.12212, "ax":-3.36549, "ay":-3.14695, "alpha":0.00402, "fx":[-55.03076,-55.00154,-55.00802,-55.03725], "fy":[-51.43462,-51.46582,-51.45874,-51.42751]}, + {"t":1.10859, "x":3.34946, "y":2.22902, "heading":0.98222, "vx":1.55711, "vy":1.45509, "omega":0.12227, "ax":-3.3665, "ay":-3.14789, "alpha":0.00306, "fx":[-55.04452,-55.02226,-55.02731,-55.04957], "fy":[-51.45282,-51.47659,-51.47108,-51.4473]}, + {"t":1.14682, "x":3.40652, "y":2.28234, "heading":0.9869, "vx":1.42842, "vy":1.33475, "omega":0.12239, "ax":-3.36731, "ay":-3.14863, "alpha":0.00229, "fx":[-55.05553,-55.03888,-55.04274,-55.05939], "fy":[-51.46737,-51.48515,-51.48095,-51.46316]}, + {"t":1.18505, "x":3.45866, "y":2.33106, "heading":0.99158, "vx":1.29969, "vy":1.21439, "omega":0.12248, "ax":-3.36797, "ay":-3.14924, "alpha":0.00166, "fx":[-55.06454,-55.05251,-55.05536,-55.0674], "fy":[-51.47928,-51.49213,-51.48903,-51.47618]}, + {"t":1.22327, "x":3.50589, "y":2.37518, "heading":0.99626, "vx":1.17094, "vy":1.094, "omega":0.12254, "ax":-3.36853, "ay":-3.14975, "alpha":0.00113, "fx":[-55.07206,-55.06389,-55.06587,-55.07404], "fy":[-51.48919,-51.49792,-51.49578,-51.48705]}, + {"t":1.2615, "x":3.54819, "y":2.4147, "heading":1.00094, "vx":1.04217, "vy":0.97359, "omega":0.12258, "ax":-3.36899, "ay":-3.15019, "alpha":0.00068, "fx":[-55.07844,-55.07355,-55.07476,-55.07965], "fy":[-51.49757,-51.5028,-51.50149,-51.49626]}, + {"t":1.29973, "x":3.58557, "y":2.44962, "heading":1.00563, "vx":0.91339, "vy":0.85317, "omega":0.12261, "ax":0.09524, "ay":-0.11403, "alpha":0.18612, "fx":[0.84597,1.72255,2.26779,1.39144], "fy":[-2.01624,-2.5709,-1.71253,-1.157]}, + {"t":1.33225, "x":3.61532, "y":2.47731, "heading":1.00962, "vx":0.91648, "vy":0.84946, "omega":0.12866, "ax":0.01311, "ay":-0.01415, "alpha":0.03201, "fx":[0.09221,0.2434,0.33644,0.18525], "fy":[-0.25785,-0.35288,-0.20488,-0.10984]}, + {"t":1.36477, "x":3.64513, "y":2.50492, "heading":1.0138, "vx":0.91691, "vy":0.849, "omega":0.1297, "ax":0.0017, "ay":-0.00184, "alpha":-0.12236, "fx":[0.49415,-0.08526,-0.4385,0.14091], "fy":[0.07313,0.43397,-0.13324,-0.49408]}, + {"t":1.39729, "x":3.67495, "y":2.53253, "heading":1.01802, "vx":0.91697, "vy":0.84894, "omega":0.12572, "ax":0.00022, "ay":-0.00024, "alpha":-0.27722, "fx":[1.05919,-0.25703,-1.05196,0.26426], "fy":[0.23434,1.04636,-0.24215,-1.05417]}, + {"t":1.42981, "x":3.70477, "y":2.56014, "heading":1.02211, "vx":0.91697, "vy":0.84894, "omega":0.11671, "ax":0.00003, "ay":-0.00003, "alpha":-0.43323, "fx":[1.64851,-0.41356,-1.64757,0.4145], "fy":[0.37855,1.6391,-0.37956,-1.64011]}, + {"t":1.46233, "x":3.73459, "y":2.58775, "heading":1.0259, "vx":0.91697, "vy":0.84893, "omega":0.10262, "ax":0.0, "ay":0.0, "alpha":-0.591, "fx":[2.2463,-0.57323,-2.24618,0.57335], "fy":[0.52556,2.23449,-0.52569,-2.23462]}, + {"t":1.49485, "x":3.76441, "y":2.61536, "heading":1.02924, "vx":0.91697, "vy":0.84893, "omega":0.0834, "ax":0.0, "ay":0.0, "alpha":-0.75117, "fx":[2.85277,-0.73814,-2.85277,0.73814], "fy":[0.6776,2.83771,-0.6776,-2.83771]}, + {"t":1.52737, "x":3.79423, "y":2.64296, "heading":1.03195, "vx":0.91697, "vy":0.84893, "omega":0.05897, "ax":-0.00007, "ay":-0.00006, "alpha":-0.91438, "fx":[3.46913,-0.90911,-3.47157,0.90665], "fy":[0.83322,3.4508,-0.83527,-3.45284]}, + {"t":1.55989, "x":3.82406, "y":2.67057, "heading":1.03387, "vx":0.91697, "vy":0.84893, "omega":0.02923, "ax":-1.16985, "ay":-1.08306, "alpha":-0.85354, "fx":[-15.96955,-20.39596,-22.18884,-17.94513], "fy":[-17.32205,-14.3404,-18.13458,-21.02687]}, + {"t":1.59242, "x":3.85326, "y":2.69761, "heading":1.03482, "vx":0.87893, "vy":0.81371, "omega":0.00148, "ax":-3.35541, "ay":-3.10643, "alpha":-0.01318, "fx":[-54.82079,-54.9143,-54.88826,-54.79485], "fy":[-50.81945,-50.71901,-50.74923,-50.84946]}, + {"t":1.62494, "x":3.88007, "y":2.72243, "heading":1.03487, "vx":0.76981, "vy":0.71269, "omega":0.00105, "ax":-3.37272, "ay":-3.12246, "alpha":-0.0075, "fx":[-55.11842,-55.17182,-55.15664,-55.10327], "fy":[-51.06657,-51.00905,-51.02605,-51.0835]}, + {"t":1.65746, "x":3.90332, "y":2.74395, "heading":1.0349, "vx":0.66012, "vy":0.61114, "omega":0.0008, "ax":-3.37852, "ay":-3.12783, "alpha":-0.0056, "fx":[-55.21816,-55.25809,-55.24666,-55.20674], "fy":[-51.14934,-51.10628,-51.11893,-51.16195]}, + {"t":1.68998, "x":3.923, "y":2.76217, "heading":1.03493, "vx":0.55025, "vy":0.50942, "omega":0.00062, "ax":-3.38143, "ay":-3.13053, "alpha":-0.00466, "fx":[-55.2681,-55.30129,-55.29176,-55.25858], "fy":[-51.19079,-51.15499,-51.16547,-51.20125]}, + {"t":1.7225, "x":3.93911, "y":2.77709, "heading":1.03495, "vx":0.44028, "vy":0.40761, "omega":0.00047, "ax":-3.38318, "ay":-3.13214, "alpha":-0.00409, "fx":[-55.29809,-55.32723,-55.31884,-55.28971], "fy":[-51.21567,-51.18423,-51.19342,-51.22484]}, + {"t":1.75502, "x":3.95164, "y":2.78869, "heading":1.03496, "vx":0.33026, "vy":0.30575, "omega":0.00034, "ax":-3.38434, "ay":-3.13322, "alpha":-0.00371, "fx":[-55.31809,-55.34453,-55.33691,-55.31048], "fy":[-51.23227,-51.20374,-51.21207,-51.24058]}, + {"t":1.78754, "x":3.96059, "y":2.79697, "heading":1.03497, "vx":0.2202, "vy":0.20386, "omega":0.00022, "ax":-3.38517, "ay":-3.13399, "alpha":-0.00343, "fx":[-55.33238,-55.35689,-55.34982,-55.32532], "fy":[-51.24412,-51.21767,-51.22539,-51.25183]}, + {"t":1.82006, "x":3.96596, "y":2.80194, "heading":1.03498, "vx":0.11011, "vy":0.10194, "omega":0.00011, "ax":-3.3858, "ay":-3.13457, "alpha":-0.00323, "fx":[-55.3431,-55.36616,-55.3595,-55.33645], "fy":[-51.25302,-51.22813,-51.23539,-51.26026]}, + {"t":1.85258, "x":3.96775, "y":2.8036, "heading":1.03499, "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/ROtoE.traj b/src/main/deploy/choreo/ROtoE.traj index e44794f5..b30213f4 100644 --- a/src/main/deploy/choreo/ROtoE.traj +++ b/src/main/deploy/choreo/ROtoE.traj @@ -4,25 +4,25 @@ "snapshot":{ "waypoints":[ {"x":7.046594619750977, "y":1.881866693496704, "heading":2.356194490192345, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.508034706115723, "y":2.496836423873902, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.508034706115723, "y":2.496836423873902, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":5.009561061859131, "y":2.7982044219970703, "heading":2.0928880900706415, "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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":1.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ {"x":{"exp":"7.046594619750977 m", "val":7.046594619750977}, "y":{"exp":"1.881866693496704 m", "val":1.881866693496704}, "heading":{"exp":"180 deg - 45 deg", "val":2.356194490192345}, "intervals":43, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.508034706115723 m", "val":5.508034706115723}, "y":{"exp":"2.4968364238739014 m", "val":2.496836423873902}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"5.508034706115723 m", "val":5.508034706115723}, "y":{"exp":"2.4968364238739014 m", "val":2.496836423873902}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"E.x", "val":5.009561061859131}, "y":{"exp":"E.y", "val":2.7982044219970703}, "heading":{"exp":"2.0928880900706415 rad", "val":2.0928880900706415}, "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":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"slow", "val":1.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -30,59 +30,58 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.02144,1.71334], + "waypoints":[0.0,0.98844,1.5904], "samples":[ - {"t":0.0, "x":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.29489, "ay":1.68764, "alpha":-0.01898, "fx":[-70.16348,-70.23349,-70.26308,-70.19312], "fy":[27.71647,27.53885,27.46267,27.64072]}, - {"t":0.03522, "x":7.04393, "y":1.88291, "heading":2.35619, "vx":-0.15127, "vy":0.05944, "omega":-0.00067, "ax":-4.29454, "ay":1.68761, "alpha":-0.01954, "fx":[-70.15628,-70.22836,-70.25881,-70.18678], "fy":[27.71971,27.53686,27.45842,27.64173]}, - {"t":0.07044, "x":7.03594, "y":1.88605, "heading":2.35617, "vx":-0.30254, "vy":0.11888, "omega":-0.00136, "ax":-4.29414, "ay":1.68757, "alpha":-0.02017, "fx":[-70.14811,-70.22255,-70.25398,-70.1796], "fy":[27.72338,27.53461,27.45361,27.64288]}, - {"t":0.10567, "x":7.02262, "y":1.89129, "heading":2.35612, "vx":-0.45378, "vy":0.17832, "omega":-0.00207, "ax":-4.29369, "ay":1.68753, "alpha":-0.0209, "fx":[-70.13879,-70.21592,-70.24845,-70.17139], "fy":[27.72758,27.53204,27.44811,27.64418]}, - {"t":0.14089, "x":7.00397, "y":1.89862, "heading":2.35605, "vx":-0.60502, "vy":0.23776, "omega":-0.0028, "ax":-4.29317, "ay":1.68749, "alpha":-0.02174, "fx":[-70.12803,-70.20826,-70.24208,-70.16192], "fy":[27.73243,27.52907,27.44176,27.64568]}, - {"t":0.17611, "x":6.98, "y":1.90804, "heading":2.35595, "vx":-0.75623, "vy":0.2972, "omega":-0.00357, "ax":-4.29255, "ay":1.68744, "alpha":-0.02272, "fx":[-70.11547,-70.19933,-70.23465,-70.15087], "fy":[27.73808,27.52562,27.43435,27.64743]}, - {"t":0.21133, "x":6.9507, "y":1.91955, "heading":2.35583, "vx":-0.90742, "vy":0.35663, "omega":-0.00437, "ax":-4.29183, "ay":1.68737, "alpha":-0.02388, "fx":[-70.10064,-70.18878,-70.22587,-70.13781], "fy":[27.74476,27.52154,27.42559,27.64949]}, - {"t":0.24655, "x":6.91608, "y":1.93316, "heading":2.35567, "vx":-1.05859, "vy":0.41606, "omega":-0.00521, "ax":-4.29097, "ay":1.6873, "alpha":-0.02527, "fx":[-70.08284,-70.17612,-70.21534,-70.12215], "fy":[27.75278,27.51666,27.41509,27.65197]}, - {"t":0.28178, "x":6.87613, "y":1.94886, "heading":2.35549, "vx":-1.20973, "vy":0.47549, "omega":-0.0061, "ax":-4.28991, "ay":1.68721, "alpha":-0.02696, "fx":[-70.0611,-70.16066,-70.20246,-70.103], "fy":[27.76256,27.51069,27.40225,27.65498]}, - {"t":0.317, "x":6.83086, "y":1.96666, "heading":2.35527, "vx":-1.36082, "vy":0.53492, "omega":-0.00705, "ax":-4.28858, "ay":1.68709, "alpha":-0.02908, "fx":[-70.03393,-70.14135,-70.18637,-70.07907], "fy":[27.77479,27.50325,27.3862,27.65875]}, - {"t":0.35222, "x":6.78027, "y":1.98654, "heading":2.35503, "vx":-1.51188, "vy":0.59434, "omega":-0.00807, "ax":-4.28688, "ay":1.68694, "alpha":-0.03181, "fx":[-69.99901,-70.11655,-70.1657,-70.04831], "fy":[27.79049,27.49368,27.36557,27.66359]}, - {"t":0.38744, "x":6.72436, "y":2.00852, "heading":2.35474, "vx":-1.66287, "vy":0.65376, "omega":-0.00919, "ax":-4.28462, "ay":1.68675, "alpha":-0.03545, "fx":[-69.95249,-70.08352,-70.13813,-70.00729], "fy":[27.81139,27.48095,27.33808,27.67002]}, - {"t":0.42266, "x":6.66313, "y":2.0326, "heading":2.35442, "vx":-1.81378, "vy":0.71317, "omega":-0.01044, "ax":-4.28145, "ay":1.68647, "alpha":-0.04054, "fx":[-69.88742,-70.03736,-70.09957,-69.94986], "fy":[27.84059,27.46315,27.29962,27.67902]}, - {"t":0.45789, "x":6.59659, "y":2.05876, "heading":2.35405, "vx":-1.96458, "vy":0.77257, "omega":-0.01187, "ax":-4.2767, "ay":1.68606, "alpha":-0.04817, "fx":[-69.78996,-69.9683,-70.04176,-69.86374], "fy":[27.88424,27.43652,27.24199,27.69246]}, - {"t":0.49311, "x":6.52474, "y":2.08702, "heading":2.35363, "vx":-2.11522, "vy":0.83196, "omega":-0.01357, "ax":-4.26881, "ay":1.68536, "alpha":-0.06088, "fx":[-69.62791,-69.85369,-69.94553,-69.72025], "fy":[27.95658,27.39233,27.14613,27.71477]}, - {"t":0.52833, "x":6.44759, "y":2.11737, "heading":2.35315, "vx":-2.26557, "vy":0.89132, "omega":-0.01571, "ax":-4.25307, "ay":1.68396, "alpha":-0.08626, "fx":[-69.3053,-69.6263,-69.75345,-69.43341], "fy":[28.09969,27.30464,26.95514,27.75894]}, - {"t":0.56355, "x":6.36516, "y":2.14981, "heading":2.3526, "vx":-2.41537, "vy":0.95063, "omega":-0.01875, "ax":-4.20634, "ay":1.6797, "alpha":-0.16201, "fx":[-68.34944,-68.95836,-69.18038,-68.5743], "fy":[28.51629,27.04698,26.38831,27.8879]}, - {"t":0.59877, "x":6.27747, "y":2.18433, "heading":2.35194, "vx":-2.56353, "vy":1.0098, "omega":-0.02446, "ax":-0.45392, "ay":0.74781, "alpha":-6.28297, "fx":[-7.03614,-31.37611,-9.50868,18.23821], "fy":[35.19987,12.48229,-13.24336,14.46201]}, - {"t":0.634, "x":6.1869, "y":2.22036, "heading":2.35108, "vx":-2.57952, "vy":1.03613, "omega":-0.24576, "ax":4.22467, "ay":-1.62933, "alpha":-0.14629, "fx":[69.43044,68.90177,68.69875,69.23052], "fy":[-25.6603,-27.01123,-27.60039,-26.27363]}, - {"t":0.66922, "x":6.09866, "y":2.25585, "heading":2.34242, "vx":-2.43072, "vy":0.97875, "omega":-0.25091, "ax":4.26267, "ay":-1.65863, "alpha":-0.06865, "fx":[69.86321,69.61391,69.50949,69.75957], "fy":[-26.65471,-27.29057,-27.5736,-26.94315]}, - {"t":0.70444, "x":6.01569, "y":2.28929, "heading":2.33358, "vx":-2.28058, "vy":0.92033, "omega":-0.25333, "ax":4.27531, "ay":-1.66843, "alpha":-0.04282, "fx":[70.00469,69.84932,69.78136,69.93706], "fy":[-26.98705,-27.38305,-27.56333,-27.16942]}, - {"t":0.73966, "x":5.93802, "y":2.32067, "heading":2.32466, "vx":-2.12999, "vy":0.86156, "omega":-0.25483, "ax":4.28162, "ay":-1.67334, "alpha":-0.02989, "fx":[70.0748,69.9666,69.91769,70.02607], "fy":[-27.15363,-27.4293,-27.55769,-27.28303]}, - {"t":0.77488, "x":5.86565, "y":2.34998, "heading":2.31569, "vx":-1.97918, "vy":0.80262, "omega":-0.25589, "ax":4.2854, "ay":-1.67628, "alpha":-0.02212, "fx":[70.11662,70.0368,69.99966,70.07959], "fy":[-27.25384,-27.45715,-27.55402,-27.35126]}, - {"t":0.8101, "x":5.7986, "y":2.37721, "heading":2.30667, "vx":-1.82824, "vy":0.74358, "omega":-0.25667, "ax":4.28792, "ay":-1.67825, "alpha":-0.01693, "fx":[70.14436,70.08349,70.05441,70.11534], "fy":[-27.32083,-27.47584,-27.55138,-27.39669]}, - {"t":0.84533, "x":5.73686, "y":2.40236, "heading":2.29763, "vx":-1.67722, "vy":0.68447, "omega":-0.25726, "ax":4.28973, "ay":-1.67965, "alpha":-0.01321, "fx":[70.16409,70.11677,70.09358,70.14095], "fy":[-27.36882,-27.48931,-27.54936,-27.42906]}, - {"t":0.88055, "x":5.68045, "y":2.42543, "heading":2.28857, "vx":-1.52612, "vy":0.62531, "omega":-0.25773, "ax":4.29108, "ay":-1.68071, "alpha":-0.01041, "fx":[70.17882,70.14166,70.12301,70.1602], "fy":[-27.40492,-27.49952,-27.54773,-27.45324]}, - {"t":0.91577, "x":5.62936, "y":2.44641, "heading":2.27949, "vx":-1.37498, "vy":0.56611, "omega":-0.25809, "ax":4.29213, "ay":-1.68153, "alpha":-0.00824, "fx":[70.19024,70.16098,70.14594,70.17522], "fy":[-27.43308,-27.50758,-27.54638,-27.47196]}, - {"t":0.95099, "x":5.58359, "y":2.4653, "heading":2.2704, "vx":-1.22381, "vy":0.50688, "omega":-0.25838, "ax":4.29297, "ay":-1.68218, "alpha":-0.00649, "fx":[70.19934,70.17638,70.16431,70.18728], "fy":[-27.45568,-27.51412,-27.54523,-27.48683]}, - {"t":0.98621, "x":5.54315, "y":2.48211, "heading":2.2613, "vx":-1.0726, "vy":0.44763, "omega":-0.25861, "ax":4.29365, "ay":-1.68272, "alpha":-0.00506, "fx":[70.20676,70.18894,70.17936,70.19718], "fy":[-27.47422,-27.51957,-27.54424,-27.49892]}, - {"t":1.02144, "x":5.50803, "y":2.49684, "heading":2.25219, "vx":-0.92137, "vy":0.38837, "omega":-0.25879, "ax":1.6242, "ay":3.27316, "alpha":-0.49769, "fx":[25.93353,23.72883,27.25891,29.28902], "fy":[54.60101,54.60261,52.40239,52.4338]}, - {"t":1.05438, "x":5.47856, "y":2.51141, "heading":2.24367, "vx":-0.86785, "vy":0.49621, "omega":-0.27519, "ax":0.42819, "ay":0.72538, "alpha":-1.65936, "fx":[7.65701,0.39714,6.41552,13.53059], "fy":[18.07444,12.78763,5.45811,11.11394]}, - {"t":1.08733, "x":5.4502, "y":2.52815, "heading":2.2346, "vx":-0.85375, "vy":0.52011, "omega":-0.32986, "ax":0.03999, "ay":0.06546, "alpha":-1.38973, "fx":[1.36978,-4.74032,-0.05823,6.04408], "fy":[6.4406,1.67831,-4.31251,0.47445]}, - {"t":1.12028, "x":5.42209, "y":2.54532, "heading":2.22373, "vx":-0.85243, "vy":0.52227, "omega":-0.37565, "ax":0.00352, "ay":0.00574, "alpha":-1.01018, "fx":[0.61991,-3.85657,-0.50477,3.97137], "fy":[3.99639,0.57328,-3.80937,-0.38511]}, - {"t":1.15323, "x":5.39401, "y":2.56253, "heading":2.21136, "vx":-0.85231, "vy":0.52246, "omega":-0.40893, "ax":0.00031, "ay":0.0005, "alpha":-0.63189, "fx":[0.38697,-2.43934,-0.37692,2.44937], "fy":[2.44496,0.33822,-2.4286,-0.32182]}, - {"t":1.18618, "x":5.36592, "y":2.57975, "heading":2.19788, "vx":-0.8523, "vy":0.52247, "omega":-0.42975, "ax":0.00003, "ay":0.00004, "alpha":-0.25488, "fx":[0.16773,-0.98363,-0.16685,0.9845], "fy":[0.98144,0.1471,-0.98001,-0.14567]}, - {"t":1.21912, "x":5.33784, "y":2.59696, "heading":2.18372, "vx":-0.8523, "vy":0.52247, "omega":-0.43815, "ax":0.0, "ay":0.0, "alpha":0.12161, "fx":[-0.0864,0.46853,0.08647,-0.46845], "fy":[-0.4667,-0.07642,0.46682,0.07655]}, - {"t":1.25207, "x":5.30976, "y":2.61418, "heading":2.16929, "vx":-0.8523, "vy":0.52247, "omega":-0.43414, "ax":0.0, "ay":0.0, "alpha":0.49835, "fx":[-0.38178,1.91512,0.38179,-1.91511], "fy":[-1.90742,-0.34111,1.90743,0.34112]}, - {"t":1.28502, "x":5.28168, "y":2.63139, "heading":2.15498, "vx":-0.8523, "vy":0.52247, "omega":-0.41772, "ax":0.0, "ay":0.0, "alpha":0.8761, "fx":[-0.71908,3.35789,0.71908,-3.35789], "fy":[-3.34335,-0.64779,3.34335,0.64779]}, - {"t":1.31797, "x":5.2536, "y":2.64861, "heading":2.14122, "vx":-0.8523, "vy":0.52247, "omega":-0.38886, "ax":0.0, "ay":0.0, "alpha":1.25562, "fx":[-1.09643,4.79926,1.09643,-4.79926], "fy":[-4.77702,-0.99455,4.77702,0.99455]}, - {"t":1.35092, "x":5.22552, "y":2.66582, "heading":2.12841, "vx":-0.8523, "vy":0.52247, "omega":-0.34749, "ax":0.0, "ay":0.0, "alpha":1.63759, "fx":[-1.50967,6.24209,1.50967,-6.24209], "fy":[-6.21138,-1.37718,6.21138,1.37718]}, - {"t":1.38386, "x":5.19744, "y":2.68303, "heading":2.11696, "vx":-0.8523, "vy":0.52247, "omega":-0.29353, "ax":0.0, "ay":0.0, "alpha":2.02262, "fx":[-1.95233,7.68977,1.95234,-7.68976], "fy":[-7.64995,-1.78914,7.64997,1.78915]}, - {"t":1.41681, "x":5.16935, "y":2.70025, "heading":2.10729, "vx":-0.8523, "vy":0.52247, "omega":-0.22689, "ax":0.0, "ay":0.00001, "alpha":2.41124, "fx":[-2.41549,9.14625,2.41559,-9.14615], "fy":[-9.09679,-2.22138,9.09696,2.22155]}, - {"t":1.44976, "x":5.14127, "y":2.71746, "heading":2.09981, "vx":-0.8523, "vy":0.52247, "omega":-0.14745, "ax":0.00015, "ay":-0.00001, "alpha":2.80387, "fx":[-2.88535,10.61831,2.89038,-10.61346], "fy":[-10.55704,-2.66279,10.55663,2.66246]}, - {"t":1.48271, "x":5.11319, "y":2.73468, "heading":2.09495, "vx":-0.8523, "vy":0.52247, "omega":-0.05507, "ax":2.31114, "ay":-1.41689, "alpha":1.58625, "fx":[34.72184,42.23227,41.13315,33.04396], "fy":[-30.36074,-23.24701,-15.63473,-23.41128]}, - {"t":1.51565, "x":5.08636, "y":2.75112, "heading":2.09314, "vx":-0.77615, "vy":0.47579, "omega":-0.0028, "ax":3.90802, "ay":-2.39566, "alpha":0.02955, "fx":[63.77526,63.92479,64.002,63.85256], "fy":[-39.35008,-39.109,-38.97834,-39.22034]}, - {"t":1.5486, "x":5.06291, "y":2.7655, "heading":2.09305, "vx":-0.64739, "vy":0.39686, "omega":-0.00183, "ax":3.92328, "ay":-2.40502, "alpha":0.0166, "fx":[64.07416,64.15817,64.20225,64.11827], "fy":[-39.4222,-39.28598,-39.21272,-39.34924]}, - {"t":1.58155, "x":5.04371, "y":2.77727, "heading":2.09299, "vx":-0.51812, "vy":0.31762, "omega":-0.00128, "ax":3.92843, "ay":-2.40818, "alpha":0.01224, "fx":[64.17498,64.23696,64.26964,64.20769], "fy":[-39.44643,-39.34575,-39.29175,-39.39259]}, - {"t":1.6145, "x":5.02877, "y":2.78643, "heading":2.09294, "vx":-0.38869, "vy":0.23827, "omega":-0.00088, "ax":3.93101, "ay":-2.40976, "alpha":0.01006, "fx":[64.22561,64.27653,64.30346,64.25255], "fy":[-39.45858,-39.37578,-39.33142,-39.41433]}, - {"t":1.64745, "x":5.0181, "y":2.79297, "heading":2.09291, "vx":-0.25917, "vy":0.15888, "omega":-0.00055, "ax":3.93256, "ay":-2.41072, "alpha":0.00875, "fx":[64.25605,64.30032,64.32378,64.27951], "fy":[-39.46588,-39.39384,-39.35528,-39.4274]}, - {"t":1.68039, "x":5.0117, "y":2.7969, "heading":2.0929, "vx":-0.1296, "vy":0.07945, "omega":-0.00026, "ax":3.9336, "ay":-2.41135, "alpha":0.00787, "fx":[64.27637,64.31621,64.33734,64.29751], "fy":[-39.47075,-39.4059,-39.3712,-39.43612]}, - {"t":1.71334, "x":5.00956, "y":2.7982, "heading":2.09289, "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":7.04659, "y":1.88187, "heading":2.35619, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.29685, "ay":1.68221, "alpha":-0.03642, "fx":[-70.14995,-70.28385,-70.34056,-70.20685], "fy":[27.74423,27.40376,27.25669,27.59874]}, + {"t":0.03408, "x":7.0441, "y":1.88284, "heading":2.35619, "vx":-0.14645, "vy":0.05734, "omega":-0.00124, "ax":-4.29649, "ay":1.68224, "alpha":-0.03731, "fx":[-70.14166,-70.27889,-70.33696,-70.19994], "fy":[27.75069,27.40188,27.25118,27.60166]}, + {"t":0.06817, "x":7.03661, "y":1.88578, "heading":2.35615, "vx":-0.2929, "vy":0.11467, "omega":-0.00251, "ax":-4.29608, "ay":1.68227, "alpha":-0.03832, "fx":[-70.13234,-70.2733,-70.33291,-70.19217], "fy":[27.75796,27.39978,27.24498,27.60492]}, + {"t":0.10225, "x":7.02413, "y":1.89066, "heading":2.35607, "vx":-0.43933, "vy":0.17201, "omega":-0.00382, "ax":-4.29561, "ay":1.68231, "alpha":-0.03947, "fx":[-70.12178,-70.26696,-70.32833,-70.18337], "fy":[27.76621,27.3974,27.23794,27.60861]}, + {"t":0.13634, "x":7.00666, "y":1.8975, "heading":2.35594, "vx":-0.58574, "vy":0.22935, "omega":-0.00516, "ax":-4.29508, "ay":1.68235, "alpha":-0.04078, "fx":[-70.1097,-70.25972,-70.32309,-70.17331], "fy":[27.77563,27.3947,27.2299,27.61282]}, + {"t":0.17042, "x":6.9842, "y":1.9063, "heading":2.35576, "vx":-0.73213, "vy":0.2867, "omega":-0.00655, "ax":-4.29447, "ay":1.6824, "alpha":-0.04229, "fx":[-70.09576,-70.25136,-70.31704,-70.16171], "fy":[27.78651,27.39159,27.22061,27.61767]}, + {"t":0.20451, "x":6.95676, "y":1.91704, "heading":2.35554, "vx":-0.87851, "vy":0.34404, "omega":-0.008, "ax":-4.29376, "ay":1.68246, "alpha":-0.04405, "fx":[-70.0795,-70.2416,-70.30999,-70.14817], "fy":[27.7992,27.38798,27.20977,27.62331]}, + {"t":0.23859, "x":6.92432, "y":1.92975, "heading":2.35526, "vx":-1.02486, "vy":0.40138, "omega":-0.0095, "ax":-4.29292, "ay":1.68253, "alpha":-0.04613, "fx":[-70.06028,-70.23008,-70.30166,-70.13217], "fy":[27.8142,27.38372,27.19695,27.62997]}, + {"t":0.27267, "x":6.88689, "y":1.94441, "heading":2.35494, "vx":-1.17118, "vy":0.45873, "omega":-0.01107, "ax":-4.29191, "ay":1.68262, "alpha":-0.04863, "fx":[-70.03721,-70.21625,-70.29166,-70.11296], "fy":[27.83219,27.37862,27.18157,27.63796]}, + {"t":0.30676, "x":6.84448, "y":1.96102, "heading":2.35456, "vx":-1.31746, "vy":0.51608, "omega":-0.01273, "ax":-4.29067, "ay":1.68272, "alpha":-0.05169, "fx":[-70.00901,-70.19936,-70.27944,-70.08948], "fy":[27.85416,27.37239,27.16276,27.64772]}, + {"t":0.34084, "x":6.79708, "y":1.97959, "heading":2.35413, "vx":-1.46371, "vy":0.57344, "omega":-0.01449, "ax":-4.28913, "ay":1.68285, "alpha":-0.05551, "fx":[-69.97376,-70.17826,-70.26415,-70.06011], "fy":[27.88162,27.36462,27.13924,27.65991]}, + {"t":0.37493, "x":6.7447, "y":2.00011, "heading":2.35364, "vx":-1.6099, "vy":0.6308, "omega":-0.01638, "ax":-4.28714, "ay":1.68301, "alpha":-0.06042, "fx":[-69.92844,-70.15116,-70.2445,-70.02231], "fy":[27.91688,27.35465,27.10899,27.67557]}, + {"t":0.40901, "x":6.68734, "y":2.02259, "heading":2.35308, "vx":-1.75602, "vy":0.68816, "omega":-0.01844, "ax":-4.28449, "ay":1.68323, "alpha":-0.06698, "fx":[-69.86801,-70.11507,-70.21828,-69.97187], "fy":[27.96384,27.34137,27.06865,27.69645]}, + {"t":0.4431, "x":6.625, "y":2.04702, "heading":2.35245, "vx":-1.90206, "vy":0.74553, "omega":-0.02072, "ax":-4.28079, "ay":1.68353, "alpha":-0.07616, "fx":[-69.78341,-70.06464,-70.18155,-69.90115], "fy":[28.02945,27.32281,27.01217,27.72568]}, + {"t":0.47718, "x":6.55768, "y":2.07341, "heading":2.35174, "vx":-2.04797, "vy":0.80291, "omega":-0.02332, "ax":-4.27522, "ay":1.68398, "alpha":-0.08995, "fx":[-69.65652,-69.98921,-70.12639,-69.79486], "fy":[28.12757,27.29505,26.92743,27.76952]}, + {"t":0.51126, "x":6.4854, "y":2.10175, "heading":2.35095, "vx":-2.19368, "vy":0.86031, "omega":-0.02638, "ax":-4.26595, "ay":1.68472, "alpha":-0.11294, "fx":[-69.4451,-69.86403,-70.0343,-69.61714], "fy":[28.29029,27.24895,26.78619,27.84255]}, + {"t":0.54535, "x":6.40815, "y":2.13206, "heading":2.35005, "vx":-2.33908, "vy":0.91773, "omega":-0.03023, "ax":-4.2474, "ay":1.68616, "alpha":-0.15898, "fx":[-69.02254,-69.61574,-69.84949,-69.25958], "fy":[28.61252,27.15746,26.50378,27.98841]}, + {"t":0.57943, "x":6.32595, "y":2.16432, "heading":2.34902, "vx":-2.48385, "vy":0.9752, "omega":-0.03565, "ax":-4.1917, "ay":1.69016, "alpha":-0.29744, "fx":[-67.7581,-68.88695,-69.28989,-68.17009], "fy":[29.55318,26.8887,25.65784,28.4239]}, + {"t":0.61352, "x":6.23886, "y":2.19854, "heading":2.3478, "vx":-2.62672, "vy":1.03281, "omega":-0.04579, "ax":0.48096, "ay":0.5564, "alpha":-9.13365, "fx":[9.21905,-29.62251,10.04063,41.81374], "fy":[42.62723,12.16863,-28.58004,10.16854]}, + {"t":0.6476, "x":6.14961, "y":2.23406, "heading":2.34624, "vx":-2.61033, "vy":1.05178, "omega":-0.3571, "ax":4.22778, "ay":-1.60063, "alpha":-0.25597, "fx":[69.74402,68.83973,68.48275,69.39804], "fy":[-24.44306,-26.81238,-27.85534,-25.55844]}, + {"t":0.68168, "x":6.06309, "y":2.26898, "heading":2.33407, "vx":-2.46623, "vy":0.99722, "omega":-0.36583, "ax":4.26547, "ay":-1.6408, "alpha":-0.11893, "fx":[70.03598,69.61101,69.42723,69.85489], "fy":[-26.01974,-27.11939,-27.62037,-26.53659]}, + {"t":0.71577, "x":5.98151, "y":2.30202, "heading":2.3216, "vx":-2.32085, "vy":0.94129, "omega":-0.36988, "ax":4.27801, "ay":-1.65434, "alpha":-0.073, "fx":[70.1268,69.86591,69.74724,70.00926], "fy":[-26.54946,-27.2216,-27.53813,-26.87191]}, + {"t":0.74985, "x":5.90489, "y":2.33314, "heading":2.30899, "vx":-2.17503, "vy":0.88491, "omega":-0.37237, "ax":4.28428, "ay":-1.66113, "alpha":-0.04995, "fx":[70.17077,69.99276,69.90842,70.08702], "fy":[-26.81558,-27.27316,-27.49575,-27.0409]}, + {"t":0.78394, "x":5.83325, "y":2.36234, "heading":2.2963, "vx":-2.02901, "vy":0.82829, "omega":-0.37407, "ax":4.28803, "ay":-1.66522, "alpha":-0.03608, "fx":[70.19658,70.06854,70.00561,70.13398], "fy":[-26.97594,-27.3046,-27.46964,-27.14238]}, + {"t":0.81802, "x":5.76658, "y":2.3896, "heading":2.28355, "vx":-1.88285, "vy":0.77153, "omega":-0.3753, "ax":4.29054, "ay":-1.66795, "alpha":-0.0268, "fx":[70.21347,70.11882,70.07067,70.16552], "fy":[-27.0833,-27.32603,-27.45179,-27.20981]}, + {"t":0.85211, "x":5.7049, "y":2.41493, "heading":2.27076, "vx":-1.73661, "vy":0.71468, "omega":-0.37622, "ax":4.29233, "ay":-1.6699, "alpha":-0.02016, "fx":[70.22534,70.15454,70.11731,70.18824], "fy":[-27.16031,-27.34176,-27.43871,-27.25768]}, + {"t":0.88619, "x":5.6482, "y":2.43832, "heading":2.25794, "vx":-1.59031, "vy":0.65776, "omega":-0.3769, "ax":4.29367, "ay":-1.67136, "alpha":-0.01517, "fx":[70.23411,70.18116,70.15241,70.20544], "fy":[-27.21832,-27.35396,-27.42866,-27.29325]}, + {"t":0.92027, "x":5.59649, "y":2.45977, "heading":2.24509, "vx":-1.44397, "vy":0.6008, "omega":-0.37742, "ax":4.29471, "ay":-1.6725, "alpha":-0.01128, "fx":[70.24083,70.20171,70.1798,70.21897], "fy":[-27.26364,-27.3638,-27.42064,-27.3206]}, + {"t":0.95436, "x":5.54977, "y":2.47927, "heading":2.23223, "vx":-1.29758, "vy":0.54379, "omega":-0.3778, "ax":4.29554, "ay":-1.67341, "alpha":-0.00816, "fx":[70.24613,70.21801,70.20178,70.22992], "fy":[-27.30005,-27.37202,-27.41407,-27.34217]}, + {"t":0.98844, "x":5.50803, "y":2.49684, "heading":2.21935, "vx":-1.15117, "vy":0.48675, "omega":-0.37808, "ax":1.69706, "ay":3.5182, "alpha":-0.42607, "fx":[27.20457,25.18554,28.35205,30.23272], "fy":[58.29849,58.56483,56.73115,56.46929]}, + {"t":1.01854, "x":5.47416, "y":2.51308, "heading":2.20797, "vx":-1.1001, "vy":0.59264, "omega":-0.39091, "ax":0.99104, "ay":1.73812, "alpha":-1.28091, "fx":[16.3411,10.58259,16.20995,21.67312], "fy":[32.66817,29.93993,23.95521,27.09652]}, + {"t":1.04864, "x":5.44149, "y":2.5317, "heading":2.1962, "vx":-1.07027, "vy":0.64496, "omega":-0.42946, "ax":0.20087, "ay":0.32987, "alpha":-1.17008, "fx":[4.03949,-1.25698,2.53927,7.81385], "fy":[9.85294,6.12057,0.88796,4.70939]}, + {"t":1.07874, "x":5.40937, "y":2.55127, "heading":2.18328, "vx":-1.06422, "vy":0.65489, "omega":-0.46468, "ax":0.03202, "ay":0.05195, "alpha":-0.49616, "fx":[0.87684,-1.38813,0.17038,2.43481], "fy":[2.75261,1.1631,-1.05534,0.53673]}, + {"t":1.10883, "x":5.37736, "y":2.571, "heading":2.16929, "vx":-1.06326, "vy":0.65645, "omega":-0.47961, "ax":0.005, "ay":0.0081, "alpha":0.19584, "fx":[-0.06822,0.83439,0.23183,-0.6708], "fy":[-0.61713,-0.00157,0.88201,0.26652]}, + {"t":1.13893, "x":5.34536, "y":2.59076, "heading":2.15486, "vx":-1.06311, "vy":0.65669, "omega":-0.47371, "ax":0.00078, "ay":0.00126, "alpha":0.88726, "fx":[-0.71591,3.41331,0.7414,-3.38785], "fy":[-3.36528,-0.63579,3.40642,0.67713]}, + {"t":1.16903, "x":5.31336, "y":2.61053, "heading":2.1406, "vx":-1.06308, "vy":0.65673, "omega":-0.44701, "ax":0.00012, "ay":0.0002, "alpha":1.57533, "fx":[-1.37731,6.02246,1.38129,-6.01849], "fy":[-5.9893,-1.24827,5.99568,1.25475]}, + {"t":1.19913, "x":5.28136, "y":2.63029, "heading":2.12715, "vx":-1.06308, "vy":0.65674, "omega":-0.3996, "ax":0.00002, "ay":0.00003, "alpha":2.25748, "fx":[-2.09161,8.6029,2.09224,-8.60227], "fy":[-8.55952,-1.90883,8.56052,1.90986]}, + {"t":1.22922, "x":5.24937, "y":2.65006, "heading":2.11512, "vx":-1.06308, "vy":0.65674, "omega":-0.33165, "ax":0.0, "ay":0.00001, "alpha":2.93129, "fx":[-2.84973,11.1397,2.84987,-11.13957], "fy":[-11.08141,-2.6133,11.08162,2.61352]}, + {"t":1.25932, "x":5.21737, "y":2.66983, "heading":2.10514, "vx":-1.06308, "vy":0.65674, "omega":-0.24343, "ax":0.00001, "ay":0.00001, "alpha":3.59446, "fx":[-3.62976,13.62744,3.63025,-13.62697], "fy":[-13.55294,-3.34074,13.55315,3.34097]}, + {"t":1.28942, "x":5.18538, "y":2.68959, "heading":2.09781, "vx":-1.06308, "vy":0.65674, "omega":-0.13524, "ax":0.09125, "ay":-0.05639, "alpha":4.24059, "fx":[-2.83029,17.45034,5.98188,-14.63487], "fy":[-16.8811,-4.97731,15.04351,3.12734]}, + {"t":1.31952, "x":5.15342, "y":2.70933, "heading":2.09374, "vx":-1.06033, "vy":0.65504, "omega":-0.00761, "ax":3.87672, "ay":-2.39492, "alpha":0.07248, "fx":[63.09996,63.46942,63.6538,63.28479], "fy":[-39.60327,-39.0181,-38.69873,-39.28939]}, + {"t":1.34962, "x":5.12326, "y":2.72796, "heading":2.09351, "vx":-0.94365, "vy":0.58296, "omega":-0.00543, "ax":3.9051, "ay":-2.41245, "alpha":0.03901, "fx":[63.6903,63.88924,63.99157,63.79278], "fy":[-39.68345,-39.36484,-39.19373,-39.51397]}, + {"t":1.37971, "x":5.09663, "y":2.74442, "heading":2.09335, "vx":-0.82612, "vy":0.51035, "omega":-0.00425, "ax":3.91387, "ay":-2.41787, "alpha":0.02871, "fx":[63.87309,64.0195,64.09552,63.94919], "fy":[-39.70785,-39.47257,-39.34682,-39.58298]}, + {"t":1.40981, "x":5.07354, "y":2.75868, "heading":2.09322, "vx":-0.70832, "vy":0.43758, "omega":-0.00339, "ax":3.91813, "ay":-2.4205, "alpha":0.0237, "fx":[63.96201,64.0829,64.14597,64.02513], "fy":[-39.71965,-39.52504,-39.42125,-39.61647]}, + {"t":1.43991, "x":5.05399, "y":2.77075, "heading":2.09312, "vx":-0.59039, "vy":0.36473, "omega":-0.00268, "ax":3.92065, "ay":-2.42206, "alpha":0.02075, "fx":[64.01458,64.1204,64.17577,64.06998], "fy":[-39.7266,-39.55608,-39.46525,-39.63624]}, + {"t":1.47001, "x":5.038, "y":2.78064, "heading":2.09304, "vx":-0.47239, "vy":0.29183, "omega":-0.00205, "ax":3.92231, "ay":-2.42309, "alpha":0.0188, "fx":[64.0493,64.14518,64.19543,64.09959], "fy":[-39.73118,-39.57659,-39.49432,-39.64929]}, + {"t":1.5001, "x":5.02556, "y":2.78832, "heading":2.09298, "vx":-0.35434, "vy":0.2189, "omega":-0.00149, "ax":3.9235, "ay":-2.42382, "alpha":0.01742, "fx":[64.07395,64.16277,64.20939,64.1206], "fy":[-39.73443,-39.59115,-39.51494,-39.65855]}, + {"t":1.5302, "x":5.01667, "y":2.79381, "heading":2.09293, "vx":-0.23625, "vy":0.14595, "omega":-0.00096, "ax":3.92438, "ay":-2.42436, "alpha":0.01638, "fx":[64.09235,64.1759,64.2198,64.13628], "fy":[-39.73685,-39.60202,-39.53034,-39.66546]}, + {"t":1.5603, "x":5.01134, "y":2.79711, "heading":2.0929, "vx":-0.11814, "vy":0.07298, "omega":-0.00047, "ax":3.92506, "ay":-2.42478, "alpha":0.01558, "fx":[64.10661,64.18608,64.22787,64.14842], "fy":[-39.73872,-39.61045,-39.54227,-39.67081]}, + {"t":1.5904, "x":5.00956, "y":2.7982, "heading":2.09289, "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/choreo.chor b/src/main/deploy/choreo/choreo.chor index f73ce214..6f163365 100644 --- a/src/main/deploy/choreo/choreo.chor +++ b/src/main/deploy/choreo/choreo.chor @@ -3,7 +3,15 @@ "version":1, "type":"Swerve", "variables":{ - "expressions":{}, + "expressions":{ + "slow":{ + "dimension":"LinVel", + "var":{ + "exp":"1.25 m / s", + "val":1.25 + } + } + }, "poses":{ "A":{ "x":{ diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 84d7d9f6..f6cdaa9e 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -27,6 +27,7 @@ import java.util.HashMap; import java.util.Optional; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Autos { @@ -35,8 +36,9 @@ public class Autos { private final FunnelSubsystem funnel; private final AutoFactory factory; - public static boolean autoPreScore = false; - public static boolean autoScore = false; // TODO perhaps this should not be static + @AutoLogOutput public static boolean autoPreScore = true; + @AutoLogOutput public static boolean autoScore = false; // TODO perhaps this should not be static + @AutoLogOutput public static boolean autoGroundIntake = false; public Autos(SwerveSubsystem swerve, ManipulatorSubsystem manipulator, FunnelSubsystem funnel) { this.swerve = swerve; @@ -141,6 +143,7 @@ public void runPath( public Command LOtoJ() { final var routine = factory.newRoutine("LO to J"); bindElevatorExtension(routine); + routine.active().onTrue(Commands.print("Auto!")); HashMap steps = new HashMap(); // key - name of path, value - traj String[] stops = { @@ -164,11 +167,25 @@ public Command LOtoJ() { runPath(routine, startPos, endPos, nextPos, steps); } + // final var groundTraj = routine.trajectory("LtoAGround"); + + // routine + // .observe(steps.get("PLOtoL").recentlyDone()) + // .onTrue(scoreInAuto(() -> steps.get("PLOtoL").getFinalPose().get())) + // .and(() -> !manipulator.getSecondBeambreak() && !manipulator.getFirstBeambreak()) + // .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))) + // .onTrue(groundTraj.cmd().andThen(scoreInAuto(() -> groundTraj.getFinalPose().get()))) + // .onTrue(Commands.runOnce(() -> autoGroundIntake = true)); + + // routine + // .observe(groundTraj.done()) + // .onTrue(Commands.runOnce(() -> autoGroundIntake = false).ignoringDisable(true)); + routine .observe(steps.get("LtoPLM").done()) .onTrue(Commands.runOnce(() -> Robot.setCurrentTarget(ReefTarget.L2))); - return routine.cmd(); + return routine.cmd().alongWith(Commands.print("auto :(")); } public Command ROtoE() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c40b48ce..8ee2599c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -119,27 +120,29 @@ private RobotHardware(SwerveConstants swerveConstants) { } } - public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.REPLAY; + public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; // For replay to work properly this should match the hardware used in the log public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE; public static enum ReefTarget { L1( ElevatorSubsystem.L1_EXTENSION_METERS, - 12.0, + 3.0, WristSubsystem.WRIST_SCORE_L1_POS, ShoulderSubsystem.SHOULDER_SCORE_POS), L2( ElevatorSubsystem.L2_EXTENSION_METERS, + -15.0, WristSubsystem.WRIST_SCORE_L2_POS, ShoulderSubsystem.SHOULDER_SCORE_POS), L3( ElevatorSubsystem.L3_EXTENSION_METERS, + -15.0, WristSubsystem.WRIST_SCORE_L3_POS, ShoulderSubsystem.SHOULDER_SCORE_POS), L4( ElevatorSubsystem.L4_EXTENSION_METERS, - 20.0, + -20.0, WristSubsystem.WRIST_SCORE_L4_POS, ShoulderSubsystem.SHOULDER_SCORE_L4_POS); @@ -182,8 +185,9 @@ public static enum AlgaeScoreTarget { private static ReefTarget currentTarget = ReefTarget.L4; private AlgaeIntakeTarget algaeIntakeTarget = AlgaeIntakeTarget.STACK; private AlgaeScoreTarget algaeScoreTarget = AlgaeScoreTarget.NET; + private boolean leftHandedTarget = false; - @AutoLogOutput private boolean killVisionIK = false; + @AutoLogOutput private boolean killVisionIK = true; @AutoLogOutput private boolean haveAutosGenerated = false; @@ -284,7 +288,9 @@ public static enum AlgaeScoreTarget { }, PhoenixOdometryThread.getInstance(), swerveDriveSimulation, - new VisionIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants())); + ROBOT_TYPE != RobotType.SIM + ? new VisionIOReal(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants()) + : new VisionIOSim(ROBOT_HARDWARE.swerveConstants.getAlgaeVisionConstants())); private final ElevatorSubsystem elevator = new ElevatorSubsystem( @@ -299,19 +305,20 @@ public static enum AlgaeScoreTarget { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(60.0) - .withSupplyCurrentLimit(20.0) + .withStatorCurrentLimit(120.0) + .withSupplyCurrentLimit(30.0) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)) - .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(2)) - .withSlot0(new Slot0Configs().withKV(0.24).withKP(0.5)) - .withSlot1(new Slot1Configs().withKP(20).withKD(0.1).withKS(0.27))) + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) + .withFeedback( + new FeedbackConfigs() + .withSensorToMechanismRatio(ManipulatorSubsystem.GEAR_RATIO)) + .withSlot0(new Slot0Configs().withKV(0.928).withKP(0.5)) + .withSlot1(new Slot1Configs().withKP(7.5).withKD(0.5).withKS(0.39))) : new RollerIOSim( 0.01, - 2, - new SimpleMotorFeedforward(0.0, 0.24), + 5.8677, + new SimpleMotorFeedforward(0.0, 0.7), new ProfiledPIDController( 0.5, 0.0, 0.0, new TrapezoidProfile.Constraints(15, 1))), new BeambreakIOReal(1, true), @@ -327,7 +334,7 @@ public static enum AlgaeScoreTarget { 12, WristIOReal.getDefaultConfiguration() .withSlot0( - new Slot0Configs().withKP(1000.0).withKD(5.0).withKS(0.3).withKV(3.6)) + new Slot0Configs().withKP(1000.0).withKD(30.0).withKS(0.3).withKV(3.2)) .withMotionMagic(WristSubsystem.DEFAULT_MOTION_MAGIC) .withCurrentLimits( new CurrentLimitsConfigs() @@ -337,7 +344,11 @@ public static enum AlgaeScoreTarget { .withSupplyCurrentLimitEnable(true)) .withFeedback( new FeedbackConfigs() - .withSensorToMechanismRatio(WristSubsystem.WRIST_GEAR_RATIO))) + .withSensorToMechanismRatio(WristSubsystem.WRIST_GEAR_RATIO)) + .withSoftwareLimitSwitch( + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(0.5))) : new WristIOSim()); private final FunnelSubsystem funnel = @@ -424,7 +435,7 @@ public static enum AlgaeScoreTarget { }) .debounce(0.15)) // .or(() -> AutoAim.isInToleranceCoral(swerve.getPose())) - .or(() -> Autos.autoScore) + .or(() -> Autos.autoScore && DriverStation.isAutonomous()) // .or( // new Trigger( // () -> @@ -446,8 +457,24 @@ public static enum AlgaeScoreTarget { // .debounce(0.08) // .and(() -> swerve.hasFrontTags)) , - driver.rightTrigger().or(() -> Autos.autoPreScore), + driver + .rightTrigger() + .or(() -> Autos.autoPreScore && DriverStation.isAutonomous()) + .or( + () -> + swerve + .getPose() + .getTranslation() + .minus( + DriverStation.getAlliance().orElse(Alliance.Blue) + == Alliance.Blue + ? AutoAim.BLUE_REEF_CENTER + : AutoAim.RED_REEF_CENTER) + .getNorm() + < 3.25 + && DriverStation.isAutonomous()), driver.leftTrigger(), + driver.leftBumper().or(() -> Autos.autoGroundIntake && DriverStation.isAutonomous()), driver .x() .and(driver.pov(-1).negate()) @@ -463,9 +490,9 @@ public static enum AlgaeScoreTarget { operator.rightBumper(), operator.leftBumper(), new Trigger(() -> killVisionIK) - .or(() -> currentTarget == ReefTarget.L4) + .or(() -> currentTarget == ReefTarget.L1) .or(() -> DriverStation.isAutonomous()), - () -> MathUtil.clamp(operator.getLeftY(), -1.0, 0.0)); + () -> MathUtil.clamp(-operator.getLeftY(), -0.5, 0.5)); private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal()); @@ -576,6 +603,21 @@ public Robot() { .setCurrent(60.0) .raceWith(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS))); + SmartDashboard.putData( + "Check Clear", + Commands.parallel( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS))); + + SmartDashboard.putData( + "Manual Zero Extension", + Commands.runOnce( + () -> { + elevator.resetExtension(0.0); + wrist.resetPosition(Rotation2d.k180deg); + }) + .ignoringDisable(true)); + // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); @@ -591,12 +633,23 @@ public Robot() { || superstructure.getState() == SuperState.READY_CORAL) .onTrue(driver.rumbleCmd(1.0, 1.0).withTimeout(0.5)); - new Trigger(() -> DriverStation.isEnabled()) + new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoScore = false)); - new Trigger(() -> DriverStation.isEnabled()) + new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) .onTrue(Commands.runOnce(() -> Autos.autoPreScore = false)); + new Trigger(() -> DriverStation.isEnabled() && DriverStation.isTeleop()) + .onTrue(Commands.runOnce(() -> Autos.autoGroundIntake = false)); + + new Trigger(() -> DriverStation.isAutonomousEnabled() && !wrist.hasZeroed) + .onTrue( + Commands.runOnce( + () -> { + wrist.resetPosition(Rotation2d.fromRadians(3.059)); + elevator.resetExtension(0.0); + })); + new Trigger( () -> { var allianceChange = !DriverStation.getAlliance().equals(lastAlliance); @@ -646,7 +699,7 @@ public Robot() { elevator.setVoltage(0.0)) .withName("Elevator Default Command")); - manipulator.setDefaultCommand(manipulator.index()); + manipulator.setDefaultCommand(manipulator.hold()); shoulder.setDefaultCommand(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS)); @@ -671,7 +724,7 @@ public Robot() { DriverStation.getAlliance() .map((a) -> a == Alliance.Blue ? Color.kBlue : Color.kRed) .orElse(Color.kWhite), - () -> LEDSubsystem.PURPLE, + () -> wrist.hasZeroed ? LEDSubsystem.PURPLE : Color.kOrange, 4, 1.0) .until(() -> DriverStation.isEnabled()), @@ -716,7 +769,6 @@ public Robot() { .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); driver .rightBumper() - .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.INTAKE_ALGAE_HIGH @@ -747,7 +799,10 @@ public Robot() { Units.inchesToMeters(1.0), Units.degreesToRadians(1.0)) && elevator.isNearTarget() - && shoulder.isNearTarget()), + && shoulder.isNearAngle( + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS) + && wrist.isNearAngle( + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)), AutoAim.approachAlgae( swerve, () -> AlgaeIntakeTargets.getClosestTargetPose(swerve.getPose()), @@ -821,7 +876,6 @@ public Robot() { driver .rightBumper() - .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.PRE_CLIMB @@ -839,7 +893,6 @@ public Robot() { driver .rightBumper() - .or(driver.leftBumper()) .and( () -> superstructure.getState() == SuperState.READY_ALGAE @@ -939,6 +992,9 @@ public Robot() { .rightTrigger() .onTrue(Commands.runOnce(() -> algaeScoreTarget = AlgaeScoreTarget.PROCESSOR)); + operator.povLeft().onTrue(Commands.runOnce(() -> leftHandedTarget = true)); + operator.povRight().onTrue(Commands.runOnce(() -> leftHandedTarget = false)); + operator .back() .and(operator.start()) @@ -985,6 +1041,15 @@ public Robot() { Stream.of(AlgaeIntakeTargets.values()) .map((target) -> AlgaeIntakeTargets.getRobotTargetLocation(target.location)) .toArray(Pose2d[]::new)); + + Logger.recordOutput("IK/L1 FK Pose", ExtensionKinematics.L1_POSE); + System.out.println("ExtensionKinematics.L1_POSE: " + ExtensionKinematics.L1_POSE); + Logger.recordOutput("IK/L2 FK Pose", ExtensionKinematics.L2_POSE); + System.out.println("ExtensionKinematics.L2_POSE: " + ExtensionKinematics.L2_POSE); + Logger.recordOutput("IK/L3 FK Pose", ExtensionKinematics.L3_POSE); + System.out.println("ExtensionKinematics.L3_POSE: " + ExtensionKinematics.L3_POSE); + Logger.recordOutput("IK/L4 FK Pose", ExtensionKinematics.L4_POSE); + System.out.println("ExtensionKinematics.L4_POSE: " + ExtensionKinematics.L4_POSE); } private void addAutos() { @@ -1066,12 +1131,15 @@ public void robotPeriodic() { 0, -Units.degreesToRadians(2.794042) - shoulder.getAngle().getRadians(), 0.0)), new Pose3d( // Manipulator new Translation3d( - ShoulderSubsystem.X_OFFSET_METERS - + shoulder.getAngle().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, - 0, - elevator.getExtensionMeters() - + ShoulderSubsystem.Z_OFFSET_METERS - + shoulder.getAngle().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS), + ShoulderSubsystem.X_OFFSET_METERS + + shoulder.getAngle().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, + 0, + elevator.getExtensionMeters() + + ShoulderSubsystem.Z_OFFSET_METERS + + shoulder.getAngle().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS) + .plus( + new Translation3d(Units.inchesToMeters(1.0), 0.0, Units.inchesToMeters(-8.0)) + .rotateBy(new Rotation3d(0.0, -wrist.getAngle().getRadians(), 0.0))), new Rotation3d(0, wrist.getAngle().getRadians(), Math.PI)) }); @@ -1094,12 +1162,16 @@ public void robotPeriodic() { 0.0)), new Pose3d( // Manipulator new Translation3d( - ShoulderSubsystem.X_OFFSET_METERS - + shoulder.getSetpoint().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, - 0, - elevator.getSetpoint() - + ShoulderSubsystem.Z_OFFSET_METERS - + shoulder.getSetpoint().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS), + ShoulderSubsystem.X_OFFSET_METERS + + shoulder.getSetpoint().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, + 0, + elevator.getSetpoint() + + ShoulderSubsystem.Z_OFFSET_METERS + + shoulder.getSetpoint().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS) + .plus( + new Translation3d( + Units.inchesToMeters(1.0), 0.0, Units.inchesToMeters(-8.0)) + .rotateBy(new Rotation3d(0.0, -wrist.getAngle().getRadians(), 0.0))), new Rotation3d(0, wrist.getSetpoint().getRadians(), Math.PI)) }); @@ -1155,9 +1227,15 @@ public void robotPeriodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput("Autos/Score", Autos.autoScore); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput( - "Manipulator FK Pose", + "IK/Manipulator FK Pose", ExtensionKinematics.getManipulatorPose( swerve.getPose(), superstructure.getExtensionState())); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "IK/Extension FK Pose", + ExtensionKinematics.solveFK( + new ExtensionState( + elevator.getExtensionMeters(), shoulder.getAngle(), wrist.getAngle()))); state = superstructure::getState; } diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 51327652..fb4190ea 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -24,31 +24,28 @@ import java.util.function.Supplier; public class ExtensionKinematics { + + // These need to be here bc their main constants arent loaded when we need the constants in this + // class + private static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); + static final Transform2d IK_WRIST_TO_CORAL = + new Transform2d( + Units.inchesToMeters(12.0), Units.inchesToMeters(-6.842), Rotation2d.fromDegrees(0.0)); + private static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); + // Not super accurate bc of whack - public static final ExtensionState L1_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L1_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - WristSubsystem.WRIST_SCORE_L1_POS); - public static final Pose2d L1_POSE = solveFK(L1_EXTENSION); + public static final Pose2d L1_POSE = + new Pose2d(0.26, 0.4, Rotation2d.fromDegrees(15.0)); // solveFK(L1_EXTENSION); + public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); public static final ExtensionState L2_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L2_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_POS, - WristSubsystem.WRIST_SCORE_L2_POS); + new ExtensionState(0.33, Rotation2d.fromRadians(0.569), Rotation2d.fromRadians(2.447)); public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); public static final ExtensionState L3_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L3_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_POS, - WristSubsystem.WRIST_SCORE_L3_POS); + new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); - public static final ExtensionState L4_EXTENSION = - new ExtensionState( - ElevatorSubsystem.L4_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L4_POS, - WristSubsystem.WRIST_SCORE_L4_POS); - public static final Pose2d L4_POSE = solveFK(L4_EXTENSION); + public static final Pose2d L4_POSE = + new Pose2d(new Translation2d(0.25, 2.05), Rotation2d.fromDegrees(110.0)); + public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION = new ExtensionState( @@ -75,9 +72,9 @@ private ExtensionKinematics() {} */ public static ExtensionState solveIK(Pose2d target) { // Offset wrist pose from target - final var wristPose = target.transformBy(ManipulatorSubsystem.IK_WRIST_TO_CORAL.inverse()); + final var wristPose = target.transformBy(IK_WRIST_TO_CORAL.inverse()); // Find shoulder angle from needed horizontal extension - var shoulderAngle = Math.acos(wristPose.getX() / ShoulderSubsystem.ARM_LENGTH_METERS); + var shoulderAngle = Math.acos(wristPose.getX() / ARM_LENGTH_METERS); // Set angle to horizontal if we can't reach if (Double.isNaN(shoulderAngle)) shoulderAngle = 0.0; // Elevator goes to remaining needed height @@ -86,16 +83,13 @@ public static ExtensionState solveIK(Pose2d target) { .getTranslation() .minus( new Translation2d( - ShoulderSubsystem.ARM_LENGTH_METERS * Math.cos(shoulderAngle), - ShoulderSubsystem.ARM_LENGTH_METERS * Math.sin(shoulderAngle))) + ARM_LENGTH_METERS * Math.cos(shoulderAngle), + ARM_LENGTH_METERS * Math.sin(shoulderAngle))) .getY(); // If we're extending higher than we can reach, prioritize matching Z instead of X - if (elevatorHeight > ElevatorSubsystem.MAX_EXTENSION_METERS) { - elevatorHeight = ElevatorSubsystem.MAX_EXTENSION_METERS; - shoulderAngle = - Math.asin( - (target.getY() - ElevatorSubsystem.MAX_EXTENSION_METERS) - / ShoulderSubsystem.ARM_LENGTH_METERS); + if (elevatorHeight > MAX_EXTENSION_METERS) { + elevatorHeight = MAX_EXTENSION_METERS - Units.inchesToMeters(1.0); + shoulderAngle = Math.asin((wristPose.getY() - MAX_EXTENSION_METERS) / ARM_LENGTH_METERS); // Limit shoulder angle if (Double.isNaN(shoulderAngle) || shoulderAngle > Units.degreesToRadians(85.0)) { shoulderAngle = Units.degreesToRadians(85.0); @@ -105,16 +99,15 @@ public static ExtensionState solveIK(Pose2d target) { return new ExtensionState( MathUtil.clamp(elevatorHeight, 0.0, ElevatorSubsystem.MAX_EXTENSION_METERS), Rotation2d.fromRadians(shoulderAngle), - Rotation2d.fromDegrees(MathUtil.clamp(wristPose.getRotation().getDegrees(), -45.0, 45.0))); + Rotation2d.fromDegrees(MathUtil.clamp(wristPose.getRotation().getDegrees(), -45.0, 120.0))); } public static Pose2d solveFK(ExtensionState state) { return new Pose2d( - state.shoulderAngle().getCos() * ShoulderSubsystem.ARM_LENGTH_METERS, - state.elevatorHeightMeters() - + state.shoulderAngle().getSin() * ShoulderSubsystem.ARM_LENGTH_METERS, + state.shoulderAngle().getCos() * ARM_LENGTH_METERS, + state.elevatorHeightMeters() + state.shoulderAngle().getSin() * ARM_LENGTH_METERS, state.wristAngle()) - .transformBy(ManipulatorSubsystem.IK_WRIST_TO_CORAL); + .transformBy(IK_WRIST_TO_CORAL); } public static ExtensionState getPoseCompensatedExtension(Pose2d pose, ExtensionState target) { @@ -165,14 +158,50 @@ public static Command holdStateCommand( ShoulderSubsystem shoulder, WristSubsystem wrist, Supplier target) { - final LinearFilter elevatorFilter = LinearFilter.movingAverage(5); - final LinearFilter shoulderFilter = LinearFilter.movingAverage(5); - final LinearFilter wristFilter = LinearFilter.movingAverage(5); + final LinearFilter elevatorFilter = LinearFilter.movingAverage(8); + final LinearFilter shoulderFilter = LinearFilter.movingAverage(8); + final LinearFilter wristFilter = LinearFilter.movingAverage(8); return Commands.runOnce( () -> { - elevatorFilter.reset(); - shoulderFilter.reset(); - wristFilter.reset(); + elevatorFilter.reset( + new double[] { + // i hate java surely theres a better way to do this + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters(), + elevator.getExtensionMeters() + }, + new double[0]); + shoulderFilter.reset( + new double[] { + // i hate java surely theres a better way to do this + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations(), + shoulder.getAngle().getRotations() + }, + new double[0]); + wristFilter.reset( + new double[] { + // i hate java surely theres a better way to do this + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations(), + wrist.getAngle().getRotations() + }, + new double[0]); }) .andThen( Commands.parallel( diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 5103ceca..664b90d3 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -4,13 +4,14 @@ package frc.robot.subsystems; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; import frc.robot.Robot.RobotType; import frc.robot.subsystems.beambreak.BeambreakIO; @@ -24,12 +25,16 @@ public class ManipulatorSubsystem extends RollerSubsystem { public static final String NAME = "Manipulator"; - public static final double ALGAE_INTAKE_VOLTAGE = -10.0; - public static final double ALGAE_HOLDING_VOLTAGE = -3.0; - public static final double ALGAE_CURRENT_THRESHOLD = 30.0; - public static final Transform2d IK_WRIST_TO_CORAL = - new Transform2d( - Units.inchesToMeters(1.0), Units.inchesToMeters(3.0), Rotation2d.fromDegrees(0.0)); + public static final double CORAL_INTAKE_VELOCITY = -18.0; + public static final double JOG_POS = 0.75; + public static final double ALGAE_INTAKE_VOLTAGE = 10.0; + public static final double ALGAE_HOLDING_VOLTAGE = 1.0; + public static final double ALGAE_CURRENT_THRESHOLD = 6.0; + public static final Transform2d IK_WRIST_TO_CORAL = ExtensionKinematics.IK_WRIST_TO_CORAL; + + public static final double CORAL_HOLD_POS = 0.5; + + public static final double GEAR_RATIO = (58.0 / 10.0) * (24.0 / 18.0); private final BeambreakIO firstBBIO, secondBBIO; private final BeambreakIOInputsAutoLogged firstBBInputs = new BeambreakIOInputsAutoLogged(), @@ -44,6 +49,8 @@ public class ManipulatorSubsystem extends RollerSubsystem { private Timer zeroTimer = new Timer(); + private double positionSetpoint = 0.0; + /** Creates a new Manipulator. */ public ManipulatorSubsystem(RollerIO rollerIO, BeambreakIO firstBBIO, BeambreakIO secondBBIO) { super(rollerIO, NAME); @@ -75,8 +82,16 @@ public void periodic() { Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.0)); zeroTimer.reset(); } + + if (!firstBBInputs.get && secondBBInputs.get) { + // Number calculated from coral length, may need tuning + Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(1.0)); + zeroTimer.reset(); + } } + /** For the old ee */ + @Deprecated public Command index() { return Commands.sequence( setVelocity(9.0) @@ -95,27 +110,69 @@ public Command index() { public Command jog(double rotations) { return Commands.sequence( // this.runOnce(() -> io.resetEncoder(0.0)), - this.run(() -> io.setPosition(Rotation2d.fromRotations(rotations)))); + this.run( + () -> { + io.setPosition(Rotation2d.fromRotations(rotations)); + positionSetpoint = rotations; + })); } public Command jog(DoubleSupplier rotations) { return Commands.sequence( // this.runOnce(() -> io.resetEncoder(0.0)), - this.run(() -> io.setPosition(Rotation2d.fromRotations(rotations.getAsDouble())))); + this.run( + () -> { + io.setPosition(Rotation2d.fromRotations(rotations.getAsDouble())); + positionSetpoint = rotations.getAsDouble(); + })); } public Command hold() { - return this.jog(inputs.positionRotations).until(() -> true).andThen(this.run(() -> {})); + return this.jog(() -> inputs.positionRotations) + .until(() -> true) + .andThen(this.run(() -> {})) + .until(() -> !MathUtil.isNear(positionSetpoint, inputs.positionRotations, 2.0)) + .repeatedly(); + } + + public void resetPosition(final double rotations) { + io.resetEncoder(rotations); } - public Command backIndex() { + public Command intakeCoral() { + return intakeCoral(CORAL_INTAKE_VELOCITY); + } + + public Command intakeCoralAir(double vel) { return Commands.sequence( - setVelocity(-INDEXING_VELOCITY).until(() -> !secondBBInputs.get), index()); + setVelocity(vel) + .until(() -> secondBBInputs.get) + .finallyDo( + () -> { + io.setPosition(Rotation2d.fromRotations(0.63)); + positionSetpoint = 0.63; + }), + setVoltage(2.0).until(() -> !firstBBInputs.get), + jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); + } + + public Command intakeCoral(double vel) { + return Commands.sequence( + setVelocity(vel).until(new Trigger(() -> secondBBInputs.get).debounce(0.5)), + Commands.runOnce( + () -> { + io.setPosition(Rotation2d.fromRotations(0.5)); + positionSetpoint = 0.5; + }), + setVelocity(1.0).until(() -> !firstBBInputs.get), + jog(CORAL_HOLD_POS).until(() -> !secondBBInputs.get && !firstBBInputs.get)); } public Command intakeAlgae() { return this.run(() -> io.setVoltage(ALGAE_INTAKE_VOLTAGE)) - .until(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + .until( + new Trigger(() -> Math.abs(currentFilterValue) > ALGAE_CURRENT_THRESHOLD) + .debounce(0.75)) .andThen(this.run(() -> io.setVoltage(ALGAE_HOLDING_VOLTAGE))); } @@ -123,6 +180,10 @@ public double getStatorCurrentAmps() { return currentFilterValue; } + public double getTimeSinceZero() { + return zeroTimer.get(); + } + public boolean getFirstBeambreak() { return firstBBInputs.get || bb1; } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index b4ab4447..5551b756 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,10 +1,8 @@ package frc.robot.subsystems; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -22,6 +20,7 @@ import frc.robot.subsystems.shoulder.ShoulderSubsystem; import frc.robot.subsystems.wrist.WristSubsystem; import frc.robot.utils.autoaim.AlgaeIntakeTargets; +import frc.robot.utils.autoaim.CoralTargets; import frc.robot.utils.autoaim.HumanPlayerTargets; import java.util.HashMap; import java.util.Map; @@ -35,6 +34,7 @@ public class Superstructure { public static enum SuperState { IDLE, HOME, + INTAKE_CORAL_GROUND, READY_CORAL, SPIT_CORAL, PRE_L1, @@ -73,6 +73,9 @@ public static enum SuperState { @AutoLogOutput(key = "Superstructure/Algae Intake Request") private final Trigger intakeAlgaeReq; + @AutoLogOutput(key = "Superstructure/Coral Intake Request") + private final Trigger intakeCoralReq; + @AutoLogOutput(key = "Superstructure/Pre Climb Request") private final Trigger preClimbReq; @@ -129,6 +132,7 @@ public Superstructure( Trigger scoreReq, Trigger preScoreReq, Trigger intakeAlgaeReq, + Trigger intakeCoralReq, Trigger climbReq, Trigger climbConfReq, Trigger climbCancelReq, @@ -155,6 +159,7 @@ public Superstructure( this.scoreReq = scoreReq; this.intakeAlgaeReq = intakeAlgaeReq; + this.intakeCoralReq = intakeCoralReq; this.preClimbReq = climbReq; this.climbConfReq = climbConfReq; @@ -196,10 +201,11 @@ private void configureStateTransitionCommands() { // .until(() -> elevator.isNearExtension(Units.inchesToMeters(5.0))) // .andThen( extendWithClearance( - ElevatorSubsystem.HP_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_HP_POS, - WristSubsystem.WRIST_HP_POS)) // ) - .whileTrue(manipulator.index().repeatedly()) + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS) + .repeatedly()) // ) + .whileTrue(manipulator.intakeCoralAir(-7.0).repeatedly()) .whileTrue( funnel.setVoltage( () -> @@ -216,11 +222,17 @@ private void configureStateTransitionCommands() { .min(Double::compare) .get() < 1.0) - ? 5.0 + ? 1.0 : 0.0))) .and(manipulator::getFirstBeambreak) + .and(() -> manipulator.getTimeSinceZero() < 1.0) .onTrue(this.forceState(SuperState.READY_CORAL)); + stateTriggers + .get(SuperState.IDLE) + .and(intakeCoralReq) + .onTrue(this.forceState(SuperState.INTAKE_CORAL_GROUND)); + // IDLE -> INTAKE_ALGAE_{location} stateTriggers .get(SuperState.IDLE) @@ -274,17 +286,64 @@ private void configureStateTransitionCommands() { .get(SuperState.HOME) .whileTrue( Commands.parallel( - elevator.runCurrentZeroing(), wrist.currentZero(() -> shoulder.getInputs()))) + shoulder.setTargetAngle(Rotation2d.fromDegrees(55.0)), + elevator.runCurrentZeroing(), + Commands.waitUntil(() -> shoulder.getAngle().getDegrees() > 20.0) + .andThen(wrist.currentZero(() -> shoulder.getInputs())))) .and(() -> elevator.hasZeroed && wrist.hasZeroed && !homeReq.getAsBoolean()) .onTrue(this.forceState(prevState)); + stateTriggers + .get(SuperState.INTAKE_CORAL_GROUND) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.GROUND_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS, + WristSubsystem.WRIST_CORAL_GROUND) + .until(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CORAL_GROUND_POS)) + .andThen( + Commands.parallel( + shoulder.setVoltage(-1.0), + elevator.setExtension(ElevatorSubsystem.GROUND_EXTENSION_METERS), + wrist + .setTargetAngle(WristSubsystem.WRIST_CORAL_GROUND) + .until(wrist::isNearTarget) + .andThen(wrist.setVoltage(-1.0))))) + .whileTrue( + Commands.waitUntil(() -> shoulder.getAngle().getDegrees() < 20.0) + .andThen(Commands.runOnce(() -> shoulder.rezero()))) + .whileTrue(manipulator.intakeCoral().repeatedly().until(intakeCoralReq.negate())); + + stateTriggers + .get(SuperState.INTAKE_CORAL_GROUND) + .and(() -> manipulator.getSecondBeambreak() && manipulator.getFirstBeambreak()) + .and(intakeCoralReq.negate()) + .debounce(0.060) + .onTrue(Commands.runOnce(() -> manipulator.resetPosition(0.792))) + .onTrue(this.forceState(SuperState.READY_CORAL)); + + stateTriggers + .get(SuperState.INTAKE_CORAL_GROUND) + .and(intakeCoralReq.negate()) + .and(() -> !manipulator.getFirstBeambreak() || !manipulator.getSecondBeambreak()) + .onTrue(this.forceState(SuperState.IDLE)); + // READY_CORAL logic stateTriggers .get(SuperState.READY_CORAL) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_RETRACTED_POS)) - .whileTrue(elevator.setExtension(ElevatorSubsystem.HP_EXTENSION_METERS)) - .whileTrue(manipulator.index()); + .whileTrue( + extendWithClearance( + ElevatorSubsystem.HP_EXTENSION_METERS, + ShoulderSubsystem.SHOULDER_HP_POS, + WristSubsystem.WRIST_HP_POS)) + .whileTrue( + manipulator + .hold() + .until( + () -> + shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_HP_POS) + && wrist.isNearAngle(WristSubsystem.WRIST_HP_POS)) + .andThen(manipulator.jog(ManipulatorSubsystem.JOG_POS))); // keep indexing to make sure its chilling stateTriggers @@ -348,12 +407,19 @@ private void configureStateTransitionCommands() { // PRE_L{1-4} logic + -> SCORE_CORAL stateTriggers .get(SuperState.PRE_L1) + // .whileTrue( + // this.extendWithClearance( + // ElevatorSubsystem.L1_EXTENSION_METERS, + // ShoulderSubsystem.SHOULDER_SCORE_L1_POS, + // WristSubsystem.WRIST_SCORE_L1_POS)) .whileTrue( this.extendWithClearance( - ElevatorSubsystem.L1_EXTENSION_METERS, - ShoulderSubsystem.SHOULDER_SCORE_L1_POS, - WristSubsystem.WRIST_SCORE_L1_POS)) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.L1_EXTENSION + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), ExtensionKinematics.L1_EXTENSION))) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(() -> elevator.isNearExtension(ElevatorSubsystem.L1_EXTENSION_METERS)) .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS)) .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_SCORE_L1_POS)) @@ -374,7 +440,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L2_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L2_EXTENSION))) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -392,7 +458,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L3_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L3_EXTENSION))) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -410,7 +476,7 @@ private void configureStateTransitionCommands() { ? ExtensionKinematics.L4_EXTENSION : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), ExtensionKinematics.L4_EXTENSION))) - .whileTrue(manipulator.jog(() -> 1.4 + coralAdjust.getAsDouble())) + .whileTrue(manipulator.jog(() -> ManipulatorSubsystem.JOG_POS + coralAdjust.getAsDouble())) .and(scoreReq) .onTrue(this.forceState(SuperState.SCORE_CORAL)); @@ -423,50 +489,81 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.SCORE_CORAL) .whileTrue( + // Commands.either( + // Commands.parallel( + // ExtensionKinematics.holdStateCommand( + // elevator, + // shoulder, + // wrist, + // () -> + // killVisionIK.getAsBoolean() + // ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + // : ExtensionKinematics.getPoseCompensatedExtension( + // pose.get(), + // ExtensionKinematics.getExtensionForLevel( + // reefTarget.get())))), Commands.either( - Commands.parallel( - elevator.setExtension(ElevatorSubsystem.L1_EXTENSION_METERS), - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SCORE_L1_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_L1_POS)), - // Score on L2-4 - Commands.either( - Commands.parallel( - ExtensionKinematics.holdStateCommand( - elevator, - shoulder, - wrist, - () -> - killVisionIK.getAsBoolean() - ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) - : ExtensionKinematics.getPoseCompensatedExtension( + this.holdExtension( + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), + ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), + this.extendWithClearance( + () -> + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + : ExtensionKinematics.getPoseCompensatedExtension( + pose.get(), + ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), + () -> + elevator.isNearExtension( + killVisionIK.getAsBoolean() + ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + .elevatorHeightMeters() + : ExtensionKinematics.getPoseCompensatedExtension( pose.get(), - ExtensionKinematics.getExtensionForLevel( - reefTarget.get())))), - this.extendWithClearance( - () -> + ExtensionKinematics.getExtensionForLevel(reefTarget.get())) + .elevatorHeightMeters()) + && shoulder.isNearAngle( killVisionIK.getAsBoolean() ? ExtensionKinematics.getExtensionForLevel(reefTarget.get()) + .shoulderAngle() : ExtensionKinematics.getPoseCompensatedExtension( - pose.get(), - ExtensionKinematics.getExtensionForLevel(reefTarget.get()))), - // End L2-4 - () -> - MathUtil.isNear( - elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0))), - () -> reefTarget.get() == ReefTarget.L1)) - .and(() -> elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) - .whileTrue(manipulator.setVelocity(() -> reefTarget.get().outtakeSpeed)) - .and(() -> reefTarget.get() == ReefTarget.L1) - .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) + pose.get(), + ExtensionKinematics.getExtensionForLevel(reefTarget.get())) + .shoulderAngle())) + // // End + // () -> + // MathUtil.isNear( + // elevator.getExtensionMeters(), 0.0, Units.inchesToMeters(4.0)))) + ) .whileTrue( - Commands.waitSeconds(0.1) - .andThen(wrist.setTargetAngle(WristSubsystem.WRIST_WHACK_L1_POS))); + manipulator + .hold() + .until( + () -> + elevator.isNearTarget() && shoulder.isNearTarget() && wrist.isNearTarget()) + .andThen(manipulator.setVelocity(() -> reefTarget.get().outtakeSpeed))) + // .and(() -> reefTarget.get() == ReefTarget.L1) + // .whileTrue(elevator.setExtension(ElevatorSubsystem.L1_WHACK_CORAL_EXTENSION_METERS)) + // .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_WHACK_L1_POS)) + // .whileTrue( + // Commands.waitSeconds(0.1) + // .andThen(wrist.setTargetAngle(WristSubsystem.WRIST_WHACK_L1_POS))) + ; stateTriggers .get(SuperState.SCORE_CORAL) .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) + .and( + () -> + CoralTargets.getClosestTarget(pose.get()) + .getTranslation() + .getDistance(pose.get().getTranslation()) + > 0.3) .debounce(0.15) .onTrue(forceState(SuperState.IDLE)); @@ -475,6 +572,12 @@ private void configureStateTransitionCommands() { .and(() -> !manipulator.getFirstBeambreak() && !manipulator.getSecondBeambreak()) .and(() -> !intakeAlgaeReq.getAsBoolean() || !intakeTargetOnReef()) .and(killVisionIK) + .and( + () -> + CoralTargets.getClosestTarget(pose.get()) + .getTranslation() + .getDistance(pose.get().getTranslation()) + > 0.3) .onTrue(forceState(SuperState.IDLE)); stateTriggers @@ -495,8 +598,10 @@ private void configureStateTransitionCommands() { // ANTI_JAM logic stateTriggers .get(SuperState.ANTI_JAM) - .whileTrue(elevator.setExtension(ElevatorSubsystem.L2_EXTENSION_METERS)) - .whileTrue(manipulator.setVelocity(10)) + .whileTrue(elevator.hold()) + .whileTrue(wrist.hold()) + .whileTrue(shoulder.hold()) + .whileTrue(manipulator.setVelocity(-10)) .whileTrue(funnel.setVoltage(-10.0)); stateTriggers @@ -526,13 +631,11 @@ private void configureStateTransitionCommands() { // INTAKE_ALGAE_{location} -> READY_ALGAE stateTriggers .get(SuperState.INTAKE_ALGAE_GROUND) - .whileTrue(elevator.setExtension(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( - Commands.waitUntil( - () -> elevator.isNearExtension(ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION)) - .andThen( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS))) + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( Commands.waitUntil( () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS)) @@ -540,33 +643,33 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.INTAKE_ALGAE_LOW) - .whileTrue(this.extendWithClearance(() -> ExtensionKinematics.LOW_ALGAE_EXTENSION)) + .whileTrue( + this.extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_LOW_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); stateTriggers .get(SuperState.INTAKE_ALGAE_HIGH) - .whileTrue(this.extendWithClearance(() -> ExtensionKinematics.HIGH_ALGAE_EXTENSION)) + .whileTrue( + this.extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_HIGH_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_REEF_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_REEF_POS)) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)); stateTriggers .get(SuperState.INTAKE_ALGAE_STACK) - .whileTrue(elevator.setExtension(ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION)) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) .whileTrue( Commands.waitUntil( () -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .andThen(manipulator.setVoltage(-12.0))) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) - .and( - () -> - Robot.ROBOT_TYPE == RobotType.REAL - ? manipulator.getStatorCurrentAmps() - > ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD - : manipulator.hasAlgae()) - .and(() -> elevator.isNearExtension(ElevatorSubsystem.INTAKE_ALGAE_STACK_EXTENSION)) - .and(() -> wrist.isNearAngle(WristSubsystem.WRIST_INTAKE_ALGAE_STACK_POS)) - .and(() -> shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_STACK_POS)) - .onTrue(forceState(SuperState.CHECK_ALGAE)); + .andThen(manipulator.setVoltage(12.0))); // leave intake stateTriggers @@ -596,7 +699,10 @@ private void configureStateTransitionCommands() { ? WristSubsystem.WRIST_RETRACTED_POS : WristSubsystem.WRIST_INTAKE_ALGAE_REEF_RETRACT_POS)) .and(() -> stateTimer.hasElapsed(1.0)) - .and(() -> manipulator.getStatorCurrentAmps() > 20.0 || Robot.ROBOT_TYPE == RobotType.SIM) + .and( + () -> + manipulator.getStatorCurrentAmps() > ManipulatorSubsystem.ALGAE_CURRENT_THRESHOLD + || Robot.ROBOT_TYPE == RobotType.SIM) .and( () -> AlgaeIntakeTargets.getClosestTargetPose(pose.get()) @@ -612,8 +718,10 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.READY_ALGAE) .whileTrue( - extendWithClearance( - 0.0, ShoulderSubsystem.SHOULDER_RETRACTED_POS, WristSubsystem.WRIST_RETRACTED_POS)) + extendWithClearanceSlow( + () -> 0.1, + () -> ShoulderSubsystem.SHOULDER_RETRACTED_POS, + () -> WristSubsystem.WRIST_READY_ALGAE)) .whileTrue(manipulator.intakeAlgae()); // READY_ALGAE -> PRE_NET stateTriggers @@ -651,8 +759,8 @@ private void configureStateTransitionCommands() { // PRE_PROCESSOR logic stateTriggers .get(SuperState.PRE_PROCESSOR) - .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) + .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) + .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) .whileTrue(manipulator.setVoltage(ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) .and(() -> elevator.isNearExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) @@ -661,7 +769,7 @@ private void configureStateTransitionCommands() { // PRE_NET logic stateTriggers .get(SuperState.PRE_NET) - .whileTrue(manipulator.setVoltage(2 * ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) + .whileTrue(manipulator.setVoltage(3 * ManipulatorSubsystem.ALGAE_HOLDING_VOLTAGE)) .whileTrue( Commands.parallel( elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_NET_EXTENSION), @@ -676,24 +784,31 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.SCORE_ALGAE_NET) .onTrue(Commands.runOnce(() -> stateTimer.reset())) - .whileTrue(manipulator.setVoltage(13.0)) + .whileTrue(manipulator.setVoltage(-13.0)) .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_NET_EXTENSION)) .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SHOOT_NET_POS)) .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SHOOT_NET_POS)) - .and(() -> stateTimer.hasElapsed(1)) + .and(() -> stateTimer.hasElapsed(0.5)) + .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS)) + .and(() -> stateTimer.hasElapsed(1.0)) .onTrue(forceState(SuperState.IDLE)); stateTriggers .get(SuperState.SCORE_ALGAE_PROCESSOR) - .whileTrue(elevator.setExtension(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) - .whileTrue(shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_RETRACTED_POS)) - .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_RETRACTED_POS)) - .whileTrue(manipulator.setVoltage(-ManipulatorSubsystem.ALGAE_INTAKE_VOLTAGE)) - .and(() -> stateTimer.hasElapsed(1.0)) + .whileTrue(elevator.setExtensionSlow(ElevatorSubsystem.ALGAE_PROCESSOR_EXTENSION)) + .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS)) + .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS)) + .whileTrue(manipulator.setVoltage(-2.0)) + .and(() -> stateTimer.hasElapsed(2.0)) .onTrue(this.forceState(SuperState.IDLE)); stateTriggers .get(SuperState.PRE_CLIMB) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue(climber.setPosition(ClimberSubsystem.CLIMB_EXTENDED_POSITION)) .onTrue(funnel.unlatch()) // !! .and(climbConfReq) @@ -701,6 +816,11 @@ private void configureStateTransitionCommands() { stateTriggers .get(SuperState.CLIMB) + .whileTrue( + extendWithClearance( + ElevatorSubsystem.INTAKE_ALGAE_GROUND_EXTENSION, + ShoulderSubsystem.SHOULDER_INTAKE_ALGAE_GROUND_POS, + WristSubsystem.WRIST_INTAKE_ALGAE_GROUND_POS)) .whileTrue( climber .setPositionSlow(1.35) @@ -722,12 +842,16 @@ private Command extendWithClearance(ExtensionState extension) { extension.elevatorHeightMeters(), extension.shoulderAngle(), extension.wristAngle()); } - private Command extendWithClearance(Supplier extension) { + public Command extendWithClearance(Supplier extension) { return extendWithClearance( () -> extension.get().elevatorHeightMeters(), () -> extension.get().shoulderAngle(), () -> extension.get().wristAngle()) - .until(() -> elevator.isNearExtension(extension.get().elevatorHeightMeters())) + .until( + () -> + elevator.isNearExtension(extension.get().elevatorHeightMeters()) + && shoulder.isNearAngle(extension.get().shoulderAngle()) + && wrist.isNearAngle(extension.get().wristAngle())) .andThen(ExtensionKinematics.holdStateCommand(elevator, shoulder, wrist, extension)); } @@ -736,31 +860,182 @@ private Command extendWithClearance( return extendWithClearance(() -> elevatorExtension, () -> shoulderAngle, () -> wristAngle); } + private boolean shouldntTuck(Rotation2d shoulderAngle) { + return wrist.getAngle().getDegrees() < 100.0 + || elevator.getExtensionMeters() > 0.75 + || shoulderAngle.getDegrees() > 60.0; + } + private Command extendWithClearance( DoubleSupplier elevatorExtension, Supplier shoulderAngle, Supplier wristAngle) { + return Commands.sequence( + // Retract shoulder + wrist + Commands.parallel( + shoulder + .run(() -> {}) + .until( + () -> + wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 + > shoulder.getAngle().getDegrees()) + .andThen( + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle( + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get()))), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get())), + elevator.hold()) + // .unless( + // () -> + // shoulder.getAngle().getDegrees() + // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() + // && wrist.getAngle().getDegrees() < 90.0) + .until( + () -> + shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 + > shoulder.getAngle().getDegrees() + && wrist.isNearTarget()) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), + // extend elevator + Commands.parallel( + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get())), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get())), + elevator.setExtension(elevatorExtension)) + .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), + // re-extend joints + Commands.parallel( + shoulder + .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) + // .unless( + // () -> + // shouldntTuck() + // || shoulderAngle.get().getDegrees() + // < + // ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .until( + () -> + wrist.isNearTarget() + && (wrist.getAngle().getDegrees() < 120.0 + || !elevator.isNearExtension(1.4, 0.25))) + .withTimeout(1.0) + .andThen(shoulder.setTargetAngle(shoulderAngle)), + wrist + .hold() + .until( + new Trigger(() -> shoulder.isNearAngle(shoulderAngle.get())) + .debounce(0.040)) + .withTimeout(0.5) + .unless( + () -> + wristAngle.get().getDegrees() < 90.0 + || shoulderAngle.get().getDegrees() + > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees() + + 5) + .andThen(wrist.setTargetAngle(wristAngle)), + elevator.setExtension(elevatorExtension))) + .finallyDo( + (interrupted) -> { + if (interrupted) { + System.out.println("interrupted clearance extend"); + } + }); + } + + private Command holdExtension(Supplier state) { + return Commands.parallel( + elevator.setExtension(() -> state.get().elevatorHeightMeters()), + shoulder.setTargetAngle(() -> state.get().shoulderAngle()), + wrist.setTargetAngle(() -> state.get().wristAngle())); + } + + private Command extendWithClearanceSlow( + DoubleSupplier elevatorExtension, + Supplier shoulderAngle, + Supplier wristAngle) { return Commands.sequence( // Retract shoulder + wrist Commands.parallel( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + shoulder + .run(() -> {}) + .until( + () -> + wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 + > shoulder.getAngle().getDegrees()) + .andThen( + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle( + ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get()))), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get())), elevator.hold()) + // .unless( + // () -> + // shoulder.getAngle().getDegrees() + // < ShoulderSubsystem.SHOULDER_CLEARANCE_POS.getDegrees() + // && wrist.getAngle().getDegrees() < 90.0) .until( () -> - shoulder.isNearAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS) - && wrist.isNearAngle(WristSubsystem.WRIST_CLEARANCE_POS)), + shoulder.isNearTarget() && wrist.getAngle().getDegrees() < 90.0 + || wrist.isNearAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS) + || wrist.getAngle().getDegrees() - 115.0 > shoulder.getAngle().getDegrees() + && wrist.isNearTarget()) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.150)), // extend elevator Commands.parallel( - shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), - wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), - elevator.setExtension(elevatorExtension)) - .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)), + Commands.either( + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_CLEARANCE_POS), + shoulder.setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get())), + Commands.either( + wrist.setTargetAngle(WristSubsystem.WRIST_CLEARANCE_POS), + wrist.setTargetAngle(WristSubsystem.WRIST_TUCKED_CLEARANCE_POS), + () -> shouldntTuck(shoulderAngle.get())), + elevator.setExtensionSlow(elevatorExtension)) + .until(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.08)) + .unless(() -> elevator.isNearExtension(elevatorExtension.getAsDouble(), 0.080)), // re-extend joints Commands.parallel( - shoulder.setTargetAngle(shoulderAngle), - wrist.setTargetAngle(wristAngle), - elevator.setExtension(elevatorExtension))); + shoulder + .setTargetAngle(ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS) + .unless( + () -> + shouldntTuck(shoulderAngle.get()) + || shoulderAngle.get().getDegrees() + < ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .until(() -> wrist.isNearTarget()) + .andThen(shoulder.setTargetAngle(shoulderAngle)), + wrist + .hold() + .until(() -> shoulder.isNearTarget()) + .unless( + () -> + wristAngle.get().getDegrees() < 90.0 + || shoulderAngle.get().getDegrees() + > ShoulderSubsystem.SHOULDER_TUCKED_CLEARANCE_POS.getDegrees()) + .andThen(wrist.setTargetAngle(wristAngle)), + elevator.setExtensionSlow(elevatorExtension))); } public SuperState getState() { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 3f2ffaf4..f4b56ccb 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -68,7 +68,6 @@ public void resetEncoder(double position) { @Override public void setPosition(double position, double vel) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); + // We dont simulate the climber really so im gonna ignore this } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 8a9e5a9c..d8ac1320 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.ExtensionKinematics; import java.util.function.DoubleSupplier; import java.util.function.Function; import org.littletonrobotics.junction.Logger; @@ -40,21 +41,26 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(63.50); - public static final double L1_EXTENSION_METERS = Units.inchesToMeters(12.0); + public static final double L1_EXTENSION_METERS = + ExtensionKinematics.L1_EXTENSION.elevatorHeightMeters(); public static final double L1_WHACK_CORAL_EXTENSION_METERS = Units.inchesToMeters(24.0); - public static final double L2_EXTENSION_METERS = Units.inchesToMeters(17.0); - public static final double L3_EXTENSION_METERS = Units.inchesToMeters(31.5); - public static final double L4_EXTENSION_METERS = Units.inchesToMeters(58.0); - - public static final double INTAKE_ALGAE_GROUND_EXTENSION = Units.inchesToMeters(5.0); - public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(12.5); - public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(25.4); - public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(40.5); + public static final double L2_EXTENSION_METERS = + ExtensionKinematics.L2_EXTENSION.elevatorHeightMeters(); + public static final double L3_EXTENSION_METERS = + ExtensionKinematics.L3_EXTENSION.elevatorHeightMeters(); + public static final double L4_EXTENSION_METERS = + ExtensionKinematics.L4_EXTENSION.elevatorHeightMeters(); + + public static final double INTAKE_ALGAE_GROUND_EXTENSION = 0.135; + public static final double INTAKE_ALGAE_STACK_EXTENSION = Units.inchesToMeters(11.5); + public static final double INTAKE_ALGAE_LOW_EXTENSION = Units.inchesToMeters(20.0); + public static final double INTAKE_ALGAE_HIGH_EXTENSION = Units.inchesToMeters(35.0); public static final double ALGAE_NET_EXTENSION = Units.inchesToMeters(61.5); - public static final double ALGAE_PROCESSOR_EXTENSION = 0.0; + public static final double ALGAE_PROCESSOR_EXTENSION = 0.01; public static final double HP_EXTENSION_METERS = Units.inchesToMeters(0.0); + public static final double GROUND_EXTENSION_METERS = Units.inchesToMeters(0.0); public static final double MAX_ACCELERATION = 10.0; public static final double SLOW_ACCELERATION = 5.0; @@ -126,8 +132,7 @@ public Command setExtension(DoubleSupplier meters) { () -> { io.setTarget(meters.getAsDouble(), MAX_ACCELERATION); setpoint = meters.getAsDouble(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Elevator/Setpoint", setpoint); + Logger.recordOutput("Elevator/Setpoint", setpoint); }); } @@ -264,4 +269,9 @@ public boolean isNearExtension(double expected) { public boolean isNearExtension(double expected, double toleranceMeters) { return MathUtil.isNear(expected, inputs.positionMeters, toleranceMeters); } + + public void resetExtension(double extension) { + io.resetEncoder(extension); + hasZeroed = true; + } } diff --git a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java index 3fe028c8..3a37e16b 100644 --- a/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/roller/RollerIOSim.java @@ -66,18 +66,22 @@ public void registerSimulationCallback(Consumer callba @Override public void setPosition(Rotation2d rot) { - // TODO Actually simulate + setVelocity(0.0); + // i cba to impl this properly rn + resetEncoder(rot.getRotations()); } @Override public void resetEncoder(double position) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'resetEncoder'"); + motorSim.setAngle(position); } @Override public void setCurrent(double amps) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setCurrent'"); + setVoltage( + DCMotor.getKrakenX60Foc(1) + .getVoltage( + DCMotor.getKrakenX60Foc(1).getTorque(amps), + motorSim.getAngularVelocityRadPerSec())); } } diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java index 0ba8f4b0..fae583d4 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOReal.java @@ -51,8 +51,8 @@ public ShoulderIOReal() { final TalonFXConfiguration config = new TalonFXConfiguration(); config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.kG = 8.6; - config.Slot0.kS = 2.5; + config.Slot0.kG = 8.0; + config.Slot0.kS = 3.5; config.Slot0.kV = 0.0; config.Slot0.kA = 0.0; config.Slot0.kP = 8000.0; diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java index 12086ad4..49be3de9 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSim.java @@ -28,7 +28,7 @@ public class ShoulderIOSim implements ShoulderIO { private final ArmFeedforward feedforward = new ArmFeedforward(0.0, 0.0, 0.0); // 1.31085, 0.278); private final ProfiledPIDController pid = - new ProfiledPIDController(100.0, 0.0, 6.0, new TrapezoidProfile.Constraints(10.0, 10.0)); + new ProfiledPIDController(60.0, 0.0, 6.0, new TrapezoidProfile.Constraints(10.0, 10.0)); private double appliedVoltage = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java index 270e6e47..2d3a3db2 100644 --- a/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderSubsystem.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.ExtensionKinematics; import frc.robot.utils.Tracer; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -28,26 +29,32 @@ public class ShoulderSubsystem extends SubsystemBase { public static final double X_OFFSET_METERS = 0.1016254; public static final double Z_OFFSET_METERS = 0.207645; public static final double ARM_LENGTH_METERS = Units.inchesToMeters(13.5); - public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(90.0); + public static final Rotation2d SHOULDER_HP_POS = Rotation2d.fromDegrees(53.0); + public static final Rotation2d SHOULDER_CORAL_GROUND_POS = Rotation2d.fromDegrees(8.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(0.0); + public static final Rotation2d SHOULDER_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(0.505); public static final Rotation2d SHOULDER_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(30.0); - public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(34.0); + public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(45.0); public static final Rotation2d SHOULDER_INTAKE_ALGAE_REEF_RETRACT_POS = Rotation2d.fromDegrees(60.0); - public static final Rotation2d SHOULDER_SCORE_POS = Rotation2d.fromDegrees(60); + // may be incorrect as l2-3 poses are derived from ExtensionKinematics now + public static final Rotation2d SHOULDER_SCORE_POS = + ExtensionKinematics.L2_EXTENSION.shoulderAngle(); public static final Rotation2d SHOULDER_WHACK_L1_POS = Rotation2d.fromDegrees(45); - public static final Rotation2d SHOULDER_SCORE_L1_POS = Rotation2d.fromDegrees(45); - public static final Rotation2d SHOULDER_SCORE_L4_POS = Rotation2d.fromDegrees(55); + public static final Rotation2d SHOULDER_SCORE_L1_POS = + ExtensionKinematics.L1_EXTENSION.shoulderAngle(); + public static final Rotation2d SHOULDER_SCORE_L4_POS = + ExtensionKinematics.L4_EXTENSION.shoulderAngle(); public static final Rotation2d SHOULDER_PRE_NET_POS = Rotation2d.fromDegrees(40); public static final Rotation2d SHOULDER_SHOOT_NET_POS = Rotation2d.fromDegrees(90); - public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(75.0); + public static final Rotation2d SHOULDER_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(60.0); public static final Rotation2d SHOULDER_CLEARANCE_POS = Rotation2d.fromDegrees(80.0); + public static final Rotation2d SHOULDER_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(35.0); private static final MotionMagicConfigs DEFAULT_CONFIGS = new MotionMagicConfigs().withMotionMagicCruiseVelocity(1.0).withMotionMagicAcceleration(4.0); private static final MotionMagicConfigs TOSS_CONFIGS = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.6).withMotionMagicAcceleration(4.0); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(0.5).withMotionMagicAcceleration(4.0); private final ShoulderIO io; private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); @@ -93,8 +100,7 @@ public Command setTargetAngle(final Supplier target) { () -> { io.setMotorPosition(target.get()); setpoint = target.get(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); + Logger.recordOutput("Carriage/Shoulder/Setpoint", setpoint); }); } @@ -116,7 +122,16 @@ public Command setTargetAngleSlow(final Supplier target) { } public Command setTargetAngleSlow(final Rotation2d target) { - return setTargetAngle(() -> target); + return setTargetAngleSlow(() -> target); + } + + public Command setVoltage(final double volts) { + return this.run(() -> io.setMotorVoltage(volts)); + } + + public Command hold() { + return Commands.sequence( + setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); } public Rotation2d getAngle() { diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java index 40fb204b..521dd93e 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristIOReal.java @@ -57,7 +57,7 @@ public WristIOReal(final int motorId, final TalonFXConfiguration config) { motor.optimizeBusUtilization(); - motor.setPosition(edu.wpi.first.math.util.Units.degreesToRotations(-10.0)); + motor.setPosition(edu.wpi.first.math.util.Units.degreesToRotations(10.0)); } @Override @@ -107,7 +107,7 @@ public static TalonFXConfiguration getDefaultConfiguration() { .withSlot0(new Slot0Configs().withGravityType(GravityTypeValue.Arm_Cosine)) .withMotorOutput( new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) + .withNeutralMode(NeutralModeValue.Coast) .withInverted(InvertedValue.Clockwise_Positive)); } } diff --git a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java index ef5da9ae..65dd63ad 100644 --- a/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/wrist/WristSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotType; +import frc.robot.subsystems.ExtensionKinematics; import frc.robot.subsystems.shoulder.ShoulderIOInputsAutoLogged; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -16,31 +17,36 @@ public class WristSubsystem extends SubsystemBase { public static final double WRIST_GEAR_RATIO = 4.0 * 4.0 * (64.0 / 34.0); // TODO: UPDATE WHEN CAD IS FINISHED - public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(180.0); + public static final Rotation2d MAX_ARM_ROTATION = Rotation2d.fromDegrees(220.0); public static final Rotation2d MIN_ARM_ROTATION = Rotation2d.fromDegrees(-90.0); - public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromDegrees(180 - 49.519570 + 5); - - public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(-30.0); - public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(-23.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromDegrees(-50); - public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-50); - public static final Rotation2d WRIST_SCORE_L1_POS = Rotation2d.fromDegrees(-70); + public static final Rotation2d ZEROING_OFFSET = Rotation2d.fromRadians(1.451); + + public static final Rotation2d WRIST_RETRACTED_POS = Rotation2d.fromDegrees(20.0); + public static final Rotation2d WRIST_READY_ALGAE = Rotation2d.fromDegrees(-10.0); + public static final Rotation2d WRIST_HP_POS = Rotation2d.fromDegrees(178.0); + public static final Rotation2d WRIST_CORAL_GROUND = Rotation2d.fromDegrees(0.0); + public static final Rotation2d WRIST_INTAKE_ALGAE_GROUND_POS = Rotation2d.fromRadians(-0.9); + public static final Rotation2d WRIST_INTAKE_ALGAE_STACK_POS = Rotation2d.fromDegrees(-10); + public static final Rotation2d WRIST_SCORE_L1_POS = ExtensionKinematics.L1_EXTENSION.wristAngle(); public static final Rotation2d WRIST_WHACK_L1_POS = Rotation2d.fromDegrees(-70); - public static final Rotation2d WRIST_SCORE_L2_POS = Rotation2d.fromDegrees(-35); - public static final Rotation2d WRIST_SCORE_L3_POS = Rotation2d.fromDegrees(-35); - public static final Rotation2d WRIST_SCORE_L4_POS = Rotation2d.fromDegrees(-45); - public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(-50.0); - public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-44.0); + public static final Rotation2d WRIST_SCORE_L2_POS = ExtensionKinematics.L2_EXTENSION.wristAngle(); + public static final Rotation2d WRIST_SCORE_L3_POS = ExtensionKinematics.L3_EXTENSION.wristAngle(); + public static final Rotation2d WRIST_SCORE_L4_POS = ExtensionKinematics.L4_EXTENSION.wristAngle(); + + public static final Rotation2d WRIST_CLEARANCE_POS = Rotation2d.fromDegrees(30.0); + public static final Rotation2d WRIST_TUCKED_CLEARANCE_POS = Rotation2d.fromDegrees(170.0); + + public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_POS = Rotation2d.fromDegrees(-20.0); public static final Rotation2d WRIST_INTAKE_ALGAE_REEF_RETRACT_POS = - Rotation2d.fromDegrees(-44.0); - public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(70); - public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = WRIST_RETRACTED_POS; + Rotation2d.fromDegrees(-20.0); + public static final Rotation2d WRIST_SHOOT_NET_POS = Rotation2d.fromDegrees(105); + public static final Rotation2d WRIST_SCORE_PROCESSOR_POS = Rotation2d.fromDegrees(-30.0); public static MotionMagicConfigs DEFAULT_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(6); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(5); public static MotionMagicConfigs SLOW_MOTION_MAGIC = - new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(4); + new MotionMagicConfigs().withMotionMagicCruiseVelocity(4).withMotionMagicAcceleration(3); public static MotionMagicConfigs CRAWL_MOTION_MAGIC = new MotionMagicConfigs().withMotionMagicCruiseVelocity(2).withMotionMagicAcceleration(2); @@ -70,8 +76,7 @@ public Command setTargetAngle(final Supplier target) { () -> { io.setMotorPosition(target.get()); setpoint = target.get(); - if (Robot.ROBOT_TYPE != RobotType.REAL) - Logger.recordOutput("Carriage/Wrist/Setpoint", setpoint); + Logger.recordOutput("Carriage/Wrist/Setpoint", setpoint); }); } @@ -104,6 +109,10 @@ public Command hold() { setTargetAngle(() -> inputs.position).until(() -> true), this.run(() -> {})); } + public Command setVoltage(final double volts) { + return this.run(() -> io.setMotorVoltage(volts)); + } + public Rotation2d getAngle() { return inputs.position; } @@ -128,11 +137,16 @@ public Command currentZero(Supplier shoulderInputs) System.out.println("Wrist Zeroing"); }), this.run(() -> io.setMotorVoltage(-1.0)) - .until(() -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 10.0), + .until(() -> Math.abs(currentFilter.calculate(inputs.statorCurrentAmps)) > 15.0), this.runOnce( () -> { hasZeroed = true; io.resetEncoder(shoulderInputs.get().position.minus(ZEROING_OFFSET)); })); } + + public void resetPosition(Rotation2d angle) { + io.resetEncoder(angle); + hasZeroed = true; + } }