From 7b771086a5b90ed3c9ba77610267e9815ec1c01e Mon Sep 17 00:00:00 2001 From: David Shen Date: Thu, 29 May 2025 20:11:58 -0400 Subject: [PATCH 1/4] reverse in danger zone --- .../robotState/ForceBargeCommand.java | 2 +- .../robot/constants/BargeAlignConstants.java | 4 +++ .../robotState/RobotStateSubsystem.java | 22 +++++++-------- .../tagAlign/BargeAlignSubsystem.java | 27 ++++++++++++------- 4 files changed, 33 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java b/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java index ae025a3..878e74d 100644 --- a/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java +++ b/src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java @@ -58,7 +58,7 @@ public boolean isFinished() { return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE && !robotStateSubsystem.getIsAutoPlacing()) - || !robotStateSubsystem.getIsBargeSafe(); + /*|| !robotStateSubsystem.getIsBargeSafe() */ ; } } } diff --git a/src/main/java/frc/robot/constants/BargeAlignConstants.java b/src/main/java/frc/robot/constants/BargeAlignConstants.java index 6efecf5..52ec593 100644 --- a/src/main/java/frc/robot/constants/BargeAlignConstants.java +++ b/src/main/java/frc/robot/constants/BargeAlignConstants.java @@ -4,12 +4,16 @@ public class BargeAlignConstants { public static final double kXSpeed = 0.45; + public static final double kXRevSpeed = 2; public static final double kBlueEjectAlgaeX = 7.67; // 7.72 public static final double kRedEjectAlgaeX = 9.86; // 9.81 public static final double kBlueRaiseElevatorX = 6.82; // 7.02 public static final double kRedRaiseElevatorX = 10.71; // 10.51 + public static final double kBlueRevDoneX = kBlueRaiseElevatorX + 0.25; + public static final double kRedRevDoneX = kRedRaiseElevatorX - 0.25; + public static final double kBlueUnsafeX = 7.02; public static final double kRedUnsafeX = 10.51; diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index a0ab829..a2c752d 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -741,18 +741,18 @@ public void toScoreAlgae() { isBargeSafe = poseX > RobotStateConstants.kRedBargeSafeX || poseX < RobotStateConstants.kBlueBargeSafeX; - if (isBargeSafe) { - driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); - if (isAutoPlacing) { - setAutoPlacingLed(true); - driveSubsystem.setIgnoreSticks(true); - bargeAlignSubsystem.startBargeAlign(allianceColor); - setState(RobotStates.BARGE_ALIGN); - } else { - elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); - setState(RobotStates.TO_BARGE_ALGAE); - } + // if (isBargeSafe) { + driveSubsystem.setDriveMultiplier(DriveConstants.kBargeScoreStickMultiplier); + if (isAutoPlacing) { + setAutoPlacingLed(true); + driveSubsystem.setIgnoreSticks(true); + bargeAlignSubsystem.startBargeAlign(allianceColor); + setState(RobotStates.BARGE_ALIGN); + } else { + elevatorSubsystem.setPosition(ElevatorConstants.kBargeSetpoint); + setState(RobotStates.TO_BARGE_ALGAE); } + // } } } } diff --git a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java index 6649c50..b69961d 100644 --- a/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java +++ b/src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java @@ -39,12 +39,12 @@ public BargeAlignSubsystem(FlyskyJoystick flysky, DriveSubsystem driveSubsystem) public void startBargeAlign(Alliance alliance) { this.alliance = alliance; - if (isSafe()) { - this.isOnBlueSide = isOnBlueSide(); - setState(BargeAlignStates.DRIVE); - setupBargeAlign(); - driveOmega.reset(); - } + // if (isSafe()) { + this.isOnBlueSide = isOnBlueSide(); + setState(BargeAlignStates.DRIVE); + setupBargeAlign(); + driveOmega.reset(); + // } } public void killBargeAlign() { @@ -96,6 +96,13 @@ private boolean shouldDriveBackwards() { : poseX < BargeAlignConstants.kRedUnsafeX; } + private boolean shouldStopDrivingBackwards() { + double poseX = driveSubsystem.getPoseMeters().getX(); + return isOnBlueSide + ? poseX < BargeAlignConstants.kBlueRevDoneX + : poseX > BargeAlignConstants.kRedRevDoneX; + } + private boolean shouldEjectAlgae() { double poseX = driveSubsystem.getPoseMeters().getX(); return isOnBlueSide @@ -160,14 +167,14 @@ public void periodic() { double vOmega = driveOmega.calculate( driveSubsystem.getPoseMeters().getRotation().getRadians(), targetYaw.getRadians()); + double revX = BargeAlignConstants.kXRevSpeed * (isOnBlueSide ? -1 : 1); + driveSubsystem.move(revX, getYStickReading(), vOmega, true); - driveSubsystem.move(-vX, getYStickReading(), vOmega, true); - - Logger.recordOutput("BargeAlign/Vx", vX); + Logger.recordOutput("BargeAlign/Vx", revX); Logger.recordOutput("BargeAlign/OmegaErr", driveOmega.getError()); Logger.recordOutput("BargeAlign/Vomega", vOmega); - if (shouldRaiseElevator()) { + if (shouldStopDrivingBackwards()) { setState(BargeAlignStates.RAISE_ELEV); } } From 766519bd826d1aa463af8f85d0c00f1dd1d51921 Mon Sep 17 00:00:00 2001 From: Isaac Hoekstra <122327870+PotatoBoyH4@users.noreply.github.com> Date: Tue, 1 Jul 2025 20:11:34 -0400 Subject: [PATCH 2/4] created the IRI auton have not tested --- src/main/deploy/choreo/2025-project.chor | 14 + src/main/deploy/choreo/bargeToG.traj | 399 ++++++++++++++++++ src/main/deploy/choreo/deepBargeToE.traj | 74 ++++ .../robot/commands/auton/IRIAutonCommand.java | 148 +++++++ .../robotState/SetAutoPlacingCommand.java | 19 + .../frc/robot/constants/AutonConstants.java | 2 + .../frc/robot/subsystems/auto/AutoSwitch.java | 19 + 7 files changed, 675 insertions(+) create mode 100644 src/main/deploy/choreo/bargeToG.traj create mode 100644 src/main/deploy/choreo/deepBargeToE.traj create mode 100644 src/main/java/frc/robot/commands/auton/IRIAutonCommand.java create mode 100644 src/main/java/frc/robot/commands/robotState/SetAutoPlacingCommand.java diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor index f94de15..056852b 100644 --- a/src/main/deploy/choreo/2025-project.chor +++ b/src/main/deploy/choreo/2025-project.chor @@ -216,6 +216,20 @@ "val":0.0 } }, + "bargeDeep":{ + "x":{ + "exp":"barge.x", + "val":7.7 + }, + "y":{ + "exp":"7.092 m", + "val":7.092 + }, + "heading":{ + "exp":"barge.heading", + "val":0.0 + } + }, "fetch":{ "x":{ "exp":"1.69577419757843 m", diff --git a/src/main/deploy/choreo/bargeToG.traj b/src/main/deploy/choreo/bargeToG.traj new file mode 100644 index 0000000..70d7eac --- /dev/null +++ b/src/main/deploy/choreo/bargeToG.traj @@ -0,0 +1,399 @@ +{ + "name":"bargeToG", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.7, "y":5.08871412277, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.520762920379639, "y":3.873906135559082, "heading":-1.8760194001518105, "intervals":254, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.827323, "y":3.7209, "heading":3.141592653589793, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.2}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":3.0}}, "enabled":false}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":0.5}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"barge.x", "val":7.7}, "y":{"exp":"barge.y", "val":5.08871412277}, "heading":{"exp":"barge.heading", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.520762920379639 m", "val":6.520762920379639}, "y":{"exp":"3.873906135559082 m", "val":3.873906135559082}, "heading":{"exp":"-1.8760194001518105 rad", "val":-1.8760194001518105}, "intervals":254, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"G.x", "val":5.827323}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"5 m / s ^ 2", "val":5.0}}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":false}, + {"from":1, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0.2 rad / s", "val":0.2}}}, "enabled":true}, + {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"3 m / s ^ 2", "val":3.0}}}, "enabled":false}, + {"from":0, "to":0, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0.5 rad / s", "val":0.5}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,0.88691,1.43005], + "samples":[ + {"t":0.0, "x":7.7, "y":5.08871, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.66601, "ay":-4.22468, "alpha":45.70087, "fx":[-197.46069,-138.24335,7.81141,150.12835], "fy":[2.88068,-141.82333,-197.37601,54.6252]}, + {"t":0.02688, "x":7.69904, "y":5.08719, "heading":0.0, "vx":-0.07165, "vy":-0.11354, "omega":1.22827, "ax":-2.69197, "ay":-4.21072, "alpha":45.51205, "fx":[-196.9853,-137.77026,9.02866,146.2315], "fy":[3.43734,-141.83158,-196.877,54.50861]}, + {"t":0.05375, "x":7.69614, "y":5.08262, "heading":0.03301, "vx":-0.144, "vy":-0.22671, "omega":2.45146, "ax":-2.7223, "ay":-4.19106, "alpha":45.08534, "fx":[-196.25311,-132.73026,16.49573,130.96976], "fy":[-1.86316,-145.84393,-195.65315,63.90861]}, + {"t":0.08063, "x":7.69129, "y":5.07501, "heading":0.0989, "vx":-0.21717, "vy":-0.33935, "omega":3.66318, "ax":-2.75495, "ay":-4.16954, "alpha":44.3678, "fx":[-194.48556,-122.63143,31.44635,101.97558], "fy":[-11.25678,-153.11467,-192.35321,78.7077]}, + {"t":0.1075, "x":7.68445, "y":5.06438, "heading":0.19735, "vx":-0.29121, "vy":-0.45141, "omega":4.85561, "ax":-2.78504, "ay":-4.14937, "alpha":42.90363, "fx":[-189.89846,-107.22846,54.91215,56.51352], "fy":[-19.63462,-160.87755,-182.84299,86.68331]}, + {"t":0.13438, "x":7.67562, "y":5.05075, "heading":0.32785, "vx":-0.36606, "vy":-0.56293, "omega":6.0087, "ax":-2.80947, "ay":-4.13271, "alpha":31.53347, "fx":[-153.90856,-82.26245,55.21665,-6.37544], "fy":[-26.86771,-145.50841,-139.79109,36.60577]}, + {"t":0.16126, "x":7.66477, "y":5.03413, "heading":0.48934, "vx":-0.44157, "vy":-0.674, "omega":6.8562, "ax":-2.83175, "ay":-4.11728, "alpha":-6.67708, "fx":[-26.07308,-39.53884,-68.02358,-55.18032], "fy":[-74.49288,-44.98963,-63.85879,-91.19121]}, + {"t":0.18813, "x":7.65188, "y":5.01453, "heading":0.67361, "vx":-0.51768, "vy":-0.78466, "omega":6.67675, "ax":-2.85203, "ay":-4.10305, "alpha":-4.17825, "fx":[-31.67625,-48.09367,-62.87633,-47.5217], "fy":[-72.76006,-55.16693,-64.6183,-81.03787]}, + {"t":0.21501, "x":7.63694, "y":4.99196, "heading":0.85305, "vx":-0.59433, "vy":-0.89493, "omega":6.56445, "ax":-2.87326, "ay":-4.08796, "alpha":-6.39901, "fx":[-23.55876,-54.21395,-69.93661,-43.87397], "fy":[-72.31674,-46.22074,-66.12345,-87.91638]}, + {"t":0.24189, "x":7.61992, "y":4.96643, "heading":1.02948, "vx":-0.67155, "vy":-1.0048, "omega":6.39247, "ax":-2.92068, "ay":-4.05393, "alpha":-18.67293, "fx":[20.94965,-77.87617,-107.28099,-30.53753], "fy":[-65.50946,-3.18234,-75.9635,-125.65293]}, + {"t":0.26876, "x":7.60082, "y":4.93796, "heading":1.20129, "vx":-0.75005, "vy":-1.11376, "omega":5.89061, "ax":-2.9757, "ay":-4.01339, "alpha":-17.04569, "fx":[11.11822,-87.4288,-97.73975,-24.36347], "fy":[-56.75381,-10.02449,-81.93025,-118.89661]}, + {"t":0.29564, "x":7.57959, "y":4.90658, "heading":1.3596, "vx":-0.83002, "vy":-1.22162, "omega":5.43249, "ax":-3.03501, "ay":-3.96835, "alpha":-15.16959, "fx":[-2.20469,-90.67055,-89.14422,-20.349], "fy":[-48.34906,-19.16138,-85.51216,-111.57938]}, + {"t":0.32251, "x":7.55618, "y":4.87231, "heading":1.50561, "vx":-0.91159, "vy":-1.32828, "omega":5.02479, "ax":-3.09975, "ay":-3.91754, "alpha":-13.4231, "fx":[-15.13511,-90.89961,-81.39437,-19.256], "fy":[-42.22424,-27.14964,-87.76416,-104.07628]}, + {"t":0.34939, "x":7.53056, "y":4.8352, "heading":1.64066, "vx":-0.9949, "vy":-1.43356, "omega":4.66403, "ax":-3.17188, "ay":-3.85883, "alpha":-11.89023, "fx":[-26.96223,-89.88658,-75.2986,-19.34775], "fy":[-38.10946,-34.20442,-88.10307,-96.88266]}, + {"t":0.37627, "x":7.50268, "y":4.79527, "heading":1.76601, "vx":-1.08015, "vy":-1.53728, "omega":4.34446, "ax":-3.25419, "ay":-3.78903, "alpha":-10.5913, "fx":[-35.82945,-88.7178,-70.80475,-21.63094], "fy":[-36.25334,-39.14096,-87.35205,-89.89897]}, + {"t":0.40314, "x":7.47247, "y":4.75259, "heading":1.88277, "vx":-1.16761, "vy":-1.63911, "omega":4.05981, "ax":-3.35046, "ay":-3.70335, "alpha":-9.51669, "fx":[-44.40479,-87.44924,-67.24388,-24.30463], "fy":[-34.07865,-43.38367,-86.00238,-83.46772]}, + {"t":0.43002, "x":7.43988, "y":4.7072, "heading":1.99188, "vx":-1.25766, "vy":-1.73864, "omega":3.80404, "ax":-3.46596, "ay":-3.59444, "alpha":-8.64189, "fx":[-50.43932,-87.01998,-65.40268,-28.2414], "fy":[-33.24217,-45.1566,-83.87584,-77.39589]}, + {"t":0.4569, "x":7.40483, "y":4.65917, "heading":2.09412, "vx":-1.35081, "vy":-1.83525, "omega":3.57178, "ax":-3.60806, "ay":-3.45038, "alpha":-7.93435, "fx":[-57.26405,-86.59638,-64.40216,-32.31594], "fy":[-30.69362,-47.03079,-81.19779,-71.14249]}, + {"t":0.48377, "x":7.36722, "y":4.6086, "heading":2.19012, "vx":-1.44778, "vy":-1.92798, "omega":3.35853, "ax":-3.78719, "ay":-3.25081, "alpha":-7.34909, "fx":[-62.5428,-87.54305,-64.98325,-37.45325], "fy":[-28.62409,-45.75331,-77.5866,-64.79354]}, + {"t":0.51065, "x":7.32694, "y":4.55561, "heading":2.28038, "vx":-1.54956, "vy":-2.01535, "omega":3.16102, "ax":-4.01746, "ay":-2.9586, "alpha":-6.82202, "fx":[-68.84889,-88.58876,-66.84949,-43.5896], "fy":[-23.87103,-44.55206,-72.36791,-56.4827]}, + {"t":0.53752, "x":7.28385, "y":4.50038, "heading":2.36534, "vx":-1.65754, "vy":-2.09487, "omega":2.97767, "ax":-4.31433, "ay":-2.50117, "alpha":-6.67946, "fx":[-74.67822,-92.45378,-70.65365,-49.88544], "fy":[-16.56244,-38.11458,-66.04946,-46.04673]}, + {"t":0.5644, "x":7.23774, "y":4.44317, "heading":2.44536, "vx":-1.77349, "vy":-2.16209, "omega":2.79815, "ax":-4.67949, "ay":-1.71403, "alpha":-15.23998, "fx":[-85.14563,-124.40619,-74.88299,-27.58474], "fy":[28.56981,-32.37529,-83.951,-26.53164]}, + {"t":0.59128, "x":7.18839, "y":4.38445, "heading":2.52057, "vx":-1.89926, "vy":-2.20815, "omega":2.38855, "ax":-4.96806, "ay":-0.33655, "alpha":-13.32919, "fx":[-88.45426,-124.36212,-79.25162,-39.19264], "fy":[41.31625,-12.29067,-55.42361,3.95722]}, + {"t":0.61815, "x":7.13555, "y":4.32498, "heading":2.58476, "vx":-2.03278, "vy":-2.2172, "omega":2.03032, "ax":-4.69018, "ay":1.6638, "alpha":-8.27343, "fx":[-83.01874,-105.56066,-73.98706,-50.16556], "fy":[55.87931,18.86163,0.31717,35.88085]}, + {"t":0.64503, "x":7.07922, "y":4.26599, "heading":2.63933, "vx":-2.15883, "vy":-2.17248, "omega":1.80796, "ax":-3.52021, "ay":3.51947, "alpha":-2.59035, "fx":[-60.6352,-66.98506,-55.01637,-52.08442], "fy":[67.91035,56.20314,48.87573,61.68205]}, + {"t":0.6719, "x":7.01993, "y":4.20887, "heading":2.68792, "vx":-2.25344, "vy":-2.07789, "omega":1.73834, "ax":-2.13728, "ay":4.50002, "alpha":1.51825, "fx":[-34.36424,-29.09362,-36.73437,-42.31748], "fy":[71.36255,76.50393,78.47989,73.70632]}, + {"t":0.69878, "x":6.95859, "y":4.15465, "heading":2.73464, "vx":-2.31089, "vy":-1.95695, "omega":1.77914, "ax":-1.09835, "ay":4.86304, "alpha":3.85196, "fx":[-12.71993,-3.88978,-22.40944,-34.21677], "fy":[70.85588,85.29625,90.66,77.44601]}, + {"t":0.72566, "x":6.89609, "y":4.10381, "heading":2.78246, "vx":-2.34041, "vy":-1.82625, "omega":1.88267, "ax":-0.40215, "ay":4.97208, "alpha":5.1988, "fx":[2.25398,11.65028,-14.42755,-26.29149], "fy":[68.71838,89.44485,96.25651,77.1093]}, + {"t":0.75253, "x":6.83304, "y":4.05652, "heading":2.83306, "vx":-2.35121, "vy":-1.69262, "omega":2.02239, "ax":0.0685, "ay":4.98985, "alpha":6.10579, "fx":[13.46749,21.47717,-8.80905,-21.56793], "fy":[66.96377,91.36728,98.88579,75.49708]}, + {"t":0.77941, "x":6.76987, "y":4.01284, "heading":2.88741, "vx":-2.34937, "vy":-1.55851, "omega":2.18649, "ax":0.39921, "ay":4.9758, "alpha":6.90632, "fx":[20.73089,28.10766,-6.16719,-16.05251], "fy":[63.94988,94.20365,101.41191,72.21136]}, + {"t":0.80629, "x":6.70687, "y":3.97275, "heading":2.94618, "vx":-2.33864, "vy":-1.42478, "omega":2.37211, "ax":0.63815, "ay":4.95194, "alpha":5.75531, "fx":[21.93683,13.25991,17.43981,-10.08598], "fy":[58.66809,102.24243,103.38615,65.88904]}, + {"t":0.83316, "x":6.64425, "y":3.93624, "heading":3.00993, "vx":-2.32149, "vy":-1.29169, "omega":2.52679, "ax":0.85725, "ay":4.91958, "alpha":-42.30393, "fx":[-58.78241,-157.22332,164.88896,108.27672], "fy":[176.50673,18.86309,-19.02379,151.68202]}, + {"t":0.86004, "x":6.58217, "y":3.9033, "heading":3.07784, "vx":-2.29845, "vy":-1.15947, "omega":1.38982, "ax":1.02156, "ay":4.88874, "alpha":-46.5634, "fx":[-54.64345,-183.84924,189.83159,116.77657], "fy":[186.04649,1.2918,-16.90831,155.54179]}, + {"t":0.88691, "x":6.52076, "y":3.87391, "heading":3.11519, "vx":-2.271, "vy":-1.02808, "omega":0.13838, "ax":1.1058, "ay":4.70805, "alpha":-8.19443, "fx":[-7.71175,6.83694,37.95266,36.65506], "fy":[100.3882,57.62058,57.05073,98.864]}, + {"t":0.88864, "x":6.51685, "y":3.87214, "heading":3.11543, "vx":-2.26909, "vy":-1.01996, "omega":0.12425, "ax":1.16449, "ay":4.7774, "alpha":-6.93365, "fx":[1.21547,0.93398,38.54873,36.94791], "fy":[95.98836,66.3852,62.06195,94.11226]}, + {"t":0.89036, "x":6.51294, "y":3.87039, "heading":3.11565, "vx":-2.26708, "vy":-1.01173, "omega":0.11229, "ax":1.20522, "ay":4.76722, "alpha":-5.72099, "fx":[5.14965,5.80871,35.23391,34.16958], "fy":[93.47974,66.8619,65.56204,91.96513]}, + {"t":0.89209, "x":6.50903, "y":3.86865, "heading":3.11584, "vx":-2.265, "vy":-1.00351, "omega":0.10243, "ax":1.2459, "ay":4.75666, "alpha":-4.66407, "fx":[8.38443,8.38561,33.557,32.74747], "fy":[90.23114,70.89059,67.06606,88.97719]}, + {"t":0.89381, "x":6.50513, "y":3.86693, "heading":3.11602, "vx":-2.26286, "vy":-0.9953, "omega":0.09438, "ax":1.28676, "ay":4.74569, "alpha":-3.93262, "fx":[11.03979,11.91964,31.69473,31.1444], "fy":[88.81233,70.38321,69.45986,87.77837]}, + {"t":0.89554, "x":6.50123, "y":3.86522, "heading":3.11618, "vx":-2.26064, "vy":-0.98712, "omega":0.0876, "ax":1.32735, "ay":4.73442, "alpha":-3.22917, "fx":[13.47666,13.51365,30.97265,30.54213], "fy":[86.43248,73.58469,70.10216,85.56255]}, + {"t":0.89726, "x":6.49733, "y":3.86352, "heading":3.11633, "vx":-2.25835, "vy":-0.97896, "omega":0.08204, "ax":1.36825, "ay":4.72268, "alpha":-2.78127, "fx":[15.3373,16.30267,29.94392,29.64872], "fy":[85.65105,72.49393,71.83271,84.92186]}, + {"t":0.89898, "x":6.49344, "y":3.86184, "heading":3.11647, "vx":-2.25599, "vy":-0.97082, "omega":0.07724, "ax":1.40875, "ay":4.71069, "alpha":-2.29343, "fx":[17.28718,17.29277,29.79436,29.55853], "fy":[83.79338,75.1898,71.9446,83.17187]}, + {"t":0.90071, "x":6.48955, "y":3.86018, "heading":3.1166, "vx":-2.25356, "vy":-0.96269, "omega":0.07329, "ax":1.44965, "ay":4.6982, "alpha":-2.01903, "fx":[18.63994,19.65538,29.26219,29.10207], "fy":[83.4021,73.73515,73.25713,82.8726]}, + {"t":0.90243, "x":6.48567, "y":3.85852, "heading":3.11673, "vx":-2.25106, "vy":-0.95459, "omega":0.06981, "ax":1.49002, "ay":4.68549, "alpha":-1.66727, "fx":[20.28491,20.24922,29.47391,29.34338], "fy":[81.86379,76.11791,73.03025,81.40751]}, + {"t":0.90416, "x":6.48179, "y":3.85688, "heading":3.11685, "vx":-2.24849, "vy":-0.94651, "omega":0.06693, "ax":1.53086, "ay":4.67225, "alpha":-1.50158, "fx":[21.30692,22.36885,29.24235,29.15655], "fy":[81.71931,74.4213,74.07226,81.32392]}, + {"t":0.90588, "x":6.47791, "y":3.85526, "heading":3.11697, "vx":-2.24585, "vy":-0.93846, "omega":0.06434, "ax":1.57109, "ay":4.65883, "alpha":-1.23854, "fx":[22.75886,22.68892,29.69087,29.61885], "fy":[80.38071,76.598,73.62623,80.0366]}, + {"t":0.9076, "x":6.47404, "y":3.85365, "heading":3.11708, "vx":-2.24314, "vy":-0.93042, "omega":0.06221, "ax":1.61182, "ay":4.64485, "alpha":-1.1413, "fx":[23.56058,24.67394,29.64144,29.59704], "fy":[80.39226,74.74303,74.48575,80.08875]}, + {"t":0.90933, "x":6.47018, "y":3.85205, "heading":3.11718, "vx":-2.24036, "vy":-0.92242, "omega":0.06024, "ax":1.65189, "ay":4.63071, "alpha":-0.93743, "fx":[24.88959,24.79515,30.24987,30.21043], "fy":[79.17873,76.78271,73.89355,78.91214]}, + {"t":0.91105, "x":6.46632, "y":3.85047, "heading":3.11729, "vx":-2.23752, "vy":-0.91443, "omega":0.05862, "ax":1.69245, "ay":4.61602, "alpha":-0.88373, "fx":[25.54062,26.71106,30.30982,30.28805], "fy":[79.29009,74.81867,74.62766,79.05065]}, + {"t":0.91278, "x":6.46246, "y":3.8489, "heading":3.11739, "vx":-2.2346, "vy":-0.90647, "omega":0.0571, "ax":1.73234, "ay":4.60117, "alpha":-0.7204, "fx":[26.79144,26.68089,31.02924,31.00736], "fy":[78.15512,76.76396,73.9353,77.94301]}, + {"t":0.9145, "x":6.45861, "y":3.84734, "heading":3.11749, "vx":-2.23161, "vy":-0.89854, "omega":0.05586, "ax":1.77269, "ay":4.58576, "alpha":-0.69452, "fx":[27.33569,28.56723,31.1533,31.14313], "fy":[78.33022,74.72285,74.5806,78.13626]}, + {"t":0.91623, "x":6.45477, "y":3.8458, "heading":3.11758, "vx":-2.22855, "vy":-0.89063, "omega":0.05466, "ax":1.81235, "ay":4.57022, "alpha":-0.55981, "fx":[28.537,28.41656,31.95186,31.93867], "fy":[77.24475,76.60013,73.81733,77.07159]}, + {"t":0.91795, "x":6.45093, "y":3.84427, "heading":3.11768, "vx":-2.22543, "vy":-0.88275, "omega":0.05369, "ax":1.85244, "ay":4.55412, "alpha":-0.55175, "fx":[29.00227,30.29703,32.11161,32.10655], "fy":[77.46008,74.50319,74.39767,77.299]}, + {"t":0.91967, "x":6.44709, "y":3.84275, "heading":3.11777, "vx":-2.22224, "vy":-0.8749, "omega":0.05274, "ax":1.89186, "ay":4.53789, "alpha":-0.43797, "fx":[30.17254,30.04641,32.96815,32.95842], "fy":[76.40583,76.32886,73.58231,76.26103]}, + {"t":0.9214, "x":6.44327, "y":3.84125, "heading":3.11786, "vx":-2.21897, "vy":-0.86707, "omega":0.05199, "ax":1.93165, "ay":4.52111, "alpha":-0.44131, "fx":[30.57653,31.93493,33.14563,33.1419], "fy":[76.64595,74.1905,74.11337,76.50911]}, + {"t":0.92312, "x":6.43944, "y":3.83976, "heading":3.11795, "vx":-2.21564, "vy":-0.85928, "omega":0.05123, "ax":1.97079, "ay":4.50421, "alpha":-0.34342, "fx":[31.72755,31.5984,34.04581,34.03653], "fy":[75.61138,75.97473,73.25828,75.48767]}, + {"t":0.92485, "x":6.43562, "y":3.83829, "heading":3.11804, "vx":-2.21225, "vy":-0.85151, "omega":0.05063, "ax":2.01024, "ay":4.48677, "alpha":-0.354, "fx":[32.08168,33.50288,34.22955,34.22505], "fy":[75.86603,73.80502,73.75046,75.74745]}, + {"t":0.92657, "x":6.43181, "y":3.83683, "heading":3.11813, "vx":-2.20878, "vy":-0.84378, "omega":0.05002, "ax":2.04906, "ay":4.46921, "alpha":-0.26866, "fx":[33.22091,33.09032,35.16343,35.15297], "fy":[74.84384,75.55418,72.86396,74.73611]}, + {"t":0.9283, "x":6.42801, "y":3.83538, "heading":3.11821, "vx":-2.20525, "vy":-0.83607, "omega":0.04956, "ax":2.08814, "ay":4.45112, "alpha":-0.28375, "fx":[33.53266,35.01494,35.34601,35.33967], "fy":[75.10614,73.36036,73.32428,75.00159]}, + {"t":0.93002, "x":6.42421, "y":3.83395, "heading":3.1183, "vx":-2.20165, "vy":-0.8284, "omega":0.04907, "ax":2.12659, "ay":4.43293, "alpha":-0.20759, "fx":[34.66438,34.53267,36.30623,36.29384], "fy":[74.08621,75.0956,72.40669,73.99089]}, + {"t":0.93174, "x":6.42042, "y":3.83252, "heading":3.11838, "vx":-2.19798, "vy":-0.82075, "omega":0.04871, "ax":2.16528, "ay":4.41422, "alpha":-0.22648, "fx":[34.93922,36.48021,36.48301,36.47441], "fy":[74.35689,72.86601,72.84543,74.26335]}, + {"t":0.93347, "x":6.41663, "y":3.83111, "heading":3.11847, "vx":-2.19425, "vy":-0.81314, "omega":0.04832, "ax":2.20336, "ay":4.3954, "alpha":-0.15896, "fx":[36.06654,35.93471,37.46455,37.44992], "fy":[73.34158,74.57292,71.90652,73.25603]}, + {"t":0.93519, "x":6.41285, "y":3.82972, "heading":3.11855, "vx":-2.19045, "vy":-0.80556, "omega":0.04805, "ax":2.2416, "ay":4.3761, "alpha":-0.17934, "fx":[36.30772,37.90466,37.63203,37.62109], "fy":[73.61203,72.32879,72.32155,73.5273]}, + {"t":0.93692, "x":6.40908, "y":3.82834, "heading":3.11863, "vx":-2.18658, "vy":-0.79802, "omega":0.04774, "ax":2.27927, "ay":4.35667, "alpha":-0.11931, "fx":[37.43217,37.3004,38.63082,38.61398], "fy":[72.59943,74.00896,71.36456,72.52175]}, + {"t":0.93864, "x":6.40531, "y":3.82697, "heading":3.11872, "vx":-2.18265, "vy":-0.79051, "omega":0.04753, "ax":2.31703, "ay":4.3368, "alpha":-0.14032, "fx":[37.64237,39.29219,38.78679,38.77365], "fy":[72.86737,71.75387,71.7583,72.78978]}, + {"t":0.94037, "x":6.40155, "y":3.82561, "heading":3.1188, "vx":-2.17866, "vy":-0.78303, "omega":0.04729, "ax":2.35427, "ay":4.31679, "alpha":-0.08691, "fx":[38.76466,38.63301,39.79956,39.7807], "fy":[71.85625,73.40829,70.7859,71.78499]}, + {"t":0.94209, "x":6.3978, "y":3.82427, "heading":3.11888, "vx":-2.1746, "vy":-0.77559, "omega":0.04714, "ax":2.39151, "ay":4.29637, "alpha":-0.10795, "fx":[38.94599,40.64548,39.94251,39.92741], "fy":[72.12003,71.14531,71.16012,72.04831]}, + {"t":0.94381, "x":6.39405, "y":3.82294, "heading":3.11896, "vx":-2.17047, "vy":-0.76818, "omega":0.04696, "ax":2.42829, "ay":4.27581, "alpha":-0.06046, "fx":[40.06631,39.93478,40.96652,40.94588], "fy":[71.10959,72.77466,70.17457,71.04369]}, + {"t":0.94554, "x":6.39031, "y":3.82162, "heading":3.11904, "vx":-2.16629, "vy":-0.7608, "omega":0.04685, "ax":2.46499, "ay":4.25487, "alpha":-0.0811, "fx":[40.22053,41.96634,41.0954,41.0786], "fy":[71.36805,70.50648,70.53063,71.30121]}, + {"t":0.94726, "x":6.38658, "y":3.82031, "heading":3.11912, "vx":-2.16204, "vy":-0.75347, "omega":0.04671, "ax":2.50128, "ay":4.23377, "alpha":-0.03896, "fx":[41.33871,41.20729,42.12823,42.1061], "fy":[70.35792,72.11119,69.53391,70.29636]}, + {"t":0.94899, "x":6.38286, "y":3.81902, "heading":3.1192, "vx":-2.15772, "vy":-0.74617, "omega":0.04664, "ax":2.53741, "ay":4.21234, "alpha":-0.05888, "fx":[41.46733,43.25607,42.24236,42.22411], "fy":[70.61008,69.8403,69.87291,70.54731]}, + {"t":0.95071, "x":6.37914, "y":3.81774, "heading":3.11928, "vx":-2.15335, "vy":-0.73891, "omega":0.04654, "ax":2.57318, "ay":4.19073, "alpha":-0.02161, "fx":[42.58299,42.45167,43.28184,43.25846], "fy":[69.60003,71.42059,68.86679,69.54217]}, + {"t":0.95243, "x":6.37543, "y":3.81647, "heading":3.11936, "vx":-2.14891, "vy":-0.73168, "omega":0.04651, "ax":2.60873, "ay":4.16884, "alpha":-0.04056, "fx":[42.68741,44.51561,43.38076,43.36132], "fy":[69.84521,69.14934,69.18966,69.78585]}, + {"t":0.95416, "x":6.37173, "y":3.81522, "heading":3.11944, "vx":-2.14441, "vy":-0.72449, "omega":0.04644, "ax":2.64396, "ay":4.14674, "alpha":-0.00774, "fx":[43.79996,43.66879,44.4249,44.40052], "fy":[68.83523,70.70533,68.17576,68.78048]}, + {"t":0.95588, "x":6.36803, "y":3.81397, "heading":3.11952, "vx":-2.13986, "vy":-0.71734, "omega":0.04642, "ax":2.67889, "ay":4.12442, "alpha":-0.02555, "fx":[43.88148,45.74563,44.50837,44.48798], "fy":[69.07287,68.43592,68.48331,69.01639]}, + {"t":0.95761, "x":6.36435, "y":3.81274, "heading":3.1196, "vx":-2.13524, "vy":-0.71023, "omega":0.04638, "ax":2.71356, "ay":4.10187, "alpha":0.0032, "fx":[44.99025,44.85928,45.55532,45.53015], "fy":[68.06308,69.96767,67.46315,68.01095]}, + {"t":0.95933, "x":6.36067, "y":3.81152, "heading":3.11968, "vx":-2.13056, "vy":-0.70316, "omega":0.04638, "ax":2.74786, "ay":4.07915, "alpha":-0.01336, "fx":[45.05003,46.94652,45.62324,45.60246], "fy":[68.29273,67.70217,67.75604,68.23867]}, + {"t":0.96106, "x":6.357, "y":3.81032, "heading":3.11976, "vx":-2.12582, "vy":-0.69612, "omega":0.04636, "ax":2.78194, "ay":4.05617, "alpha":0.01168, "fx":[46.15431,46.02363,46.67128,46.64549], "fy":[67.28339,69.20967,66.73106,67.23348]}, + {"t":0.96278, "x":6.35334, "y":3.80912, "heading":3.11984, "vx":-2.12102, "vy":-0.68913, "omega":0.04638, "ax":2.8168, "ay":4.03223, "alpha":-0.00925, "fx":[46.24157,48.05506,46.77204,46.75016], "fy":[67.49007,66.93789,66.99549,67.43798]}, + {"t":0.9645, "x":6.34969, "y":3.80794, "heading":3.11992, "vx":-2.11617, "vy":-0.68218, "omega":0.04636, "ax":2.84908, "ay":4.0097, "alpha":0.01806, "fx":[47.29243,47.16215,47.77129,47.74502], "fy":[66.49623,68.43318,65.98137,66.4482]}, + {"t":0.96623, "x":6.34604, "y":3.80677, "heading":3.12, "vx":-2.11125, "vy":-0.67526, "omega":0.0464, "ax":2.88321, "ay":3.98543, "alpha":-0.00114, "fx":[47.35816,49.20241,47.85427,47.83192], "fy":[66.69466,66.1693,66.2325,66.64431]}, + {"t":0.96795, "x":6.34241, "y":3.80561, "heading":3.12008, "vx":-2.10628, "vy":-0.66839, "omega":0.04639, "ax":2.91492, "ay":3.96252, "alpha":0.02266, "fx":[48.4048,48.27506,48.85405,48.82742], "fy":[65.70183,67.63992,65.21582,65.65538]}, + {"t":0.96968, "x":6.33878, "y":3.80447, "heading":3.12016, "vx":-2.10126, "vy":-0.66156, "omega":0.04643, "ax":2.94831, "ay":3.93795, "alpha":0.00513, "fx":[48.44985,50.32094,48.9199,48.89717], "fy":[65.89189,65.38595,65.45426,65.84298]}, + {"t":0.9714, "x":6.33516, "y":3.80333, "heading":3.12024, "vx":-2.09617, "vy":-0.65477, "omega":0.04644, "ax":2.97945, "ay":3.91468, "alpha":0.02577, "fx":[49.49169,49.36264,49.91835,49.89147], "fy":[64.90044,66.83162,64.43613,64.85534]}, + {"t":0.97313, "x":6.33155, "y":3.80221, "heading":3.12032, "vx":-2.09104, "vy":-0.64802, "omega":0.04649, "ax":3.0121, "ay":3.88985, "alpha":0.01016, "fx":[49.51604,51.41503,49.96641,49.94341], "fy":[65.082,64.58923,64.66237,65.03432]}, + {"t":0.97485, "x":6.32795, "y":3.8011, "heading":3.1204, "vx":-2.08584, "vy":-0.64131, "omega":0.0465, "ax":3.04264, "ay":3.86626, "alpha":0.02765, "fx":[50.55329,50.42509,50.96317,50.93611], "fy":[64.09243,66.00991,63.64393,64.04846]}, + {"t":0.97657, "x":6.32436, "y":3.8, "heading":3.12048, "vx":-2.0806, "vy":-0.63465, "omega":0.04655, "ax":3.07453, "ay":3.8412, "alpha":0.01391, "fx":[50.55816,52.48083,50.99406,50.97085], "fy":[64.26543,63.78092,63.85848,64.21879]}, + {"t":0.9783, "x":6.32078, "y":3.79891, "heading":3.12056, "vx":-2.07529, "vy":-0.62802, "omega":0.04658, "ax":3.10448, "ay":3.8173, "alpha":0.02853, "fx":[51.58981,51.46263,51.98761,51.96044], "fy":[63.27822,65.17632,62.84075,63.23522]}, + {"t":0.98002, "x":6.3172, "y":3.79783, "heading":3.12065, "vx":-2.06994, "vy":-0.62144, "omega":0.04662, "ax":3.1356, "ay":3.79204, "alpha":0.01659, "fx":[51.57637,53.51867,52.00201,51.97864], "fy":[63.44266,62.9625,63.04408,63.39688]}, + {"t":0.98175, "x":6.31364, "y":3.79676, "heading":3.12073, "vx":-2.06454, "vy":-0.6149, "omega":0.04665, "ax":3.16493, "ay":3.76788, "alpha":0.02858, "fx":[52.60143,52.47544,52.99096,52.96372], "fy":[62.45834,64.33232,62.02802,62.41616]}, + {"t":0.98347, "x":6.31008, "y":3.79571, "heading":3.12081, "vx":-2.05908, "vy":-0.60841, "omega":0.0467, "ax":3.19529, "ay":3.74244, "alpha":0.01867, "fx":[52.56952,54.53303,52.98831,52.96484], "fy":[62.6142,62.13509,62.22052,62.56916]}, + {"t":0.9852, "x":6.30654, "y":3.79467, "heading":3.12089, "vx":-2.05357, "vy":-0.60195, "omega":0.04673, "ax":3.224, "ay":3.71804, "alpha":0.02799, "fx":[53.58831,53.46368,53.97259,53.94533], "fy":[61.63334,63.4793,61.20711,61.59189]}, + {"t":0.98692, "x":6.303, "y":3.79363, "heading":3.12097, "vx":-2.04801, "vy":-0.59554, "omega":0.04678, "ax":3.25359, "ay":3.69246, "alpha":0.01998, "fx":[53.53925,55.51951,53.95385,53.9303], "fy":[61.78062,61.30023,61.38916,61.73623]}, + {"t":0.98864, "x":6.29947, "y":3.79261, "heading":3.12105, "vx":-2.0424, "vy":-0.58918, "omega":0.04682, "ax":3.28167, "ay":3.66784, "alpha":0.02634, "fx":[54.55069,54.42797,54.93202,54.90477], "fy":[60.80649,62.61012,60.38199,60.76567]}, + {"t":0.99037, "x":6.29596, "y":3.7916, "heading":3.12113, "vx":-2.03674, "vy":-0.58285, "omega":0.04686, "ax":3.31048, "ay":3.64215, "alpha":0.02074, "fx":[54.48527,56.47954,54.89777,54.87418], "fy":[60.94261,60.45912,60.55125,60.89878]}, + {"t":0.99209, "x":6.29245, "y":3.7906, "heading":3.12121, "vx":-2.03103, "vy":-0.57657, "omega":0.0469, "ax":3.33793, "ay":3.61734, "alpha":0.02489, "fx":[55.48859,55.36751,55.86889,55.84167], "fy":[59.97275,61.7442,59.54807,59.9325]}, + {"t":0.99382, "x":6.28895, "y":3.78961, "heading":3.12129, "vx":-2.02528, "vy":-0.57034, "omega":0.04694, "ax":3.36598, "ay":3.59157, "alpha":0.02105, "fx":[55.40772,57.41341,55.81973,55.79611], "fy":[60.10084,59.61293,59.70798,60.05751]}, + {"t":0.99554, "x":6.28547, "y":3.78864, "heading":3.12137, "vx":-2.01947, "vy":-0.56414, "omega":0.04698, "ax":3.39278, "ay":3.56661, "alpha":0.02314, "fx":[56.40228,56.28297,56.78292,56.75575], "fy":[59.13582,60.87306,58.70957,59.09608]}, + {"t":0.99726, "x":6.28199, "y":3.78767, "heading":3.12145, "vx":-2.01362, "vy":-0.55799, "omega":0.04702, "ax":3.42006, "ay":3.54078, "alpha":0.02073, "fx":[56.30806,58.31728,56.72074,56.69712], "fy":[59.25599,58.76296,58.8605,59.21311]}, + {"t":0.99899, "x":6.27852, "y":3.78671, "heading":3.12153, "vx":-2.00773, "vy":-0.55189, "omega":0.04705, "ax":3.44622, "ay":3.51568, "alpha":0.02116, "fx":[57.29195,57.17457,57.67394,57.64685], "fy":[58.29639,59.99782,57.86756,58.25713]}, + {"t":1.00071, "x":6.27507, "y":3.78577, "heading":3.12161, "vx":-2.00179, "vy":-0.54583, "omega":0.04709, "ax":3.47275, "ay":3.48982, "alpha":0.02059, "fx":[57.18296,59.2026,57.59718,57.57358], "fy":[58.40877,57.90968,58.00976,58.36631]}, + {"t":1.00244, "x":6.27162, "y":3.78483, "heading":3.1217, "vx":-1.9958, "vy":-0.53981, "omega":0.04713, "ax":3.49826, "ay":3.46462, "alpha":0.01901, "fx":[58.15783,58.0425,58.54185,58.51485], "fy":[57.45515,59.11955,57.02304,57.41633]}, + {"t":1.00416, "x":6.26818, "y":3.7839, "heading":3.12178, "vx":-1.98977, "vy":-0.53384, "omega":0.04716, "ax":3.52404, "ay":3.43875, "alpha":0.02008, "fx":[58.03556,60.06029,58.45186,58.42828], "fy":[57.55988,57.05457,57.15687,57.51782]}, + {"t":1.00589, "x":6.26476, "y":3.78299, "heading":3.12186, "vx":-1.98369, "vy":-0.52791, "omega":0.04719, "ax":3.54889, "ay":3.41347, "alpha":0.01677, "fx":[59.00015,58.88699,59.38663,59.35973], "fy":[56.61278,58.23926,56.17699,56.57439]}, + {"t":1.00761, "x":6.26134, "y":3.78208, "heading":3.12194, "vx":-1.97757, "vy":-0.52202, "omega":0.04722, "ax":3.57393, "ay":3.38761, "alpha":0.0197, "fx":[58.8639,60.89749,59.28261,59.25908], "fy":[56.71003,56.19823,56.30271,56.66836]}, + {"t":1.00933, "x":6.25794, "y":3.78119, "heading":3.12202, "vx":-1.97141, "vy":-0.51618, "omega":0.04726, "ax":3.59814, "ay":3.36228, "alpha":0.01446, "fx":[59.81917,59.70829,60.20831,60.18152], "fy":[55.76996,57.35793,55.33031,55.73199]}, + {"t":1.01106, "x":6.25455, "y":3.7803, "heading":3.1221, "vx":-1.9652, "vy":-0.51038, "omega":0.04728, "ax":3.62245, "ay":3.33645, "alpha":0.01924, "fx":[59.66954,61.71058,60.09077,60.06728], "fy":[55.85988,55.3417,55.44817,55.81859]}, + {"t":1.01278, "x":6.25116, "y":3.77943, "heading":3.12218, "vx":-1.95896, "vy":-0.50463, "omega":0.04731, "ax":3.64601, "ay":3.31109, "alpha":0.01215, "fx":[60.61518,60.50669,61.00699,60.98033], "fy":[54.92734,56.47646,54.48385,54.8898]}, + {"t":1.01451, "x":6.24779, "y":3.77856, "heading":3.12227, "vx":-1.95267, "vy":-0.49892, "omega":0.04733, "ax":3.6696, "ay":3.28533, "alpha":0.0184, "fx":[60.45441,62.49466,60.87807,60.85465], "fy":[55.01008,54.48609,54.59415,54.96917]}, + {"t":1.01623, "x":6.24443, "y":3.77771, "heading":3.12235, "vx":-1.94634, "vy":-0.49326, "omega":0.04737, "ax":3.69252, "ay":3.25996, "alpha":0.00971, "fx":[61.38848,61.28257,61.78282,61.7563], "fy":[54.08628,55.59349,53.63911,54.04916]}, + {"t":1.01796, "x":6.24108, "y":3.77686, "heading":3.12243, "vx":-1.93998, "vy":-0.48764, "omega":0.04738, "ax":3.71539, "ay":3.23427, "alpha":0.01821, "fx":[61.21375,63.26549,61.63973,61.61637], "fy":[54.16141,53.6315,53.74141,54.1209]}, + {"t":1.01968, "x":6.23774, "y":3.77603, "heading":3.12251, "vx":-1.93357, "vy":-0.48206, "omega":0.04741, "ax":3.73768, "ay":3.20893, "alpha":0.00749, "fx":[62.13938,62.03598,62.53604,62.50965], "fy":[53.24585,54.71486,52.79525,53.20917]}, + {"t":1.0214, "x":6.23441, "y":3.7752, "heading":3.12259, "vx":-1.92713, "vy":-0.47653, "omega":0.04743, "ax":3.75985, "ay":3.18333, "alpha":0.01775, "fx":[61.95266,64.00911,62.38062,62.35735], "fy":[53.31435,52.77923,52.89065,53.27424]}, + {"t":1.02313, "x":6.23109, "y":3.77438, "heading":3.12267, "vx":-1.92064, "vy":-0.47104, "omega":0.04746, "ax":3.7815, "ay":3.15803, "alpha":0.00534, "fx":[62.86823,62.76741,63.26685,63.24063], "fy":[52.40752,53.83851,51.95388,52.37128]}, + {"t":1.02485, "x":6.22779, "y":3.77358, "heading":3.12276, "vx":-1.91412, "vy":-0.46559, "omega":0.04747, "ax":3.80299, "ay":3.13254, "alpha":0.01732, "fx":[62.67013,64.72983,63.09968,63.0765], "fy":[52.46955,51.92986,52.04261,52.42987]}, + {"t":1.02658, "x":6.22449, "y":3.77278, "heading":3.12284, "vx":-1.90757, "vy":-0.46019, "omega":0.0475, "ax":3.82401, "ay":3.1073, "alpha":0.00327, "fx":[63.5754,63.47722,63.97556,63.9495], "fy":[51.57187,52.96515,51.11567,51.53609]}, + {"t":1.0283, "x":6.22121, "y":3.77199, "heading":3.12292, "vx":-1.90097, "vy":-0.45483, "omega":0.0475, "ax":3.84483, "ay":3.08194, "alpha":0.01691, "fx":[63.36655,65.42795,63.79722,63.77415], "fy":[51.6276,51.08403,51.19796,51.58836]}, + {"t":1.03002, "x":6.21794, "y":3.77121, "heading":3.123, "vx":-1.89434, "vy":-0.44952, "omega":0.04753, "ax":3.86523, "ay":3.05678, "alpha":0.00128, "fx":[64.26127,64.16579,64.66247,64.63658], "fy":[50.73958,52.09505,50.28136,50.70428]}, + {"t":1.03175, "x":6.21468, "y":3.77044, "heading":3.12308, "vx":-1.88768, "vy":-0.44425, "omega":0.04753, "ax":3.88539, "ay":3.03157, "alpha":0.01511, "fx":[64.04951,66.08223,64.48068,64.45771], "fy":[50.78882,50.24321,50.35719,50.75005]}, + {"t":1.03347, "x":6.21143, "y":3.76968, "heading":3.12317, "vx":-1.88098, "vy":-0.43902, "omega":0.04756, "ax":3.90518, "ay":3.00651, "alpha":-0.00058, "fx":[64.9262,64.83342,65.32797,65.30226], "fy":[49.91088,51.22986,49.45116,49.87608]}, + {"t":1.0352, "x":6.20819, "y":3.76892, "heading":3.12325, "vx":-1.87425, "vy":-0.43384, "omega":0.04756, "ax":3.92469, "ay":2.98145, "alpha":0.01497, "fx":[64.70381,66.7396,65.13519,65.11234], "fy":[49.9543,49.40603,49.52101,49.91602]}, + {"t":1.03692, "x":6.20496, "y":3.76818, "heading":3.12333, "vx":-1.86748, "vy":-0.4287, "omega":0.04758, "ax":3.94388, "ay":2.9565, "alpha":-0.00235, "fx":[65.57055,65.48051,65.97246,65.94695], "fy":[49.0866,50.36922,48.62586,49.05231]}, + {"t":1.03865, "x":6.20175, "y":3.76745, "heading":3.12341, "vx":-1.86068, "vy":-0.4236, "omega":0.04758, "ax":3.96276, "ay":2.93162, "alpha":0.01527, "fx":[65.33616,67.38146,65.76732,65.74461], "fy":[49.12437,48.57384,48.68989,49.08659]}, + {"t":1.04037, "x":6.19855, "y":3.76672, "heading":3.12349, "vx":-1.85385, "vy":-0.41854, "omega":0.04761, "ax":3.98136, "ay":2.90681, "alpha":-0.00403, "fx":[66.19476,66.10748,66.59632,66.57101], "fy":[48.26717,49.51376,47.80595,48.23341]}, + {"t":1.04209, "x":6.19536, "y":3.766, "heading":3.12358, "vx":-1.84698, "vy":-0.41353, "omega":0.0476, "ax":3.99963, "ay":2.8821, "alpha":0.01515, "fx":[65.95132,67.99548,66.3817,66.35913], "fy":[48.29927,47.74757,47.86425,48.26202]}, + {"t":1.04382, "x":6.19218, "y":3.76529, "heading":3.12366, "vx":-1.84009, "vy":-0.40856, "omega":0.04763, "ax":4.01765, "ay":2.85744, "alpha":-0.00561, "fx":[66.79927,66.71477,67.19996,67.17486], "fy":[47.45299,48.66406,46.99187,47.41977]}, + {"t":1.04554, "x":6.18901, "y":3.76459, "heading":3.12374, "vx":-1.83316, "vy":-0.40364, "omega":0.04762, "ax":4.03531, "ay":2.83293, "alpha":0.01507, "fx":[66.54742,68.5889,66.97647,66.95406], "fy":[47.47955,46.92743,47.04458,47.44284]}, + {"t":1.04727, "x":6.18586, "y":3.7639, "heading":3.12382, "vx":-1.8262, "vy":-0.39875, "omega":0.04764, "ax":4.05276, "ay":2.80843, "alpha":-0.00706, "fx":[67.38453,67.3028,67.78379,67.75891], "fy":[46.64446,47.82055,46.18411,46.61181]}, + {"t":1.04899, "x":6.18271, "y":3.76322, "heading":3.1239, "vx":-1.81921, "vy":-0.39391, "omega":0.04763, "ax":4.06984, "ay":2.78413, "alpha":0.01503, "fx":[67.12499,69.16191,67.55215,67.52991], "fy":[46.6656,46.11384,46.23132,46.62947]}, + {"t":1.05072, "x":6.17958, "y":3.76254, "heading":3.12399, "vx":-1.8122, "vy":-0.38911, "omega":0.04766, "ax":4.08672, "ay":2.75981, "alpha":-0.00842, "fx":[67.95097,67.87202,68.34826,68.32361], "fy":[45.84202,46.98363,45.38294,45.80995]}, + {"t":1.05244, "x":6.17647, "y":3.76188, "heading":3.12407, "vx":-1.80515, "vy":-0.38435, "omega":0.04764, "ax":4.10352, "ay":2.73529, "alpha":0.01599, "fx":[67.68424,69.73442,68.10902,68.08696], "fy":[45.85092,45.29965,45.41784,45.8154]}, + {"t":1.05416, "x":6.17336, "y":3.76122, "heading":3.12415, "vx":-1.79807, "vy":-0.37963, "omega":0.04767, "ax":4.11957, "ay":2.71158, "alpha":-0.00966, "fx":[68.49907,68.42289,68.89381,68.86939], "fy":[45.04601,46.15356,44.58887,45.01455]}, + {"t":1.05589, "x":6.17026, "y":3.76057, "heading":3.12423, "vx":-1.79097, "vy":-0.37496, "omega":0.04765, "ax":4.13578, "ay":2.68732, "alpha":0.01388, "fx":[68.2364,70.23534,68.65798,68.63611], "fy":[45.04975,44.50197,44.61886,45.01485]}, + {"t":1.05761, "x":6.16718, "y":3.75992, "heading":3.12431, "vx":-1.78384, "vy":-0.37032, "omega":0.04768, "ax":4.15133, "ay":2.66378, "alpha":-0.01075, "fx":[69.02923,68.95579,69.42093,69.39676], "fy":[44.25655,45.33148,43.80188,44.22571]}, + {"t":1.05934, "x":6.16411, "y":3.75929, "heading":3.1244, "vx":-1.77668, "vy":-0.36573, "omega":0.04766, "ax":4.16697, "ay":2.63978, "alpha":0.01404, "fx":[68.75983,70.75153,69.17799,69.1563], "fy":[44.25582,43.71053,43.82737,44.22156]}, + {"t":1.06106, "x":6.16106, "y":3.75866, "heading":3.12448, "vx":-1.7695, "vy":-0.36118, "omega":0.04768, "ax":4.18202, "ay":2.61642, "alpha":-0.01175, "fx":[69.54186,69.47114,69.93014,69.90621], "fy":[43.47417,44.51707,43.0224,43.44396]}, + {"t":1.06279, "x":6.15801, "y":3.75804, "heading":3.12456, "vx":-1.76229, "vy":-0.35667, "omega":0.04766, "ax":4.19712, "ay":2.59268, "alpha":0.01417, "fx":[69.26663,71.24857,69.68101,69.65951], "fy":[43.46908,42.92687,43.04349,43.43546]}, + {"t":1.06451, "x":6.15498, "y":3.75743, "heading":3.12464, "vx":-1.75505, "vy":-0.3522, "omega":0.04769, "ax":4.21168, "ay":2.56951, "alpha":-0.0127, "fx":[70.03741,69.96938,70.42191,70.39824], "fy":[42.69933,43.71012,42.25084,42.66974]}, + {"t":1.06623, "x":6.15196, "y":3.75683, "heading":3.12473, "vx":-1.74779, "vy":-0.34777, "omega":0.04766, "ax":4.22624, "ay":2.54605, "alpha":0.01383, "fx":[69.75923,71.72075,70.16948,70.14818], "fy":[42.68979,42.1515,42.26749,42.65682]}, + {"t":1.06796, "x":6.14895, "y":3.75623, "heading":3.12481, "vx":-1.7405, "vy":-0.34338, "omega":0.04769, "ax":4.24033, "ay":2.52308, "alpha":-0.01359, "fx":[70.51628,70.45094,70.89676,70.87333], "fy":[41.93217,42.91125,41.48725,41.90321]}, + {"t":1.06968, "x":6.14596, "y":3.75565, "heading":3.12489, "vx":-1.73319, "vy":-0.33903, "omega":0.04767, "ax":4.25438, "ay":2.49989, "alpha":0.01395, "fx":[70.23347,72.18245,70.63941,70.6183], "fy":[41.91829,41.38397,41.49947,41.88596]}, + {"t":1.07141, "x":6.14298, "y":3.75507, "heading":3.12497, "vx":-1.72585, "vy":-0.33472, "omega":0.04769, "ax":4.268, "ay":2.47713, "alpha":-0.01444, "fx":[70.97891,70.91623,71.35518,71.332], "fy":[41.17295,42.12066,40.73182,41.14461]}, + {"t":1.07313, "x":6.14001, "y":3.75449, "heading":3.12505, "vx":-1.71849, "vy":-0.33045, "omega":0.04766, "ax":4.28155, "ay":2.45421, "alpha":0.01431, "fx":[70.69064,72.63143,71.09214,71.07121], "fy":[41.15482,40.62459,40.73964,41.12314]}, + {"t":1.07485, "x":6.13705, "y":3.75393, "heading":3.12514, "vx":-1.71111, "vy":-0.32621, "omega":0.04769, "ax":4.29472, "ay":2.43167, "alpha":-0.01525, "fx":[71.42572,71.36566,71.79767,71.77474], "fy":[40.42188,41.33854,39.9847,40.39414]}, + {"t":1.07658, "x":6.1341, "y":3.75337, "heading":3.12522, "vx":-1.70371, "vy":-0.32202, "omega":0.04766, "ax":4.30778, "ay":2.40904, "alpha":0.01443, "fx":[71.13366,73.06071,71.53053,71.50979], "fy":[40.39953,39.87379,39.98816,40.36851]}, + {"t":1.0783, "x":6.13117, "y":3.75282, "heading":3.1253, "vx":-1.69628, "vy":-0.31787, "omega":0.04769, "ax":4.32051, "ay":2.38672, "alpha":-0.01602, "fx":[71.85716,71.79965,72.22469,72.20204], "fy":[39.67905,40.5651,39.24614,39.65193]}, + {"t":1.08003, "x":6.12826, "y":3.75227, "heading":3.12538, "vx":-1.68883, "vy":-0.31375, "omega":0.04766, "ax":4.33311, "ay":2.36437, "alpha":0.01478, "fx":[71.56086,73.4775,71.95275,71.93223], "fy":[39.65258,39.13147,39.24525,39.62231]}, + {"t":1.08175, "x":6.12535, "y":3.75173, "heading":3.12547, "vx":-1.68136, "vy":-0.30968, "omega":0.04769, "ax":4.34541, "ay":2.34229, "alpha":-0.01636, "fx":[72.27382,72.21846,72.63662,72.6144], "fy":[38.94269,39.80588,38.51487,38.91626]}, + {"t":1.08348, "x":6.12246, "y":3.7512, "heading":3.12555, "vx":-1.67387, "vy":-0.30564, "omega":0.04766, "ax":4.35755, "ay":2.32022, "alpha":0.01559, "fx":[71.97177,73.8853,72.35809,72.33793], "fy":[38.91405,38.39754,38.51145,38.88467]}, + {"t":1.0852, "x":6.11958, "y":3.75068, "heading":3.12563, "vx":-1.66635, "vy":-0.30164, "omega":0.04768, "ax":4.36943, "ay":2.29838, "alpha":-0.01688, "fx":[72.67602,72.62292,73.03392,73.01213], "fy":[38.21605,39.05194,37.79358,38.19031]}, + {"t":1.08692, "x":6.11671, "y":3.75016, "heading":3.12571, "vx":-1.65882, "vy":-0.29767, "omega":0.04765, "ax":4.38113, "ay":2.27659, "alpha":0.01507, "fx":[72.37486,74.25903,72.75581,72.73589], "fy":[38.18409,37.67335,37.78602,38.15545]}, + {"t":1.08865, "x":6.11386, "y":3.74965, "heading":3.12579, "vx":-1.65127, "vy":-0.29375, "omega":0.04768, "ax":4.3926, "ay":2.255, "alpha":-0.01747, "fx":[73.06412,73.01337,73.41716,73.39567], "fy":[37.49858,38.30636,37.08094,37.47346]}, + {"t":1.09037, "x":6.11102, "y":3.74915, "heading":3.12588, "vx":-1.64369, "vy":-0.28986, "omega":0.04765, "ax":4.40389, "ay":2.23349, "alpha":0.01562, "fx":[72.75871,74.63485,73.13451,73.1148], "fy":[37.46304,36.95757,37.06946,37.43508]}, + {"t":1.0921, "x":6.10819, "y":3.74865, "heading":3.12596, "vx":-1.6361, "vy":-0.28601, "omega":0.04768, "ax":4.41496, "ay":2.21216, "alpha":-0.01805, "fx":[73.43863,73.39024,73.78669,73.76548], "fy":[36.79007,37.56945,36.37728,36.76556]}, + {"t":1.09382, "x":6.10537, "y":3.74816, "heading":3.12604, "vx":-1.62849, "vy":-0.2822, "omega":0.04765, "ax":4.42584, "ay":2.19093, "alpha":0.01702, "fx":[73.12501,75.00962,73.49564,73.47615], "fy":[36.75093,36.25052,36.36198,36.72363]}, + {"t":1.09555, "x":6.10257, "y":3.74768, "heading":3.12612, "vx":-1.62085, "vy":-0.27842, "omega":0.04768, "ax":4.43652, "ay":2.16985, "alpha":-0.0186, "fx":[73.80001,73.75393,74.14291,74.12198], "fy":[36.09049,36.84146,35.68275,36.06659]}, + {"t":1.09727, "x":6.09979, "y":3.7472, "heading":3.12621, "vx":-1.61321, "vy":-0.27468, "omega":0.04764, "ax":4.44701, "ay":2.1489, "alpha":0.01771, "fx":[73.48229,75.36,73.84748,73.82823], "fy":[36.04749,35.55293,35.6635,36.02087]}, + {"t":1.09899, "x":6.09701, "y":3.74673, "heading":3.12629, "vx":-1.60554, "vy":-0.27097, "omega":0.04767, "ax":4.45732, "ay":2.12808, "alpha":-0.01912, "fx":[74.1487,74.10486,74.48623,74.46559], "fy":[35.3998,36.12258,34.99735,35.37654]}, + {"t":1.10072, "x":6.09425, "y":3.74627, "heading":3.12637, "vx":-1.59785, "vy":-0.2673, "omega":0.04764, "ax":4.46742, "ay":2.10742, "alpha":0.0185, "fx":[73.82677,75.69874,74.18635,74.16736], "fy":[35.35295,34.86448,34.97414,35.32701]}, + {"t":1.10244, "x":6.0915, "y":3.74581, "heading":3.12645, "vx":-1.59015, "vy":-0.26367, "omega":0.04767, "ax":4.47737, "ay":2.08686, "alpha":-0.0196, "fx":[74.48507,74.44341,74.81709,74.79675], "fy":[34.71803,35.41298,34.32108,34.69541]}, + {"t":1.10417, "x":6.08877, "y":3.74536, "heading":3.12653, "vx":-1.58243, "vy":-0.26007, "omega":0.04764, "ax":4.48711, "ay":2.06648, "alpha":0.01947, "fx":[74.15845,76.02739,74.51229,74.49356], "fy":[34.66738,34.18516,34.29393,34.64212]}, + {"t":1.10589, "x":6.08604, "y":3.74491, "heading":3.12662, "vx":-1.57469, "vy":-0.25651, "omega":0.04767, "ax":4.4967, "ay":2.04618, "alpha":-0.02003, "fx":[74.80952,74.76999,75.13589,75.11587], "fy":[34.04514,34.71282,33.6539,34.02316]}, + {"t":1.10762, "x":6.08333, "y":3.74447, "heading":3.1267, "vx":-1.56694, "vy":-0.25298, "omega":0.04764, "ax":4.50608, "ay":2.02608, "alpha":0.02048, "fx":[74.47834,76.34458,74.82627,74.80783], "fy":[33.99075,33.51502,33.62286,33.96618]}, + {"t":1.10934, "x":6.08064, "y":3.74404, "heading":3.12678, "vx":-1.55917, "vy":-0.24949, "omega":0.04767, "ax":4.51534, "ay":2.00603, "alpha":-0.02249, "fx":[75.12238,75.08601,75.44275,75.42306], "fy":[33.39157,33.98973,33.00642,33.37026]}, + {"t":1.11106, "x":6.07796, "y":3.74361, "heading":3.12686, "vx":-1.55138, "vy":-0.24603, "omega":0.04764, "ax":4.52438, "ay":1.98622, "alpha":0.02159, "fx":[74.78649,76.65148,75.12847,75.1103], "fy":[33.32314,32.85401,32.96091,33.29926]}, + {"t":1.11279, "x":6.07529, "y":3.74319, "heading":3.12694, "vx":-1.54358, "vy":-0.2426, "omega":0.04767, "ax":4.53329, "ay":1.96645, "alpha":-0.02365, "fx":[75.42395,75.38983,75.73848,75.7191], "fy":[32.74085,33.29653,32.36169,32.72018]}, + {"t":1.11451, "x":6.07264, "y":3.74278, "heading":3.12703, "vx":-1.53577, "vy":-0.23921, "omega":0.04763, "ax":4.54205, "ay":1.94681, "alpha":0.02293, "fx":[75.08316,76.95151,75.41922,75.40135], "fy":[32.66293,32.20038,32.30633,32.63975]}, + {"t":1.11624, "x":6.06999, "y":3.74237, "heading":3.12711, "vx":-1.52794, "vy":-0.23585, "omega":0.04767, "ax":4.5505, "ay":1.92766, "alpha":-0.02433, "fx":[75.71303,75.68086,76.0218,76.00274], "fy":[32.10063,32.62396,31.72741,32.08059]}, + {"t":1.11796, "x":6.06737, "y":3.74196, "heading":3.12719, "vx":-1.52009, "vy":-0.23253, "omega":0.04763, "ax":4.55904, "ay":1.90805, "alpha":0.02406, "fx":[75.36942,77.23695,75.69957,75.68199], "fy":[32.01353,31.55769,31.66259,31.99103]}, + {"t":1.11968, "x":6.06475, "y":3.74157, "heading":3.12727, "vx":-1.51223, "vy":-0.22924, "omega":0.04767, "ax":4.56719, "ay":1.88915, "alpha":-0.02458, "fx":[75.99355,75.96319,76.29659,76.27785], "fy":[31.46294,31.96245,31.09566,31.44352]}, + {"t":1.12141, "x":6.06215, "y":3.74117, "heading":3.12736, "vx":-1.50435, "vy":-0.22598, "omega":0.04763, "ax":4.57541, "ay":1.86983, "alpha":0.02473, "fx":[75.64779,77.50498,75.97197,75.95468], "fy":[31.37303,30.92428,31.02786,31.35121]}, + {"t":1.12313, "x":6.05956, "y":3.74079, "heading":3.12744, "vx":-1.49646, "vy":-0.22276, "omega":0.04767, "ax":4.58327, "ay":1.85117, "alpha":-0.02479, "fx":[76.26396,76.23534,76.56125,76.54283], "fy":[30.83402,31.31061,30.47274,30.81522]}, + {"t":1.12486, "x":6.05699, "y":3.74041, "heading":3.12752, "vx":-1.48856, "vy":-0.21957, "omega":0.04763, "ax":4.59118, "ay":1.83215, "alpha":0.02074, "fx":[75.9398,77.69313,76.25757,76.24056], "fy":[30.74079,30.30176,30.40186,30.71963]}, + {"t":1.12658, "x":6.05443, "y":3.74003, "heading":3.1276, "vx":-1.48065, "vy":-0.21641, "omega":0.04766, "ax":4.59876, "ay":1.81374, "alpha":-0.02499, "fx":[76.5245,76.49756,76.81623,76.79814], "fy":[30.21394,30.66832,29.85848,30.19575]}, + {"t":1.12831, "x":6.05189, "y":3.73966, "heading":3.12768, "vx":-1.47272, "vy":-0.21328, "omega":0.04762, "ax":4.60638, "ay":1.795, "alpha":0.02159, "fx":[76.19714,77.94524,76.50935,76.49264], "fy":[30.11837,29.68597,29.78482,30.09787]}, + {"t":1.13003, "x":6.04935, "y":3.73929, "heading":3.12777, "vx":-1.46477, "vy":-0.21019, "omega":0.04766, "ax":4.61369, "ay":1.77684, "alpha":-0.02524, "fx":[76.77539,76.75006,77.06199,77.04422], "fy":[29.60275,30.03541,29.25271,29.58514]}, + {"t":1.13175, "x":6.04683, "y":3.73893, "heading":3.12785, "vx":-1.45682, "vy":-0.20712, "omega":0.04761, "ax":4.62102, "ay":1.75838, "alpha":0.02243, "fx":[76.44469,78.18889,76.75173,76.73531], "fy":[29.50501,29.07887,29.17646,29.48514]}, + {"t":1.13348, "x":6.04433, "y":3.73858, "heading":3.12793, "vx":-1.44885, "vy":-0.20409, "omega":0.04765, "ax":4.62806, "ay":1.74047, "alpha":-0.02552, "fx":[77.01698,76.99319,77.29882,77.28138], "fy":[29.00038,29.41186,28.65538,28.98333]}, + {"t":1.1352, "x":6.04184, "y":3.73823, "heading":3.12801, "vx":-1.44087, "vy":-0.20109, "omega":0.04761, "ax":4.63512, "ay":1.72229, "alpha":0.0231, "fx":[76.68356,78.422,76.9858,76.96967], "fy":[28.9006,28.48045,28.5767,28.88135]}, + {"t":1.13693, "x":6.03936, "y":3.73789, "heading":3.12809, "vx":-1.43288, "vy":-0.19812, "omega":0.04765, "ax":4.64191, "ay":1.70462, "alpha":-0.02583, "fx":[77.24961,77.22728,77.52704,77.50992], "fy":[28.40674,28.79757,28.06646,28.39025]}, + {"t":1.13865, "x":6.0369, "y":3.73755, "heading":3.12818, "vx":-1.42488, "vy":-0.19518, "omega":0.0476, "ax":4.64871, "ay":1.68673, "alpha":0.02491, "fx":[76.90761,78.66414,77.20547,77.18964], "fy":[28.30528,27.89021,27.98559,28.28662]}, + {"t":1.14038, "x":6.03445, "y":3.73721, "heading":3.12826, "vx":-1.41686, "vy":-0.19227, "omega":0.04765, "ax":4.65525, "ay":1.6693, "alpha":-0.02615, "fx":[77.4736,77.45269,77.7469,77.73011], "fy":[27.82176,28.19248,27.48592,27.8058]}, + {"t":1.1421, "x":6.03201, "y":3.73688, "heading":3.12834, "vx":-1.40883, "vy":-0.18939, "omega":0.0476, "ax":4.66179, "ay":1.65168, "alpha":0.02471, "fx":[77.13309,78.86851,77.42662,77.41109], "fy":[27.7184,27.30927,27.4029,27.70033]}, + {"t":1.14382, "x":6.02959, "y":3.73656, "heading":3.12842, "vx":-1.4008, "vy":-0.18655, "omega":0.04764, "ax":4.6681, "ay":1.6345, "alpha":-0.02649, "fx":[77.68929,77.66973,77.9587,77.94223], "fy":[27.24533,27.59648,26.91372,27.2299]}, + {"t":1.14555, "x":6.02718, "y":3.73624, "heading":3.12851, "vx":-1.39275, "vy":-0.18373, "omega":0.0476, "ax":4.67439, "ay":1.61715, "alpha":0.02539, "fx":[77.34578,79.07842,77.63531,77.62008], "fy":[27.14035,26.73647,26.82871,27.12286]}, + {"t":1.14727, "x":6.02479, "y":3.73593, "heading":3.12859, "vx":-1.38469, "vy":-0.18094, "omega":0.04764, "ax":4.68047, "ay":1.60021, "alpha":-0.02685, "fx":[77.89695,77.87868,78.16271,78.14656], "fy":[26.6774,27.00944,26.34978,26.66248]}, + {"t":1.149, "x":6.0224, "y":3.73562, "heading":3.12867, "vx":-1.37662, "vy":-0.17818, "omega":0.0476, "ax":4.68653, "ay":1.58313, "alpha":0.02619, "fx":[77.54969,79.28302,77.83549,77.82055], "fy":[26.57093,26.17201,26.26291,26.554]}, + {"t":1.15072, "x":6.02004, "y":3.73531, "heading":3.12875, "vx":-1.36854, "vy":-0.17545, "omega":0.04764, "ax":4.69239, "ay":1.56643, "alpha":-0.02722, "fx":[78.09687,78.07984,78.35921,78.34338], "fy":[26.11788,26.43124,25.79404,26.10346]}, + {"t":1.15244, "x":6.01769, "y":3.73501, "heading":3.12883, "vx":-1.36045, "vy":-0.17275, "omega":0.04759, "ax":4.69822, "ay":1.54961, "alpha":0.02754, "fx":[77.74307,79.4888,78.02537,78.01073], "fy":[26.01009,25.6157,25.70546,25.9937]}, + {"t":1.15417, "x":6.01535, "y":3.73472, "heading":3.12892, "vx":-1.35234, "vy":-0.17008, "omega":0.04764, "ax":4.70386, "ay":1.53315, "alpha":-0.02761, "fx":[78.28924,78.27342,78.54852,78.53303], "fy":[25.56758,25.86265,25.24566,25.55174]}, + {"t":1.15589, "x":6.01302, "y":3.73443, "heading":3.129, "vx":-1.34423, "vy":-0.16744, "omega":0.04759, "ax":4.70947, "ay":1.51659, "alpha":0.02934, "fx":[77.92677,79.69437,78.20573,78.19138], "fy":[25.45767,25.06751,25.15629,25.44181]}, + {"t":1.15762, "x":6.01071, "y":3.73414, "heading":3.12908, "vx":-1.33611, "vy":-0.16482, "omega":0.04765, "ax":4.7149, "ay":1.50037, "alpha":-0.02785, "fx":[78.47481,78.45987,78.7306,78.71542], "fy":[25.02306,25.30239,24.70667,25.00961]}, + {"t":1.15934, "x":6.00841, "y":3.73386, "heading":3.12916, "vx":-1.32798, "vy":-0.16223, "omega":0.0476, "ax":4.7203, "ay":1.48406, "alpha":0.03099, "fx":[78.10443,79.89035,78.37996,78.36589], "fy":[24.91337,24.52768,24.61536,24.89805]}, + {"t":1.16107, "x":6.00613, "y":3.73358, "heading":3.12924, "vx":-1.31985, "vy":-0.15967, "omega":0.04765, "ax":4.72554, "ay":1.46808, "alpha":-0.02813, "fx":[78.65344,78.63959,78.9058,78.89093], "fy":[24.48793,24.75028,24.17539,24.47496]}, + {"t":1.16279, "x":6.00386, "y":3.73331, "heading":3.12933, "vx":-1.3117, "vy":-0.15714, "omega":0.0476, "ax":4.73074, "ay":1.452, "alpha":0.03328, "fx":[78.27271,80.08844,78.54459,78.53081], "fy":[24.37677,23.99561,24.08238,24.36197]}, + {"t":1.16451, "x":6.00161, "y":3.73304, "heading":3.12941, "vx":-1.30354, "vy":-0.15464, "omega":0.04766, "ax":4.73578, "ay":1.43627, "alpha":-0.02832, "fx":[78.82564,78.81284,79.07423,79.05967], "fy":[23.96056,24.20679,23.65227,23.94806]}, + {"t":1.16624, "x":5.99937, "y":3.73277, "heading":3.12949, "vx":-1.29538, "vy":-0.15216, "omega":0.04761, "ax":4.74078, "ay":1.42045, "alpha":0.03608, "fx":[78.43245,80.28651,78.70027,78.68679], "fy":[23.84836,23.47206,23.55807,23.8341]}, + {"t":1.16796, "x":5.99714, "y":3.73251, "heading":3.12957, "vx":-1.2872, "vy":-0.14971, "omega":0.04767, "ax":4.74563, "ay":1.40494, "alpha":-0.02837, "fx":[78.99178,78.97997,79.236,79.22175], "fy":[23.44074,23.67188,23.13735,23.42873]}, + {"t":1.16969, "x":5.99493, "y":3.73226, "heading":3.12965, "vx":-1.27902, "vy":-0.14729, "omega":0.04762, "ax":4.75044, "ay":1.38936, "alpha":0.03778, "fx":[78.59265,80.45984,78.8556,78.8424], "fy":[23.3273,22.95722,23.04198,23.31358]}, + {"t":1.17141, "x":5.99273, "y":3.732, "heading":3.12974, "vx":-1.27083, "vy":-0.1449, "omega":0.04769, "ax":4.75512, "ay":1.37408, "alpha":-0.02824, "fx":[79.15216,79.14131,79.39125,79.3773], "fy":[22.92828,23.14552,22.63062,22.91677]}, + {"t":1.17314, "x":5.99055, "y":3.73176, "heading":3.12982, "vx":-1.26263, "vy":-0.14253, "omega":0.04764, "ax":4.75975, "ay":1.35875, "alpha":0.02457, "fx":[78.82309,80.40253,79.07907,79.06615], "fy":[22.81157,22.45531,22.53349,22.79842]}, + {"t":1.17486, "x":5.98838, "y":3.73151, "heading":3.1299, "vx":-1.25442, "vy":-0.14018, "omega":0.04768, "ax":4.76425, "ay":1.34369, "alpha":-0.02805, "fx":[79.30674,79.2968,79.54048,79.52682], "fy":[22.4234,22.62724,22.13172,22.41238]}, + {"t":1.17658, "x":5.98622, "y":3.73127, "heading":3.12998, "vx":-1.24621, "vy":-0.13787, "omega":0.04763, "ax":4.7687, "ay":1.3286, "alpha":0.02549, "fx":[78.97604,80.55121,79.22666,79.21401], "fy":[22.30567,21.95641,22.03314,22.29305]}, + {"t":1.17831, "x":5.98408, "y":3.73104, "heading":3.13007, "vx":-1.23798, "vy":-0.13558, "omega":0.04768, "ax":4.77303, "ay":1.31376, "alpha":-0.02793, "fx":[79.45541,79.44634,79.6842,79.67084], "fy":[21.92631,22.11658,21.64021,21.91577]}, + {"t":1.18003, "x":5.98195, "y":3.73081, "heading":3.13015, "vx":-1.22976, "vy":-0.13331, "omega":0.04763, "ax":4.77732, "ay":1.2989, "alpha":0.0264, "fx":[79.12284,80.69515,79.36847,79.35611], "fy":[21.80754,21.46492,21.54019,21.79545]}, + {"t":1.18176, "x":5.97984, "y":3.73058, "heading":3.13023, "vx":-1.22152, "vy":-0.13107, "omega":0.04768, "ax":4.78149, "ay":1.28428, "alpha":-0.02786, "fx":[79.59843,79.5902,79.82258,79.80951], "fy":[21.43688,21.61347,21.15605,21.4268]}, + {"t":1.18348, "x":5.97774, "y":3.73035, "heading":3.13031, "vx":-1.21327, "vy":-0.12886, "omega":0.04763, "ax":4.78561, "ay":1.26965, "alpha":0.02701, "fx":[79.26539,80.82976,79.50627,79.49418], "fy":[21.317,20.98085,21.05459,21.30541]}, + {"t":1.18521, "x":5.97565, "y":3.73013, "heading":3.13039, "vx":-1.20502, "vy":-0.12667, "omega":0.04767, "ax":4.78963, "ay":1.25524, "alpha":-0.02784, "fx":[79.73602,79.72859,79.95578,79.94299], "fy":[20.95495,21.11783,20.67915,20.94531]}, + {"t":1.18693, "x":5.97358, "y":3.72992, "heading":3.13048, "vx":-1.19676, "vy":-0.1245, "omega":0.04763, "ax":4.7936, "ay":1.24084, "alpha":0.02723, "fx":[79.40432,80.95403,79.64064,79.62881], "fy":[20.8339,20.50413,20.57624,20.8228]}, + {"t":1.18865, "x":5.97153, "y":3.7297, "heading":3.13056, "vx":-1.1885, "vy":-0.12236, "omega":0.04767, "ax":4.79746, "ay":1.22665, "alpha":-0.02784, "fx":[79.86841,79.86174,80.08396,80.07146], "fy":[20.48038,20.62963,20.20943,20.47116]}, + {"t":1.19038, "x":5.96949, "y":3.7295, "heading":3.13064, "vx":-1.18023, "vy":-0.12025, "omega":0.04763, "ax":4.80128, "ay":1.21247, "alpha":0.02587, "fx":[79.54596,81.04996,79.77777,79.76621], "fy":[20.35792,20.03518,20.10496,20.34729]}, + {"t":1.1921, "x":5.96746, "y":3.72929, "heading":3.13072, "vx":-1.17195, "vy":-0.11816, "omega":0.04767, "ax":4.805, "ay":1.19849, "alpha":-0.02788, "fx":[79.99573,79.98979,80.20736,80.19514], "fy":[20.01305,20.14878,19.74672,20.00425]}, + {"t":1.19383, "x":5.96544, "y":3.72909, "heading":3.13081, "vx":-1.16366, "vy":-0.11609, "omega":0.04762, "ax":4.80867, "ay":1.18453, "alpha":0.02577, "fx":[79.67588,81.16083,79.9036,79.8923], "fy":[19.88948,19.57265,19.64076,19.87932]}, + {"t":1.19555, "x":5.96344, "y":3.72889, "heading":3.13089, "vx":-1.15537, "vy":-0.11405, "omega":0.04767, "ax":4.81225, "ay":1.17075, "alpha":-0.02795, "fx":[80.11814,80.11287,80.32617,80.31424], "fy":[19.55281,19.67527,19.29089,19.54442]}, + {"t":1.19727, "x":5.96146, "y":3.72869, "heading":3.13097, "vx":-1.14707, "vy":-0.11203, "omega":0.04762, "ax":4.81578, "ay":1.15701, "alpha":0.02538, "fx":[79.80228,81.26323,80.02608,80.01506], "fy":[19.42817,19.11708,19.18349,19.41849]}, + {"t":1.199, "x":5.95949, "y":3.7285, "heading":3.13105, "vx":-1.13877, "vy":-0.11004, "omega":0.04766, "ax":4.81922, "ay":1.14343, "alpha":-0.0279, "fx":[80.23587,80.23116,80.4405,80.42894], "fy":[19.09874,19.21102,18.84134,19.09084]}, + {"t":1.20072, "x":5.95753, "y":3.72831, "heading":3.13113, "vx":-1.13046, "vy":-0.10806, "omega":0.04761, "ax":4.82261, "ay":1.12991, "alpha":0.02524, "fx":[79.92447,81.36364,80.14267,80.13189], "fy":[18.97393,18.66811,18.73308,18.96485]}, + {"t":1.20245, "x":5.95559, "y":3.72813, "heading":3.13122, "vx":-1.12215, "vy":-0.10612, "omega":0.04766, "ax":4.82593, "ay":1.11653, "alpha":-0.02803, "fx":[80.349,80.34488,80.55061,80.53934], "fy":[18.65235,18.75203,18.3988,18.64484]}, + {"t":1.20417, "x":5.95366, "y":3.72795, "heading":3.1313, "vx":-1.11383, "vy":-0.10419, "omega":0.04761, "ax":4.82919, "ay":1.10321, "alpha":0.02517, "fx":[80.03657,81.46649,80.25443,80.24389], "fy":[18.52683,18.22582,18.28917,18.51798]}, + {"t":1.2059, "x":5.95175, "y":3.72777, "heading":3.13138, "vx":-1.1055, "vy":-0.10229, "omega":0.04765, "ax":4.83238, "ay":1.09003, "alpha":-0.02817, "fx":[80.45776,80.45421,80.65658,80.64558], "fy":[18.21268,18.30028,17.96266,18.20554]}, + {"t":1.20762, "x":5.94985, "y":3.7276, "heading":3.13146, "vx":-1.09717, "vy":-0.10041, "omega":0.0476, "ax":4.83552, "ay":1.07692, "alpha":0.0247, "fx":[80.14897,81.55696,80.36383,80.35354], "fy":[18.08644,17.79028,17.85196,18.07799]}, + {"t":1.20934, "x":5.94797, "y":3.72742, "heading":3.13154, "vx":-1.08883, "vy":-0.09855, "omega":0.04765, "ax":4.83859, "ay":1.06393, "alpha":-0.02832, "fx":[80.5623,80.5593,80.75857,80.74782], "fy":[17.77959,17.85575,17.53281,17.77279]}, + {"t":1.21107, "x":5.9461, "y":3.72726, "heading":3.13163, "vx":-1.08049, "vy":-0.09672, "omega":0.0476, "ax":4.84161, "ay":1.05102, "alpha":0.02445, "fx":[80.25598,81.64702,80.46806,80.45802], "fy":[17.65279,17.36115,17.42126,17.64473]}, + {"t":1.21279, "x":5.94424, "y":3.72709, "heading":3.13171, "vx":-1.07214, "vy":-0.09491, "omega":0.04764, "ax":4.84456, "ay":1.03823, "alpha":-0.02847, "fx":[80.66279,80.66028,80.85671,80.84622], "fy":[17.35292,17.41838,17.10916,17.34646]}, + {"t":1.21452, "x":5.9424, "y":3.72693, "heading":3.13179, "vx":-1.06379, "vy":-0.09312, "omega":0.04759, "ax":4.84746, "ay":1.02551, "alpha":0.02408, "fx":[80.35936,81.73201,80.56886,80.55907], "fy":[17.22574,16.93841,16.99697,17.21805]}, + {"t":1.21624, "x":5.94057, "y":3.72677, "heading":3.13187, "vx":-1.05543, "vy":-0.09135, "omega":0.04763, "ax":4.8503, "ay":1.01291, "alpha":-0.02861, "fx":[80.75936,80.75731,80.95114,80.94091], "fy":[16.93255,16.98809,16.69161,16.92642]}, + {"t":1.21797, "x":5.93876, "y":3.72661, "heading":3.13196, "vx":-1.04706, "vy":-0.0896, "omega":0.04758, "ax":4.85309, "ay":1.00039, "alpha":0.02365, "fx":[80.45899,81.8129,80.6661,80.65655], "fy":[16.80516,16.52196,16.57897,16.79785]}, + {"t":1.21969, "x":5.93696, "y":3.72646, "heading":3.13204, "vx":-1.0387, "vy":-0.08788, "omega":0.04762, "ax":4.85582, "ay":0.98797, "alpha":-0.02875, "fx":[80.85216,80.85054,81.04199,81.03201], "fy":[16.51836,16.56475,16.28008,16.51255]}, + {"t":1.22141, "x":5.93518, "y":3.72631, "heading":3.13212, "vx":-1.03032, "vy":-0.08617, "omega":0.04757, "ax":4.8585, "ay":0.97564, "alpha":0.02332, "fx":[80.55424,81.89206,80.75915,80.74985], "fy":[16.39096,16.11165,16.16718,16.38401]}, + {"t":1.22314, "x":5.93341, "y":3.72616, "heading":3.1322, "vx":-1.02195, "vy":-0.08449, "omega":0.04761, "ax":4.86113, "ay":0.9634, "alpha":-0.02887, "fx":[80.94134,80.94012,81.12938,81.11965], "fy":[16.11028,16.14815,15.87453,16.10478]}, + {"t":1.22486, "x":5.93165, "y":3.72602, "heading":3.13228, "vx":-1.01356, "vy":-0.08283, "omega":0.04756, "ax":4.8637, "ay":0.95126, "alpha":0.02345, "fx":[80.6435,81.97485,80.84642,80.83735], "fy":[15.98304,15.7074,15.76147,15.97642]}, + {"t":1.22659, "x":5.92991, "y":3.72588, "heading":3.13237, "vx":-1.00518, "vy":-0.08119, "omega":0.0476, "ax":4.86623, "ay":0.9392, "alpha":-0.02899, "fx":[81.02705,81.02619,81.21341,81.20393], "fy":[15.70824,15.73801,15.4749,15.70304]}, + {"t":1.22831, "x":5.92819, "y":3.72574, "heading":3.13245, "vx":-0.99679, "vy":-0.07957, "omega":0.04755, "ax":4.8687, "ay":0.92725, "alpha":0.02296, "fx":[80.73237,82.04536,80.9333,80.92447], "fy":[15.58119,15.30923,15.36181,15.57491]}, + {"t":1.23004, "x":5.92648, "y":3.7256, "heading":3.13253, "vx":-0.98839, "vy":-0.07797, "omega":0.04759, "ax":4.87113, "ay":0.91536, "alpha":-0.02912, "fx":[81.10942,81.10891,81.29419,81.28494], "fy":[15.31222,15.33402,15.0812,15.3073]}, + {"t":1.23176, "x":5.92478, "y":3.72547, "heading":3.13261, "vx":-0.97999, "vy":-0.07639, "omega":0.04754, "ax":4.8735, "ay":0.90359, "alpha":0.02261, "fx":[80.8172,82.11486,81.01623,81.00763], "fy":[15.18536,14.91693,14.96809,15.1794]}, + {"t":1.23348, "x":5.9231, "y":3.72534, "heading":3.13269, "vx":-0.97159, "vy":-0.07484, "omega":0.04758, "ax":4.87584, "ay":0.89188, "alpha":-0.02927, "fx":[81.18859,81.18842,81.37181,81.36279], "fy":[14.92218,14.93579,14.69341,14.91754]}, + {"t":1.23521, "x":5.92143, "y":3.72521, "heading":3.13278, "vx":-0.96318, "vy":-0.0733, "omega":0.04753, "ax":4.87812, "ay":0.88029, "alpha":0.02217, "fx":[80.8993,82.18,81.09646,81.08807], "fy":[14.79542,14.53047,14.5802,14.78977]}, + {"t":1.23693, "x":5.91977, "y":3.72509, "heading":3.13286, "vx":-0.95477, "vy":-0.07178, "omega":0.04757, "ax":4.88037, "ay":0.86875, "alpha":-0.02945, "fx":[81.26467,81.26484,81.44638,81.43758], "fy":[14.53811,14.543,14.31152,14.53373]}, + {"t":1.23866, "x":5.91814, "y":3.72497, "heading":3.13294, "vx":-0.94636, "vy":-0.07028, "omega":0.04752, "ax":4.88256, "ay":0.85733, "alpha":0.02194, "fx":[80.979,82.24385,81.17433,81.16251], "fy":[14.41111,14.14955,14.19789,14.4064]}, + {"t":1.24038, "x":5.91651, "y":3.72485, "heading":3.13302, "vx":-0.93794, "vy":-0.0688, "omega":0.04756, "ax":4.88472, "ay":0.84595, "alpha":-0.02965, "fx":[81.33783,81.33833,81.51796,81.50937], "fy":[14.15989,14.15541,13.93556,14.15577]}, + {"t":1.2421, "x":5.9149, "y":3.72473, "heading":3.1331, "vx":-0.92952, "vy":-0.06734, "omega":0.04751, "ax":4.88682, "ay":0.83471, "alpha":0.02162, "fx":[81.05299,82.30642,81.24625,81.23827], "fy":[14.03288,13.77421,13.82185,14.02783]}, + {"t":1.24383, "x":5.91331, "y":3.72461, "heading":3.13319, "vx":-0.92109, "vy":-0.06591, "omega":0.04754, "ax":4.88889, "ay":0.8235, "alpha":-0.02986, "fx":[81.40824,81.40905,81.5866,81.57822], "fy":[13.78739,13.77286,13.56556,13.78352]}, + {"t":1.24555, "x":5.91172, "y":3.7245, "heading":3.13327, "vx":-0.91266, "vy":-0.06449, "omega":0.04749, "ax":4.89092, "ay":0.81242, "alpha":0.02146, "fx":[81.12548,82.36609,81.31658,81.3088], "fy":[13.65977,13.40481,13.45115,13.65502]}, + {"t":1.24728, "x":5.91016, "y":3.72439, "heading":3.13335, "vx":-0.90423, "vy":-0.06308, "omega":0.04753, "ax":4.89291, "ay":0.80137, "alpha":-0.03007, "fx":[81.47599,81.4771,81.65238,81.64422], "fy":[13.42052,13.39522,13.20144,13.4169]}, + {"t":1.249, "x":5.90861, "y":3.72428, "heading":3.13343, "vx":-0.89579, "vy":-0.0617, "omega":0.04748, "ax":4.89485, "ay":0.79046, "alpha":0.02077, "fx":[81.1982,82.41476,81.38689,81.37931], "fy":[13.29196,13.04108,13.08603,13.28749]}, + {"t":1.25073, "x":5.90707, "y":3.72418, "heading":3.13351, "vx":-0.88735, "vy":-0.06034, "omega":0.04751, "ax":4.89676, "ay":0.77957, "alpha":-0.03028, "fx":[81.5412,81.54261,81.71541,81.70745], "fy":[13.05918,13.02242,12.8431,13.05581]}, + {"t":1.25245, "x":5.90555, "y":3.72407, "heading":3.13359, "vx":-0.87891, "vy":-0.059, "omega":0.04746, "ax":4.89863, "ay":0.76883, "alpha":0.0206, "fx":[81.26607,82.46791,81.45218,81.44479], "fy":[12.92946,12.68271,12.7264,12.92527]}, + {"t":1.25417, "x":5.90404, "y":3.72397, "heading":3.13368, "vx":-0.87046, "vy":-0.05767, "omega":0.0475, "ax":4.90046, "ay":0.75809, "alpha":-0.03048, "fx":[81.60396,81.60566,81.77577,81.76801], "fy":[12.70323,12.65448,12.49039,12.70011]}, + {"t":1.2559, "x":5.90255, "y":3.72388, "heading":3.13376, "vx":-0.86201, "vy":-0.05636, "omega":0.04744, "ax":4.90225, "ay":0.7475, "alpha":0.02091, "fx":[81.3294,82.52502,81.51272,81.50553], "fy":[12.57215,12.32962,12.37218,12.56824]}, + {"t":1.25762, "x":5.90107, "y":3.72378, "heading":3.13384, "vx":-0.85356, "vy":-0.05507, "omega":0.04748, "ax":4.90401, "ay":0.73693, "alpha":-0.03065, "fx":[81.6644,81.66637,81.83352,81.82596], "fy":[12.35248,12.29155,12.14318,12.34959]}, + {"t":1.25935, "x":5.8996, "y":3.72369, "heading":3.13392, "vx":-0.84511, "vy":-0.0538, "omega":0.04743, "ax":4.90573, "ay":0.72649, "alpha":0.02132, "fx":[81.39027,82.58046,81.57049,81.56348], "fy":[12.21987,11.98185,12.0233,12.21623]}, + {"t":1.26107, "x":5.89815, "y":3.72359, "heading":3.134, "vx":-0.83665, "vy":-0.05255, "omega":0.04746, "ax":4.90742, "ay":0.71607, "alpha":-0.03075, "fx":[81.72264,81.72487,81.88871,81.88134], "fy":[12.00672,11.93381,11.80133,12.00407]}, + {"t":1.2628, "x":5.89672, "y":3.7235, "heading":3.13409, "vx":-0.82819, "vy":-0.05132, "omega":0.04741, "ax":4.90907, "ay":0.70579, "alpha":0.02146, "fx":[81.45075,82.62858,81.62744,81.62061], "fy":[11.87244,11.63942,11.67973,11.86906]}, + {"t":1.26452, "x":5.8953, "y":3.72342, "heading":3.13417, "vx":-0.81972, "vy":-0.0501, "omega":0.04745, "ax":4.91069, "ay":0.69551, "alpha":-0.03076, "fx":[81.7788,81.78129,81.94139,81.93421], "fy":[11.66575,11.58144,11.46471,11.66332]}, + {"t":1.26624, "x":5.89389, "y":3.72333, "heading":3.13425, "vx":-0.81125, "vy":-0.0489, "omega":0.0474, "ax":4.91227, "ay":0.68538, "alpha":0.02198, "fx":[81.50776,82.67891,81.68051,81.67384], "fy":[11.52982,11.30214,11.34138,11.52669]}, + {"t":1.26797, "x":5.8925, "y":3.72325, "heading":3.13433, "vx":-0.80278, "vy":-0.04772, "omega":0.04743, "ax":4.91383, "ay":0.67525, "alpha":-0.03065, "fx":[81.833,81.83572,81.99162,81.98461], "fy":[11.3294,11.23453,11.13318,11.32719]}, + {"t":1.26969, "x":5.89112, "y":3.72317, "heading":3.13441, "vx":-0.79431, "vy":-0.04655, "omega":0.04738, "ax":4.91535, "ay":0.66527, "alpha":0.02241, "fx":[81.56384,82.7244,81.73211,81.7256], "fy":[11.19184,10.97003,11.0082,11.18896]}, + {"t":1.27142, "x":5.88976, "y":3.72309, "heading":3.13449, "vx":-0.78584, "vy":-0.04541, "omega":0.04742, "ax":4.91684, "ay":0.65528, "alpha":-0.0304, "fx":[81.88534,81.88829,82.03944,82.03259], "fy":[10.99752,10.89315,10.80666,10.99553]}, + {"t":1.27314, "x":5.88841, "y":3.72301, "heading":3.13458, "vx":-0.77736, "vy":-0.04428, "omega":0.04737, "ax":4.9183, "ay":0.64545, "alpha":0.02338, "fx":[81.61554,82.77527,81.77904,81.77264], "fy":[10.85872,10.64227,10.68032,10.85599]}, + {"t":1.27486, "x":5.88708, "y":3.72294, "heading":3.13466, "vx":-0.76888, "vy":-0.04316, "omega":0.04741, "ax":4.91973, "ay":0.6356, "alpha":-0.02997, "fx":[81.93598,81.93914,82.08483,82.07814], "fy":[10.6699,10.55734,10.48514,10.66812]}, + {"t":1.27659, "x":5.88576, "y":3.72286, "heading":3.13474, "vx":-0.7604, "vy":-0.04207, "omega":0.04736, "ax":4.92112, "ay":0.62591, "alpha":0.02382, "fx":[81.66969,82.81299,81.82724,81.82098], "fy":[10.52954,10.32054,10.35747,10.52706]}, + {"t":1.27831, "x":5.88446, "y":3.72279, "heading":3.13482, "vx":-0.75191, "vy":-0.04099, "omega":0.0474, "ax":4.9225, "ay":0.6162, "alpha":-0.02932, "fx":[81.98508,81.98843,82.1278,82.12126], "fy":[10.34637,10.22712,10.16863,10.3448]}, + {"t":1.28004, "x":5.88317, "y":3.72272, "heading":3.1349, "vx":-0.74342, "vy":-0.03993, "omega":0.04735, "ax":4.92383, "ay":0.60665, "alpha":0.02488, "fx":[81.72018,82.85564,81.8709,81.86478], "fy":[10.20455,10.0038,10.03972,10.20232]}, + {"t":1.28176, "x":5.88189, "y":3.72265, "heading":3.13498, "vx":-0.73493, "vy":-0.03888, "omega":0.04739, "ax":4.92515, "ay":0.59707, "alpha":-0.02846, "fx":[82.03273,82.03627,82.16838,82.16198], "fy":[10.02683,9.9024,9.85706,10.02547]}, + {"t":1.28349, "x":5.88063, "y":3.72259, "heading":3.13507, "vx":-0.72644, "vy":-0.03785, "omega":0.04734, "ax":4.92642, "ay":0.58767, "alpha":0.02594, "fx":[81.77024,82.89399,81.91316,81.90717], "fy":[9.88364,9.6921,9.72705,9.88164]}, + {"t":1.28521, "x":5.87939, "y":3.72252, "heading":3.13515, "vx":-0.71795, "vy":-0.03684, "omega":0.04738, "ax":4.92769, "ay":0.57822, "alpha":-0.02735, "fx":[82.07906,82.08276,82.2066,82.20033], "fy":[9.71117,9.5831,9.55042,9.71002]}, + {"t":1.28693, "x":5.87816, "y":3.72246, "heading":3.13523, "vx":-0.70945, "vy":-0.03584, "omega":0.04734, "ax":4.92891, "ay":0.56895, "alpha":0.02605, "fx":[81.82493,82.91356,81.95885,81.95298], "fy":[9.56655,9.38565,9.41942,9.56479]}, + {"t":1.28866, "x":5.87694, "y":3.7224, "heading":3.13531, "vx":-0.70095, "vy":-0.03486, "omega":0.04738, "ax":4.93012, "ay":0.55964, "alpha":-0.02601, "fx":[82.12413,82.12799,82.2425,82.23636], "fy":[9.39931,9.26911,9.24865,9.39836]}, + {"t":1.29038, "x":5.87574, "y":3.72234, "heading":3.13539, "vx":-0.69245, "vy":-0.0339, "omega":0.04734, "ax":4.93129, "ay":0.5505, "alpha":0.02733, "fx":[81.87361,82.94601,81.9976,81.99184], "fy":[9.25342,9.08395,9.11673,9.25189]}, + {"t":1.29211, "x":5.87455, "y":3.72228, "heading":3.13547, "vx":-0.68395, "vy":-0.03295, "omega":0.04738, "ax":4.93245, "ay":0.54131, "alpha":-0.02442, "fx":[82.16801,82.17203,82.27614,82.27011], "fy":[9.09118,8.96032,8.95167,9.09043]}, + {"t":1.29383, "x":5.87338, "y":3.72223, "heading":3.13556, "vx":-0.67544, "vy":-0.03201, "omega":0.04734, "ax":4.93357, "ay":0.5323, "alpha":0.02934, "fx":[81.91858,82.98495,82.03157,82.02592], "fy":[8.94412,8.78703,8.81892,8.94281]}, + {"t":1.29556, "x":5.87222, "y":3.72217, "heading":3.13564, "vx":-0.66694, "vy":-0.03109, "omega":0.04739, "ax":4.93468, "ay":0.52324, "alpha":-0.02258, "fx":[82.21082,82.21498,82.30753,82.30161], "fy":[8.78668,8.65664,8.65947,8.78612]}, + {"t":1.29728, "x":5.87108, "y":3.72212, "heading":3.13572, "vx":-0.65843, "vy":-0.03019, "omega":0.04735, "ax":4.93575, "ay":0.51436, "alpha":0.03158, "fx":[81.96266,83.02242,82.06346,82.0579], "fy":[8.63846,8.49494,8.52597,8.63738]}, + {"t":1.299, "x":5.86995, "y":3.72207, "heading":3.1358, "vx":-0.64992, "vy":-0.02931, "omega":0.04741, "ax":4.93681, "ay":0.50543, "alpha":-0.02047, "fx":[82.25266,82.25697,82.33668,82.33086], "fy":[8.4857,8.35803,8.37202,8.48533]}, + {"t":1.30073, "x":5.86884, "y":3.72202, "heading":3.13588, "vx":-0.64141, "vy":-0.02843, "omega":0.04737, "ax":4.93784, "ay":0.49667, "alpha":0.02923, "fx":[82.03018,82.98687,82.11698,82.1115], "fy":[8.33595,8.20843,8.23785,8.33506]}, + {"t":1.30245, "x":5.86774, "y":3.72197, "heading":3.13596, "vx":-0.63289, "vy":-0.02758, "omega":0.04742, "ax":4.93885, "ay":0.48786, "alpha":-0.01812, "fx":[82.2935,82.29794,82.36373,82.358], "fy":[8.18827,8.06426,8.08915,8.18808]}, + {"t":1.30418, "x":5.86666, "y":3.72192, "heading":3.13605, "vx":-0.62438, "vy":-0.02674, "omega":0.04739, "ax":4.93983, "ay":0.47923, "alpha":0.03163, "fx":[82.0735,83.01844,82.146,82.14058], "fy":[8.03755,7.92554,7.95426,8.03686]}, + {"t":1.3059, "x":5.86559, "y":3.72188, "heading":3.13613, "vx":-0.61586, "vy":-0.02591, "omega":0.04745, "ax":4.9408, "ay":0.47054, "alpha":-0.01559, "fx":[82.33329,82.33787,82.38885,82.38318], "fy":[7.89445,7.77515,7.81068,7.89443]}, + {"t":1.30763, "x":5.86453, "y":3.72183, "heading":3.13621, "vx":-0.60734, "vy":-0.0251, "omega":0.04742, "ax":4.94174, "ay":0.46203, "alpha":0.03419, "fx":[82.11596,83.0486,82.17321,82.16786], "fy":[7.74274,7.64723,7.67511,7.74223]}, + {"t":1.30935, "x":5.86349, "y":3.72179, "heading":3.13629, "vx":-0.59882, "vy":-0.0243, "omega":0.04748, "ax":4.94267, "ay":0.45346, "alpha":-0.01286, "fx":[82.37209,82.37681,82.41206,82.40646], "fy":[7.60416,7.49062,7.53653,7.60429]}, + {"t":1.31107, "x":5.86247, "y":3.72175, "heading":3.13637, "vx":-0.5903, "vy":-0.02352, "omega":0.04746, "ax":4.94356, "ay":0.44507, "alpha":0.03739, "fx":[82.15508,83.08471,82.19628,82.19098], "fy":[7.45151,7.37318,7.40029,7.45116]}, + {"t":1.3128, "x":5.86146, "y":3.72171, "heading":3.13646, "vx":-0.58177, "vy":-0.02275, "omega":0.04752, "ax":4.94444, "ay":0.43661, "alpha":-0.00996, "fx":[82.40994,82.41479,82.43345,82.42789], "fy":[7.31737,7.21055,7.26662,7.31763]}, + {"t":1.31452, "x":5.86046, "y":3.72167, "heading":3.13654, "vx":-0.57325, "vy":-0.022, "omega":0.0475, "ax":4.9453, "ay":0.42833, "alpha":0.04048, "fx":[82.19448,83.11622,82.21878,82.2135], "fy":[7.16385,7.10314,7.12984,7.16365]}, + {"t":1.31625, "x":5.85948, "y":3.72163, "heading":3.13662, "vx":-0.56472, "vy":-0.02126, "omega":0.04757, "ax":4.94614, "ay":0.41999, "alpha":-0.00688, "fx":[82.44689,82.45187,82.45304,82.44752], "fy":[7.03399,6.93489,7.00088,7.03436]}, + {"t":1.31797, "x":5.85852, "y":3.7216, "heading":3.1367, "vx":-0.55619, "vy":-0.02054, "omega":0.04756, "ax":4.94696, "ay":0.41183, "alpha":0.04364, "fx":[82.23365,83.14492,82.24015,82.23491], "fy":[6.87951,6.83762,6.86352,6.87944]}, + {"t":1.31969, "x":5.85756, "y":3.72156, "heading":3.13678, "vx":-0.54766, "vy":-0.01983, "omega":0.04764, "ax":4.94776, "ay":0.4036, "alpha":-0.00362, "fx":[82.48295,82.48806,82.47092,82.46544], "fy":[6.75396,6.66358,6.73921,6.75444]}, + {"t":1.32142, "x":5.85663, "y":3.72153, "heading":3.13687, "vx":-0.53913, "vy":-0.01913, "omega":0.04763, "ax":4.94854, "ay":0.39555, "alpha":0.04564, "fx":[82.27833,83.15348,82.26628,82.26106], "fy":[6.59852,6.57627,6.60129,6.59856]}, + {"t":1.32314, "x":5.8557, "y":3.7215, "heading":3.13695, "vx":-0.5306, "vy":-0.01845, "omega":0.04771, "ax":4.94931, "ay":0.38743, "alpha":-0.00025, "fx":[82.51804,82.52327,82.48727,82.48181], "fy":[6.47738,6.3964,6.4814,6.47795]}, + {"t":1.32487, "x":5.8548, "y":3.72147, "heading":3.13703, "vx":-0.52207, "vy":-0.01778, "omega":0.04771, "ax":4.95005, "ay":0.37949, "alpha":0.04947, "fx":[82.3132,83.18706,82.28234,82.27714], "fy":[6.32121,6.31853,6.34283,6.32135]}, + {"t":1.32659, "x":5.8539, "y":3.72144, "heading":3.13711, "vx":-0.51353, "vy":-0.01713, "omega":0.04779, "ax":4.95078, "ay":0.37148, "alpha":0.00318, "fx":[82.55205,82.55741,82.50227,82.49683], "fy":[6.20433,6.13314,6.22721,6.20497]}, + {"t":1.32832, "x":5.85303, "y":3.72141, "heading":3.13719, "vx":-0.50499, "vy":-0.01649, "omega":0.0478, "ax":4.95149, "ay":0.36365, "alpha":0.0528, "fx":[82.34939,83.21216,82.29959,82.29441], "fy":[6.04751,6.06444,6.08797,6.04773]}, + {"t":1.33004, "x":5.85216, "y":3.72138, "heading":3.13728, "vx":-0.49646, "vy":-0.01586, "omega":0.04789, "ax":4.95218, "ay":0.35575, "alpha":0.00658, "fx":[82.58487,82.59033,82.51613,82.51071], "fy":[5.93494,5.87354,5.9764,5.93562]}, + {"t":1.33176, "x":5.85131, "y":3.72135, "heading":3.13736, "vx":-0.48792, "vy":-0.01525, "omega":0.0479, "ax":4.95285, "ay":0.34802, "alpha":0.05608, "fx":[82.38414,83.2364,82.3157,82.31052], "fy":[5.77762,5.81365,5.83643,5.77789]}, + {"t":1.33349, "x":5.85048, "y":3.72133, "heading":3.13744, "vx":-0.47938, "vy":-0.01465, "omega":0.048, "ax":4.95352, "ay":0.34022, "alpha":0.00987, "fx":[82.61629,82.62186,82.52913,82.52372], "fy":[5.66939,5.61727,5.72866,5.6701]}, + {"t":1.33521, "x":5.84966, "y":3.7213, "heading":3.13753, "vx":-0.47084, "vy":-0.01406, "omega":0.04802, "ax":4.95415, "ay":0.3326, "alpha":0.05922, "fx":[82.41714,83.2598,82.33088,82.32571], "fy":[5.51173,5.56583,5.58785,5.51203]}, + {"t":1.33694, "x":5.84886, "y":3.72128, "heading":3.13761, "vx":-0.4623, "vy":-0.01349, "omega":0.04812, "ax":4.95479, "ay":0.32491, "alpha":0.01291, "fx":[82.64608,82.65175,82.54158,82.53617], "fy":[5.40795,5.36395,5.48364,5.40865]}, + {"t":1.33866, "x":5.84807, "y":3.72125, "heading":3.13769, "vx":-0.45375, "vy":-0.01293, "omega":0.04814, "ax":4.95539, "ay":0.31739, "alpha":0.06234, "fx":[82.44685,83.28585,82.34423,82.33906], "fy":[5.25016,5.32057,5.34186,5.25045]}, + {"t":1.34039, "x":5.84729, "y":3.72123, "heading":3.13777, "vx":-0.44521, "vy":-0.01238, "omega":0.04825, "ax":4.95599, "ay":0.3098, "alpha":0.01557, "fx":[82.67394,82.6797,82.55385,82.54846], "fy":[5.15091,5.11311,5.24094,5.15158]}, + {"t":1.34211, "x":5.84653, "y":3.72121, "heading":3.13786, "vx":-0.43666, "vy":-0.01184, "omega":0.04827, "ax":4.95657, "ay":0.30238, "alpha":0.06465, "fx":[82.47611,83.30473,82.35933,82.35416], "fy":[4.99314,5.07759,5.09793,4.99339]}, + {"t":1.34383, "x":5.84579, "y":3.72119, "heading":3.13794, "vx":-0.42812, "vy":-0.01132, "omega":0.04839, "ax":4.95714, "ay":0.29488, "alpha":0.01765, "fx":[82.69944,82.70528,82.56645,82.56106], "fy":[4.89873,4.86419,5.00001,4.89934]}, + {"t":1.34556, "x":5.84505, "y":3.72117, "heading":3.13802, "vx":-0.41957, "vy":-0.01081, "omega":0.04842, "ax":4.95768, "ay":0.28757, "alpha":0.06656, "fx":[82.50115,83.32575,82.37346,82.3683], "fy":[4.74132,4.83594,4.85552,4.7415]}, + {"t":1.34728, "x":5.84434, "y":3.72115, "heading":3.13811, "vx":-0.41102, "vy":-0.01032, "omega":0.04853, "ax":4.95822, "ay":0.28017, "alpha":0.01891, "fx":[82.72207,82.72798,82.57996,82.57458], "fy":[4.65196,4.61649,4.7602,4.65246]}, + {"t":1.34901, "x":5.84364, "y":3.72114, "heading":3.13819, "vx":-0.40247, "vy":-0.00984, "omega":0.04856, "ax":4.95874, "ay":0.27295, "alpha":0.06749, "fx":[82.52318,83.34308,82.38899,82.38384], "fy":[4.49521,4.59512,4.61392,4.49527]}, + {"t":1.35073, "x":5.84295, "y":3.72112, "heading":3.13827, "vx":-0.39392, "vy":-0.00937, "omega":0.04868, "ax":4.95925, "ay":0.26565, "alpha":0.01907, "fx":[82.7412,82.74719,82.59506,82.58968], "fy":[4.41121,4.36926,4.52079,4.41156]}, + {"t":1.35246, "x":5.84228, "y":3.7211, "heading":3.13836, "vx":-0.38537, "vy":-0.00891, "omega":0.04871, "ax":4.95974, "ay":0.25852, "alpha":0.06712, "fx":[82.54158,83.35588,82.40675,82.40161], "fy":[4.25548,4.35435,4.37231,4.25537]}, + {"t":1.35418, "x":5.84162, "y":3.72109, "heading":3.13844, "vx":-0.37682, "vy":-0.00846, "omega":0.04883, "ax":4.96022, "ay":0.25132, "alpha":0.0178, "fx":[82.75609,82.76213,82.61258,82.60721], "fy":[4.17727,4.12162,4.2809,4.17743]}, + {"t":1.3559, "x":5.84098, "y":3.72108, "heading":3.13853, "vx":-0.36827, "vy":-0.00803, "omega":0.04886, "ax":4.96068, "ay":0.24428, "alpha":0.06442, "fx":[82.55882,83.35351,82.43086,82.42574], "fy":[4.02298,4.11266,4.12974,4.02266]}, + {"t":1.35763, "x":5.84035, "y":3.72106, "heading":3.13861, "vx":-0.35971, "vy":-0.00761, "omega":0.04897, "ax":4.96114, "ay":0.23717, "alpha":0.01469, "fx":[82.76579,82.77187,82.63352,82.62817], "fy":[3.95111,3.87247,4.03946,3.95102]}, + {"t":1.35935, "x":5.83974, "y":3.72105, "heading":3.1387, "vx":-0.35116, "vy":-0.0072, "omega":0.049, "ax":4.96158, "ay":0.23022, "alpha":0.06038, "fx":[82.56667,83.35625,82.45536,82.45026], "fy":[3.79886,3.86876,3.88498,3.79828]}, + {"t":1.36108, "x":5.83914, "y":3.72104, "heading":3.13878, "vx":-0.34261, "vy":-0.0068, "omega":0.0491, "ax":4.96201, "ay":0.22321, "alpha":0.00927, "fx":[82.76917,82.77527,82.65907,82.65374], "fy":[3.73389,3.62054,3.79521,3.7335]}, + {"t":1.3628, "x":5.83856, "y":3.72103, "heading":3.13886, "vx":-0.33405, "vy":-0.00642, "omega":0.04912, "ax":4.96242, "ay":0.21635, "alpha":0.0542, "fx":[82.56597,83.35824,82.48283,82.47776], "fy":[3.58434,3.62126,3.63676,3.58349]}, + {"t":1.36452, "x":5.83799, "y":3.72102, "heading":3.13895, "vx":-0.32549, "vy":-0.00604, "omega":0.04921, "ax":4.96283, "ay":0.20943, "alpha":0.00097, "fx":[82.76495,82.77105,82.69057,82.68527], "fy":[3.52689,3.36443,3.54676,3.52615]}, + {"t":1.36625, "x":5.83744, "y":3.721, "heading":3.13903, "vx":-0.31694, "vy":-0.00568, "omega":0.04921, "ax":4.96322, "ay":0.20266, "alpha":0.04439, "fx":[82.5579,83.34643,82.51924,82.51422], "fy":[3.38084,3.36885,3.38348,3.37955]}, + {"t":1.36797, "x":5.8369, "y":3.721, "heading":3.13912, "vx":-0.30838, "vy":-0.00533, "omega":0.04929, "ax":4.9636, "ay":0.19582, "alpha":-0.01086, "fx":[82.7516,82.75768,82.72961,82.72436], "fy":[3.33169,3.10247,3.29244,3.33054]}, + {"t":1.3697, "x":5.83637, "y":3.72099, "heading":3.1392, "vx":-0.29982, "vy":-0.00499, "omega":0.04927, "ax":4.96397, "ay":0.18914, "alpha":0.03036, "fx":[82.54216,83.31912,82.56567,82.56071], "fy":[3.19,3.10965,3.12334,3.18827]}, + {"t":1.37142, "x":5.83586, "y":3.72098, "heading":3.13929, "vx":-0.29126, "vy":-0.00467, "omega":0.04932, "ax":4.96432, "ay":0.18239, "alpha":-0.02699, "fx":[82.72726,82.73329,82.7781,82.77291], "fy":[3.1502,2.83261,3.03026,3.14857]}, + {"t":1.37315, "x":5.83537, "y":3.72097, "heading":3.13937, "vx":-0.2827, "vy":-0.00435, "omega":0.04928, "ax":4.96467, "ay":0.17579, "alpha":0.01191, "fx":[82.51277,83.28478,82.62091,82.61603], "fy":[3.01402,2.84134,2.85416,3.01179]}, + {"t":1.37487, "x":5.83489, "y":3.72096, "heading":3.13946, "vx":-0.27414, "vy":-0.00405, "omega":0.0493, "ax":4.965, "ay":0.16913, "alpha":-0.04836, "fx":[82.68976,82.69572,82.83826,82.83315], "fy":[2.98466,2.55252,2.75789,2.98249]}, + {"t":1.37659, "x":5.83442, "y":3.72096, "heading":3.13954, "vx":-0.26558, "vy":-0.00376, "omega":0.04921, "ax":4.96533, "ay":0.16261, "alpha":-0.01247, "fx":[82.47005,83.23322,82.68995,82.68516], "fy":[2.85527,2.56154,2.57339,2.85246]}, + {"t":1.37832, "x":5.83397, "y":3.72095, "heading":3.13963, "vx":-0.25702, "vy":-0.00348, "omega":0.04919, "ax":4.96564, "ay":0.15604, "alpha":-0.07604, "fx":[82.63656,82.64244,82.91268,82.90765], "fy":[2.83769,2.25944,2.47262,2.83491]}, + {"t":1.38004, "x":5.83354, "y":3.72094, "heading":3.13971, "vx":-0.24846, "vy":-0.00321, "omega":0.04906, "ax":4.96594, "ay":0.1496, "alpha":-0.04369, "fx":[82.40986,83.16587,82.7742,82.7695], "fy":[2.71666,2.26718,2.27807,2.71319]}, + {"t":1.38177, "x":5.83311, "y":3.72094, "heading":3.1398, "vx":-0.23989, "vy":-0.00295, "omega":0.04898, "ax":4.96623, "ay":0.14312, "alpha":-0.1113, "fx":[82.56468,82.57046,83.0044,82.99945], "fy":[2.71245,1.95011,2.17124,2.70896]}, + {"t":1.38349, "x":5.83271, "y":3.72093, "heading":3.13988, "vx":-0.23133, "vy":-0.00271, "omega":0.04879, "ax":4.96652, "ay":0.13675, "alpha":-0.08323, "fx":[82.32959,83.07767,82.87755,82.87295], "fy":[2.60162,1.95476,1.96469,2.5974]}, + {"t":1.38522, "x":5.83232, "y":3.72093, "heading":3.13997, "vx":-0.22277, "vy":-0.00247, "omega":0.04865, "ax":4.96679, "ay":0.13035, "alpha":-0.15564, "fx":[82.47058,82.47626,83.117,83.11213], "fy":[2.61267,1.6207,1.84995,2.60836]}, + {"t":1.38694, "x":5.83194, "y":3.72093, "heading":3.14005, "vx":-0.2142, "vy":-0.00224, "omega":0.04838, "ax":4.96705, "ay":0.12407, "alpha":-0.13269, "fx":[82.22515,82.96555,83.00363,82.9991], "fy":[2.51426,1.62008,1.62907,2.50916]}, + {"t":1.38866, "x":5.83158, "y":3.72092, "heading":3.14013, "vx":-0.20564, "vy":-0.00203, "omega":0.04815, "ax":4.9673, "ay":0.11775, "alpha":-0.21086, "fx":[82.35006,82.35567,83.25473,83.24989], "fy":[2.54281,1.26659,1.50422,2.53756]}, + {"t":1.39039, "x":5.83123, "y":3.72092, "heading":3.14022, "vx":-0.19708, "vy":-0.00183, "omega":0.04779, "ax":4.96755, "ay":0.11154, "alpha":-0.19613, "fx":[82.10257,82.79375,83.16737,83.16286], "fy":[2.4594,1.25827,1.26627,2.45327]}, + {"t":1.39211, "x":5.8309, "y":3.72092, "heading":3.1403, "vx":-0.18851, "vy":-0.00164, "omega":0.04745, "ax":4.96778, "ay":0.1053, "alpha":-0.27914, "fx":[82.19806,82.20363,83.42269,83.41783], "fy":[2.50842,0.88216,1.1285,2.50203]}, + {"t":1.39384, "x":5.83058, "y":3.72091, "heading":3.14038, "vx":-0.17994, "vy":-0.00145, "omega":0.04697, "ax":4.96801, "ay":0.09917, "alpha":-0.27163, "fx":[81.93428,82.62148,83.353,83.34844], "fy":[2.44349,0.86273,0.86989,2.43612]}, + {"t":1.39556, "x":5.83028, "y":3.72091, "heading":3.14046, "vx":-0.17138, "vy":-0.00128, "omega":0.0465, "ax":4.96822, "ay":0.093, "alpha":-0.36314, "fx":[82.00841,82.01403,83.6271,83.62211], "fy":[2.51626,0.46051,0.71597,2.5085]}, + {"t":1.39728, "x":5.82999, "y":3.72091, "heading":3.14054, "vx":-0.16281, "vy":-0.00112, "omega":0.04587, "ax":4.96843, "ay":0.08695, "alpha":-0.36438, "fx":[81.72497,82.40828,83.57847,83.57374], "fy":[2.47396,0.42601,0.43241,2.46506]}, + {"t":1.39901, "x":5.82972, "y":3.72091, "heading":3.14062, "vx":-0.15425, "vy":-0.00097, "omega":0.04525, "ax":4.96863, "ay":0.08086, "alpha":-0.46614, "fx":[81.77358,81.77941,83.87552,83.87024], "fy":[2.5748,-0.0069,0.2582,2.5653]}, + {"t":1.40073, "x":5.82946, "y":3.72091, "heading":3.1407, "vx":-0.14568, "vy":-0.00083, "omega":0.04444, "ax":4.96882, "ay":0.07488, "alpha":-0.47859, "fx":[81.46874,82.13838,83.85472,83.84958], "fy":[2.56044,-0.06154,-0.05582,2.54957]}, + {"t":1.40246, "x":5.82921, "y":3.7209, "heading":3.14078, "vx":-0.13711, "vy":-0.0007, "omega":0.04362, "ax":4.969, "ay":0.06886, "alpha":-0.59231, "fx":[81.48404,81.49033,84.17753,84.17168], "fy":[2.69494,-0.53107,-0.25569,2.68318]}, + {"t":1.40418, "x":5.82899, "y":3.7209, "heading":3.14085, "vx":-0.12854, "vy":-0.00059, "omega":0.0426, "ax":4.96918, "ay":0.06296, "alpha":-0.61802, "fx":[81.14969,81.81636,84.18748,84.18163], "fy":[2.71562,-0.61261,-0.60741,2.70214]}, + {"t":1.40591, "x":5.82877, "y":3.7209, "heading":3.14092, "vx":-0.11998, "vy":-0.00048, "omega":0.04153, "ax":4.96934, "ay":0.057, "alpha":-0.74712, "fx":[81.12749,81.13461,84.54546,84.53867], "fy":[2.89106,-1.12639,-0.83992,2.87622]}, + {"t":1.40763, "x":5.82857, "y":3.7209, "heading":3.141, "vx":-0.11141, "vy":-0.00038, "omega":0.04024, "ax":4.9695, "ay":0.05118, "alpha":-0.78945, "fx":[80.75612,81.42134,84.59315,84.58611], "fy":[2.95637,-1.24401,-1.23909,2.93926]}, + {"t":1.40935, "x":5.82839, "y":3.7209, "heading":3.14107, "vx":-0.10284, "vy":-0.00029, "omega":0.03888, "ax":4.96965, "ay":0.04529, "alpha":-0.93782, "fx":[80.68764,80.69614,84.99564,84.98733], "fy":[3.18249,-1.8122,-1.51361,3.16331]}, + {"t":1.41108, "x":5.82822, "y":3.7209, "heading":3.14113, "vx":-0.09427, "vy":-0.00021, "omega":0.03726, "ax":4.96979, "ay":0.03955, "alpha":-1.00174, "fx":[80.27104,80.92964,85.09223,85.08331], "fy":[3.30575,-1.97866,-1.97365,3.28339]}, + {"t":1.4128, "x":5.82806, "y":3.7209, "heading":3.1412, "vx":-0.0857, "vy":-0.00014, "omega":0.03554, "ax":4.96993, "ay":0.03372, "alpha":-1.17434, "fx":[80.1424,80.1531,85.5502,85.53954], "fy":[3.59596,-2.61515,-2.30298,3.5704]}, + {"t":1.41453, "x":5.82792, "y":3.7209, "heading":3.14126, "vx":-0.07713, "vy":-0.00009, "omega":0.03351, "ax":4.97005, "ay":0.02805, "alpha":-1.26683, "fx":[79.66924,80.31399,85.71114,85.69933], "fy":[3.79615,-2.84844,-2.84305,3.76584]}, + {"t":1.41625, "x":5.8278, "y":3.7209, "heading":3.14132, "vx":-0.06856, "vy":-0.00004, "omega":0.03133, "ax":4.97018, "ay":0.02228, "alpha":-1.47066, "fx":[79.46091,79.475,86.24001,86.22582], "fy":[4.1695,-3.57303,-3.24506,4.1341]}, + {"t":1.41798, "x":5.82769, "y":3.7209, "heading":3.14137, "vx":-0.05999, "vy":0.0, "omega":0.02879, "ax":4.97029, "ay":0.0167, "alpha":-1.60053, "fx":[78.9092,79.55405,86.48105,86.46495], "fy":[4.47457,-3.90042,-3.89242,4.43168]}, + {"t":1.4197, "x":5.82759, "y":3.7209, "heading":3.14142, "vx":-0.05142, "vy":0.00003, "omega":0.02603, "ax":4.97039, "ay":0.01097, "alpha":-1.84664, "fx":[78.59955,78.61873,87.10872,87.08932], "fy":[4.95769,-4.74006,-4.39251,4.90646]}, + {"t":1.42142, "x":5.82751, "y":3.7209, "heading":3.14146, "vx":-0.04285, "vy":0.00005, "omega":0.02285, "ax":4.97049, "ay":0.00548, "alpha":-2.02749, "fx":[77.94269,78.58934,87.45662,87.43424], "fy":[5.40847,-5.2012,-5.1866,5.3447]}, + {"t":1.42315, "x":5.82744, "y":3.7209, "heading":3.1415, "vx":-0.03428, "vy":0.00006, "omega":0.01935, "ax":4.97058, "ay":-0.00021, "alpha":-2.33089, "fx":[77.4957,77.5224,88.21894,88.192], "fy":[6.04,-6.19505,-5.82074,5.96205]}, + {"t":1.42487, "x":5.82739, "y":3.7209, "heading":3.14154, "vx":-0.02571, "vy":0.00006, "omega":0.01533, "ax":4.97067, "ay":-0.00561, "alpha":-2.58036, "fx":[76.68374,77.38028,88.70106,88.66963], "fy":[6.69772,-6.84949,-6.81948,6.59751]}, + {"t":1.4266, "x":5.82735, "y":3.7209, "heading":3.14156, "vx":-0.01714, "vy":0.00005, "omega":0.01088, "ax":4.97075, "ay":-0.01126, "alpha":-2.96522, "fx":[76.05823,76.09585,89.66179,89.62408], "fy":[7.53422,-8.05474,-7.63904,7.40891]}, + {"t":1.42832, "x":5.82733, "y":3.7209, "heading":3.14158, "vx":-0.00857, "vy":0.00003, "omega":0.00577, "ax":4.97082, "ay":-0.01652, "alpha":-3.34711, "fx":[75.05867,75.10131,90.35157,90.93328], "fy":[8.19672,-8.82727,-8.76555,8.29432]}, + {"t":1.43005, "x":5.82732, "y":3.7209, "heading":3.14159, "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/deepBargeToE.traj b/src/main/deploy/choreo/deepBargeToE.traj new file mode 100644 index 0000000..f1f9d55 --- /dev/null +++ b/src/main/deploy/choreo/deepBargeToE.traj @@ -0,0 +1,74 @@ +{ + "name":"deepBargeToE", + "version":1, + "snapshot":{ + "waypoints":[ + {"x":7.7, "y":7.092, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.422460748154253, "y":5.032141990263579, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":10.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"bargeDeep.x", "val":7.7}, "y":{"exp":"bargeDeep.y", "val":7.092}, "heading":{"exp":"bargeDeep.heading", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"I.x", "val":5.422460748154253}, "y":{"exp":"I.y", "val":5.032141990263579}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true}, + {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"10 m / s ^ 2", "val":10.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":"Swerve", + "waypoints":[0.0,1.21908], + "samples":[ + {"t":0.0, "x":7.7, "y":7.092, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.92358, "ay":-5.01833, "alpha":-25.51924, "fx":[-2.81779,-184.47719,-199.16664,-141.86725], "fy":[-199.14368,4.39322,0.01542,-139.87727]}, + {"t":0.03483, "x":7.69519, "y":7.08896, "heading":0.0, "vx":-0.27599, "vy":-0.17479, "omega":-0.88886, "ax":-7.56482, "ay":-6.43547, "alpha":-21.87993, "fx":[-2.85819,-161.16128,-199.10488,-141.28324], "fy":[-199.08925,-87.98946,-1.61385,-140.41239]}, + {"t":0.06966, "x":7.68099, "y":7.07896, "heading":-0.03096, "vx":-0.53947, "vy":-0.39895, "omega":-1.65096, "ax":-6.56358, "ay":-7.49306, "alpha":-21.77628, "fx":[-8.322,-86.08917,-199.01352,-144.222], "fy":[-198.84899,-164.88925,1.41569,-137.30003]}, + {"t":0.10449, "x":7.65822, "y":7.06052, "heading":-0.08846, "vx":-0.76809, "vy":-0.65994, "omega":-2.40944, "ax":-6.3062, "ay":-7.72156, "alpha":-22.23992, "fx":[-20.51849,-52.72119,-198.83687,-148.40896], "fy":[-197.81139,-185.67973,1.22134,-132.58884]}, + {"t":0.13932, "x":7.62764, "y":7.03285, "heading":-0.17239, "vx":-0.98774, "vy":-0.92888, "omega":-3.18408, "ax":-6.40238, "ay":-7.65209, "alpha":-22.53822, "fx":[-35.14395,-40.14054,-198.30655,-153.30762], "fy":[-195.30897,-188.92036,0.36255,-126.36003]}, + {"t":0.17415, "x":7.58935, "y":6.99586, "heading":-0.28329, "vx":-1.21074, "vy":-1.19541, "omega":-3.9691, "ax":-6.62639, "ay":-7.4675, "alpha":-5.91184, "fx":[-88.6434,-97.2752,-134.02985,-121.88646], "fy":[-146.06861,-129.19469,-99.41897,-123.23657]}, + {"t":0.20899, "x":7.54316, "y":6.94969, "heading":-0.42154, "vx":-1.44154, "vy":-1.45551, "omega":-4.17502, "ax":-6.78948, "ay":-7.31123, "alpha":18.46137, "fx":[-181.94481,-153.52507,-66.76909,-50.47065], "fy":[-31.49769,-112.7858,-177.0498,-166.1655]}, + {"t":0.24382, "x":7.48883, "y":6.89456, "heading":-0.56696, "vx":-1.67803, "vy":-1.71017, "omega":-3.53199, "ax":-7.20535, "ay":-6.88787, "alpha":19.64709, "fx":[-185.01525,-164.13648,-82.41421,-48.87312], "fy":[-16.66583,-98.8397,-172.04903,-171.71537]}, + {"t":0.27865, "x":7.42602, "y":6.83081, "heading":-0.68998, "vx":-1.929, "vy":-1.95008, "omega":-2.84767, "ax":-7.81554, "ay":-6.11908, "alpha":22.41507, "fx":[-190.83882,-178.64658,-99.97831,-51.66124], "fy":[17.18313,-78.44613,-167.02739,-179.7183]}, + {"t":0.31348, "x":7.35409, "y":6.75918, "heading":-0.78917, "vx":-2.20122, "vy":-2.16321, "omega":-2.06693, "ax":-7.97752, "ay":-4.74944, "alpha":26.80128, "fx":[-179.88677,-189.48288,-115.63813,-46.91772], "fy":[69.1749,-48.89924,-157.22409,-179.73534]}, + {"t":0.34831, "x":7.27258, "y":6.68095, "heading":-0.86116, "vx":-2.47908, "vy":-2.32864, "omega":-1.13342, "ax":-7.13851, "ay":-5.25482, "alpha":26.30491, "fx":[-175.59973,-180.76115,-103.42389,-16.1972], "fy":[36.89183,-58.88533,-159.05614,-169.33162]}, + {"t":0.38314, "x":7.1819, "y":6.59666, "heading":-0.90064, "vx":-2.72772, "vy":-2.51167, "omega":-0.2172, "ax":-1.33857, "ay":-1.17954, "alpha":5.40644, "fx":[-25.1708,-36.78325,-24.21448,-3.08446], "fy":[1.9355,-16.33803,-38.34543,-25.90128]}, + {"t":0.41797, "x":7.08608, "y":6.50846, "heading":-0.9082, "vx":-2.77435, "vy":-2.55276, "omega":-0.02888, "ax":-0.45103, "ay":0.49556, "alpha":-0.00698, "fx":[-8.19075,-7.25394,-7.13455,-7.49438], "fy":[8.37418,8.38353,8.12819,8.15684]}, + {"t":0.4528, "x":6.98917, "y":6.41984, "heading":-0.90921, "vx":-2.79006, "vy":-2.53549, "omega":-0.02913, "ax":0.06495, "ay":-0.07213, "alpha":0.00224, "fx":[1.18034,-0.13353,1.81869,1.46493], "fy":[-1.89209,-1.57227,-0.49383,-0.85132]}, + {"t":0.48763, "x":6.89203, "y":6.33148, "heading":-0.91022, "vx":-2.78779, "vy":-2.53801, "omega":-0.02905, "ax":-0.06315, "ay":0.07004, "alpha":-0.00126, "fx":[-1.24173,-0.79325,-0.86547,-1.31019], "fy":[1.83015,0.19711,1.11677,1.52637]}, + {"t":0.52246, "x":6.79489, "y":6.24313, "heading":-0.91124, "vx":-2.78999, "vy":-2.53557, "omega":-0.02909, "ax":0.01232, "ay":-0.01454, "alpha":0.00247, "fx":[0.38289,-0.54984,0.48808,0.50049], "fy":[-0.72202,-0.41186,0.23781,-0.07324]}, + {"t":0.55729, "x":6.69772, "y":6.1548, "heading":-0.91225, "vx":-2.78956, "vy":-2.53607, "omega":-0.02901, "ax":0.00635, "ay":-0.00605, "alpha":-0.00154, "fx":[0.05471,0.27122,0.15715,-0.05953], "fy":[0.12888,-0.24386,-0.21531,-0.07306]}, + {"t":0.59213, "x":6.60056, "y":6.06646, "heading":-0.91326, "vx":-2.78934, "vy":-2.53628, "omega":-0.02906, "ax":0.00856, "ay":-0.01119, "alpha":0.00412, "fx":[0.19318,-0.01167,0.22876,0.1602], "fy":[-0.23951,-0.20284,-0.13363,-0.17032]}, + {"t":0.62696, "x":6.50341, "y":5.97812, "heading":-0.91427, "vx":-2.78905, "vy":-2.53667, "omega":-0.02892, "ax":0.0569, "ay":-0.06334, "alpha":0.00241, "fx":[0.9351,0.86348,0.96172,1.03358], "fy":[-1.3027,-0.67042,-1.10846,-1.1421]}, + {"t":0.66179, "x":6.4063, "y":5.88972, "heading":-0.91528, "vx":-2.78706, "vy":-2.53888, "omega":-0.02883, "ax":-0.00487, "ay":0.00889, "alpha":-0.007, "fx":[-0.35293,0.4989,-0.1344,-0.33603], "fy":[0.52792,0.17258,-0.23204,0.12431]}, + {"t":0.69662, "x":6.30922, "y":5.8013, "heading":-0.91628, "vx":-2.78723, "vy":-2.53857, "omega":-0.02908, "ax":-0.04118, "ay":0.03579, "alpha":0.01969, "fx":[-0.92436,-0.90257,-0.44799,-0.47079], "fy":[0.10866,1.39175,0.31176,0.57455]}, + {"t":0.73145, "x":6.21212, "y":5.7129, "heading":-0.9173, "vx":-2.78867, "vy":-2.53732, "omega":-0.02839, "ax":-0.54675, "ay":0.61851, "alpha":-0.02943, "fx":[-10.26835,-7.85707,-9.06193,-9.26874], "fy":[10.95211,9.78516,9.64716,10.85665]}, + {"t":0.76628, "x":6.11465, "y":5.6249, "heading":-0.91828, "vx":-2.80771, "vy":-2.51578, "omega":-0.02942, "ax":-0.9813, "ay":1.29004, "alpha":-0.37269, "fx":[-18.90541,-15.1744,-13.70401,-17.64749], "fy":[20.64573,18.8577,21.88575,24.62803]}, + {"t":0.80111, "x":6.01626, "y":5.53805, "heading":-0.91931, "vx":-2.84189, "vy":-2.47085, "omega":-0.0424, "ax":2.76878, "ay":6.71121, "alpha":-19.75716, "fx":[40.39127,121.13249,51.83289,-28.73985], "fy":[45.13422,97.75426,162.35888,142.24352]}, + {"t":0.83594, "x":5.91896, "y":5.45606, "heading":-0.92079, "vx":-2.74545, "vy":-2.23709, "omega":-0.73056, "ax":5.98065, "ay":6.90801, "alpha":-25.69105, "fx":[170.95689,173.12117,81.84599,-27.14571], "fy":[19.1198,82.61132,174.28594,184.59562]}, + {"t":0.87077, "x":5.82696, "y":5.38233, "heading":-0.94623, "vx":-2.53714, "vy":-1.99648, "omega":-1.6254, "ax":7.34682, "ay":6.3842, "alpha":-24.18211, "fx":[188.82757,181.29619,103.92894,15.81904], "fy":[-1.38349,71.68157,165.16834,190.21962]}, + {"t":0.9056, "x":5.74304, "y":5.31666, "heading":-1.00285, "vx":-2.28124, "vy":-1.77411, "omega":-2.46768, "ax":7.96472, "ay":5.94874, "alpha":-21.38901, "fx":[185.28581,183.39924,118.23906,44.14802], "fy":[2.29909,59.90193,152.69946,181.75017]}, + {"t":0.94043, "x":5.66842, "y":5.25848, "heading":-1.0888, "vx":-2.00383, "vy":-1.56691, "omega":-3.21268, "ax":8.31145, "ay":5.49899, "alpha":-19.26284, "fx":[179.5081,183.74968,129.03601,61.89769], "fy":[6.5644,48.41591,139.98071,171.70132]}, + {"t":0.97526, "x":5.60366, "y":5.20724, "heading":-1.2007, "vx":-1.71433, "vy":-1.37538, "omega":-3.88362, "ax":8.53407, "ay":5.16378, "alpha":-18.67276, "fx":[177.06905,185.65669,137.51627,68.79319], "fy":[7.58833,36.75193,130.99285,168.97802]}, + {"t":1.0101, "x":5.54913, "y":5.16247, "heading":-1.33597, "vx":-1.41708, "vy":-1.19552, "omega":-4.53401, "ax":8.64638, "ay":4.98307, "alpha":-18.81115, "fx":[174.51507,188.17085,143.10026,70.7376], "fy":[10.01707,24.97524,126.35234,170.91673]}, + {"t":1.04493, "x":5.50501, "y":5.12385, "heading":-1.49389, "vx":-1.11592, "vy":-1.02195, "omega":-5.18922, "ax":8.59518, "ay":5.05184, "alpha":23.07942, "fx":[146.58574,33.77809,194.63796,198.10815], "fy":[134.04745,195.31007,-3.89776,11.38707]}, + {"t":1.07976, "x":5.47136, "y":5.09132, "heading":-1.67464, "vx":-0.81654, "vy":-0.84599, "omega":-4.38534, "ax":8.43236, "ay":5.29077, "alpha":21.87331, "fx":[156.18842,28.31643,179.72016,198.02839], "fy":[123.33385,196.82727,50.8605,-18.24323]}, + {"t":1.11459, "x":5.44803, "y":5.06506, "heading":-1.82738, "vx":-0.52284, "vy":-0.66171, "omega":-3.62348, "ax":5.92091, "ay":7.08136, "alpha":27.54934, "fx":[164.83572,44.53603,-11.4016,196.82478], "fy":[111.74537,194.01876,195.89283,-29.4857]}, + {"t":1.14942, "x":5.43341, "y":5.04631, "heading":-1.95359, "vx":-0.31661, "vy":-0.41506, "omega":-2.66391, "ax":4.60548, "ay":6.19964, "alpha":36.90159, "fx":[171.10899,48.34177,-105.17446,192.80842], "fy":[101.97834,193.19056,167.75984,-49.54843]}, + {"t":1.18425, "x":5.42518, "y":5.03561, "heading":-2.04638, "vx":-0.15619, "vy":-0.19912, "omega":-1.37859, "ax":4.48432, "ay":5.7168, "alpha":39.57963, "fx":[175.64958,53.2177,-118.32491,188.46338], "fy":[94.00814,191.95401,159.45264,-64.22969]}, + {"t":1.21908, "x":5.42246, "y":5.03214, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} diff --git a/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java b/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java new file mode 100644 index 0000000..99f6438 --- /dev/null +++ b/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java @@ -0,0 +1,148 @@ +package frc.robot.commands.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.drive.DriveAutonCommand; +import frc.robot.commands.drive.PrepOdomForAutoCommand; +import frc.robot.commands.elevator.ZeroElevatorCommand; +import frc.robot.commands.robotState.AutoScoreAlgaeCommand; +import frc.robot.commands.robotState.ForceBargeCommand; +import frc.robot.commands.robotState.SetAutoPlacingCommand; +import frc.robot.subsystems.algae.AlgaeSubsystem; +import frc.robot.subsystems.biscuit.BiscuitSubsystem; +import frc.robot.subsystems.coral.CoralSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide; +import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel; +import frc.robot.subsystems.tagAlign.TagAlignSubsystem; +import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.ArrayList; +import java.util.List; + +public class IRIAutonCommand extends SequentialCommandGroup implements AutoCommandInterface { + + private DriveSubsystem driveSubsystem; + private CoralSubsystem coralSubsystem; + private RobotStateSubsystem robotStateSubsystem; + private VisionSubsystem visionSubsystem; + + private ArrayList pathCommands = new ArrayList<>(); + + public IRIAutonCommand( + DriveSubsystem driveSubsystem, + RobotStateSubsystem robotStateSubsystem, + AlgaeSubsystem algaeSubsystem, + BiscuitSubsystem biscuitSubsystem, + CoralSubsystem coralSubsystem, + ElevatorSubsystem elevatorSubsystem, + TagAlignSubsystem tagAlignSubsystem, + VisionSubsystem visionSubsystem, + List grabPaths, + List grabYOffsets, + List delays, + List bargePaths, + List algaeLevels, + Pose2d startPose) { + addRequirements( + driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem); + this.driveSubsystem = driveSubsystem; + this.coralSubsystem = coralSubsystem; + this.robotStateSubsystem = robotStateSubsystem; + this.visionSubsystem = visionSubsystem; + + // BooleanSupplier awayCondition = + // () -> DriverStation.getMatchTime() < AutonConstants.kBargeScoreMinTime; + + addCommands( + new ParallelCommandGroup( + new PrepOdomForAutoCommand( + robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(90.0), startPose), + new ZeroElevatorCommand(elevatorSubsystem)), + new SetAutoPlacingCommand(robotStateSubsystem, true), + new ForceBargeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem), + new SetAutoPlacingCommand(robotStateSubsystem, false)); + + for (int i = 0; i < grabPaths.size(); i++) { + boolean last = i == grabPaths.size() - 1; + var algaeDrive = + new DriveAlgaeAutonServoCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + grabPaths.get(i), + i == 0, + true, + i == 0, + grabYOffsets.get(i), + algaeLevels.get(i)); + var bargeDrive = + last + ? new DriveAutonCommand(driveSubsystem, bargePaths.get(i), true, false, false) + : new DriveBargeAutonCommand( + driveSubsystem, + tagAlignSubsystem, + elevatorSubsystem, + biscuitSubsystem, + robotStateSubsystem, + visionSubsystem, + bargePaths.get(i), + true, + false); + + pathCommands.add(algaeDrive); + pathCommands.add(bargeDrive); + + addCommands(algaeDrive, new WaitCommand(delays.get(i)), bargeDrive); + + if (!last) { + addCommands( + new AutoScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); + } + } + /* + addCommands( + new ConditionalCommand( + new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false), + new SequentialCommandGroup( + algaeDrive, + new WaitCommand(delays.get(i))), + awayCondition), + new ConditionalCommand( + new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false), + new SequentialCommandGroup( + bargeDrive, + new AutoScoreAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)) + .until(awayCondition), + awayCondition)); + } + */ + + // addCommands(new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false)); + } + + @Override + public void reassignAlliance() { + driveSubsystem.teleResetGyro(); + coralSubsystem.setAutoPreload(); + robotStateSubsystem.setIsAutoPlacing(true); + robotStateSubsystem.setScoringLevel(ScoringLevel.L4); + robotStateSubsystem.setGetAlgaeOnCycle(true); + robotStateSubsystem.setScoreSide(ScoreSide.LEFT); + visionSubsystem.setVisionUpdating(true); + + for (var command : pathCommands) { + command.reassignAlliance(); + } + } +} diff --git a/src/main/java/frc/robot/commands/robotState/SetAutoPlacingCommand.java b/src/main/java/frc/robot/commands/robotState/SetAutoPlacingCommand.java new file mode 100644 index 0000000..ff9eda5 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/SetAutoPlacingCommand.java @@ -0,0 +1,19 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.robotState.RobotStateSubsystem; + +public class SetAutoPlacingCommand extends InstantCommand { + private RobotStateSubsystem robotStateSubsystem; + private boolean isAutoPlacing; + + public SetAutoPlacingCommand(RobotStateSubsystem robotState, boolean isAutoPlacing) { + this.robotStateSubsystem = robotState; + this.isAutoPlacing = isAutoPlacing; + } + + @Override + public void initialize() { + robotStateSubsystem.setIsAutoPlacing(isAutoPlacing); + } +} diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index 56b7226..609760c 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -30,4 +30,6 @@ public class AutonConstants { public static final Pose2d kMiddleBargeStart = new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180)); + + public static final Pose2d kDeepBarge = new Pose2d(7.7, 7.092, Rotation2d.fromDegrees(0)); } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index 78daca4..1d5701f 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.commands.auton.AutoCommandInterface; import frc.robot.commands.auton.DefaultAutonCommand; +import frc.robot.commands.auton.IRIAutonCommand; import frc.robot.commands.auton.MiddleBargeAutonCommand; import frc.robot.commands.auton.NonProcessorShallowAutonCommand; import frc.robot.commands.auton.NonProcessorShallowSlowAutonCommand; @@ -288,6 +289,24 @@ private AutoCommandInterface getAutoCommand(int switchPos) { AutonConstants.kNonProcessorMid); } + case 0x03 -> { + return new IRIAutonCommand( + driveSubsystem, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + new ArrayList(Arrays.asList("deepBargeToE", "bargeToG", null)), + new ArrayList(Arrays.asList(0.0, 0.0, 0.0)), + new ArrayList(Arrays.asList(0.0, 0.0, 0.0)), + new ArrayList(Arrays.asList("EToBarge", "GToBarge", "bargeAway")), + new ArrayList<>(Arrays.asList(ScoringLevel.L3, ScoringLevel.L2, ScoringLevel.L3)), + AutonConstants.kMiddleBargeStart); + } + // case 0x03 -> { // return new NonProcessorShallowSlowAutonCommand( // driveSubsystem, From 09d6362d6a227de3a240113172a3f730c18820fa Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Tue, 8 Jul 2025 10:48:08 -0400 Subject: [PATCH 3/4] use new start pos, add grab algae step --- src/main/java/frc/robot/commands/auton/IRIAutonCommand.java | 2 ++ src/main/java/frc/robot/constants/AutonConstants.java | 2 +- src/main/java/frc/robot/subsystems/auto/AutoSwitch.java | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java b/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java index 99f6438..1d4ac1c 100644 --- a/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java @@ -10,6 +10,7 @@ import frc.robot.commands.elevator.ZeroElevatorCommand; import frc.robot.commands.robotState.AutoScoreAlgaeCommand; import frc.robot.commands.robotState.ForceBargeCommand; +import frc.robot.commands.robotState.ForceLowFloorAlgaeCommand; import frc.robot.commands.robotState.SetAutoPlacingCommand; import frc.robot.subsystems.algae.AlgaeSubsystem; import frc.robot.subsystems.biscuit.BiscuitSubsystem; @@ -63,6 +64,7 @@ public IRIAutonCommand( new PrepOdomForAutoCommand( robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(90.0), startPose), new ZeroElevatorCommand(elevatorSubsystem)), + new ForceLowFloorAlgaeCommand(robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem), new SetAutoPlacingCommand(robotStateSubsystem, true), new ForceBargeCommand( robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem), diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java index 609760c..d562439 100644 --- a/src/main/java/frc/robot/constants/AutonConstants.java +++ b/src/main/java/frc/robot/constants/AutonConstants.java @@ -31,5 +31,5 @@ public class AutonConstants { public static final Pose2d kMiddleBargeStart = new Pose2d(7.1, 7.257, Rotation2d.fromDegrees(180)); - public static final Pose2d kDeepBarge = new Pose2d(7.7, 7.092, Rotation2d.fromDegrees(0)); + public static final Pose2d kDeepBarge = new Pose2d(7.7, 7.092, Rotation2d.fromDegrees(90)); } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index 1d5701f..aa8f195 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -304,7 +304,7 @@ private AutoCommandInterface getAutoCommand(int switchPos) { new ArrayList(Arrays.asList(0.0, 0.0, 0.0)), new ArrayList(Arrays.asList("EToBarge", "GToBarge", "bargeAway")), new ArrayList<>(Arrays.asList(ScoringLevel.L3, ScoringLevel.L2, ScoringLevel.L3)), - AutonConstants.kMiddleBargeStart); + AutonConstants.kDeepBarge); } // case 0x03 -> { From e08ffbf222185fc4b639dfb78759a0b82b85d678 Mon Sep 17 00:00:00 2001 From: mwitcpalek Date: Tue, 8 Jul 2025 18:44:08 -0400 Subject: [PATCH 4/4] update processor sequence - drive practice --- src/main/java/frc/robot/commands/auton/IRIAutonCommand.java | 3 ++- src/main/java/frc/robot/constants/AlgaeConstants.java | 2 +- .../frc/robot/subsystems/robotState/RobotStateSubsystem.java | 1 + 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java b/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java index 1d4ac1c..00c8437 100644 --- a/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java +++ b/src/main/java/frc/robot/commands/auton/IRIAutonCommand.java @@ -64,7 +64,8 @@ public IRIAutonCommand( new PrepOdomForAutoCommand( robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(90.0), startPose), new ZeroElevatorCommand(elevatorSubsystem)), - new ForceLowFloorAlgaeCommand(robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem), + new ForceLowFloorAlgaeCommand( + robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem), new SetAutoPlacingCommand(robotStateSubsystem, true), new ForceBargeCommand( robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem), diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java index fba8521..f3cf1cd 100644 --- a/src/main/java/frc/robot/constants/AlgaeConstants.java +++ b/src/main/java/frc/robot/constants/AlgaeConstants.java @@ -29,7 +29,7 @@ public class AlgaeConstants { public static final double kHoldSpeed = 1; public static final double kCoralHoldSpeed = -0.05; public static final double kBargeScoreSpeed = -1; - public static final double kProcessorScoreSpeed = -1; + public static final double kProcessorScoreSpeed = -0.5; // was -1.0 public static final double kCoralScoreSpeed = 0.3; // 0.5; was 0.4 public static final double kIntakingSpeed = 1; // 0.75 public static final double kCoralIntakingSpeed = -0.3; // -0.75; diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java index a2c752d..47cbd1a 100644 --- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java @@ -1182,6 +1182,7 @@ public void periodic() { FastMath.pow2(currentPose.getX() - processorReleasePose.getX()) + FastMath.pow2(currentPose.getY() - processorReleasePose.getY())); Logger.recordOutput("RobotState/Processor Release Distance", distanceFromRelease); + algaeSubsystem.holdAlgae(); // stop wheels once algae is out if (distanceFromRelease > RobotStateConstants.kProcessorStowRadius) { isEjectingAlgae = false;