From 0adee431eac02090d9d20184c4f7af71990d9a89 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Mon, 10 Mar 2025 13:05:21 -0400 Subject: [PATCH 01/24] Convert cameras to subsystems, add function to get vision-only pose estimate, use it in pose-seek. --- src/main/java/frc/robot/LEDs/LEDs.java | 16 ++---- src/main/java/frc/robot/Robot.java | 5 +- src/main/java/frc/robot/vision/Camera.java | 60 ++++++++++++++-------- src/main/java/frc/robot/vision/Vision.java | 19 ++++++- 4 files changed, 63 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/LEDs/LEDs.java b/src/main/java/frc/robot/LEDs/LEDs.java index 64a03506..0144380d 100644 --- a/src/main/java/frc/robot/LEDs/LEDs.java +++ b/src/main/java/frc/robot/LEDs/LEDs.java @@ -371,18 +371,6 @@ public Command createStandardDisplayCommand( () -> displayDefaultInfo(isAlgeMode.getAsBoolean(), Optional.ofNullable(gamepiece.get()))); } - /** - * Create a command to display pose seeking information on the LEDs. - * - * @param targetPoseSupplier provides the target pose - * @param currentPoseSupplier provides the current pose - * @return a command to display pose seeking information - */ - public Command createPoseSeekingCommand( - Supplier targetPoseSupplier, Supplier currentPoseSupplier) { - return newCommand(() -> displayPoseSeek(currentPoseSupplier.get(), targetPoseSupplier.get())); - } - /** * Create a pattern to display stacked blocks on the left and right strips. * @@ -446,8 +434,10 @@ public void displayAutoMode( */ auto -> { var hasInitialPose = auto.getInitialPose().isPresent(); - if (hasInitialPose) { + if (hasInitialPose && currentPose != null) { displayPoseSeek(currentPose, auto.getInitialPose().get()); + } else if (currentPose == null) { + fill(Color.kDarkGray, Segments.ALL); } displayAutoSelection( auto.getAllianceColor(), auto.getOptionNumber(), agreement, hasInitialPose); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1401545f..ec3e8bdf 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -137,7 +137,10 @@ public void disabledInit() { leds.replaceDefaultCommandImmediately( leds.createAutoOptionDisplayCommand( autoSelector, - () -> swerve.getPose(), + () -> + vision.getEstimatedGlobalPose().isPresent() + ? vision.getEstimatedGlobalPose().get().estimatedPose.toPose2d() + : null, allianceSelector.getAgreementInAllianceColor()) .ignoringDisable(true)); diff --git a/src/main/java/frc/robot/vision/Camera.java b/src/main/java/frc/robot/vision/Camera.java index 0afd53cf..8d57dac4 100644 --- a/src/main/java/frc/robot/vision/Camera.java +++ b/src/main/java/frc/robot/vision/Camera.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Constants; import java.util.List; import java.util.Optional; @@ -18,7 +19,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonTrackedTarget; -public enum Camera { +public enum Camera implements Subsystem { FrontRight("OV2311_TH_8", new Translation3d(0.248, -0.318, 0.513), new Rotation3d(0.0, 0.0, 0.0)), FrontLeft( "OV2311_TH_5", new Translation3d(0.222, 0.331, 0.513), new Rotation3d(0.0, 0, Math.PI / 2.0)), @@ -32,13 +33,16 @@ public enum Camera { public final String name; public final Transform3d transform; public final PhotonCamera device; - public PhotonPoseEstimator pose; + + private Optional pose; + private final PhotonPoseEstimator poseEstimator; private Matrix curStdDevs; private Camera(String name, Translation3d translation, Rotation3d rotation) { this.name = name; this.transform = new Transform3d(translation, rotation); this.device = new PhotonCamera(name); + this.pose = Optional.empty(); // AprilTagFieldLayout tagLayout; // try { @@ -51,13 +55,30 @@ private Camera(String name, Translation3d translation, Rotation3d rotation) { AprilTagFieldLayout tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); - this.pose = + this.poseEstimator = new PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, transform); + register(); + } + + /** Updates the pose estimate for this camera */ + @Override + public void periodic() { + device + .getAllUnreadResults() + .forEach( + change -> { + pose = poseEstimator.update(change); + updateEstimationStdDevs(pose, change.getTargets()); + }); + } + + @Override + public String getName() { + return "Camera." + toString(); } /** - * The latest estimated robot pose on the field from vision data. This may be empty. This should - * only be called once per loop. + * The latest estimated robot pose on the field from vision data. This may be empty. * *

Also includes updates for the standard deviations, which can (optionally) be retrieved with * {@link getEstimationStdDevs} @@ -66,12 +87,17 @@ private Camera(String name, Translation3d translation, Rotation3d rotation) { * used for estimation. */ public Optional getEstimatedGlobalPose() { - Optional est = Optional.empty(); - for (var change : device.getAllUnreadResults()) { - est = pose.update(change); - updateEstimationStdDevs(est, change.getTargets()); - } - return est; + return pose; + } + + /** + * Returns the latest standard deviations of the estimated pose from {@link + * #getEstimatedGlobalPose()}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; } /** @@ -95,7 +121,7 @@ private void updateEstimationStdDevs( // Precalculation - see how many tags we found, and calculate an average-distance metric for (var tgt : targets) { - var tagPose = pose.getFieldTags().getTagPose(tgt.getFiducialId()); + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); if (tagPose.isEmpty()) continue; numTags++; avgDist += @@ -122,14 +148,4 @@ private void updateEstimationStdDevs( } } } - - /** - * Returns the latest standard deviations of the estimated pose from {@link - * #getEstimatedGlobalPose()}, for use with {@link - * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should - * only be used when there are targets visible. - */ - public Matrix getEstimationStdDevs() { - return curStdDevs; - } } diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 44897e3c..abbd2a74 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -3,12 +3,29 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Arrays; import java.util.List; +import java.util.Optional; import java.util.stream.Collectors; import org.photonvision.EstimatedRobotPose; -public class Vision { +public class Vision extends SubsystemBase { + + /** Choose the pose estimate with the lowest maximum stdev. */ + public Optional getEstimatedGlobalPose() { + return Arrays.stream(Camera.values()) + .sorted( + (lhs, rhs) -> + (int) + Math.signum( + lhs.getEstimationStdDevs().max() - rhs.getEstimationStdDevs().max())) + .map(cam -> cam.getEstimatedGlobalPose()) + .filter(Optional::isPresent) + .map(Optional::get) + .findFirst(); + } + public record EstimatedPoseWithStdevs( EstimatedRobotPose pose, Matrix stdev, String name) {} From f872d1f4e291917e388e020f110042f1442527b9 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Fri, 21 Mar 2025 19:32:44 -0400 Subject: [PATCH 02/24] Switch to field April tags --- src/main/java/frc/robot/Constants.java | 4 ---- src/main/java/frc/robot/vision/Camera.java | 22 ++++++++++------------ 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3b4a4dc4..15b71a88 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,6 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Time; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.util.Color; import frc.game.FeederStation; @@ -39,9 +38,6 @@ public static final class NEOVortexConstants { } public static final class VisionConstants { - public static final String kAprilTagLayoutPath = - Filesystem.getDeployDirectory() + "/" + "stemgym.json"; - // Define the standard deviations for the pose estimator, which determine how fast the pose // estimate converges to the vision measurement. This should depend on the vision measurement // noise and how many or how frequently vision measurements are applied to the pose estimator. diff --git a/src/main/java/frc/robot/vision/Camera.java b/src/main/java/frc/robot/vision/Camera.java index dae4b616..ba51a325 100644 --- a/src/main/java/frc/robot/vision/Camera.java +++ b/src/main/java/frc/robot/vision/Camera.java @@ -10,8 +10,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.robot.Constants; -import frc.robot.Constants.VisionConstants; -import java.io.IOException; import java.util.List; import java.util.Optional; import org.photonvision.EstimatedRobotPose; @@ -42,17 +40,17 @@ private Camera(String name, Translation3d translation, Rotation3d rotation) { this.transform = new Transform3d(translation, rotation); this.device = new PhotonCamera(name); - // TODO: switch back to official field layout - AprilTagFieldLayout tagLayout; - try { - tagLayout = new AprilTagFieldLayout(VisionConstants.kAprilTagLayoutPath); - } catch (IOException e) { - System.err.println("Error loading custom AprilTag layout: " + e.getMessage()); - tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - } + // // TODO: switch back to official field layout + // AprilTagFieldLayout tagLayout; + // try { + // tagLayout = new AprilTagFieldLayout(VisionConstants.kAprilTagLayoutPath); + // } catch (IOException e) { + // System.err.println("Error loading custom AprilTag layout: " + e.getMessage()); + // tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + // } - // AprilTagFieldLayout tagLayout = - // AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); + AprilTagFieldLayout tagLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); this.pose = new PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, transform); From d5500d2489219191ce0047f2bb0c7f51cf9c2ce0 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Fri, 21 Mar 2025 22:02:16 -0400 Subject: [PATCH 03/24] Added Aapril tag json, removed reef offset, updated climber servo constants --- src/main/deploy/2025-reefscape-andymark.json | 404 +++++++++++++++++++ src/main/java/frc/game/Reef.java | 2 +- src/main/java/frc/robot/Constants.java | 4 +- 3 files changed, 407 insertions(+), 3 deletions(-) create mode 100644 src/main/deploy/2025-reefscape-andymark.json diff --git a/src/main/deploy/2025-reefscape-andymark.json b/src/main/deploy/2025-reefscape-andymark.json new file mode 100644 index 00000000..60b60e5e --- /dev/null +++ b/src/main/deploy/2025-reefscape-andymark.json @@ -0,0 +1,404 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 16.687292, + "y": 0.628142, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.4539904997395468, + "X": 0.0, + "Y": 0.0, + "Z": 0.8910065241883678 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.687292, + "y": 7.414259999999999, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.45399049973954675, + "X": -0.0, + "Y": 0.0, + "Z": 0.8910065241883679 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.49096, + "y": 8.031733999999998, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 6.132575999999999, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 1.9098259999999998, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 13.474446, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 13.890498, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 13.474446, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.643358, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.227305999999999, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.643358, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 0.8613139999999999, + "y": 0.628142, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.8910065241883679, + "X": 0.0, + "Y": 0.0, + "Z": 0.45399049973954675 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 0.8613139999999999, + "y": 7.414259999999999, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.8910065241883678, + "X": -0.0, + "Y": 0.0, + "Z": 0.45399049973954686 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 8.272272, + "y": 6.132575999999999, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 8.272272, + "y": 1.9098259999999998, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 6.057646, + "y": 0.010667999999999999, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 3.6576, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 4.740402, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 5.321046, + "y": 4.0208200000000005, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 3.3012379999999997, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + } + ], + "field": { + "length": 17.548, + "width": 8.042 + } +} diff --git a/src/main/java/frc/game/Reef.java b/src/main/java/frc/game/Reef.java index caf253d1..4af2b289 100644 --- a/src/main/java/frc/game/Reef.java +++ b/src/main/java/frc/game/Reef.java @@ -19,7 +19,7 @@ */ public enum Reef { // TODO remove reef ceter offset - Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5 + 4.5), Rotation2d.kZero)), + Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5), Rotation2d.kZero)), Red(new Pose2d(Inches.of(514.125), Inches.of(158.5), Rotation2d.kPi)); /** The radius of the reef hexagon */ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 15b71a88..646ccead 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -224,8 +224,8 @@ public static final class RotationControllerGains { public static final class ClimberConstants { public static final int kClimberPort = 17; public static final int kRatchetServoPort = 1; - public static final double kEngagedPosition = 0 / 1024.0; - public static final double kDisengedPosition = 1024.0 / 1024.0; + public static final double kEngagedPosition = 800 / 1024.0; + public static final double kDisengedPosition = 900 / 1024.0; public static final int kCageSensorPort = 6; From cdb9ab7cedefbe0ea04152c0df4da9254c6c8031 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Fri, 21 Mar 2025 22:15:11 -0400 Subject: [PATCH 04/24] Remove reef offset todo --- src/main/java/frc/game/Reef.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/game/Reef.java b/src/main/java/frc/game/Reef.java index 4af2b289..89be7d06 100644 --- a/src/main/java/frc/game/Reef.java +++ b/src/main/java/frc/game/Reef.java @@ -18,7 +18,6 @@ * to a pose. */ public enum Reef { - // TODO remove reef ceter offset Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5), Rotation2d.kZero)), Red(new Pose2d(Inches.of(514.125), Inches.of(158.5), Rotation2d.kPi)); From 5b25bccac9e33467ef5a942cdf8cd360245db4f8 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sat, 22 Mar 2025 12:09:20 -0400 Subject: [PATCH 05/24] morning tweaks --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 646ccead..71b94890 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -225,7 +225,7 @@ public static final class ClimberConstants { public static final int kClimberPort = 17; public static final int kRatchetServoPort = 1; public static final double kEngagedPosition = 800 / 1024.0; - public static final double kDisengedPosition = 900 / 1024.0; + public static final double kDisengedPosition = 950 / 1024.0; public static final int kCageSensorPort = 6; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 903a0f89..6e5b849f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -210,7 +210,9 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { autoSelector.cancelAuto(); - lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID())); + elevator.coralIntakeCG().schedule(); + lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand()); + // lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID())); leds.replaceDefaultCommandImmediately( leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier)); @@ -343,7 +345,7 @@ public boolean getAsBoolean() { // Force joystick operation of the elevator Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1); Trigger elevatorTriggerLow = operator.axisGreaterThan(Axis.kLeftY.value, -0.9, loop).debounce(0.1); - elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID())); + // elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID())); // Actuate climber winch // Trigger climbTrigger = operator.axisGreaterThan(Axis.kRightY.value, -0.9, loop).debounce(0.1); From fa35e3d82b20056d5bb30ced1b8ad80e5997062d Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sat, 22 Mar 2025 12:33:55 -0400 Subject: [PATCH 06/24] Fix elevator rocket --- src/main/java/frc/robot/elevator/Elevator.java | 1 + src/main/java/frc/robot/elevator/Lifter.java | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/elevator/Elevator.java b/src/main/java/frc/robot/elevator/Elevator.java index db610525..44000b55 100644 --- a/src/main/java/frc/robot/elevator/Elevator.java +++ b/src/main/java/frc/robot/elevator/Elevator.java @@ -49,6 +49,7 @@ public AlgaeWrist getAlgaeWrist() { public Command resetPositionControllers() { return new InstantCommand( () -> { + lifter.matchHeight(); lifter.resetController(); coralWrist.resetController(); algaeWrist.resetController(); diff --git a/src/main/java/frc/robot/elevator/Lifter.java b/src/main/java/frc/robot/elevator/Lifter.java index 8f8cf789..4a064345 100644 --- a/src/main/java/frc/robot/elevator/Lifter.java +++ b/src/main/java/frc/robot/elevator/Lifter.java @@ -181,12 +181,16 @@ public Command createSetHeightCommand(LifterState state) { this); } + public void matchHeight() { + feedback.setGoal(encoder.getPosition()); + } + public Command createRemainAtCurrentHeightCommand() { return new FunctionalCommand( // initialize () -> { if (targetState == LifterState.Initial) { - feedback.setGoal(encoder.getPosition()); + matchHeight(); // Users should call reset() when they first start running the controller to avoid // unwanted behavior. resetController(); From 32127f4c1c5ed09fc3b0c2524c0f7f7221dfd430 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sat, 22 Mar 2025 14:10:38 -0400 Subject: [PATCH 07/24] Move zorro controller command into teleop init. --- src/main/java/frc/robot/Robot.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e5b849f..3447eda8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -125,9 +125,6 @@ public void robotInit() { DataLogManager.start(); DriverStation.startDataLog(DataLogManager.getLog()); - swerve.setDefaultCommand( - new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID())); - reefTargetPositionsPublisher.set(DriveConstants.kReefTargetPoses); } @@ -210,7 +207,9 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { autoSelector.cancelAuto(); - elevator.coralIntakeCG().schedule(); + swerve.setDefaultCommand( + new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID())); + lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand()); // lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID())); leds.replaceDefaultCommandImmediately( From a1f46fda18316454e1f0830d57540ff882bbb7e5 Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sat, 22 Mar 2025 14:13:53 -0400 Subject: [PATCH 08/24] sycronized blue and red auto logic --- src/main/deploy/choreo/redCenterToL4G.traj | 159 ++++++++++---------- src/main/java/frc/robot/auto/RedL4Auto.java | 10 +- 2 files changed, 83 insertions(+), 86 deletions(-) diff --git a/src/main/deploy/choreo/redCenterToL4G.traj b/src/main/deploy/choreo/redCenterToL4G.traj index 5af929e4..09f66473 100644 --- a/src/main/deploy/choreo/redCenterToL4G.traj +++ b/src/main/deploy/choreo/redCenterToL4G.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":10.35, "y":4.2, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":11.279606819152832, "y":4.188701152801514, "heading":0.0, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":11.75, "y":4.2, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":11.279606819152832, "y":4.188701152801514, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.65, "y":4.2, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,8 +18,8 @@ "params":{ "waypoints":[ {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"redStart.y", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.188701152801514 m", "val":4.188701152801514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"redL4G.x", "val":11.75}, "y":{"exp":"redL4G.y", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.188701152801514 m", "val":4.188701152801514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.2 m", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,86 +34,79 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.67494,3.39455], + "waypoints":[0.0,1.67494,3.06116], "samples":[ - {"t":0.0, "x":10.35, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99944, "ay":-0.01292, "alpha":-0.00018, "fx":[11.33025,11.33965,11.3319,11.3319], "fy":[-0.14608,-0.14685,-0.14685,-0.14608]}, - {"t":0.04295, "x":10.35092, "y":4.19999, "heading":0.0, "vx":0.04292, "vy":-0.00055, "omega":-0.00001, "ax":0.99967, "ay":-0.01291, "alpha":-0.00012, "fx":[11.33386,11.33993,11.33514,11.33514], "fy":[-0.14628,-0.14661,-0.14661,-0.14628]}, - {"t":0.08589, "x":10.35369, "y":4.19995, "heading":0.0, "vx":0.08586, "vy":-0.00111, "omega":-0.00001, "ax":0.99965, "ay":-0.01291, "alpha":0.00003, "fx":[11.3359,11.3359,11.33586,11.33586], "fy":[-0.14623,-0.14554,-0.14751,-0.14623]}, - {"t":0.12884, "x":10.3583, "y":4.19989, "heading":0.0, "vx":0.12879, "vy":-0.00166, "omega":-0.00001, "ax":0.99964, "ay":-0.0129, "alpha":-0.00027, "fx":[11.3338,11.34023,11.33443,11.33443], "fy":[-0.14649,-0.14612,-0.14612,-0.14649]}, - {"t":0.17179, "x":10.36475, "y":4.19981, "heading":0.0, "vx":0.17172, "vy":-0.00222, "omega":-0.00002, "ax":0.99962, "ay":-0.01289, "alpha":-0.0001, "fx":[11.33582,11.33582,11.33528,11.33529], "fy":[-0.14646,-0.14507,-0.14691,-0.14646]}, - {"t":0.21474, "x":10.37305, "y":4.1997, "heading":0.0, "vx":0.21465, "vy":-0.00277, "omega":-0.00003, "ax":0.99961, "ay":-0.01289, "alpha":-0.00042, "fx":[11.33364,11.34047,11.33367,11.33367], "fy":[-0.14663,-0.14563,-0.14563,-0.14663]}, - {"t":0.25768, "x":10.38319, "y":4.19957, "heading":0.0, "vx":0.25758, "vy":-0.00333, "omega":-0.00005, "ax":0.99959, "ay":-0.01288, "alpha":-0.00023, "fx":[11.33565,11.33565,11.33465,11.33465], "fy":[-0.14661,-0.14462,-0.1463,-0.14661]}, - {"t":0.30063, "x":10.39517, "y":4.19942, "heading":-0.00001, "vx":0.30051, "vy":-0.00388, "omega":-0.00006, "ax":0.99957, "ay":-0.01287, "alpha":-0.00056, "fx":[11.33335,11.34061,11.33283,11.33283], "fy":[-0.14673,-0.14511,-0.14511,-0.14673]}, - {"t":0.34358, "x":10.409, "y":4.19924, "heading":-0.00001, "vx":0.34344, "vy":-0.00443, "omega":-0.00008, "ax":0.99954, "ay":-0.01286, "alpha":-0.00035, "fx":[11.33537,11.33537,11.33389,11.33389], "fy":[-0.1467,-0.14411,-0.14565,-0.1467]}, - {"t":0.38652, "x":10.42467, "y":4.19904, "heading":-0.00001, "vx":0.38637, "vy":-0.00498, "omega":-0.00009, "ax":0.99952, "ay":-0.01284, "alpha":-0.00071, "fx":[11.33294,11.34059,11.33186,11.33186], "fy":[-0.14678,-0.1445,-0.1445,-0.14678]}, - {"t":0.42947, "x":10.44219, "y":4.19881, "heading":-0.00002, "vx":0.42929, "vy":-0.00553, "omega":-0.00012, "ax":0.99948, "ay":-0.01283, "alpha":-0.00047, "fx":[11.33494,11.33494,11.33295,11.33295], "fy":[-0.14671,-0.14353,-0.14492,-0.14671]}, - {"t":0.47242, "x":10.46154, "y":4.19856, "heading":-0.00002, "vx":0.47222, "vy":-0.00609, "omega":-0.00014, "ax":0.99944, "ay":-0.01281, "alpha":-0.00085, "fx":[11.33237,11.34021,11.33072,11.33073], "fy":[-0.14676,-0.14377,-0.14377,-0.14676]}, - {"t":0.51537, "x":10.48275, "y":4.19829, "heading":-0.00003, "vx":0.51514, "vy":-0.00664, "omega":-0.00018, "ax":0.9994, "ay":-0.01279, "alpha":-0.0006, "fx":[11.33426,11.33426,11.33171,11.33172], "fy":[-0.1466,-0.1428,-0.14407,-0.1466]}, - {"t":0.55831, "x":10.50579, "y":4.19799, "heading":-0.00003, "vx":0.55806, "vy":-0.00718, "omega":-0.00021, "ax":0.99934, "ay":-0.01276, "alpha":-0.00095, "fx":[11.33175,11.33859,11.32954,11.32954], "fy":[-0.1466,-0.14284,-0.14284,-0.1466]}, - {"t":0.60126, "x":10.53068, "y":4.19767, "heading":-0.00004, "vx":0.60098, "vy":-0.00773, "omega":-0.00025, "ax":0.99927, "ay":-0.01273, "alpha":-0.00073, "fx":[11.33314,11.33314,11.33001,11.33001], "fy":[-0.14626,-0.14187,-0.14302,-0.14626]}, - {"t":0.64421, "x":10.55741, "y":4.19733, "heading":-0.00005, "vx":0.6439, "vy":-0.00828, "omega":-0.00028, "ax":0.99919, "ay":-0.01269, "alpha":-0.00102, "fx":[11.33061,11.3359,11.32789,11.32789], "fy":[-0.14615,-0.14161,-0.14161,-0.14615]}, - {"t":0.68715, "x":10.58599, "y":4.19696, "heading":-0.00007, "vx":0.68681, "vy":-0.00882, "omega":-0.00032, "ax":0.99907, "ay":-0.01263, "alpha":-0.00084, "fx":[11.33109,11.33109,11.32742,11.32742], "fy":[-0.14543,-0.14055,-0.14163,-0.14543]}, - {"t":0.7301, "x":10.61641, "y":4.19657, "heading":-0.00008, "vx":0.72972, "vy":-0.00937, "omega":-0.00036, "ax":0.99891, "ay":-0.01256, "alpha":-0.00075, "fx":[11.32976,11.32669,11.32665,11.32665], "fy":[-0.14499,-0.13982,-0.13982,-0.145]}, - {"t":0.77305, "x":10.64867, "y":4.19615, "heading":-0.0001, "vx":0.77262, "vy":-0.00991, "omega":-0.00039, "ax":0.99867, "ay":-0.01245, "alpha":-0.0009, "fx":[11.32675,11.32675,11.3228,11.3228], "fy":[-0.14347,-0.13817,-0.1395,-0.14347]}, - {"t":0.816, "x":10.68277, "y":4.19572, "heading":-0.00011, "vx":0.81551, "vy":-0.01044, "omega":-0.00043, "ax":0.9983, "ay":-0.01227, "alpha":0.00169, "fx":[11.33681,11.27793,11.3336,11.3336], "fy":[-0.14183,-0.13644,-0.13643,-0.14183]}, - {"t":0.85894, "x":10.71871, "y":4.19526, "heading":-0.00013, "vx":0.85838, "vy":-0.01097, "omega":-0.00036, "ax":0.99759, "ay":-0.01194, "alpha":-0.00087, "fx":[11.31407,11.31407,11.31081,11.31081], "fy":[-0.13782,-0.13087,-0.1349,-0.13782]}, - {"t":0.90189, "x":10.7565, "y":4.19478, "heading":-0.00015, "vx":0.90122, "vy":-0.01148, "omega":-0.00039, "ax":0.99577, "ay":-0.01108, "alpha":0.03254, "fx":[11.48245,10.72797,11.47853,11.47853], "fy":[-0.12847,-0.12293,-0.1229,-0.12847]}, - {"t":0.94484, "x":10.79612, "y":4.19427, "heading":-0.00016, "vx":0.94399, "vy":-0.01196, "omega":0.001, "ax":0.9813, "ay":-0.00436, "alpha":0.00095, "fx":[11.1258,11.1258,11.12969,11.12969], "fy":[-0.04686,-0.05186,-0.05207,-0.04686]}, - {"t":0.98778, "x":10.83757, "y":4.19376, "heading":-0.00012, "vx":0.98613, "vy":-0.01214, "omega":0.00104, "ax":-0.99227, "ay":0.0164, "alpha":-0.03273, "fx":[-11.45946,-10.65534,-11.44685,-11.44686], "fy":[0.19397,0.17795,0.17788,0.19397]}, - {"t":1.03073, "x":10.879, "y":4.19325, "heading":-0.00008, "vx":0.94352, "vy":-0.01144, "omega":-0.00036, "ax":-0.99675, "ay":0.01443, "alpha":0.00319, "fx":[-11.30805,-11.30805,-11.29792,-11.29792], "fy":[0.17328,0.14699,0.16097,0.17328]}, - {"t":1.07368, "x":10.91861, "y":4.19277, "heading":-0.00009, "vx":0.90071, "vy":-0.01082, "omega":-0.00023, "ax":-0.99792, "ay":0.01391, "alpha":-0.00559, "fx":[-11.36714,-11.18383,-11.35703,-11.35703], "fy":[0.16478,0.15077,0.15076,0.16479]}, - {"t":1.11663, "x":10.95637, "y":4.19232, "heading":-0.0001, "vx":0.85785, "vy":-0.01022, "omega":-0.00047, "ax":-0.99846, "ay":0.01368, "alpha":0.00229, "fx":[-11.32674,-11.32673,-11.31799,-11.31799], "fy":[0.16145,0.14713,0.1503,0.16145]}, - {"t":1.15957, "x":10.99229, "y":4.19189, "heading":-0.00012, "vx":0.81497, "vy":-0.00963, "omega":-0.00037, "ax":-0.99877, "ay":0.01354, "alpha":0.00131, "fx":[-11.33293,-11.32,-11.32527,-11.32528], "fy":[0.1586,0.14846,0.14846,0.15861]}, - {"t":1.20252, "x":11.02637, "y":4.19149, "heading":-0.00014, "vx":0.77208, "vy":-0.00905, "omega":-0.00031, "ax":-0.99897, "ay":0.01345, "alpha":0.00173, "fx":[-11.33132,-11.33131,-11.32498,-11.32498], "fy":[0.15744,0.14649,0.14872,0.15745]}, - {"t":1.24547, "x":11.05861, "y":4.19112, "heading":-0.00015, "vx":0.72917, "vy":-0.00848, "omega":-0.00024, "ax":-0.99911, "ay":0.01339, "alpha":0.00153, "fx":[-11.3316,-11.33573,-11.32582,-11.32583], "fy":[0.15532,0.14831,0.14831,0.15532]}, - {"t":1.28841, "x":11.089, "y":4.19076, "heading":-0.00016, "vx":0.68627, "vy":-0.0079, "omega":-0.00017, "ax":-0.99922, "ay":0.01334, "alpha":0.00127, "fx":[-11.33305,-11.33304,-11.32881,-11.32881], "fy":[0.15505,0.14655,0.14853,0.15505]}, - {"t":1.33136, "x":11.11755, "y":4.19044, "heading":-0.00017, "vx":0.64335, "vy":-0.00733, "omega":-0.00012, "ax":-0.9993, "ay":0.01331, "alpha":0.00122, "fx":[-11.33214,-11.33933,-11.32794,-11.32794], "fy":[0.15312,0.14866,0.14866,0.15312]}, - {"t":1.37431, "x":11.14426, "y":4.19013, "heading":-0.00017, "vx":0.60044, "vy":-0.00676, "omega":-0.00006, "ax":-0.99936, "ay":0.01328, "alpha":0.00087, "fx":[-11.33383,-11.33383,-11.3313,-11.3313], "fy":[0.15331,0.14698,0.14867,0.15331]}, - {"t":1.41726, "x":11.16913, "y":4.18986, "heading":-0.00017, "vx":0.55752, "vy":-0.00619, "omega":-0.00003, "ax":-0.99941, "ay":0.01326, "alpha":0.0009, "fx":[-11.3326,-11.34059,-11.3297,-11.3297], "fy":[0.15151,0.14911,0.14911,0.15152]}, - {"t":1.4602, "x":11.19215, "y":4.1896, "heading":-0.00018, "vx":0.51459, "vy":-0.00562, "omega":0.00001, "ax":-0.99946, "ay":0.01324, "alpha":0.00055, "fx":[-11.33422,-11.33422,-11.33306,-11.33306], "fy":[0.15198,0.14749,0.14892,0.15199]}, - {"t":1.50315, "x":11.21333, "y":4.18937, "heading":-0.00018, "vx":0.47167, "vy":-0.00505, "omega":0.00004, "ax":-0.99949, "ay":0.01322, "alpha":0.00059, "fx":[-11.33303,-11.34071,-11.33123,-11.33123], "fy":[0.15029,0.14953,0.14953,0.15029]}, - {"t":1.5461, "x":11.23266, "y":4.18917, "heading":-0.00017, "vx":0.42874, "vy":-0.00448, "omega":0.00006, "ax":-0.99952, "ay":0.01321, "alpha":0.00029, "fx":[-11.33442,-11.33442,-11.33437,-11.33438], "fy":[0.15095,0.14791,0.14923,0.15095]}, - {"t":1.58904, "x":11.25015, "y":4.18899, "heading":-0.00017, "vx":0.38582, "vy":-0.00391, "omega":0.00007, "ax":-0.99955, "ay":0.01319, "alpha":0.00032, "fx":[-11.33336,-11.34041,-11.33251,-11.33252], "fy":[0.14932,0.14993,0.14993,0.14932]}, - {"t":1.63199, "x":11.2658, "y":4.18883, "heading":-0.00017, "vx":0.34289, "vy":-0.00335, "omega":0.00009, "ax":-0.99957, "ay":0.01318, "alpha":0.00005, "fx":[-11.33451,-11.33451,-11.33542,-11.33542], "fy":[0.15006,0.14836,0.14954,0.15006]}, - {"t":1.67494, "x":11.27961, "y":4.1887, "heading":-0.00016, "vx":0.29996, "vy":-0.00278, "omega":0.00009, "ax":-0.00239, "ay":0.21525, "alpha":-0.00036, "fx":[-0.02791,-0.02469,-0.02797,-0.02797], "fy":[2.44001,2.44177,2.44177,2.44001]}, - {"t":1.72019, "x":11.29318, "y":4.1888, "heading":-0.00016, "vx":0.29985, "vy":0.00696, "omega":0.00007, "ax":-0.00021, "ay":0.00888, "alpha":0.00609, "fx":[-0.00136,-0.00135,-0.00345,-0.00345], "fy":[0.12714,0.02055,0.12775,0.12714]}, - {"t":1.76544, "x":11.30675, "y":4.18912, "heading":-0.00016, "vx":0.29984, "vy":0.00736, "omega":0.00035, "ax":0.0, "ay":-0.0001, "alpha":-0.00062, "fx":[-0.00186,0.00613,-0.00208,-0.00208], "fy":[-0.00211,-0.00005,-0.00005,-0.00211]}, - {"t":1.8107, "x":11.32032, "y":4.18945, "heading":-0.00014, "vx":0.29984, "vy":0.00736, "omega":0.00032, "ax":0.0, "ay":0.00006, "alpha":-0.00037, "fx":[0.00077,0.00077,-0.0008,-0.0008], "fy":[-0.00023,0.00324,0.00014,-0.00023]}, - {"t":1.85595, "x":11.33389, "y":4.18979, "heading":-0.00013, "vx":0.29984, "vy":0.00736, "omega":0.0003, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00009,0.00052,-0.0003,-0.0003], "fy":[-0.0011,0.00103,0.00103,-0.0011]}, - {"t":1.9012, "x":11.34745, "y":4.19012, "heading":-0.00011, "vx":0.29984, "vy":0.00736, "omega":0.00029, "ax":0.0, "ay":0.0, "alpha":-0.0004, "fx":[0.0008,0.0008,-0.0008,-0.0008], "fy":[-0.0011,0.0026,-0.00051,-0.0011]}, - {"t":1.94646, "x":11.36102, "y":4.19045, "heading":-0.0001, "vx":0.29984, "vy":0.00736, "omega":0.00027, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00025,0.0003,-0.00027,-0.00027], "fy":[-0.0011,0.00109,0.00109,-0.00109]}, - {"t":1.99171, "x":11.37459, "y":4.19079, "heading":-0.00009, "vx":0.29984, "vy":0.00736, "omega":0.00026, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00082,0.00083,-0.00082,-0.00083], "fy":[-0.00069,0.00133,0.00007,-0.00069]}, - {"t":2.03696, "x":11.38816, "y":4.19112, "heading":-0.00008, "vx":0.29984, "vy":0.00736, "omega":0.00024, "ax":0.0, "ay":0.0, "alpha":-0.00032, "fx":[0.00027,0.00042,-0.00035,-0.00035], "fy":[-0.00111,0.0011,0.0011,-0.00111]}, - {"t":2.08221, "x":11.40173, "y":4.19145, "heading":-0.00006, "vx":0.29984, "vy":0.00736, "omega":0.00023, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00083,0.00083,-0.00083,-0.00083], "fy":[-0.00056,0.0008,0.00032,-0.00056]}, - {"t":2.12747, "x":11.4153, "y":4.19178, "heading":-0.00005, "vx":0.29984, "vy":0.00736, "omega":0.00022, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[0.0003,0.0005,-0.0004,-0.0004], "fy":[-0.00111,0.00111,0.00111,-0.00111]}, - {"t":2.17272, "x":11.42887, "y":4.19212, "heading":-0.00004, "vx":0.29984, "vy":0.00736, "omega":0.0002, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00084,0.00084,-0.00084,-0.00084], "fy":[-0.00056,0.00071,0.00042,-0.00056]}, - {"t":2.21797, "x":11.44244, "y":4.19245, "heading":-0.00004, "vx":0.29984, "vy":0.00736, "omega":0.00019, "ax":0.0, "ay":0.0, "alpha":-0.00034, "fx":[0.00032,0.00056,-0.00044,-0.00044], "fy":[-0.00111,0.00111,0.00111,-0.00111]}, - {"t":2.26323, "x":11.456, "y":4.19278, "heading":-0.00003, "vx":0.29984, "vy":0.00736, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":-0.00029, "fx":[0.00084,0.00084,-0.00084,-0.00084], "fy":[-0.0006,0.00075,0.00045,-0.0006]}, - {"t":2.30848, "x":11.46957, "y":4.19312, "heading":-0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00035, "fx":[0.00034,0.00061,-0.00047,-0.00047], "fy":[-0.00112,0.00112,0.00112,-0.00112]}, - {"t":2.35373, "x":11.48314, "y":4.19345, "heading":-0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00014, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00086,0.00086,-0.00086,-0.00086], "fy":[-0.00066,0.00084,0.00047,-0.00066]}, - {"t":2.39898, "x":11.49671, "y":4.19378, "heading":-0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00037, "fx":[0.00036,0.00068,-0.00052,-0.00052], "fy":[-0.00116,0.00116,0.00116,-0.00116]}, - {"t":2.44424, "x":11.51028, "y":4.19412, "heading":0.0, "vx":0.29984, "vy":0.00736, "omega":0.00011, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[0.00089,0.00089,-0.00089,-0.00089], "fy":[-0.00072,0.00094,0.0005,-0.00072]}, - {"t":2.48949, "x":11.52385, "y":4.19445, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00039, "fx":[0.00039,0.00076,-0.00057,-0.00057], "fy":[-0.00122,0.00122,0.00122,-0.00122]}, - {"t":2.53474, "x":11.53742, "y":4.19478, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00036, "fx":[0.00095,0.00095,-0.00095,-0.00095], "fy":[-0.00081,0.00107,0.00055,-0.00081]}, - {"t":2.58, "x":11.55099, "y":4.19511, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00007, "ax":0.0, "ay":0.0, "alpha":-0.00043, "fx":[0.00043,0.00085,-0.00064,-0.00064], "fy":[-0.00132,0.00131,0.00131,-0.00132]}, - {"t":2.62525, "x":11.56455, "y":4.19545, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.0004, "fx":[0.00104,0.00104,-0.00104,-0.00104], "fy":[-0.00092,0.00123,0.00061,-0.00092]}, - {"t":2.6705, "x":11.57812, "y":4.19578, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00048, "fx":[0.0005,0.00096,-0.00073,-0.00073], "fy":[-0.00146,0.00146,0.00146,-0.00146]}, - {"t":2.71575, "x":11.59169, "y":4.19611, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00046, "fx":[0.00117,0.00117,-0.00117,-0.00117], "fy":[-0.00106,0.00142,0.00071,-0.00106]}, - {"t":2.76101, "x":11.60526, "y":4.19645, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00054, "fx":[0.0006,0.00106,-0.00083,-0.00083], "fy":[-0.00167,0.00167,0.00167,-0.00167]}, - {"t":2.80626, "x":11.61883, "y":4.19678, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00053, "fx":[0.00135,0.00135,-0.00135,-0.00135], "fy":[-0.00124,0.00165,0.00084,-0.00124]}, - {"t":2.85151, "x":11.6324, "y":4.19711, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00064, "fx":[0.00068,0.00136,-0.00102,-0.00102], "fy":[-0.00194,0.00194,0.00194,-0.00194]}, - {"t":2.89677, "x":11.64597, "y":4.19745, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00063, "fx":[0.00158,0.00158,-0.00158,-0.00158], "fy":[-0.00147,0.00194,0.001,-0.00147]}, - {"t":2.94202, "x":11.65954, "y":4.19778, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00091, "fx":[-0.00005,0.00416,-0.00206,-0.00206], "fy":[-0.00229,0.00229,0.00229,-0.00229]}, - {"t":2.98727, "x":11.6731, "y":4.19811, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00078, "fx":[0.00189,0.00189,-0.00189,-0.00189], "fy":[-0.00187,0.00262,0.0011,-0.00187]}, - {"t":3.03252, "x":11.68667, "y":4.19845, "heading":0.0, "vx":0.29984, "vy":0.00736, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00577, "fx":[0.03932,-0.11234,0.03641,0.03641], "fy":[-0.003,0.00301,0.00301,-0.003]}, - {"t":3.07778, "x":11.70024, "y":4.19878, "heading":-0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00006, "ax":-0.63133, "ay":-0.0155, "alpha":-0.00094, "fx":[-7.15741,-7.15741,-7.16098,-7.16098], "fy":[-0.17836,-0.17042,-0.17581,-0.17836]}, - {"t":3.12303, "x":11.71316, "y":4.1991, "heading":-0.00001, "vx":0.27127, "vy":0.00666, "omega":0.00002, "ax":-0.99824, "ay":-0.0245, "alpha":0.00085, "fx":[-11.31075,-11.34443,-11.31219,-11.31219], "fy":[-0.28004,-0.27569,-0.27569,-0.28004]}, - {"t":3.16828, "x":11.72442, "y":4.19937, "heading":-0.00001, "vx":0.2261, "vy":0.00555, "omega":0.00006, "ax":-0.99897, "ay":-0.02452, "alpha":-0.00051, "fx":[-11.32673,-11.32673,-11.32954,-11.32954], "fy":[-0.27918,-0.27625,-0.27765,-0.27918]}, - {"t":3.21354, "x":11.73363, "y":4.1996, "heading":0.0, "vx":0.18089, "vy":0.00444, "omega":0.00004, "ax":-0.99921, "ay":-0.02453, "alpha":-0.00008, "fx":[-11.32813,-11.33737,-11.32902,-11.32902], "fy":[-0.27987,-0.2764,-0.27639,-0.27987]}, - {"t":3.25879, "x":11.74079, "y":4.19977, "heading":0.0, "vx":0.13568, "vy":0.00333, "omega":0.00003, "ax":-0.99933, "ay":-0.02453, "alpha":-0.00042, "fx":[-11.33107,-11.33107,-11.33345,-11.33345], "fy":[-0.27903,-0.2765,-0.27812,-0.27903]}, - {"t":3.30404, "x":11.74591, "y":4.1999, "heading":0.0, "vx":0.09045, "vy":0.00222, "omega":0.00001, "ax":-0.99941, "ay":-0.02453, "alpha":-0.00015, "fx":[-11.33149,-11.33692,-11.33197,-11.33197], "fy":[-0.27967,-0.2767,-0.2767,-0.27967]}, - {"t":3.34929, "x":11.74898, "y":4.19997, "heading":0.0, "vx":0.04523, "vy":0.00111, "omega":0.00001, "ax":-0.99946, "ay":-0.02453, "alpha":-0.00016, "fx":[-11.33241,-11.33674,-11.3327,-11.3327], "fy":[-0.27959,-0.27682,-0.27682,-0.27959]}, - {"t":3.39455, "x":11.75, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":10.35, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99944, "ay":-0.01305, "alpha":0.00374, "fx":[11.35777,11.26375,11.35605,11.35605], "fy":[-0.14923,-0.14682,-0.14682,-0.14923]}, + {"t":0.04295, "x":10.35092, "y":4.19999, "heading":0.0, "vx":0.04292, "vy":-0.00056, "omega":0.00016, "ax":0.99966, "ay":-0.01305, "alpha":0.00167, "fx":[11.34887,11.30102,11.34705,11.34705], "fy":[-0.14925,-0.14672,-0.14672,-0.14925]}, + {"t":0.08589, "x":10.35369, "y":4.19995, "heading":0.00001, "vx":0.08586, "vy":-0.00112, "omega":0.00023, "ax":0.99965, "ay":-0.01304, "alpha":-0.00014, "fx":[11.33683,11.33682,11.33489,11.33489], "fy":[-0.14777,-0.15076,-0.14534,-0.14777]}, + {"t":0.12884, "x":10.3583, "y":4.19989, "heading":0.00002, "vx":0.12879, "vy":-0.00168, "omega":0.00023, "ax":0.99964, "ay":-0.01304, "alpha":0.00182, "fx":[11.34994,11.29722,11.34783,11.34783], "fy":[-0.14925,-0.1464,-0.1464,-0.14925]}, + {"t":0.17179, "x":10.36475, "y":4.19981, "heading":0.00003, "vx":0.17172, "vy":-0.00224, "omega":0.0003, "ax":0.99962, "ay":-0.01303, "alpha":-0.00016, "fx":[11.3365,11.3365,11.33457,11.33457], "fy":[-0.14766,-0.15043,-0.14516,-0.14766]}, + {"t":0.21474, "x":10.37305, "y":4.1997, "heading":0.00004, "vx":0.21465, "vy":-0.0028, "omega":0.0003, "ax":0.99961, "ay":-0.01302, "alpha":0.00212, "fx":[11.35104,11.29227,11.34903,11.34903], "fy":[-0.14895,-0.1463,-0.14631,-0.14895]}, + {"t":0.25768, "x":10.38319, "y":4.19957, "heading":0.00005, "vx":0.25758, "vy":-0.00336, "omega":0.00039, "ax":0.99959, "ay":-0.01301, "alpha":-0.0001, "fx":[11.33594,11.33593,11.33432,11.33432], "fy":[-0.14734,-0.15016,-0.14518,-0.14734]}, + {"t":0.30063, "x":10.39517, "y":4.19941, "heading":0.00007, "vx":0.30051, "vy":-0.00392, "omega":0.00038, "ax":0.99957, "ay":-0.013, "alpha":0.00255, "fx":[11.35235,11.28585,11.35068,11.35068], "fy":[-0.14845,-0.1463,-0.1463,-0.14845]}, + {"t":0.34358, "x":10.409, "y":4.19923, "heading":0.00009, "vx":0.34344, "vy":-0.00448, "omega":0.00049, "ax":0.99954, "ay":-0.01298, "alpha":-0.00002, "fx":[11.33519,11.33519,11.33404,11.33404], "fy":[-0.14686,-0.14991,-0.14527,-0.14686]}, + {"t":0.38652, "x":10.42467, "y":4.19903, "heading":0.00011, "vx":0.38637, "vy":-0.00503, "omega":0.00049, "ax":0.99951, "ay":-0.01297, "alpha":0.00312, "fx":[11.35412,11.27732,11.35287,11.35287], "fy":[-0.14783,-0.14626,-0.14626,-0.14783]}, + {"t":0.42947, "x":10.44219, "y":4.1988, "heading":0.00013, "vx":0.42929, "vy":-0.00559, "omega":0.00063, "ax":0.99948, "ay":-0.01295, "alpha":0.00008, "fx":[11.33427,11.33427,11.33358,11.33358], "fy":[-0.14626,-0.14959,-0.14527,-0.14625]}, + {"t":0.47242, "x":10.46154, "y":4.19855, "heading":0.00015, "vx":0.47222, "vy":-0.00615, "omega":0.00063, "ax":0.99944, "ay":-0.01293, "alpha":0.00384, "fx":[11.35676,11.26549,11.35586,11.35585], "fy":[-0.14716,-0.14604,-0.14604,-0.14716]}, + {"t":0.51537, "x":10.48275, "y":4.19827, "heading":0.00018, "vx":0.51514, "vy":-0.0067, "omega":0.0008, "ax":0.9994, "ay":-0.0129, "alpha":0.00011, "fx":[11.33318,11.33318,11.33276,11.33276], "fy":[-0.14568,-0.1488,-0.1451,-0.14568]}, + {"t":0.55831, "x":10.50579, "y":4.19797, "heading":0.00022, "vx":0.55806, "vy":-0.00726, "omega":0.0008, "ax":0.99934, "ay":-0.01287, "alpha":0.00471, "fx":[11.36039,11.24985,11.35956,11.35956], "fy":[-0.14652,-0.1454,-0.1454,-0.14652]}, + {"t":0.60126, "x":10.53068, "y":4.19765, "heading":0.00025, "vx":0.60098, "vy":-0.00781, "omega":0.001, "ax":0.99927, "ay":-0.01283, "alpha":0.00002, "fx":[11.33186,11.33187,11.33125,11.33125], "fy":[-0.14521,-0.14721,-0.14449,-0.14521]}, + {"t":0.64421, "x":10.55741, "y":4.1973, "heading":0.00029, "vx":0.6439, "vy":-0.00836, "omega":0.001, "ax":0.99918, "ay":-0.01278, "alpha":0.00616, "fx":[11.36805,11.2209,11.36664,11.36664], "fy":[-0.14603,-0.14392,-0.14393,-0.14603]}, + {"t":0.68715, "x":10.58599, "y":4.19693, "heading":0.00034, "vx":0.68681, "vy":-0.00891, "omega":0.00127, "ax":0.99907, "ay":-0.01272, "alpha":-0.00031, "fx":[11.33009,11.33009,11.3284,11.3284], "fy":[-0.14494,-0.14405,-0.14306,-0.14494]}, + {"t":0.7301, "x":10.6164, "y":4.19653, "heading":0.00039, "vx":0.72972, "vy":-0.00946, "omega":0.00125, "ax":0.99891, "ay":-0.01263, "alpha":0.00915, "fx":[11.38519,11.16038,11.38207,11.38207], "fy":[-0.14563,-0.14086,-0.14086,-0.14563]}, + {"t":0.77305, "x":10.64867, "y":4.19612, "heading":0.00045, "vx":0.77262, "vy":-0.01, "omega":0.00165, "ax":0.99867, "ay":-0.0125, "alpha":-0.00108, "fx":[11.32678,11.32679,11.32275,11.32275], "fy":[-0.14483,-0.13716,-0.14028,-0.14483]}, + {"t":0.816, "x":10.68277, "y":4.19568, "heading":0.00052, "vx":0.81551, "vy":-0.01054, "omega":0.0016, "ax":0.9983, "ay":-0.01229, "alpha":0.0196, "fx":[11.44404,10.96302,11.43743,11.43743], "fy":[-0.14438,-0.13443,-0.13442,-0.14438]}, + {"t":0.85894, "x":10.71871, "y":4.19521, "heading":0.00058, "vx":0.85838, "vy":-0.01106, "omega":0.00244, "ax":0.99759, "ay":-0.0119, "alpha":-0.00272, "fx":[11.31587,11.31588,11.30901,11.30901], "fy":[-0.14388,-0.11588,-0.1362,-0.14388]}, + {"t":0.90189, "x":10.7565, "y":4.19473, "heading":0.00069, "vx":0.90122, "vy":-0.01157, "omega":0.00233, "ax":0.99577, "ay":-0.0109, "alpha":0.10571, "fx":[11.91216,9.45717,11.89912,11.89911], "fy":[-0.13269,-0.11461,-0.1145,-0.13268]}, + {"t":0.94484, "x":10.79612, "y":4.19422, "heading":0.00079, "vx":0.94399, "vy":-0.01204, "omega":0.00687, "ax":0.9813, "ay":-0.00302, "alpha":-0.00124, "fx":[11.12292,11.12292,11.13253,11.13253], "fy":[-0.04303,0.00548,-0.05619,-0.04303]}, + {"t":0.98778, "x":10.83757, "y":4.1937, "heading":0.00108, "vx":0.98613, "vy":-0.01217, "omega":0.00681, "ax":-0.99226, "ay":0.01717, "alpha":-0.16183, "fx":[-12.23569,-8.3771,-12.19759,-12.19758], "fy":[0.22023,0.1693,0.16911,0.22022]}, + {"t":1.03073, "x":10.879, "y":4.19319, "heading":0.00138, "vx":0.94352, "vy":-0.01143, "omega":-0.00014, "ax":-0.99675, "ay":0.01484, "alpha":0.00738, "fx":[-11.3168,-11.3168,-11.28904,-11.28905], "fy":[0.18903,0.14212,0.15308,0.18903]}, + {"t":1.07368, "x":10.91861, "y":4.19271, "heading":0.00137, "vx":0.90071, "vy":-0.0108, "omega":0.00018, "ax":-0.99792, "ay":0.01423, "alpha":-0.03144, "fx":[-11.55161,-10.67097,-11.52113,-11.52113], "fy":[0.18218,0.14062,0.14059,0.18218]}, + {"t":1.11663, "x":10.95637, "y":4.19226, "heading":0.00138, "vx":0.85785, "vy":-0.01019, "omega":-0.00117, "ax":-0.99846, "ay":0.01395, "alpha":0.00562, "fx":[-11.33528,-11.33527,-11.30936,-11.30937], "fy":[0.17218,0.14988,0.13857,0.17218]}, + {"t":1.15957, "x":10.99229, "y":4.19184, "heading":0.00133, "vx":0.81497, "vy":-0.00959, "omega":-0.00093, "ax":-0.99877, "ay":0.01379, "alpha":-0.00719, "fx":[-11.41206,-11.11498,-11.38815,-11.38814], "fy":[0.17269,0.14005,0.14005,0.17269]}, + {"t":1.20252, "x":11.02637, "y":4.19144, "heading":0.00129, "vx":0.77208, "vy":-0.00899, "omega":-0.00124, "ax":-0.99897, "ay":0.01368, "alpha":0.0045, "fx":[-11.33867,-11.33867,-11.31755,-11.31756], "fy":[0.16623,0.1496,0.13864,0.16623]}, + {"t":1.24547, "x":11.05861, "y":4.19107, "heading":0.00124, "vx":0.72918, "vy":-0.00841, "omega":-0.00105, "ax":-0.99911, "ay":0.01361, "alpha":-0.00277, "fx":[-11.38244,-11.2105,-11.36295,-11.36295], "fy":[0.16753,0.14114,0.14115,0.16753]}, + {"t":1.28841, "x":11.089, "y":4.19072, "heading":0.00119, "vx":0.68627, "vy":-0.00782, "omega":-0.00116, "ax":-0.99921, "ay":0.01356, "alpha":0.00373, "fx":[-11.33967,-11.33967,-11.32212,-11.32212], "fy":[0.16286,0.14946,0.1397,0.16286]}, + {"t":1.33136, "x":11.11755, "y":4.19039, "heading":0.00114, "vx":0.64335, "vy":-0.00724, "omega":-0.001, "ax":-0.99929, "ay":0.01351, "alpha":-0.00136, "fx":[-11.37091,-11.24765,-11.35434,-11.35433], "fy":[0.16435,0.14213,0.14213,0.16435]}, + {"t":1.37431, "x":11.14426, "y":4.1901, "heading":0.0011, "vx":0.60044, "vy":-0.00666, "omega":-0.00106, "ax":-0.99936, "ay":0.01348, "alpha":0.00319, "fx":[-11.34011,-11.34011,-11.32495,-11.32495], "fy":[0.16063,0.14974,0.14045,0.16063]}, + {"t":1.41726, "x":11.16913, "y":4.18982, "heading":0.00105, "vx":0.55752, "vy":-0.00608, "omega":-0.00093, "ax":-0.99941, "ay":0.01345, "alpha":-0.00079, "fx":[-11.36524,-11.2661,-11.35057,-11.35057], "fy":[0.1623,0.14281,0.14281,0.1623]}, + {"t":1.4602, "x":11.19215, "y":4.18957, "heading":0.00101, "vx":0.51459, "vy":-0.0055, "omega":-0.00096, "ax":-0.99945, "ay":0.01343, "alpha":0.00284, "fx":[-11.34039,-11.34039,-11.32683,-11.32682], "fy":[0.15919,0.14986,0.14096,0.15918]}, + {"t":1.50315, "x":11.21333, "y":4.18935, "heading":0.00097, "vx":0.47167, "vy":-0.00493, "omega":-0.00084, "ax":-0.99949, "ay":0.01341, "alpha":-0.00036, "fx":[-11.36131,-11.27901,-11.34788,-11.34788], "fy":[0.16093,0.14324,0.14324,0.16093]}, + {"t":1.5461, "x":11.23266, "y":4.18915, "heading":0.00093, "vx":0.42875, "vy":-0.00435, "omega":-0.00085, "ax":-0.99952, "ay":0.0134, "alpha":0.00262, "fx":[-11.34064,-11.34064,-11.3281,-11.3281], "fy":[0.15826,0.14981,0.14126,0.15826]}, + {"t":1.58904, "x":11.25015, "y":4.18898, "heading":0.0009, "vx":0.38582, "vy":-0.00377, "omega":-0.00074, "ax":-0.99955, "ay":0.01338, "alpha":-0.00003, "fx":[-11.35864,-11.2882,-11.34593,-11.34593], "fy":[0.16005,0.14343,0.14343,0.16005]}, + {"t":1.63199, "x":11.2658, "y":4.18883, "heading":0.00087, "vx":0.34289, "vy":-0.0032, "omega":-0.00074, "ax":-0.99957, "ay":0.01337, "alpha":0.00252, "fx":[-11.34096,-11.34096,-11.32892,-11.32891], "fy":[0.15771,0.14971,0.14128,0.15771]}, + {"t":1.67494, "x":11.27961, "y":4.1887, "heading":0.00083, "vx":0.29996, "vy":-0.00263, "omega":-0.00063, "ax":-0.0036, "ay":0.25734, "alpha":0.01224, "fx":[0.00577,-0.20449,0.01772,0.01772], "fy":[2.926,2.91048,2.91048,2.926]}, + {"t":1.71966, "x":11.29302, "y":4.18884, "heading":0.00081, "vx":0.2998, "vy":0.00888, "omega":-0.00009, "ax":-0.00034, "ay":0.01117, "alpha":-0.0301, "fx":[-0.01153,-0.01153,0.00381,0.00381], "fy":[-0.00599,0.54693,-0.02829,-0.00599]}, + {"t":1.76437, "x":11.30642, "y":4.18925, "heading":0.0008, "vx":0.29979, "vy":0.00938, "omega":-0.00143, "ax":0.0, "ay":0.00016, "alpha":0.00227, "fx":[-0.01045,0.00622,0.002,0.002], "fy":[0.00984,-0.00625,-0.00625,0.00983]}, + {"t":1.80909, "x":11.31983, "y":4.18967, "heading":0.00074, "vx":0.29979, "vy":0.00939, "omega":-0.00133, "ax":0.00001, "ay":-0.00036, "alpha":-0.00403, "fx":[-0.00583,-0.00584,0.00609,0.00609], "fy":[-0.02557,0.07619,-0.04156,-0.02557]}, + {"t":1.85381, "x":11.33323, "y":4.19009, "heading":0.00068, "vx":0.29979, "vy":0.00937, "omega":-0.00151, "ax":0.0, "ay":0.00013, "alpha":0.00261, "fx":[-0.00572,-0.005,0.00527,0.00527], "fy":[0.00847,-0.00555,-0.00555,0.00847]}, + {"t":1.89852, "x":11.34664, "y":4.19051, "heading":0.00061, "vx":0.29979, "vy":0.00938, "omega":-0.00139, "ax":0.0, "ay":0.00014, "alpha":0.00249, "fx":[-0.00489,-0.00489,0.00479,0.0048], "fy":[0.0084,-0.00532,-0.00529,0.0084]}, + {"t":1.94324, "x":11.36004, "y":4.19093, "heading":0.00055, "vx":0.29979, "vy":0.00938, "omega":-0.00128, "ax":0.0, "ay":-0.00005, "alpha":0.00227, "fx":[-0.0047,-0.00489,0.00483,0.00483], "fy":[0.00534,-0.00656,-0.00656,0.00533]}, + {"t":1.98796, "x":11.37345, "y":4.19135, "heading":0.00049, "vx":0.29979, "vy":0.00938, "omega":-0.00118, "ax":0.0, "ay":-0.00003, "alpha":0.00204, "fx":[-0.0041,-0.0041,0.00412,0.00413], "fy":[0.00516,-0.0051,-0.00668,0.00516]}, + {"t":2.03267, "x":11.38686, "y":4.19177, "heading":0.00044, "vx":0.29979, "vy":0.00938, "omega":-0.00109, "ax":0.0, "ay":0.00001, "alpha":0.00194, "fx":[-0.00415,-0.00417,0.00415,0.00415], "fy":[0.00521,-0.0049,-0.00491,0.00521]}, + {"t":2.07739, "x":11.40026, "y":4.19219, "heading":0.00039, "vx":0.29979, "vy":0.00938, "omega":-0.001, "ax":0.0, "ay":0.0, "alpha":0.00187, "fx":[-0.00351,-0.00351,0.00351,0.00351], "fy":[0.00532,-0.0054,-0.00506,0.00531]}, + {"t":2.12211, "x":11.41367, "y":4.19261, "heading":0.00034, "vx":0.29979, "vy":0.00938, "omega":-0.00092, "ax":0.0, "ay":0.0, "alpha":0.00168, "fx":[-0.00367,-0.00368,0.00367,0.00367], "fy":[0.00435,-0.00436,-0.00436,0.00434]}, + {"t":2.16682, "x":11.42707, "y":4.19302, "heading":0.0003, "vx":0.29979, "vy":0.00938, "omega":-0.00085, "ax":0.0, "ay":0.0, "alpha":0.00166, "fx":[-0.00305,-0.00305,0.00305,0.00305], "fy":[0.0047,-0.00487,-0.0046,0.00469]}, + {"t":2.21154, "x":11.44048, "y":4.19344, "heading":0.00027, "vx":0.29979, "vy":0.00938, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.0015, "fx":[-0.00333,-0.00335,0.00334,0.00334], "fy":[0.00386,-0.00382,-0.00382,0.00386]}, + {"t":2.25626, "x":11.45388, "y":4.19386, "heading":0.00023, "vx":0.29979, "vy":0.00938, "omega":-0.0007, "ax":0.0, "ay":0.0, "alpha":0.00152, "fx":[-0.00274,-0.00274,0.00274,0.00275], "fy":[0.00433,-0.00445,-0.00427,0.00433]}, + {"t":2.30097, "x":11.46729, "y":4.19428, "heading":0.0002, "vx":0.29979, "vy":0.00938, "omega":-0.00064, "ax":0.0, "ay":0.0, "alpha":0.0014, "fx":[-0.00315,-0.00319,0.00316,0.00317], "fy":[0.00356,-0.00352,-0.00352,0.00356]}, + {"t":2.34569, "x":11.48069, "y":4.1947, "heading":0.00017, "vx":0.29979, "vy":0.00938, "omega":-0.00057, "ax":0.0, "ay":0.0, "alpha":0.00146, "fx":[-0.00261,-0.00261,0.00261,0.00261], "fy":[0.00418,-0.00425,-0.00417,0.00417]}, + {"t":2.39041, "x":11.4941, "y":4.19512, "heading":0.00015, "vx":0.29979, "vy":0.00938, "omega":-0.00051, "ax":0.0, "ay":0.0, "alpha":0.00139, "fx":[-0.00313,-0.00324,0.00318,0.00319], "fy":[0.00351,-0.00347,-0.00347,0.00351]}, + {"t":2.43513, "x":11.50751, "y":4.19554, "heading":0.00012, "vx":0.29979, "vy":0.00938, "omega":-0.00045, "ax":0.0, "ay":0.0, "alpha":0.0015, "fx":[-0.00268,-0.00268,0.00268,0.00268], "fy":[0.00431,-0.00438,-0.00432,0.00431]}, + {"t":2.47984, "x":11.52091, "y":4.19596, "heading":0.0001, "vx":0.29979, "vy":0.00938, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.0015, "fx":[-0.00337,-0.00344,0.0034,0.0034], "fy":[0.00378,-0.00374,-0.00374,0.00378]}, + {"t":2.52456, "x":11.53432, "y":4.19638, "heading":0.00009, "vx":0.29979, "vy":0.00938, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":0.00168, "fx":[-0.00303,-0.00304,0.00303,0.00304], "fy":[0.00478,-0.00481,-0.00483,0.00478]}, + {"t":2.56928, "x":11.54772, "y":4.1968, "heading":0.00007, "vx":0.29979, "vy":0.00938, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":0.0016, "fx":[-0.00482,-0.00121,0.00302,0.00302], "fy":[0.00449,-0.00444,-0.00444,0.00449]}, + {"t":2.61399, "x":11.56113, "y":4.19722, "heading":0.00006, "vx":0.29979, "vy":0.00938, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00202, "fx":[-0.00374,-0.00374,0.00374,0.00374], "fy":[0.00569,-0.0057,-0.00579,0.00569]}, + {"t":2.65871, "x":11.57453, "y":4.19764, "heading":0.00005, "vx":0.29979, "vy":0.00938, "omega":-0.00007, "ax":0.0, "ay":0.0, "alpha":0.00172, "fx":[-0.00753,0.00335,0.00209,0.00209], "fy":[0.00571,-0.00567,-0.00567,0.00571]}, + {"t":2.70343, "x":11.58794, "y":4.19806, "heading":0.00005, "vx":0.29979, "vy":0.00938, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0032, "fx":[-0.0049,-0.0049,0.00476,0.00476], "fy":[0.00984,-0.01535,-0.0044,0.00984]}, + {"t":2.74814, "x":11.60134, "y":4.19848, "heading":0.00005, "vx":0.29979, "vy":0.00938, "omega":0.00015, "ax":-0.71067, "ay":-0.02224, "alpha":-0.00818, "fx":[-8.1275,-7.8781,-8.11504,-8.11504], "fy":[-0.24459,-0.25975,-0.25971,-0.24459]}, + {"t":2.79286, "x":11.61404, "y":4.19887, "heading":0.00006, "vx":0.26801, "vy":0.00839, "omega":-0.00022, "ax":-0.99803, "ay":-0.03123, "alpha":0.0023, "fx":[-11.32216,-11.32216,-11.31273,-11.31273], "fy":[-0.34795,-0.35851,-0.3621,-0.34795]}, + {"t":2.83758, "x":11.62503, "y":4.19922, "heading":0.00005, "vx":0.22338, "vy":0.00699, "omega":-0.00012, "ax":-0.99877, "ay":-0.03125, "alpha":-0.00415, "fx":[-11.36668,-11.22309,-11.35679,-11.35679], "fy":[-0.34866,-0.36011,-0.36012,-0.34866]}, + {"t":2.88229, "x":11.63402, "y":4.1995, "heading":0.00004, "vx":0.17872, "vy":0.00559, "omega":-0.0003, "ax":-0.99902, "ay":-0.03126, "alpha":0.00217, "fx":[-11.33217,-11.33217,-11.32511,-11.32511], "fy":[-0.34799,-0.36254,-0.35939,-0.34799]}, + {"t":2.92701, "x":11.64101, "y":4.19972, "heading":0.00003, "vx":0.13404, "vy":0.00419, "omega":-0.00021, "ax":-0.99914, "ay":-0.03126, "alpha":-0.00133, "fx":[-11.35206,-11.28086,-11.34364,-11.34364], "fy":[-0.34994,-0.3591,-0.35911,-0.34994]}, + {"t":2.97173, "x":11.646, "y":4.19987, "heading":0.00002, "vx":0.08937, "vy":0.0028, "omega":-0.00027, "ax":-0.99921, "ay":-0.03127, "alpha":0.0021, "fx":[-11.33409,-11.33409,-11.32769,-11.32769], "fy":[-0.34808,-0.36317,-0.35885,-0.34808]}, + {"t":3.01644, "x":11.649, "y":4.19997, "heading":0.00001, "vx":0.04468, "vy":0.0014, "omega":-0.00017, "ax":-0.99926, "ay":-0.03127, "alpha":0.00384, "fx":[-11.34735,-11.34735,-11.33927,-11.29182], "fy":[-0.3526,-0.35883,-0.35883,-0.34801]}, + {"t":3.06116, "x":11.65, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/auto/RedL4Auto.java b/src/main/java/frc/robot/auto/RedL4Auto.java index 4ae8563a..cfc3dd79 100644 --- a/src/main/java/frc/robot/auto/RedL4Auto.java +++ b/src/main/java/frc/robot/auto/RedL4Auto.java @@ -45,12 +45,16 @@ public Optional getInitialPose() { public AutoRoutine getAutoRoutine() { // spotless:off - redL4AutoRoutine.active().onTrue(redCenterToL4G.cmd()); + redL4AutoRoutine.active().onTrue( + Commands.parallel( + redCenterToL4G.cmd(), + Commands.sequence( + Commands.waitSeconds(1.0), + elevator.coralL4PositionCG()).withTimeout(2.0))); + redCenterToL4G.done().onTrue( Commands.sequence( - Commands.waitSeconds(0.1), - elevator.coralL4PositionCG(), Commands.waitSeconds(0.1), coralRoller.createOuttakeCommand().withTimeout(0.2), Commands.waitSeconds(0.2))); From 631ba986930ac9e26f5cf60eb811d2a3da37bda0 Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sat, 22 Mar 2025 14:25:02 -0400 Subject: [PATCH 09/24] removed withtimout on elevator --- src/main/java/frc/robot/auto/RedL4Auto.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/auto/RedL4Auto.java b/src/main/java/frc/robot/auto/RedL4Auto.java index cfc3dd79..c2418b4e 100644 --- a/src/main/java/frc/robot/auto/RedL4Auto.java +++ b/src/main/java/frc/robot/auto/RedL4Auto.java @@ -50,12 +50,12 @@ public AutoRoutine getAutoRoutine() { redCenterToL4G.cmd(), Commands.sequence( Commands.waitSeconds(1.0), - elevator.coralL4PositionCG()).withTimeout(2.0))); + elevator.coralL4PositionCG()))); redCenterToL4G.done().onTrue( Commands.sequence( - Commands.waitSeconds(0.1), + Commands.waitSeconds(0.1), coralRoller.createOuttakeCommand().withTimeout(0.2), Commands.waitSeconds(0.2))); From c690ac9319566ad76af27089e4e65d0640ea8c6e Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sat, 22 Mar 2025 14:31:09 -0400 Subject: [PATCH 10/24] added timout to both red and blue L4autos --- src/main/java/frc/robot/auto/BlueL4Auto.java | 2 +- src/main/java/frc/robot/auto/RedL4Auto.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/auto/BlueL4Auto.java b/src/main/java/frc/robot/auto/BlueL4Auto.java index 80c97da6..913a79ba 100644 --- a/src/main/java/frc/robot/auto/BlueL4Auto.java +++ b/src/main/java/frc/robot/auto/BlueL4Auto.java @@ -50,7 +50,7 @@ public AutoRoutine getAutoRoutine() { blueCenterToL4G.cmd(), Commands.sequence( Commands.waitSeconds(1.0), - elevator.coralL4PositionCG()))); + elevator.coralL4PositionCG().withTimeout(2.0)))); blueCenterToL4G.done().onTrue( Commands.sequence( diff --git a/src/main/java/frc/robot/auto/RedL4Auto.java b/src/main/java/frc/robot/auto/RedL4Auto.java index c2418b4e..2353a2a4 100644 --- a/src/main/java/frc/robot/auto/RedL4Auto.java +++ b/src/main/java/frc/robot/auto/RedL4Auto.java @@ -50,7 +50,7 @@ public AutoRoutine getAutoRoutine() { redCenterToL4G.cmd(), Commands.sequence( Commands.waitSeconds(1.0), - elevator.coralL4PositionCG()))); + elevator.coralL4PositionCG().withTimeout(2.0)))); redCenterToL4G.done().onTrue( From 355960b807c0dc9719839f12e2868103c8f82f85 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sat, 22 Mar 2025 14:35:08 -0400 Subject: [PATCH 11/24] Set default stop command for swerve in autonomous --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/drivetrain/Drivetrain.java | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3447eda8..ca14a9cd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -196,6 +196,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { autoSelector.scheduleAuto(); + swerve.setDefaultCommand(swerve.createStopCommand()); lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand()); leds.replaceDefaultCommandImmediately( leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier)); diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index a07fbc5a..7c36f1de 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -192,6 +192,10 @@ public void setHeadingOffset() { fieldRotatedSupplier.getAsBoolean() ? getHeading().rotateBy(Rotation2d.kPi) : getHeading(); } + public Command createStopCommand() { + return this.run(() -> setRobotRelativeChassisSpeeds(new ChassisSpeeds())); + } + /** * @return Array of swerve module positions */ From e13eb4780a7d739c642e81d2bbbaa4a06152ce50 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sat, 22 Mar 2025 15:04:39 -0400 Subject: [PATCH 12/24] Prevent elevator rocket in joystick command --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/elevator/Lifter.java | 18 +++++++++++++++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca14a9cd..9e03e045 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -211,7 +211,7 @@ public void teleopInit() { swerve.setDefaultCommand( new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID())); - lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand()); + lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID())); // lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID())); leds.replaceDefaultCommandImmediately( leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier)); diff --git a/src/main/java/frc/robot/elevator/Lifter.java b/src/main/java/frc/robot/elevator/Lifter.java index 4a064345..61d5acf4 100644 --- a/src/main/java/frc/robot/elevator/Lifter.java +++ b/src/main/java/frc/robot/elevator/Lifter.java @@ -213,8 +213,14 @@ private Boolean isInRange(Distance height) { } public Command createJoystickControlCommand(XboxController gamepad) { - return this.run( - () -> { + return new FunctionalCommand( + // initialize + () -> { + matchHeight(); + resetController(); + }, + // execute + () -> { Distance targetPosition = Meters.of(feedback.getGoal().position); double joystickInput = MathUtil.applyDeadband(-gamepad.getLeftY(), 0.05); @@ -224,7 +230,13 @@ public Command createJoystickControlCommand(XboxController gamepad) { if (isInRange(targetPosition)) feedback.setGoal(targetPosition.in(Meters)); control(); - }); + }, + // end + interrupted -> {}, + // isFinished + () -> false, + // requirements + this); } public Command createJoystickVoltageCommand(XboxController gamepad) { From 6010fbc4c514676eb394b89b5666a612e0a35dda Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sat, 22 Mar 2025 15:11:15 -0400 Subject: [PATCH 13/24] made the elevator and robot move at the same time. --- src/main/java/frc/robot/auto/BlueL4Auto.java | 4 +--- src/main/java/frc/robot/auto/RedL4Auto.java | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/auto/BlueL4Auto.java b/src/main/java/frc/robot/auto/BlueL4Auto.java index 913a79ba..92892d44 100644 --- a/src/main/java/frc/robot/auto/BlueL4Auto.java +++ b/src/main/java/frc/robot/auto/BlueL4Auto.java @@ -48,9 +48,7 @@ public AutoRoutine getAutoRoutine() { blueL4AutoRoutine.active().onTrue( Commands.parallel( blueCenterToL4G.cmd(), - Commands.sequence( - Commands.waitSeconds(1.0), - elevator.coralL4PositionCG().withTimeout(2.0)))); + elevator.coralL4PositionCG().withTimeout(2.0))); blueCenterToL4G.done().onTrue( Commands.sequence( diff --git a/src/main/java/frc/robot/auto/RedL4Auto.java b/src/main/java/frc/robot/auto/RedL4Auto.java index 2353a2a4..f390a331 100644 --- a/src/main/java/frc/robot/auto/RedL4Auto.java +++ b/src/main/java/frc/robot/auto/RedL4Auto.java @@ -48,9 +48,7 @@ public AutoRoutine getAutoRoutine() { redL4AutoRoutine.active().onTrue( Commands.parallel( redCenterToL4G.cmd(), - Commands.sequence( - Commands.waitSeconds(1.0), - elevator.coralL4PositionCG().withTimeout(2.0)))); + elevator.coralL4PositionCG().withTimeout(2.0))); redCenterToL4G.done().onTrue( From ac1571b2d3fa2dbe960509fb3e92a2939f51e02b Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sat, 22 Mar 2025 15:39:17 -0400 Subject: [PATCH 14/24] Tweaked the choro by 3 inches --- src/main/deploy/choreo/blueCenterToL4G.traj | 185 ++++++++++---------- src/main/deploy/choreo/redCenterToL4G.traj | 153 ++++++++-------- 2 files changed, 171 insertions(+), 167 deletions(-) diff --git a/src/main/deploy/choreo/blueCenterToL4G.traj b/src/main/deploy/choreo/blueCenterToL4G.traj index 4672ec6a..de3722c7 100644 --- a/src/main/deploy/choreo/blueCenterToL4G.traj +++ b/src/main/deploy/choreo/blueCenterToL4G.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":7.2, "y":3.8, "heading":3.141592653589793, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.300933361053467, "y":3.7982170581817623, "heading":0.0, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":5.88, "y":3.8, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":6.300933361053467, "y":3.7982170581817623, "heading":0.0, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":5.85, "y":3.7, "heading":3.141592653589793, "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}, @@ -18,8 +18,8 @@ "params":{ "waypoints":[ {"x":{"exp":"blueStartingPose.x", "val":7.2}, "y":{"exp":"blueStartingPose.y", "val":3.8}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.300933361053467 m", "val":6.300933361053467}, "y":{"exp":"3.7982170581817627 m", "val":3.7982170581817623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"blueL4H.x", "val":5.88}, "y":{"exp":"blueL4H.y", "val":3.8}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"6.300933361053467 m", "val":6.300933361053467}, "y":{"exp":"3.7982170581817627 m", "val":3.7982170581817623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"blueL4G.x", "val":5.85}, "y":{"exp":"3.7 m", "val":3.7}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "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}, @@ -34,94 +34,97 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,2.16171,3.75334], + "waypoints":[0.0,2.16185,3.88915], "samples":[ - {"t":0.0, "x":7.2, "y":3.8, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.79963, "ay":-0.00165, "alpha":-0.00048, "fx":[-9.05869,-9.08977,-9.06105,-9.06105], "fy":[-0.02159,-0.01575,-0.01575,-0.02159]}, - {"t":0.04412, "x":7.19922, "y":3.8, "heading":3.14159, "vx":-0.03528, "vy":-0.00007, "omega":-0.00002, "ax":-0.7998, "ay":-0.00165, "alpha":0.00008, "fx":[-9.06402,-9.08171,-9.06628,-9.06628], "fy":[-0.02149,-0.01586,-0.01586,-0.02149]}, - {"t":0.08823, "x":7.19689, "y":3.79999, "heading":3.14159, "vx":-0.07056, "vy":-0.00015, "omega":-0.00002, "ax":-0.79978, "ay":-0.00165, "alpha":0.00074, "fx":[-9.06736,-9.06736,-9.07141,-9.07141], "fy":[-0.02025,-0.01681,-0.01738,-0.02025]}, - {"t":0.13235, "x":7.193, "y":3.79999, "heading":3.14159, "vx":-0.10584, "vy":-0.00022, "omega":0.00001, "ax":-0.79976, "ay":-0.00165, "alpha":-0.00011, "fx":[-9.0628,-9.08396,-9.06495,-9.06495], "fy":[-0.02137,-0.01598,-0.01598,-0.02137]}, - {"t":0.17647, "x":7.18755, "y":3.79997, "heading":3.14159, "vx":-0.14113, "vy":-0.00029, "omega":0.00001, "ax":-0.79974, "ay":-0.00165, "alpha":0.0007, "fx":[-9.06695,-9.06695,-9.07084,-9.07084], "fy":[-0.02017,-0.0169,-0.01745,-0.02017]}, - {"t":0.22058, "x":7.18054, "y":3.79996, "heading":3.14159, "vx":-0.17641, "vy":-0.00036, "omega":0.00004, "ax":-0.79971, "ay":-0.00165, "alpha":-0.00034, "fx":[-9.06114,-9.08673,-9.0632,-9.0632], "fy":[-0.02128,-0.01606,-0.01606,-0.02128]}, - {"t":0.2647, "x":7.17198, "y":3.79994, "heading":-3.14159, "vx":-0.21169, "vy":-0.00044, "omega":0.00003, "ax":-0.79967, "ay":-0.00165, "alpha":0.00067, "fx":[-9.06628,-9.06628,-9.07004,-9.07004], "fy":[-0.0201,-0.01699,-0.0175,-0.0201]}, - {"t":0.30882, "x":7.16187, "y":3.79992, "heading":-3.14159, "vx":-0.24697, "vy":-0.00051, "omega":0.00006, "ax":-0.79963, "ay":-0.00165, "alpha":-0.00068, "fx":[-9.05844,-9.09127,-9.06042,-9.06042], "fy":[-0.02122,-0.01612,-0.01612,-0.02122]}, - {"t":0.35293, "x":7.15019, "y":3.7999, "heading":-3.14159, "vx":-0.28225, "vy":-0.00058, "omega":0.00003, "ax":-0.79957, "ay":-0.00165, "alpha":0.00064, "fx":[-9.06512,-9.06512,-9.06875,-9.06875], "fy":[-0.02002,-0.01707,-0.01756,-0.02002]}, - {"t":0.39705, "x":7.13696, "y":3.79987, "heading":-3.14159, "vx":-0.31752, "vy":-0.00065, "omega":0.00005, "ax":-0.79948, "ay":-0.00165, "alpha":-0.00123, "fx":[-9.05382,-9.09858,-9.05571,-9.05571], "fy":[-0.02116,-0.01618,-0.01618,-0.02116]}, - {"t":0.44117, "x":7.12218, "y":3.79984, "heading":-3.14158, "vx":-0.35279, "vy":-0.00073, "omega":0.0, "ax":-0.79935, "ay":-0.00165, "alpha":0.00061, "fx":[-9.06274,-9.06274,-9.06622,-9.06622], "fy":[-0.01991,-0.0172,-0.01764,-0.01991]}, - {"t":0.48528, "x":7.10584, "y":3.79981, "heading":-3.14158, "vx":-0.38805, "vy":-0.0008, "omega":0.00003, "ax":-0.79913, "ay":-0.00165, "alpha":-0.00229, "fx":[-9.04413,-9.11215,-9.04589,-9.04589], "fy":[-0.02108,-0.01624,-0.01624,-0.02108]}, - {"t":0.5294, "x":7.08794, "y":3.79977, "heading":-3.14158, "vx":-0.42331, "vy":-0.00087, "omega":-0.00007, "ax":-0.7987, "ay":-0.00164, "alpha":0.00054, "fx":[-9.05546,-9.05546,-9.05868,-9.05868], "fy":[-0.01972,-0.01741,-0.01775,-0.01972]}, - {"t":0.57351, "x":7.06849, "y":3.79973, "heading":-3.14159, "vx":-0.45855, "vy":-0.00094, "omega":-0.00005, "ax":-0.79738, "ay":-0.00164, "alpha":-0.00553, "fx":[-9.00638,-9.14634,-9.00788,-9.00788], "fy":[-0.02097,-0.01626,-0.01627,-0.02097]}, - {"t":0.61763, "x":7.04748, "y":3.79969, "heading":-3.14159, "vx":-0.49372, "vy":-0.00102, "omega":-0.00029, "ax":-0.13968, "ay":-0.00029, "alpha":0.00044, "fx":[-1.58264,-1.58264,-1.58529,-1.58529], "fy":[-0.00412,-0.00216,-0.00269,-0.00412]}, - {"t":0.66175, "x":7.02556, "y":3.79964, "heading":3.14158, "vx":-0.49989, "vy":-0.00103, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00336, "fx":[-0.01651,0.05058,-0.01711,-0.01711], "fy":[-0.00135,0.00138,0.00138,-0.00135]}, - {"t":0.70586, "x":7.00351, "y":3.7996, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00031, "fx":[0.00102,0.00102,-0.00102,-0.00102], "fy":[-0.00056,0.00077,0.00036,-0.00056]}, - {"t":0.74998, "x":6.98146, "y":3.79955, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":0.00105, "fx":[-0.00383,0.0125,-0.00434,-0.00434], "fy":[-0.00117,0.00116,0.00116,-0.00117]}, - {"t":0.7941, "x":6.9594, "y":3.7995, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":-0.00007, "ax":0.0, "ay":0.0, "alpha":0.00024, "fx":[0.00082,0.00082,-0.00082,-0.00082], "fy":[-0.0004,0.00057,0.00024,-0.0004]}, - {"t":0.83821, "x":6.93735, "y":3.79946, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":-0.00006, "ax":0.0, "ay":0.0, "alpha":0.0005, "fx":[-0.0011,0.00409,-0.00149,-0.00149], "fy":[-0.00097,0.00097,0.00097,-0.00097]}, - {"t":0.88233, "x":6.9153, "y":3.79941, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[0.00067,0.00067,-0.00067,-0.00067], "fy":[-0.0003,0.00044,0.00017,-0.0003]}, - {"t":0.92645, "x":6.89324, "y":3.79937, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00027, "fx":[-0.00012,0.00094,-0.00041,-0.00041], "fy":[-0.00081,0.00081,0.00081,-0.00081]}, - {"t":0.97056, "x":6.87119, "y":3.79932, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[0.00056,0.00056,-0.00056,-0.00056], "fy":[-0.00023,0.00035,0.00011,-0.00023]}, - {"t":1.01468, "x":6.84914, "y":3.79928, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[0.00004,0.00032,-0.00018,-0.00018], "fy":[-0.00068,0.00067,0.00067,-0.00068]}, - {"t":1.0588, "x":6.82708, "y":3.79923, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[0.00046,0.00046,-0.00046,-0.00046], "fy":[-0.00017,0.00028,0.00007,-0.00017]}, - {"t":1.10291, "x":6.80503, "y":3.79919, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[0.00003,0.0002,-0.00012,-0.00012], "fy":[-0.00056,0.00056,0.00056,-0.00056]}, - {"t":1.14703, "x":6.78298, "y":3.79914, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.00039,0.00039,-0.00039,-0.00039], "fy":[-0.00013,0.00022,0.00004,-0.00013]}, - {"t":1.19115, "x":6.76092, "y":3.7991, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[0.00002,0.00015,-0.00009,-0.00009], "fy":[-0.00047,0.00047,0.00047,-0.00047]}, - {"t":1.23526, "x":6.73887, "y":3.79905, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[0.00032,0.00032,-0.00032,-0.00032], "fy":[-0.00009,0.00017,0.00002,-0.00009]}, - {"t":1.27938, "x":6.71682, "y":3.79901, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.0,0.00012,-0.00006,-0.00006], "fy":[-0.00039,0.00039,0.00039,-0.00039]}, - {"t":1.3235, "x":6.69477, "y":3.79896, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[0.00027,0.00027,-0.00027,-0.00027], "fy":[-0.00006,0.00013,0.0,-0.00006]}, - {"t":1.36761, "x":6.67271, "y":3.79891, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00001,0.00009,-0.00004,-0.00004], "fy":[-0.00033,0.00033,0.00033,-0.00033]}, - {"t":1.41173, "x":6.65066, "y":3.79887, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[0.00023,0.00023,-0.00023,-0.00023], "fy":[-0.00004,0.00009,-0.00002,-0.00004]}, - {"t":1.45585, "x":6.62861, "y":3.79882, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[0.0,0.00002,-0.00001,-0.00001], "fy":[-0.00028,0.00028,0.00028,-0.00028]}, - {"t":1.49996, "x":6.60655, "y":3.79878, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[0.00019,0.00019,-0.00019,-0.00019], "fy":[-0.00002,0.00006,-0.00003,-0.00002]}, - {"t":1.54408, "x":6.5845, "y":3.79873, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.0001,-0.00035,0.00012,0.00012], "fy":[-0.00023,0.00023,0.00023,-0.00023]}, - {"t":1.58819, "x":6.56245, "y":3.79869, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.00016,0.00016,-0.00016,-0.00016], "fy":[0.0,0.00006,-0.00004,0.0]}, - {"t":1.63231, "x":6.54039, "y":3.79864, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[0.0008,-0.00247,0.00083,0.00083], "fy":[-0.00019,0.00021,0.00021,-0.00019]}, - {"t":1.67643, "x":6.51834, "y":3.7986, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.00001, "alpha":0.00002, "fx":[0.00013,0.00013,-0.00013,-0.00013], "fy":[0.00009,0.00008,0.0,0.00009]}, - {"t":1.72054, "x":6.49629, "y":3.79855, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00056, "fx":[0.00335,-0.01014,0.00339,0.00339], "fy":[-0.00023,0.00013,0.00013,-0.00023]}, - {"t":1.76466, "x":6.47423, "y":3.79851, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00008,0.00008,-0.00008,-0.00008], "fy":[0.00026,-0.00051,0.00008,0.00026]}, - {"t":1.80878, "x":6.45218, "y":3.79846, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":-0.00006, "alpha":-0.00133, "fx":[0.00755,-0.02281,0.00763,0.00763], "fy":[-0.0008,-0.00053,-0.00053,-0.0008]}, - {"t":1.85289, "x":6.43013, "y":3.79841, "heading":3.14158, "vx":-0.49989, "vy":-0.00103, "omega":-0.00004, "ax":0.00002, "ay":0.00101, "alpha":-0.00032, "fx":[0.00021,0.00021,0.00021,0.00021], "fy":[0.01282,0.00777,0.01245,0.01282]}, - {"t":1.89701, "x":6.40807, "y":3.79837, "heading":3.14157, "vx":-0.49988, "vy":-0.00099, "omega":-0.00006, "ax":0.53753, "ay":0.00627, "alpha":0.00014, "fx":[6.09398,6.09883,6.09456,6.09456], "fy":[0.07126,0.07101,0.07101,0.07126]}, - {"t":1.94113, "x":6.38654, "y":3.79833, "heading":3.14157, "vx":-0.47617, "vy":-0.00071, "omega":-0.00005, "ax":0.79743, "ay":0.00171, "alpha":-0.00013, "fx":[9.04256,9.04256,9.04287,9.04287], "fy":[0.01981,0.0187,0.01915,0.01981]}, - {"t":1.98524, "x":6.36631, "y":3.7983, "heading":3.14157, "vx":-0.44099, "vy":-0.00063, "omega":-0.00006, "ax":0.79871, "ay":0.00168, "alpha":0.00338, "fx":[9.03682,9.11695,9.03756,9.03756], "fy":[0.01953,0.01853,0.01853,0.01953]}, - {"t":2.02936, "x":6.34763, "y":3.79828, "heading":3.14157, "vx":-0.40575, "vy":-0.00056, "omega":0.00009, "ax":0.79914, "ay":0.00167, "alpha":-0.00015, "fx":[9.06185,9.06185,9.06232,9.06232], "fy":[0.01937,0.01831,0.01861,0.01937]}, - {"t":2.07348, "x":6.33051, "y":3.79825, "heading":3.14157, "vx":-0.3705, "vy":-0.00049, "omega":0.00009, "ax":0.79935, "ay":0.00166, "alpha":0.00205, "fx":[9.05175,9.10149,9.05241,9.05241], "fy":[0.01929,0.01843,0.01843,0.01929]}, - {"t":2.11759, "x":6.31494, "y":3.79823, "heading":3.14157, "vx":-0.33523, "vy":-0.00041, "omega":0.00018, "ax":0.79948, "ay":0.00166, "alpha":-0.00013, "fx":[9.06579,9.06579,9.06617,9.06617], "fy":[0.01921,0.01831,0.01857,0.01921]}, - {"t":2.16171, "x":6.30093, "y":3.79822, "heading":3.14158, "vx":-0.29996, "vy":-0.00034, "omega":0.00017, "ax":0.00077, "ay":0.03476, "alpha":-0.00117, "fx":[0.01474,-0.01007,0.01514,0.01514], "fy":[0.39431,0.39402,0.39402,0.39431]}, - {"t":2.20592, "x":6.28767, "y":3.79824, "heading":3.14159, "vx":-0.29993, "vy":0.0012, "omega":0.00012, "ax":0.00001, "ay":0.00239, "alpha":-0.00101, "fx":[0.00002,0.00002,0.00021,0.00021], "fy":[0.03125,0.01496,0.03085,0.03125]}, - {"t":2.25013, "x":6.27441, "y":3.79829, "heading":-3.14159, "vx":-0.29993, "vy":0.0013, "omega":0.00008, "ax":0.0, "ay":-0.00007, "alpha":-0.00027, "fx":[0.00101,-0.00365,0.00131,0.00131], "fy":[-0.0006,-0.00091,-0.00091,-0.0006]}, - {"t":2.29435, "x":6.26115, "y":3.79835, "heading":-3.14159, "vx":-0.29993, "vy":0.0013, "omega":0.00006, "ax":0.0, "ay":0.00001, "alpha":-0.00008, "fx":[-0.00012,-0.00012,0.00012,0.00012], "fy":[0.00039,-0.00017,-0.00001,0.00039]}, - {"t":2.33856, "x":6.24789, "y":3.79841, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.00003,-0.00042,0.00023,0.00023], "fy":[0.00021,-0.00012,-0.00012,0.00021]}, - {"t":2.38277, "x":6.23463, "y":3.79846, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.0002,-0.00022,-0.00013,0.0002]}, - {"t":2.42698, "x":6.22137, "y":3.79852, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[-0.00011,-0.00014,0.00012,0.00012], "fy":[0.00017,-0.00018,-0.00018,0.00017]}, - {"t":2.47119, "x":6.20811, "y":3.79858, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00014,-0.00014,0.00014,0.00014], "fy":[0.00017,-0.00023,-0.00012,0.00017]}, - {"t":2.5154, "x":6.19485, "y":3.79864, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.0001,-0.00012,0.00011,0.00011], "fy":[0.0002,-0.0002,-0.0002,0.0002]}, - {"t":2.55962, "x":6.18159, "y":3.79869, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00016,-0.00016,0.00016,0.00016], "fy":[0.00016,-0.0002,-0.00011,0.00016]}, - {"t":2.60383, "x":6.16833, "y":3.79875, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.0001,-0.00011,0.00011,0.00011], "fy":[0.00024,-0.00024,-0.00024,0.00024]}, - {"t":2.64804, "x":6.15507, "y":3.79881, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.0002,-0.0002,0.0002,0.0002], "fy":[0.00016,-0.00019,-0.00012,0.00016]}, - {"t":2.69225, "x":6.14181, "y":3.79887, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[-0.0001,-0.00012,0.00011,0.00011], "fy":[0.00029,-0.00029,-0.00029,0.00029]}, - {"t":2.73646, "x":6.12855, "y":3.79892, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.00024,-0.00024,0.00024,0.00024], "fy":[0.00017,-0.0002,-0.00013,0.00017]}, - {"t":2.78068, "x":6.11529, "y":3.79898, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00011,-0.00014,0.00013,0.00013], "fy":[0.00035,-0.00035,-0.00035,0.00035]}, - {"t":2.82489, "x":6.10203, "y":3.79904, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00029,-0.00029,0.00029,0.00029], "fy":[0.00019,-0.00022,-0.00015,0.00019]}, - {"t":2.8691, "x":6.08877, "y":3.7991, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00013, "fx":[-0.00012,-0.00018,0.00015,0.00015], "fy":[0.00042,-0.00042,-0.00042,0.00042]}, - {"t":2.91331, "x":6.07551, "y":3.79915, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00012, "fx":[-0.00035,-0.00035,0.00035,0.00035], "fy":[0.00022,-0.00027,-0.00018,0.00022]}, - {"t":2.95752, "x":6.06224, "y":3.79921, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[-0.00015,-0.00023,0.00019,0.00019], "fy":[0.00052,-0.00052,-0.00052,0.00052]}, - {"t":3.00174, "x":6.04898, "y":3.79927, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[-0.00043,-0.00043,0.00043,0.00043], "fy":[0.00027,-0.00033,-0.00022,0.00027]}, - {"t":3.04595, "x":6.03572, "y":3.79933, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[-0.00018,-0.00029,0.00023,0.00023], "fy":[0.00064,-0.00064,-0.00064,0.00064]}, - {"t":3.09016, "x":6.02246, "y":3.79938, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[-0.00053,-0.00053,0.00053,0.00053], "fy":[0.00034,-0.0004,-0.00028,0.00034]}, - {"t":3.13437, "x":6.0092, "y":3.79944, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[-0.00012,-0.00067,0.0004,0.0004], "fy":[0.00078,-0.00078,-0.00078,0.00078]}, - {"t":3.17858, "x":5.99594, "y":3.7995, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00022, "fx":[-0.00065,-0.00065,0.00065,0.00065], "fy":[0.00043,-0.0005,-0.00036,0.00043]}, - {"t":3.22279, "x":5.98268, "y":3.79956, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00058, "fx":[0.00136,-0.00535,0.00199,0.00199], "fy":[0.00095,-0.00095,-0.00095,0.00095]}, - {"t":3.26701, "x":5.96942, "y":3.79961, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00007, "ax":0.0, "ay":0.0, "alpha":-0.00027, "fx":[-0.00081,-0.00081,0.00081,0.00081], "fy":[0.00053,-0.00059,-0.00047,0.00053]}, - {"t":3.31122, "x":5.95616, "y":3.79967, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00222, "fx":[0.01032,-0.03224,0.01104,0.01104], "fy":[0.0011,-0.00109,-0.00109,0.0011]}, - {"t":3.35543, "x":5.9429, "y":3.79973, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":-0.00018, "ax":0.38814, "ay":-0.00168, "alpha":-0.00036, "fx":[4.40038,4.40038,4.40245,4.40245], "fy":[-0.01832,-0.01993,-0.01971,-0.01832]}, - {"t":3.39964, "x":5.93002, "y":3.79978, "heading":-3.14158, "vx":-0.28277, "vy":0.00123, "omega":-0.00019, "ax":0.79846, "ay":-0.00346, "alpha":0.00359, "fx":[9.03037,9.12354,9.03177,9.03177], "fy":[-0.03745,-0.04101,-0.04101,-0.03745]}, - {"t":3.44385, "x":5.9183, "y":3.79983, "heading":-3.14159, "vx":-0.24747, "vy":0.00107, "omega":-0.00003, "ax":0.79922, "ay":-0.00346, "alpha":-0.00046, "fx":[9.06181,9.06181,9.0643,9.0643], "fy":[-0.03827,-0.04048,-0.04005,-0.03827]}, - {"t":3.48807, "x":5.90814, "y":3.79988, "heading":-3.14159, "vx":-0.21213, "vy":0.00092, "omega":-0.00005, "ax":0.79948, "ay":-0.00346, "alpha":0.00134, "fx":[9.05442,9.09751,9.05596,9.05596], "fy":[-0.03742,-0.04114,-0.04114,-0.03742]}, - {"t":3.53228, "x":5.89954, "y":3.79992, "heading":-3.14159, "vx":-0.17679, "vy":0.00077, "omega":0.0, "ax":0.79961, "ay":-0.00346, "alpha":-0.0005, "fx":[9.06605,9.06605,9.06878,9.06878], "fy":[-0.0382,-0.04054,-0.0402,-0.0382]}, - {"t":3.57649, "x":5.89251, "y":3.79995, "heading":-3.14159, "vx":-0.14143, "vy":0.00061, "omega":-0.00002, "ax":0.79969, "ay":-0.00346, "alpha":0.0006, "fx":[9.06067,9.08784,9.0623,9.0623], "fy":[-0.03733,-0.04124,-0.04124,-0.03733]}, - {"t":3.6207, "x":5.88703, "y":3.79997, "heading":-3.14159, "vx":-0.10608, "vy":0.00046, "omega":0.00001, "ax":0.79974, "ay":-0.00346, "alpha":-0.00054, "fx":[9.0674,9.0674,9.07033,9.07033], "fy":[-0.03813,-0.04061,-0.0403,-0.03813]}, - {"t":3.66491, "x":5.88313, "y":3.79999, "heading":-3.14159, "vx":-0.07072, "vy":0.00031, "omega":-0.00001, "ax":0.79977, "ay":-0.00347, "alpha":0.00024, "fx":[9.06346,9.08325,9.0652,9.0652], "fy":[-0.03723,-0.04136,-0.04136,-0.03723]}, - {"t":3.70912, "x":5.88078, "y":3.8, "heading":-3.14159, "vx":-0.03536, "vy":0.00015, "omega":0.0, "ax":0.7998, "ay":-0.00347, "alpha":0.0001, "fx":[9.06439,9.0816,9.06618,9.06618], "fy":[-0.03717,-0.04142,-0.04142,-0.03717]}, - {"t":3.75334, "x":5.88, "y":3.8, "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]}], + {"t":0.0, "x":7.2, "y":3.8, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.79963, "ay":0.00144, "alpha":-0.00002, "fx":[-9.08898,-9.02005,-9.08078,-9.08078], "fy":[0.02625,0.00639,0.00639,0.02625]}, + {"t":0.04412, "x":7.19922, "y":3.8, "heading":3.14159, "vx":-0.03528, "vy":0.00006, "omega":0.0, "ax":-0.7998, "ay":0.00144, "alpha":-0.00131, "fx":[-9.08325,-9.04469,-9.07519,-9.07519], "fy":[0.02604,0.00661,0.00661,0.02604]}, + {"t":0.08824, "x":7.19689, "y":3.80001, "heading":3.14159, "vx":-0.07057, "vy":0.00013, "omega":-0.00006, "ax":-0.79978, "ay":0.00144, "alpha":-0.00186, "fx":[-9.07656,-9.07656,-9.06222,-9.06222], "fy":[0.0188,0.01949,0.00819,0.0188]}, + {"t":0.13236, "x":7.193, "y":3.80001, "heading":3.14159, "vx":-0.10585, "vy":0.00019, "omega":-0.00014, "ax":-0.79976, "ay":0.00144, "alpha":-0.00092, "fx":[-9.08463,-9.03867,-9.07668,-9.07668], "fy":[0.02585,0.00679,0.00679,0.02585]}, + {"t":0.17648, "x":7.18755, "y":3.80002, "heading":3.14158, "vx":-0.14114, "vy":0.00025, "omega":-0.00018, "ax":-0.79974, "ay":0.00144, "alpha":-0.00184, "fx":[-9.07596,-9.07596,-9.06184,-9.06184], "fy":[0.01877,0.01949,0.00825,0.01877]}, + {"t":0.2206, "x":7.18054, "y":3.80004, "heading":3.14158, "vx":-0.17642, "vy":0.00032, "omega":-0.00026, "ax":-0.79971, "ay":0.00144, "alpha":-0.00051, "fx":[-9.08625,-9.03145,-9.0783,-9.0783], "fy":[0.02578,0.00686,0.00686,0.02578]}, + {"t":0.26472, "x":7.17198, "y":3.80005, "heading":3.14156, "vx":-0.2117, "vy":0.00038, "omega":-0.00029, "ax":-0.79968, "ay":0.00144, "alpha":-0.00184, "fx":[-9.07519,-9.07519,-9.06114,-9.06114], "fy":[0.01882,0.0194,0.00824,0.01882]}, + {"t":0.30884, "x":7.16186, "y":3.80007, "heading":3.14155, "vx":-0.24698, "vy":0.00044, "omega":-0.00037, "ax":-0.79963, "ay":0.00144, "alpha":0.00021, "fx":[-9.08957,-9.01798,-9.08151,-9.08151], "fy":[0.02582,0.00681,0.00681,0.02582]}, + {"t":0.35295, "x":7.15019, "y":3.80009, "heading":3.14154, "vx":-0.28226, "vy":0.00051, "omega":-0.00036, "ax":-0.79957, "ay":0.00144, "alpha":-0.00187, "fx":[-9.07399,-9.07399,-9.05989,-9.05989], "fy":[0.01893,0.01921,0.00819,0.01893]}, + {"t":0.39707, "x":7.13695, "y":3.80011, "heading":3.14152, "vx":-0.31754, "vy":0.00057, "omega":-0.00044, "ax":-0.79948, "ay":0.00144, "alpha":0.00146, "fx":[-9.09528,-8.99454,-9.08701,-9.08701], "fy":[0.02596,0.00667,0.00667,0.02596]}, + {"t":0.44119, "x":7.12217, "y":3.80014, "heading":3.1415, "vx":-0.35281, "vy":0.00063, "omega":-0.00038, "ax":-0.79935, "ay":0.00144, "alpha":-0.00193, "fx":[-9.07159,-9.07159,-9.05738,-9.05738], "fy":[0.01911,0.01892,0.00811,0.01911]}, + {"t":0.48531, "x":7.10582, "y":3.80017, "heading":3.14148, "vx":-0.38808, "vy":0.0007, "omega":-0.00046, "ax":-0.79913, "ay":0.00144, "alpha":0.00413, "fx":[-9.10696,-8.94434,-9.09839,-9.09839], "fy":[0.02617,0.00645,0.00645,0.02617]}, + {"t":0.52943, "x":7.08792, "y":3.8002, "heading":3.14146, "vx":-0.42334, "vy":0.00076, "omega":-0.00028, "ax":-0.7987, "ay":0.00144, "alpha":-0.00198, "fx":[-9.06419,-9.06419,-9.04996,-9.04996], "fy":[0.01931,0.01855,0.00802,0.01931]}, + {"t":0.57355, "x":7.06847, "y":3.80024, "heading":3.14145, "vx":-0.45858, "vy":0.00083, "omega":-0.00037, "ax":-0.79738, "ay":0.00144, "alpha":0.0238, "fx":[-9.20005,-8.5875,-9.19047,-9.19047], "fy":[0.02715,0.0054,0.0054,0.02715]}, + {"t":0.61767, "x":7.04746, "y":3.80027, "heading":3.14144, "vx":-0.49375, "vy":0.00089, "omega":0.00068, "ax":-0.13895, "ay":0.00024, "alpha":-0.00208, "fx":[-1.58234,-1.58234,-1.56896,-1.56896], "fy":[0.00653,0.00252,-0.00449,0.00653]}, + {"t":0.66179, "x":7.02554, "y":3.80031, "heading":3.14147, "vx":-0.49989, "vy":0.0009, "omega":0.00059, "ax":0.0, "ay":0.0, "alpha":0.0101, "fx":[-0.07792,0.21555,-0.06888,-0.06888], "fy":[0.00906,-0.00899,-0.00899,0.00906]}, + {"t":0.70591, "x":7.00349, "y":3.80035, "heading":3.14149, "vx":-0.49989, "vy":0.0009, "omega":0.00104, "ax":0.0, "ay":0.00001, "alpha":-0.0021, "fx":[-0.00567,-0.00567,0.00567,0.00567], "fy":[0.00467,-0.00414,-0.00497,0.00467]}, + {"t":0.75003, "x":6.98143, "y":3.80039, "heading":3.14154, "vx":-0.49989, "vy":0.0009, "omega":0.00095, "ax":0.0, "ay":0.0, "alpha":-0.00188, "fx":[-0.00503,0.00201,0.00151,0.00151], "fy":[0.00675,-0.00686,-0.00686,0.00675]}, + {"t":0.79415, "x":6.95938, "y":3.80043, "heading":3.14158, "vx":-0.49989, "vy":0.0009, "omega":0.00086, "ax":0.0, "ay":0.0, "alpha":-0.00172, "fx":[-0.00456,-0.00456,0.00456,0.00456], "fy":[0.00386,-0.00367,-0.00394,0.00386]}, + {"t":0.83827, "x":6.93732, "y":3.80047, "heading":-3.14157, "vx":-0.49989, "vy":0.0009, "omega":0.00079, "ax":0.0, "ay":0.0, "alpha":-0.00201, "fx":[-0.00105,-0.00721,0.00413,0.00413], "fy":[0.00534,-0.00538,-0.00538,0.00534]}, + {"t":0.88239, "x":6.91527, "y":3.80051, "heading":-3.14153, "vx":-0.49989, "vy":0.0009, "omega":0.0007, "ax":0.0, "ay":0.0, "alpha":-0.00138, "fx":[-0.00355,-0.00356,0.00355,0.00356], "fy":[0.00315,-0.00323,-0.003,0.00315]}, + {"t":0.92651, "x":6.89321, "y":3.80055, "heading":-3.1415, "vx":-0.49989, "vy":0.0009, "omega":0.00064, "ax":0.0, "ay":0.0, "alpha":-0.00152, "fx":[-0.00098,-0.00512,0.00305,0.00305], "fy":[0.00409,-0.00412,-0.00412,0.00409]}, + {"t":0.97063, "x":6.87116, "y":3.80059, "heading":-3.14147, "vx":-0.49989, "vy":0.0009, "omega":0.00057, "ax":0.0, "ay":0.0, "alpha":-0.00102, "fx":[-0.00265,-0.00265,0.00265,0.00265], "fy":[0.00234,-0.00229,-0.00236,0.00234]}, + {"t":1.01475, "x":6.8491, "y":3.80063, "heading":-3.14145, "vx":-0.49989, "vy":0.0009, "omega":0.00053, "ax":0.0, "ay":0.0, "alpha":-0.00105, "fx":[-0.00102,-0.00293,0.00198,0.00198], "fy":[0.00296,-0.00297,-0.00297,0.00296]}, + {"t":1.05886, "x":6.82705, "y":3.80067, "heading":-3.14143, "vx":-0.49989, "vy":0.0009, "omega":0.00048, "ax":0.0, "ay":0.0, "alpha":-0.00069, "fx":[-0.00183,-0.00183,0.00183,0.00183], "fy":[0.00153,-0.0012,-0.00184,0.00153]}, + {"t":1.10298, "x":6.80499, "y":3.80071, "heading":-3.14141, "vx":-0.49989, "vy":0.0009, "omega":0.00045, "ax":0.0, "ay":0.0, "alpha":-0.00067, "fx":[-0.00081,-0.00167,0.00124,0.00124], "fy":[0.0019,-0.00191,-0.00191,0.0019]}, + {"t":1.1471, "x":6.78294, "y":3.80075, "heading":-3.14139, "vx":-0.49989, "vy":0.0009, "omega":0.00042, "ax":0.0, "ay":0.0, "alpha":-0.00036, "fx":[-0.00105,-0.00105,0.00105,0.00105], "fy":[0.00072,-0.00002,-0.00142,0.00072]}, + {"t":1.19122, "x":6.76089, "y":3.80079, "heading":-3.14137, "vx":-0.49989, "vy":0.0009, "omega":0.0004, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[-0.0005,-0.0008,0.00065,0.00065], "fy":[0.00089,-0.0009,-0.0009,0.00089]}, + {"t":1.23534, "x":6.73883, "y":3.80083, "heading":-3.14135, "vx":-0.49989, "vy":0.0009, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.0003,-0.0003,0.0003,0.0003], "fy":[-0.0001,0.00128,-0.00107,-0.0001]}, + {"t":1.27946, "x":6.71678, "y":3.80087, "heading":-3.14133, "vx":-0.49989, "vy":0.0009, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00018,-0.00004,0.00011,0.00011], "fy":[-0.00012,0.00008,0.00008,-0.00012]}, + {"t":1.32358, "x":6.69472, "y":3.80091, "heading":-3.14131, "vx":-0.49989, "vy":0.0009, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":0.0003, "fx":[0.00044,0.00044,-0.00044,-0.00044], "fy":[-0.0009,0.00258,-0.00072,-0.0009]}, + {"t":1.3677, "x":6.67267, "y":3.80095, "heading":-3.1413, "vx":-0.49989, "vy":0.0009, "omega":0.0004, "ax":0.0, "ay":-0.00001, "alpha":0.00034, "fx":[0.00013,0.0008,-0.00046,-0.00046], "fy":[-0.00116,0.00104,0.00104,-0.00116]}, + {"t":1.41182, "x":6.65061, "y":3.80099, "heading":-3.14128, "vx":-0.49989, "vy":0.0009, "omega":0.00042, "ax":0.0, "ay":0.0, "alpha":0.00055, "fx":[0.00121,0.00121,-0.00121,-0.00121], "fy":[-0.00137,0.00291,0.00001,-0.00137]}, + {"t":1.45594, "x":6.62856, "y":3.80103, "heading":-3.14126, "vx":-0.49989, "vy":0.0009, "omega":0.00044, "ax":0.0, "ay":-0.00002, "alpha":0.00071, "fx":[0.00035,0.00195,-0.00115,-0.00115], "fy":[-0.00233,0.00197,0.00197,-0.00233]}, + {"t":1.50006, "x":6.6065, "y":3.80107, "heading":-3.14124, "vx":-0.49989, "vy":0.0009, "omega":0.00047, "ax":0.0, "ay":0.00002, "alpha":0.00042, "fx":[0.00204,0.00204,-0.00204,-0.00204], "fy":[0.00002,-0.00175,0.00268,0.00002]}, + {"t":1.54418, "x":6.58445, "y":3.80111, "heading":-3.14122, "vx":-0.49989, "vy":0.0009, "omega":0.00049, "ax":0.0, "ay":-0.00003, "alpha":0.00115, "fx":[0.00036,0.00385,-0.00211,-0.00211], "fy":[-0.00363,0.00287,0.00287,-0.00363]}, + {"t":1.5883, "x":6.56239, "y":3.80115, "heading":-3.1412, "vx":-0.49989, "vy":0.0009, "omega":0.00054, "ax":0.0, "ay":0.0001, "alpha":-0.00131, "fx":[0.00299,0.00299,-0.00298,-0.00298], "fy":[0.00887,-0.02632,0.01303,0.00887]}, + {"t":1.63242, "x":6.54034, "y":3.80119, "heading":-3.14118, "vx":-0.49989, "vy":0.0009, "omega":0.00048, "ax":0.0, "ay":-0.00016, "alpha":0.00162, "fx":[0.00022,0.00624,-0.00324,-0.00324], "fy":[-0.00623,0.00256,0.00256,-0.00623]}, + {"t":1.67654, "x":6.51828, "y":3.80123, "heading":-3.14115, "vx":-0.49989, "vy":0.0009, "omega":0.00055, "ax":0.0, "ay":0.0002, "alpha":-0.00828, "fx":[0.00415,0.00415,-0.00415,-0.00415], "fy":[0.04033,-0.11775,0.04632,0.04033]}, + {"t":1.72066, "x":6.49623, "y":3.80127, "heading":-3.14113, "vx":-0.49989, "vy":0.0009, "omega":0.00019, "ax":0.0, "ay":-0.00074, "alpha":0.00161, "fx":[0.00252,0.00106,-0.00182,-0.00182], "fy":[-0.01386,-0.00303,-0.00303,-0.01386]}, + {"t":1.76477, "x":6.47418, "y":3.80131, "heading":-3.14112, "vx":-0.49989, "vy":0.00087, "omega":0.00026, "ax":0.0, "ay":0.00033, "alpha":-0.02495, "fx":[0.00546,0.00547,-0.00545,-0.00545], "fy":[0.11303,-0.33216,0.12095,0.11303]}, + {"t":1.80889, "x":6.45212, "y":3.80134, "heading":-3.14111, "vx":-0.49989, "vy":0.00089, "omega":-0.00084, "ax":0.0, "ay":0.00068, "alpha":-0.00449, "fx":[0.03826,-0.10474,0.03327,0.03327], "fy":[0.00167,0.01368,0.01368,0.00167]}, + {"t":1.85301, "x":6.43007, "y":3.80138, "heading":-3.14115, "vx":-0.49989, "vy":0.00092, "omega":-0.00104, "ax":0.00005, "ay":-0.05361, "alpha":0.0061, "fx":[0.00443,0.00443,-0.00319,-0.00319], "fy":[-0.63075,-0.54391,-0.62616,-0.63075]}, + {"t":1.89713, "x":6.40801, "y":3.80137, "heading":-3.14119, "vx":-0.49988, "vy":-0.00145, "omega":-0.00077, "ax":0.54381, "ay":-0.25032, "alpha":0.02315, "fx":[6.04389,6.53977,6.04158,6.04158], "fy":[-2.84242,-2.83469,-2.83474,-2.84242]}, + {"t":1.94125, "x":6.38649, "y":3.80106, "heading":-3.14123, "vx":-0.47589, "vy":-0.01249, "omega":0.00025, "ax":0.79742, "ay":-0.00526, "alpha":-0.00774, "fx":[9.04687,9.04687,9.03834,9.03834], "fy":[-0.02387,-0.17261,-0.01846,-0.02386]}, + {"t":1.98537, "x":6.36627, "y":3.80051, "heading":-3.14122, "vx":-0.44071, "vy":-0.01273, "omega":-0.00009, "ax":0.79871, "ay":-0.00338, "alpha":-0.00683, "fx":[9.10705,8.91537,9.10315,9.10315], "fy":[-0.04403,-0.03273,-0.03273,-0.04403]}, + {"t":2.02949, "x":6.3476, "y":3.79994, "heading":-3.14122, "vx":-0.40547, "vy":-0.01287, "omega":-0.00039, "ax":0.79914, "ay":-0.00275, "alpha":-0.00316, "fx":[9.06602,9.06602,9.05809,9.05809], "fy":[-0.01488,-0.08497,-0.00994,-0.01488]}, + {"t":2.07361, "x":6.33049, "y":3.79937, "heading":-3.14124, "vx":-0.37021, "vy":-0.013, "omega":-0.00053, "ax":0.79935, "ay":-0.00243, "alpha":-0.0034, "fx":[9.09391,8.98329,9.0904,9.0904], "fy":[-0.03259,-0.02247,-0.02247,-0.03259]}, + {"t":2.11773, "x":6.31493, "y":3.7988, "heading":-3.14126, "vx":-0.33495, "vy":-0.0131, "omega":-0.00068, "ax":0.79948, "ay":-0.00223, "alpha":-0.002, "fx":[9.06946,9.06946,9.06247,9.06247], "fy":[-0.01429,-0.06277,-0.00998,-0.01429]}, + {"t":2.16185, "x":6.30093, "y":3.79822, "heading":-3.14129, "vx":-0.29967, "vy":-0.0132, "omega":-0.00077, "ax":0.07594, "ay":-0.75118, "alpha":0.00006, "fx":[0.86928,0.84252,0.8664,0.8664], "fy":[-8.52242,-8.51403,-8.51403,-8.52241]}, + {"t":2.20614, "x":6.28774, "y":3.7969, "heading":-3.14132, "vx":-0.29631, "vy":-0.04647, "omega":-0.00077, "ax":0.07594, "ay":-0.40381, "alpha":0.04572, "fx":[0.86213,0.86215,0.86012,0.86012], "fy":[-4.77114,-4.00338,-4.7707,-4.77114]}, + {"t":2.25043, "x":6.27469, "y":3.79444, "heading":-3.14136, "vx":-0.29295, "vy":-0.06436, "omega":0.00126, "ax":0.00373, "ay":-0.01687, "alpha":0.0064, "fx":[0.01462,0.13115,0.0117,0.0117], "fy":[-0.19542,-0.18729,-0.18729,-0.19542]}, + {"t":2.29472, "x":6.26172, "y":3.79157, "heading":-3.1413, "vx":-0.29278, "vy":-0.0651, "omega":0.00154, "ax":0.00005, "ay":-0.00024, "alpha":-0.04874, "fx":[0.00646,0.00646,-0.00523,-0.00523], "fy":[0.20716,-0.64086,0.2155,0.20716]}, + {"t":2.33901, "x":6.24875, "y":3.78869, "heading":-3.14123, "vx":-0.29278, "vy":-0.06511, "omega":-0.00062, "ax":0.00004, "ay":-0.0002, "alpha":0.00343, "fx":[-0.00913,0.03631,-0.01258,-0.01258], "fy":[-0.00691,0.00237,0.00237,-0.00691]}, + {"t":2.3833, "x":6.23578, "y":3.78581, "heading":-3.14126, "vx":-0.29278, "vy":-0.06512, "omega":-0.00046, "ax":-0.00001, "ay":0.00006, "alpha":-0.00489, "fx":[0.00345,0.00345,-0.00376,-0.00376], "fy":[0.02404,-0.07415,0.02884,0.02404]}, + {"t":2.42759, "x":6.22281, "y":3.78292, "heading":-3.14128, "vx":-0.29278, "vy":-0.06512, "omega":-0.00068, "ax":0.0, "ay":-0.00002, "alpha":0.00123, "fx":[0.00156,0.00149,-0.00143,-0.00143], "fy":[-0.00432,0.00388,0.00388,-0.00432]}, + {"t":2.47188, "x":6.20985, "y":3.78004, "heading":-3.14131, "vx":-0.29278, "vy":-0.06512, "omega":-0.00063, "ax":0.0, "ay":0.00001, "alpha":0.00118, "fx":[0.0028,0.0028,-0.00283,-0.00283], "fy":[-0.00278,0.00508,0.00075,-0.00278]}, + {"t":2.51617, "x":6.19688, "y":3.77715, "heading":-3.14134, "vx":-0.29278, "vy":-0.06512, "omega":-0.00057, "ax":0.0, "ay":0.0, "alpha":0.00092, "fx":[0.00186,-0.00071,-0.00055,-0.00055], "fy":[-0.00349,0.00341,0.00341,-0.00349]}, + {"t":2.56046, "x":6.18391, "y":3.77427, "heading":-3.14137, "vx":-0.29278, "vy":-0.06512, "omega":-0.00053, "ax":0.0, "ay":0.0, "alpha":0.00124, "fx":[0.00235,0.00236,-0.00236,-0.00236], "fy":[-0.00344,0.00752,-0.00061,-0.00344]}, + {"t":2.60475, "x":6.17095, "y":3.77139, "heading":-3.14139, "vx":-0.29278, "vy":-0.06512, "omega":-0.00048, "ax":0.0, "ay":0.0, "alpha":0.0008, "fx":[0.00129,0.00001,-0.00065,-0.00065], "fy":[-0.0029,0.00291,0.00291,-0.0029]}, + {"t":2.64904, "x":6.15798, "y":3.7685, "heading":-3.14141, "vx":-0.29278, "vy":-0.06512, "omega":-0.00044, "ax":0.0, "ay":0.0, "alpha":0.00087, "fx":[0.002,0.002,-0.002,-0.002], "fy":[-0.00218,0.00425,0.00011,-0.00218]}, + {"t":2.69333, "x":6.14501, "y":3.76562, "heading":-3.14143, "vx":-0.29278, "vy":-0.06512, "omega":-0.00041, "ax":0.0, "ay":0.0, "alpha":0.0007, "fx":[0.00088,0.00045,-0.00067,-0.00067], "fy":[-0.00245,0.00246,0.00246,-0.00245]}, + {"t":2.73762, "x":6.13205, "y":3.76273, "heading":-3.14145, "vx":-0.29278, "vy":-0.06512, "omega":-0.00037, "ax":0.0, "ay":0.0, "alpha":0.00062, "fx":[0.0017,0.0017,-0.0017,-0.0017], "fy":[-0.00134,0.00218,0.00049,-0.00134]}, + {"t":2.7819, "x":6.11908, "y":3.75985, "heading":-3.14146, "vx":-0.29278, "vy":-0.06512, "omega":-0.00035, "ax":0.0, "ay":0.0, "alpha":0.0006, "fx":[0.00062,0.00056,-0.0006,-0.0006], "fy":[-0.00208,0.00209,0.00209,-0.00208]}, + {"t":2.82619, "x":6.10611, "y":3.75697, "heading":-3.14148, "vx":-0.29278, "vy":-0.06512, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00046, "fx":[0.00145,0.00145,-0.00145,-0.00145], "fy":[-0.00087,0.00116,0.00056,-0.00087]}, + {"t":2.87048, "x":6.09314, "y":3.75408, "heading":-3.14149, "vx":-0.29278, "vy":-0.06512, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00051, "fx":[0.00045,0.00052,-0.00049,-0.00049], "fy":[-0.00178,0.00179,0.00179,-0.00178]}, + {"t":2.91477, "x":6.08018, "y":3.7512, "heading":-3.14151, "vx":-0.29278, "vy":-0.06512, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00036, "fx":[0.00124,0.00124,-0.00124,-0.00124], "fy":[-0.0006,0.00067,0.00049,-0.0006]}, + {"t":2.95906, "x":6.06721, "y":3.74831, "heading":-3.14152, "vx":-0.29278, "vy":-0.06512, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00043, "fx":[0.00031,0.00044,-0.00038,-0.00038], "fy":[-0.00153,0.00154,0.00154,-0.00153]}, + {"t":3.00335, "x":6.05424, "y":3.74543, "heading":-3.14153, "vx":-0.29278, "vy":-0.06512, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":0.00029, "fx":[0.00107,0.00108,-0.00107,-0.00107], "fy":[-0.00041,0.00043,0.00037,-0.00041]}, + {"t":3.04764, "x":6.04128, "y":3.74254, "heading":-3.14154, "vx":-0.29278, "vy":-0.06512, "omega":-0.00023, "ax":0.0, "ay":0.0, "alpha":0.00037, "fx":[0.00018,0.00041,-0.0003,-0.0003], "fy":[-0.00132,0.00134,0.00134,-0.00132]}, + {"t":3.09193, "x":6.02831, "y":3.73966, "heading":-3.14155, "vx":-0.29278, "vy":-0.06512, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00023, "fx":[0.00094,0.00094,-0.00094,-0.00094], "fy":[-0.00028,0.00029,0.00024,-0.00028]}, + {"t":3.13622, "x":6.01534, "y":3.73678, "heading":-3.14156, "vx":-0.29278, "vy":-0.06512, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[-0.00001,0.00058,-0.00029,-0.00029], "fy":[-0.00116,0.00117,0.00118,-0.00116]}, + {"t":3.18051, "x":6.00237, "y":3.73389, "heading":-3.14157, "vx":-0.29278, "vy":-0.06512, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[0.00083,0.00083,-0.00083,-0.00083], "fy":[-0.00017,0.0002,0.0001,-0.00017]}, + {"t":3.2248, "x":5.98941, "y":3.73101, "heading":-3.14158, "vx":-0.29278, "vy":-0.06512, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[-0.00038,0.00136,-0.00049,-0.00049], "fy":[-0.00104,0.00105,0.00105,-0.00104]}, + {"t":3.26909, "x":5.97644, "y":3.72812, "heading":-3.14159, "vx":-0.29278, "vy":-0.06512, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[0.00076,0.00076,-0.00076,-0.00076], "fy":[-0.00007,0.00014,-0.00002,-0.00007]}, + {"t":3.31338, "x":5.96347, "y":3.72524, "heading":3.14159, "vx":-0.29278, "vy":-0.06512, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00045, "fx":[-0.00129,0.00375,-0.00123,-0.00123], "fy":[-0.00094,0.00097,0.00097,-0.00094]}, + {"t":3.35767, "x":5.95051, "y":3.72236, "heading":3.14158, "vx":-0.29278, "vy":-0.06512, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.00072,0.00072,-0.00071,-0.00071], "fy":[0.00008,-0.00021,-0.00005,0.00008]}, + {"t":3.40196, "x":5.93754, "y":3.71947, "heading":3.14158, "vx":-0.29278, "vy":-0.06512, "omega":-0.00013, "ax":0.0, "ay":0.00001, "alpha":-0.00014, "fx":[0.00188,-0.00597,0.00202,0.00202], "fy":[-0.00089,0.00101,0.00101,-0.00089]}, + {"t":3.44625, "x":5.92457, "y":3.71659, "heading":3.14157, "vx":-0.29278, "vy":-0.06512, "omega":-0.00014, "ax":0.00001, "ay":-0.00001, "alpha":-0.00094, "fx":[0.00079,0.00079,-0.00067,-0.00067], "fy":[0.00444,-0.01346,0.00426,0.00444]}, + {"t":3.49054, "x":5.91161, "y":3.7137, "heading":3.14157, "vx":-0.29278, "vy":-0.06512, "omega":-0.00018, "ax":0.36724, "ay":0.08169, "alpha":0.01464, "fx":[4.08104,4.41216,4.08222,4.08222], "fy":[0.92622,0.9264,0.92642,0.92622]}, + {"t":3.53483, "x":5.899, "y":3.7109, "heading":3.14156, "vx":-0.27651, "vy":-0.0615, "omega":0.00047, "ax":0.77942, "ay":0.17336, "alpha":-0.00253, "fx":[8.83951,8.83951,8.8375,8.8375], "fy":[1.97732,1.93168,1.97724,1.97732]}, + {"t":3.57912, "x":5.88752, "y":3.70834, "heading":3.14158, "vx":-0.24199, "vy":-0.05382, "omega":0.00035, "ax":0.78017, "ay":0.17353, "alpha":-0.00575, "fx":[8.88196,8.74258,8.88168,8.88168], "fy":[1.96598,1.96957,1.96955,1.96598]}, + {"t":3.62341, "x":5.87756, "y":3.70613, "heading":-3.14159, "vx":-0.20744, "vy":-0.04614, "omega":0.0001, "ax":0.78042, "ay":0.17358, "alpha":-0.00022, "fx":[8.85102,8.85103,8.84857,8.84857], "fy":[1.97024,1.96259,1.97053,1.97024]}, + {"t":3.6677, "x":5.86914, "y":3.70426, "heading":-3.14159, "vx":-0.17287, "vy":-0.03845, "omega":0.00009, "ax":0.78054, "ay":0.17361, "alpha":-0.0028, "fx":[8.86996,8.79582,8.86952,8.86952], "fy":[1.96679,1.97065,1.97064,1.96679]}, + {"t":3.71199, "x":5.86225, "y":3.70272, "heading":-3.14158, "vx":-0.1383, "vy":-0.03076, "omega":-0.00003, "ax":0.78062, "ay":0.17363, "alpha":0.00035, "fx":[8.85344,8.85344,8.85067,8.85067], "fy":[1.96847,1.96967,1.96901,1.96847]}, + {"t":3.75628, "x":5.85689, "y":3.70153, "heading":-3.14158, "vx":-0.10373, "vy":-0.02307, "omega":-0.00002, "ax":0.78067, "ay":0.17364, "alpha":-0.00174, "fx":[8.86576,8.81435,8.86519,8.86519], "fy":[1.96696,1.9711,1.9711,1.96696]}, + {"t":3.80057, "x":5.85306, "y":3.70068, "heading":-3.14159, "vx":-0.06916, "vy":-0.01538, "omega":-0.0001, "ax":0.7807, "ay":0.17365, "alpha":0.00061, "fx":[8.85453,8.85453,8.85151,8.85151], "fy":[1.96766,1.97278,1.96838,1.96766]}, + {"t":3.84486, "x":5.85077, "y":3.70017, "heading":-3.14159, "vx":-0.03458, "vy":-0.00769, "omega":-0.00007, "ax":0.78073, "ay":0.17365, "alpha":0.00154, "fx":[8.86336,8.86336,8.86278,8.82381], "fy":[1.97299,1.96818,1.96818,1.9674]}, + {"t":3.88915, "x":5.85, "y":3.7, "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/redCenterToL4G.traj b/src/main/deploy/choreo/redCenterToL4G.traj index 09f66473..452f0178 100644 --- a/src/main/deploy/choreo/redCenterToL4G.traj +++ b/src/main/deploy/choreo/redCenterToL4G.traj @@ -4,8 +4,8 @@ "snapshot":{ "waypoints":[ {"x":10.35, "y":4.2, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":11.279606819152832, "y":4.188701152801514, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":11.65, "y":4.2, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":11.279606819152832, "y":4.188701152801514, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.65, "y":4.3, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,8 +18,8 @@ "params":{ "waypoints":[ {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"redStart.y", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.188701152801514 m", "val":4.188701152801514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.2 m", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.188701152801514 m", "val":4.188701152801514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.3 m", "val":4.3}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -34,79 +34,80 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.67494,3.06116], + "waypoints":[0.0,1.67519,3.11656], "samples":[ - {"t":0.0, "x":10.35, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99944, "ay":-0.01305, "alpha":0.00374, "fx":[11.35777,11.26375,11.35605,11.35605], "fy":[-0.14923,-0.14682,-0.14682,-0.14923]}, - {"t":0.04295, "x":10.35092, "y":4.19999, "heading":0.0, "vx":0.04292, "vy":-0.00056, "omega":0.00016, "ax":0.99966, "ay":-0.01305, "alpha":0.00167, "fx":[11.34887,11.30102,11.34705,11.34705], "fy":[-0.14925,-0.14672,-0.14672,-0.14925]}, - {"t":0.08589, "x":10.35369, "y":4.19995, "heading":0.00001, "vx":0.08586, "vy":-0.00112, "omega":0.00023, "ax":0.99965, "ay":-0.01304, "alpha":-0.00014, "fx":[11.33683,11.33682,11.33489,11.33489], "fy":[-0.14777,-0.15076,-0.14534,-0.14777]}, - {"t":0.12884, "x":10.3583, "y":4.19989, "heading":0.00002, "vx":0.12879, "vy":-0.00168, "omega":0.00023, "ax":0.99964, "ay":-0.01304, "alpha":0.00182, "fx":[11.34994,11.29722,11.34783,11.34783], "fy":[-0.14925,-0.1464,-0.1464,-0.14925]}, - {"t":0.17179, "x":10.36475, "y":4.19981, "heading":0.00003, "vx":0.17172, "vy":-0.00224, "omega":0.0003, "ax":0.99962, "ay":-0.01303, "alpha":-0.00016, "fx":[11.3365,11.3365,11.33457,11.33457], "fy":[-0.14766,-0.15043,-0.14516,-0.14766]}, - {"t":0.21474, "x":10.37305, "y":4.1997, "heading":0.00004, "vx":0.21465, "vy":-0.0028, "omega":0.0003, "ax":0.99961, "ay":-0.01302, "alpha":0.00212, "fx":[11.35104,11.29227,11.34903,11.34903], "fy":[-0.14895,-0.1463,-0.14631,-0.14895]}, - {"t":0.25768, "x":10.38319, "y":4.19957, "heading":0.00005, "vx":0.25758, "vy":-0.00336, "omega":0.00039, "ax":0.99959, "ay":-0.01301, "alpha":-0.0001, "fx":[11.33594,11.33593,11.33432,11.33432], "fy":[-0.14734,-0.15016,-0.14518,-0.14734]}, - {"t":0.30063, "x":10.39517, "y":4.19941, "heading":0.00007, "vx":0.30051, "vy":-0.00392, "omega":0.00038, "ax":0.99957, "ay":-0.013, "alpha":0.00255, "fx":[11.35235,11.28585,11.35068,11.35068], "fy":[-0.14845,-0.1463,-0.1463,-0.14845]}, - {"t":0.34358, "x":10.409, "y":4.19923, "heading":0.00009, "vx":0.34344, "vy":-0.00448, "omega":0.00049, "ax":0.99954, "ay":-0.01298, "alpha":-0.00002, "fx":[11.33519,11.33519,11.33404,11.33404], "fy":[-0.14686,-0.14991,-0.14527,-0.14686]}, - {"t":0.38652, "x":10.42467, "y":4.19903, "heading":0.00011, "vx":0.38637, "vy":-0.00503, "omega":0.00049, "ax":0.99951, "ay":-0.01297, "alpha":0.00312, "fx":[11.35412,11.27732,11.35287,11.35287], "fy":[-0.14783,-0.14626,-0.14626,-0.14783]}, - {"t":0.42947, "x":10.44219, "y":4.1988, "heading":0.00013, "vx":0.42929, "vy":-0.00559, "omega":0.00063, "ax":0.99948, "ay":-0.01295, "alpha":0.00008, "fx":[11.33427,11.33427,11.33358,11.33358], "fy":[-0.14626,-0.14959,-0.14527,-0.14625]}, - {"t":0.47242, "x":10.46154, "y":4.19855, "heading":0.00015, "vx":0.47222, "vy":-0.00615, "omega":0.00063, "ax":0.99944, "ay":-0.01293, "alpha":0.00384, "fx":[11.35676,11.26549,11.35586,11.35585], "fy":[-0.14716,-0.14604,-0.14604,-0.14716]}, - {"t":0.51537, "x":10.48275, "y":4.19827, "heading":0.00018, "vx":0.51514, "vy":-0.0067, "omega":0.0008, "ax":0.9994, "ay":-0.0129, "alpha":0.00011, "fx":[11.33318,11.33318,11.33276,11.33276], "fy":[-0.14568,-0.1488,-0.1451,-0.14568]}, - {"t":0.55831, "x":10.50579, "y":4.19797, "heading":0.00022, "vx":0.55806, "vy":-0.00726, "omega":0.0008, "ax":0.99934, "ay":-0.01287, "alpha":0.00471, "fx":[11.36039,11.24985,11.35956,11.35956], "fy":[-0.14652,-0.1454,-0.1454,-0.14652]}, - {"t":0.60126, "x":10.53068, "y":4.19765, "heading":0.00025, "vx":0.60098, "vy":-0.00781, "omega":0.001, "ax":0.99927, "ay":-0.01283, "alpha":0.00002, "fx":[11.33186,11.33187,11.33125,11.33125], "fy":[-0.14521,-0.14721,-0.14449,-0.14521]}, - {"t":0.64421, "x":10.55741, "y":4.1973, "heading":0.00029, "vx":0.6439, "vy":-0.00836, "omega":0.001, "ax":0.99918, "ay":-0.01278, "alpha":0.00616, "fx":[11.36805,11.2209,11.36664,11.36664], "fy":[-0.14603,-0.14392,-0.14393,-0.14603]}, - {"t":0.68715, "x":10.58599, "y":4.19693, "heading":0.00034, "vx":0.68681, "vy":-0.00891, "omega":0.00127, "ax":0.99907, "ay":-0.01272, "alpha":-0.00031, "fx":[11.33009,11.33009,11.3284,11.3284], "fy":[-0.14494,-0.14405,-0.14306,-0.14494]}, - {"t":0.7301, "x":10.6164, "y":4.19653, "heading":0.00039, "vx":0.72972, "vy":-0.00946, "omega":0.00125, "ax":0.99891, "ay":-0.01263, "alpha":0.00915, "fx":[11.38519,11.16038,11.38207,11.38207], "fy":[-0.14563,-0.14086,-0.14086,-0.14563]}, - {"t":0.77305, "x":10.64867, "y":4.19612, "heading":0.00045, "vx":0.77262, "vy":-0.01, "omega":0.00165, "ax":0.99867, "ay":-0.0125, "alpha":-0.00108, "fx":[11.32678,11.32679,11.32275,11.32275], "fy":[-0.14483,-0.13716,-0.14028,-0.14483]}, - {"t":0.816, "x":10.68277, "y":4.19568, "heading":0.00052, "vx":0.81551, "vy":-0.01054, "omega":0.0016, "ax":0.9983, "ay":-0.01229, "alpha":0.0196, "fx":[11.44404,10.96302,11.43743,11.43743], "fy":[-0.14438,-0.13443,-0.13442,-0.14438]}, - {"t":0.85894, "x":10.71871, "y":4.19521, "heading":0.00058, "vx":0.85838, "vy":-0.01106, "omega":0.00244, "ax":0.99759, "ay":-0.0119, "alpha":-0.00272, "fx":[11.31587,11.31588,11.30901,11.30901], "fy":[-0.14388,-0.11588,-0.1362,-0.14388]}, - {"t":0.90189, "x":10.7565, "y":4.19473, "heading":0.00069, "vx":0.90122, "vy":-0.01157, "omega":0.00233, "ax":0.99577, "ay":-0.0109, "alpha":0.10571, "fx":[11.91216,9.45717,11.89912,11.89911], "fy":[-0.13269,-0.11461,-0.1145,-0.13268]}, - {"t":0.94484, "x":10.79612, "y":4.19422, "heading":0.00079, "vx":0.94399, "vy":-0.01204, "omega":0.00687, "ax":0.9813, "ay":-0.00302, "alpha":-0.00124, "fx":[11.12292,11.12292,11.13253,11.13253], "fy":[-0.04303,0.00548,-0.05619,-0.04303]}, - {"t":0.98778, "x":10.83757, "y":4.1937, "heading":0.00108, "vx":0.98613, "vy":-0.01217, "omega":0.00681, "ax":-0.99226, "ay":0.01717, "alpha":-0.16183, "fx":[-12.23569,-8.3771,-12.19759,-12.19758], "fy":[0.22023,0.1693,0.16911,0.22022]}, - {"t":1.03073, "x":10.879, "y":4.19319, "heading":0.00138, "vx":0.94352, "vy":-0.01143, "omega":-0.00014, "ax":-0.99675, "ay":0.01484, "alpha":0.00738, "fx":[-11.3168,-11.3168,-11.28904,-11.28905], "fy":[0.18903,0.14212,0.15308,0.18903]}, - {"t":1.07368, "x":10.91861, "y":4.19271, "heading":0.00137, "vx":0.90071, "vy":-0.0108, "omega":0.00018, "ax":-0.99792, "ay":0.01423, "alpha":-0.03144, "fx":[-11.55161,-10.67097,-11.52113,-11.52113], "fy":[0.18218,0.14062,0.14059,0.18218]}, - {"t":1.11663, "x":10.95637, "y":4.19226, "heading":0.00138, "vx":0.85785, "vy":-0.01019, "omega":-0.00117, "ax":-0.99846, "ay":0.01395, "alpha":0.00562, "fx":[-11.33528,-11.33527,-11.30936,-11.30937], "fy":[0.17218,0.14988,0.13857,0.17218]}, - {"t":1.15957, "x":10.99229, "y":4.19184, "heading":0.00133, "vx":0.81497, "vy":-0.00959, "omega":-0.00093, "ax":-0.99877, "ay":0.01379, "alpha":-0.00719, "fx":[-11.41206,-11.11498,-11.38815,-11.38814], "fy":[0.17269,0.14005,0.14005,0.17269]}, - {"t":1.20252, "x":11.02637, "y":4.19144, "heading":0.00129, "vx":0.77208, "vy":-0.00899, "omega":-0.00124, "ax":-0.99897, "ay":0.01368, "alpha":0.0045, "fx":[-11.33867,-11.33867,-11.31755,-11.31756], "fy":[0.16623,0.1496,0.13864,0.16623]}, - {"t":1.24547, "x":11.05861, "y":4.19107, "heading":0.00124, "vx":0.72918, "vy":-0.00841, "omega":-0.00105, "ax":-0.99911, "ay":0.01361, "alpha":-0.00277, "fx":[-11.38244,-11.2105,-11.36295,-11.36295], "fy":[0.16753,0.14114,0.14115,0.16753]}, - {"t":1.28841, "x":11.089, "y":4.19072, "heading":0.00119, "vx":0.68627, "vy":-0.00782, "omega":-0.00116, "ax":-0.99921, "ay":0.01356, "alpha":0.00373, "fx":[-11.33967,-11.33967,-11.32212,-11.32212], "fy":[0.16286,0.14946,0.1397,0.16286]}, - {"t":1.33136, "x":11.11755, "y":4.19039, "heading":0.00114, "vx":0.64335, "vy":-0.00724, "omega":-0.001, "ax":-0.99929, "ay":0.01351, "alpha":-0.00136, "fx":[-11.37091,-11.24765,-11.35434,-11.35433], "fy":[0.16435,0.14213,0.14213,0.16435]}, - {"t":1.37431, "x":11.14426, "y":4.1901, "heading":0.0011, "vx":0.60044, "vy":-0.00666, "omega":-0.00106, "ax":-0.99936, "ay":0.01348, "alpha":0.00319, "fx":[-11.34011,-11.34011,-11.32495,-11.32495], "fy":[0.16063,0.14974,0.14045,0.16063]}, - {"t":1.41726, "x":11.16913, "y":4.18982, "heading":0.00105, "vx":0.55752, "vy":-0.00608, "omega":-0.00093, "ax":-0.99941, "ay":0.01345, "alpha":-0.00079, "fx":[-11.36524,-11.2661,-11.35057,-11.35057], "fy":[0.1623,0.14281,0.14281,0.1623]}, - {"t":1.4602, "x":11.19215, "y":4.18957, "heading":0.00101, "vx":0.51459, "vy":-0.0055, "omega":-0.00096, "ax":-0.99945, "ay":0.01343, "alpha":0.00284, "fx":[-11.34039,-11.34039,-11.32683,-11.32682], "fy":[0.15919,0.14986,0.14096,0.15918]}, - {"t":1.50315, "x":11.21333, "y":4.18935, "heading":0.00097, "vx":0.47167, "vy":-0.00493, "omega":-0.00084, "ax":-0.99949, "ay":0.01341, "alpha":-0.00036, "fx":[-11.36131,-11.27901,-11.34788,-11.34788], "fy":[0.16093,0.14324,0.14324,0.16093]}, - {"t":1.5461, "x":11.23266, "y":4.18915, "heading":0.00093, "vx":0.42875, "vy":-0.00435, "omega":-0.00085, "ax":-0.99952, "ay":0.0134, "alpha":0.00262, "fx":[-11.34064,-11.34064,-11.3281,-11.3281], "fy":[0.15826,0.14981,0.14126,0.15826]}, - {"t":1.58904, "x":11.25015, "y":4.18898, "heading":0.0009, "vx":0.38582, "vy":-0.00377, "omega":-0.00074, "ax":-0.99955, "ay":0.01338, "alpha":-0.00003, "fx":[-11.35864,-11.2882,-11.34593,-11.34593], "fy":[0.16005,0.14343,0.14343,0.16005]}, - {"t":1.63199, "x":11.2658, "y":4.18883, "heading":0.00087, "vx":0.34289, "vy":-0.0032, "omega":-0.00074, "ax":-0.99957, "ay":0.01337, "alpha":0.00252, "fx":[-11.34096,-11.34096,-11.32892,-11.32891], "fy":[0.15771,0.14971,0.14128,0.15771]}, - {"t":1.67494, "x":11.27961, "y":4.1887, "heading":0.00083, "vx":0.29996, "vy":-0.00263, "omega":-0.00063, "ax":-0.0036, "ay":0.25734, "alpha":0.01224, "fx":[0.00577,-0.20449,0.01772,0.01772], "fy":[2.926,2.91048,2.91048,2.926]}, - {"t":1.71966, "x":11.29302, "y":4.18884, "heading":0.00081, "vx":0.2998, "vy":0.00888, "omega":-0.00009, "ax":-0.00034, "ay":0.01117, "alpha":-0.0301, "fx":[-0.01153,-0.01153,0.00381,0.00381], "fy":[-0.00599,0.54693,-0.02829,-0.00599]}, - {"t":1.76437, "x":11.30642, "y":4.18925, "heading":0.0008, "vx":0.29979, "vy":0.00938, "omega":-0.00143, "ax":0.0, "ay":0.00016, "alpha":0.00227, "fx":[-0.01045,0.00622,0.002,0.002], "fy":[0.00984,-0.00625,-0.00625,0.00983]}, - {"t":1.80909, "x":11.31983, "y":4.18967, "heading":0.00074, "vx":0.29979, "vy":0.00939, "omega":-0.00133, "ax":0.00001, "ay":-0.00036, "alpha":-0.00403, "fx":[-0.00583,-0.00584,0.00609,0.00609], "fy":[-0.02557,0.07619,-0.04156,-0.02557]}, - {"t":1.85381, "x":11.33323, "y":4.19009, "heading":0.00068, "vx":0.29979, "vy":0.00937, "omega":-0.00151, "ax":0.0, "ay":0.00013, "alpha":0.00261, "fx":[-0.00572,-0.005,0.00527,0.00527], "fy":[0.00847,-0.00555,-0.00555,0.00847]}, - {"t":1.89852, "x":11.34664, "y":4.19051, "heading":0.00061, "vx":0.29979, "vy":0.00938, "omega":-0.00139, "ax":0.0, "ay":0.00014, "alpha":0.00249, "fx":[-0.00489,-0.00489,0.00479,0.0048], "fy":[0.0084,-0.00532,-0.00529,0.0084]}, - {"t":1.94324, "x":11.36004, "y":4.19093, "heading":0.00055, "vx":0.29979, "vy":0.00938, "omega":-0.00128, "ax":0.0, "ay":-0.00005, "alpha":0.00227, "fx":[-0.0047,-0.00489,0.00483,0.00483], "fy":[0.00534,-0.00656,-0.00656,0.00533]}, - {"t":1.98796, "x":11.37345, "y":4.19135, "heading":0.00049, "vx":0.29979, "vy":0.00938, "omega":-0.00118, "ax":0.0, "ay":-0.00003, "alpha":0.00204, "fx":[-0.0041,-0.0041,0.00412,0.00413], "fy":[0.00516,-0.0051,-0.00668,0.00516]}, - {"t":2.03267, "x":11.38686, "y":4.19177, "heading":0.00044, "vx":0.29979, "vy":0.00938, "omega":-0.00109, "ax":0.0, "ay":0.00001, "alpha":0.00194, "fx":[-0.00415,-0.00417,0.00415,0.00415], "fy":[0.00521,-0.0049,-0.00491,0.00521]}, - {"t":2.07739, "x":11.40026, "y":4.19219, "heading":0.00039, "vx":0.29979, "vy":0.00938, "omega":-0.001, "ax":0.0, "ay":0.0, "alpha":0.00187, "fx":[-0.00351,-0.00351,0.00351,0.00351], "fy":[0.00532,-0.0054,-0.00506,0.00531]}, - {"t":2.12211, "x":11.41367, "y":4.19261, "heading":0.00034, "vx":0.29979, "vy":0.00938, "omega":-0.00092, "ax":0.0, "ay":0.0, "alpha":0.00168, "fx":[-0.00367,-0.00368,0.00367,0.00367], "fy":[0.00435,-0.00436,-0.00436,0.00434]}, - {"t":2.16682, "x":11.42707, "y":4.19302, "heading":0.0003, "vx":0.29979, "vy":0.00938, "omega":-0.00085, "ax":0.0, "ay":0.0, "alpha":0.00166, "fx":[-0.00305,-0.00305,0.00305,0.00305], "fy":[0.0047,-0.00487,-0.0046,0.00469]}, - {"t":2.21154, "x":11.44048, "y":4.19344, "heading":0.00027, "vx":0.29979, "vy":0.00938, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.0015, "fx":[-0.00333,-0.00335,0.00334,0.00334], "fy":[0.00386,-0.00382,-0.00382,0.00386]}, - {"t":2.25626, "x":11.45388, "y":4.19386, "heading":0.00023, "vx":0.29979, "vy":0.00938, "omega":-0.0007, "ax":0.0, "ay":0.0, "alpha":0.00152, "fx":[-0.00274,-0.00274,0.00274,0.00275], "fy":[0.00433,-0.00445,-0.00427,0.00433]}, - {"t":2.30097, "x":11.46729, "y":4.19428, "heading":0.0002, "vx":0.29979, "vy":0.00938, "omega":-0.00064, "ax":0.0, "ay":0.0, "alpha":0.0014, "fx":[-0.00315,-0.00319,0.00316,0.00317], "fy":[0.00356,-0.00352,-0.00352,0.00356]}, - {"t":2.34569, "x":11.48069, "y":4.1947, "heading":0.00017, "vx":0.29979, "vy":0.00938, "omega":-0.00057, "ax":0.0, "ay":0.0, "alpha":0.00146, "fx":[-0.00261,-0.00261,0.00261,0.00261], "fy":[0.00418,-0.00425,-0.00417,0.00417]}, - {"t":2.39041, "x":11.4941, "y":4.19512, "heading":0.00015, "vx":0.29979, "vy":0.00938, "omega":-0.00051, "ax":0.0, "ay":0.0, "alpha":0.00139, "fx":[-0.00313,-0.00324,0.00318,0.00319], "fy":[0.00351,-0.00347,-0.00347,0.00351]}, - {"t":2.43513, "x":11.50751, "y":4.19554, "heading":0.00012, "vx":0.29979, "vy":0.00938, "omega":-0.00045, "ax":0.0, "ay":0.0, "alpha":0.0015, "fx":[-0.00268,-0.00268,0.00268,0.00268], "fy":[0.00431,-0.00438,-0.00432,0.00431]}, - {"t":2.47984, "x":11.52091, "y":4.19596, "heading":0.0001, "vx":0.29979, "vy":0.00938, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.0015, "fx":[-0.00337,-0.00344,0.0034,0.0034], "fy":[0.00378,-0.00374,-0.00374,0.00378]}, - {"t":2.52456, "x":11.53432, "y":4.19638, "heading":0.00009, "vx":0.29979, "vy":0.00938, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":0.00168, "fx":[-0.00303,-0.00304,0.00303,0.00304], "fy":[0.00478,-0.00481,-0.00483,0.00478]}, - {"t":2.56928, "x":11.54772, "y":4.1968, "heading":0.00007, "vx":0.29979, "vy":0.00938, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":0.0016, "fx":[-0.00482,-0.00121,0.00302,0.00302], "fy":[0.00449,-0.00444,-0.00444,0.00449]}, - {"t":2.61399, "x":11.56113, "y":4.19722, "heading":0.00006, "vx":0.29979, "vy":0.00938, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00202, "fx":[-0.00374,-0.00374,0.00374,0.00374], "fy":[0.00569,-0.0057,-0.00579,0.00569]}, - {"t":2.65871, "x":11.57453, "y":4.19764, "heading":0.00005, "vx":0.29979, "vy":0.00938, "omega":-0.00007, "ax":0.0, "ay":0.0, "alpha":0.00172, "fx":[-0.00753,0.00335,0.00209,0.00209], "fy":[0.00571,-0.00567,-0.00567,0.00571]}, - {"t":2.70343, "x":11.58794, "y":4.19806, "heading":0.00005, "vx":0.29979, "vy":0.00938, "omega":0.0, "ax":-0.00001, "ay":0.0, "alpha":0.0032, "fx":[-0.0049,-0.0049,0.00476,0.00476], "fy":[0.00984,-0.01535,-0.0044,0.00984]}, - {"t":2.74814, "x":11.60134, "y":4.19848, "heading":0.00005, "vx":0.29979, "vy":0.00938, "omega":0.00015, "ax":-0.71067, "ay":-0.02224, "alpha":-0.00818, "fx":[-8.1275,-7.8781,-8.11504,-8.11504], "fy":[-0.24459,-0.25975,-0.25971,-0.24459]}, - {"t":2.79286, "x":11.61404, "y":4.19887, "heading":0.00006, "vx":0.26801, "vy":0.00839, "omega":-0.00022, "ax":-0.99803, "ay":-0.03123, "alpha":0.0023, "fx":[-11.32216,-11.32216,-11.31273,-11.31273], "fy":[-0.34795,-0.35851,-0.3621,-0.34795]}, - {"t":2.83758, "x":11.62503, "y":4.19922, "heading":0.00005, "vx":0.22338, "vy":0.00699, "omega":-0.00012, "ax":-0.99877, "ay":-0.03125, "alpha":-0.00415, "fx":[-11.36668,-11.22309,-11.35679,-11.35679], "fy":[-0.34866,-0.36011,-0.36012,-0.34866]}, - {"t":2.88229, "x":11.63402, "y":4.1995, "heading":0.00004, "vx":0.17872, "vy":0.00559, "omega":-0.0003, "ax":-0.99902, "ay":-0.03126, "alpha":0.00217, "fx":[-11.33217,-11.33217,-11.32511,-11.32511], "fy":[-0.34799,-0.36254,-0.35939,-0.34799]}, - {"t":2.92701, "x":11.64101, "y":4.19972, "heading":0.00003, "vx":0.13404, "vy":0.00419, "omega":-0.00021, "ax":-0.99914, "ay":-0.03126, "alpha":-0.00133, "fx":[-11.35206,-11.28086,-11.34364,-11.34364], "fy":[-0.34994,-0.3591,-0.35911,-0.34994]}, - {"t":2.97173, "x":11.646, "y":4.19987, "heading":0.00002, "vx":0.08937, "vy":0.0028, "omega":-0.00027, "ax":-0.99921, "ay":-0.03127, "alpha":0.0021, "fx":[-11.33409,-11.33409,-11.32769,-11.32769], "fy":[-0.34808,-0.36317,-0.35885,-0.34808]}, - {"t":3.01644, "x":11.649, "y":4.19997, "heading":0.00001, "vx":0.04468, "vy":0.0014, "omega":-0.00017, "ax":-0.99926, "ay":-0.03127, "alpha":0.00384, "fx":[-11.34735,-11.34735,-11.33927,-11.29182], "fy":[-0.3526,-0.35883,-0.35883,-0.34801]}, - {"t":3.06116, "x":11.65, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":10.35, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99925, "ay":-0.02332, "alpha":0.00472, "fx":[11.38806,11.19691,11.37008,11.37008], "fy":[-0.27359,-0.25524,-0.25523,-0.27359]}, + {"t":0.04295, "x":10.35092, "y":4.19998, "heading":0.0, "vx":0.04292, "vy":-0.001, "omega":0.0002, "ax":0.99948, "ay":-0.02325, "alpha":0.00143, "fx":[11.36861,11.26175,11.35261,11.35261], "fy":[-0.27166,-0.25567,-0.25566,-0.27166]}, + {"t":0.08591, "x":10.35369, "y":4.19991, "heading":0.00001, "vx":0.08585, "vy":-0.002, "omega":0.00026, "ax":0.99947, "ay":-0.02317, "alpha":-0.00203, "fx":[11.33892,11.33893,11.32863,11.32862], "fy":[-0.26747,-0.26764,-0.24848,-0.26747]}, + {"t":0.12886, "x":10.3583, "y":4.19981, "heading":0.00002, "vx":0.12878, "vy":-0.003, "omega":0.00018, "ax":0.99946, "ay":-0.02308, "alpha":0.00256, "fx":[11.37023,11.25082,11.35676,11.35676], "fy":[-0.26833,-0.25522,-0.25521,-0.26833]}, + {"t":0.17181, "x":10.36475, "y":4.19966, "heading":0.00003, "vx":0.17171, "vy":-0.00399, "omega":0.00029, "ax":0.99944, "ay":-0.02299, "alpha":-0.0017, "fx":[11.3379,11.33791,11.32908,11.32908], "fy":[-0.26454,-0.26559,-0.24802,-0.26454]}, + {"t":0.21477, "x":10.37305, "y":4.19946, "heading":0.00004, "vx":0.21464, "vy":-0.00497, "omega":0.00021, "ax":0.99943, "ay":-0.02288, "alpha":0.00347, "fx":[11.37321,11.23831,11.3609,11.3609], "fy":[-0.26547,-0.25344,-0.25342,-0.26547]}, + {"t":0.25772, "x":10.38319, "y":4.19923, "heading":0.00005, "vx":0.25757, "vy":-0.00596, "omega":0.00036, "ax":0.99941, "ay":-0.02276, "alpha":-0.00167, "fx":[11.33738,11.33738,11.32891,11.32891], "fy":[-0.26196,-0.262,-0.24638,-0.26196]}, + {"t":0.30067, "x":10.39518, "y":4.19895, "heading":0.00006, "vx":0.3005, "vy":-0.00693, "omega":0.00029, "ax":0.99939, "ay":-0.02262, "alpha":0.00432, "fx":[11.3776,11.22326,11.36544,11.36544], "fy":[-0.26266,-0.25038,-0.25036,-0.26266]}, + {"t":0.34363, "x":10.40901, "y":4.19863, "heading":0.00008, "vx":0.34343, "vy":-0.00791, "omega":0.00048, "ax":0.99937, "ay":-0.02246, "alpha":-0.00188, "fx":[11.33718,11.3372,11.32821,11.3282], "fy":[-0.25931,-0.25684,-0.24348,-0.25931]}, + {"t":0.38658, "x":10.42468, "y":4.19827, "heading":0.0001, "vx":0.38635, "vy":-0.00887, "omega":0.0004, "ax":0.99935, "ay":-0.02228, "alpha":0.00521, "fx":[11.3837,11.20431,11.37085,11.37083], "fy":[-0.25953,-0.24587,-0.24585,-0.25952]}, + {"t":0.42954, "x":10.4422, "y":4.19787, "heading":0.00011, "vx":0.42928, "vy":-0.00983, "omega":0.00062, "ax":0.99932, "ay":-0.02207, "alpha":-0.00231, "fx":[11.33724,11.33725,11.32697,11.32695], "fy":[-0.25621,-0.24976,-0.23907,-0.2562]}, + {"t":0.47249, "x":10.46156, "y":4.19743, "heading":0.00014, "vx":0.4722, "vy":-0.01078, "omega":0.00052, "ax":0.99929, "ay":-0.02183, "alpha":0.00626, "fx":[11.39208,11.17939,11.37772,11.3777], "fy":[-0.25559,-0.23945,-0.23942,-0.25558]}, + {"t":0.51544, "x":10.48276, "y":4.19695, "heading":0.00016, "vx":0.51513, "vy":-0.01171, "omega":0.00079, "ax":0.99925, "ay":-0.02153, "alpha":-0.00302, "fx":[11.33746,11.33749,11.32507,11.32505], "fy":[-0.25227,-0.23938,-0.23274,-0.25225]}, + {"t":0.5584, "x":10.50581, "y":4.19642, "heading":0.0002, "vx":0.55805, "vy":-0.01264, "omega":0.00066, "ax":0.9992, "ay":-0.02117, "alpha":0.00765, "fx":[11.4038,11.14503,11.38704,11.387], "fy":[-0.25004,-0.23016,-0.23012,-0.25003]}, + {"t":0.60135, "x":10.5307, "y":4.19586, "heading":0.00023, "vx":0.60097, "vy":-0.01355, "omega":0.00099, "ax":0.99914, "ay":-0.02073, "alpha":-0.00404, "fx":[11.33775,11.33779,11.32231,11.32227], "fy":[-0.24628,-0.22435,-0.22321,-0.24626]}, + {"t":0.6443, "x":10.55744, "y":4.19526, "heading":0.00027, "vx":0.64388, "vy":-0.01444, "omega":0.00081, "ax":0.99906, "ay":-0.02016, "alpha":0.00969, "fx":[11.42059,11.09521,11.40042,11.40037], "fy":[-0.24113,-0.21604,-0.21599,-0.2411]}, + {"t":0.68726, "x":10.58602, "y":4.19462, "heading":0.0003, "vx":0.6868, "vy":-0.0153, "omega":0.00123, "ax":0.99896, "ay":-0.0194, "alpha":-0.00547, "fx":[11.33775,11.3378,11.3182,11.31816], "fy":[-0.23578,-0.20069,-0.20792,-0.23575]}, + {"t":0.73021, "x":10.61644, "y":4.19395, "heading":0.00036, "vx":0.72971, "vy":-0.01614, "omega":0.001, "ax":0.99881, "ay":-0.01837, "alpha":0.01306, "fx":[11.44583,11.01727,11.42118,11.42112], "fy":[-0.22426,-0.19231,-0.19224,-0.22423]}, + {"t":0.77316, "x":10.6487, "y":4.19324, "heading":0.0004, "vx":0.77261, "vy":-0.01693, "omega":0.00156, "ax":0.9986, "ay":-0.01684, "alpha":-0.00747, "fx":[11.33628,11.33633,11.31159,11.31154], "fy":[-0.21317,-0.15802,-0.17943,-0.21313]}, + {"t":0.81612, "x":10.68281, "y":4.19249, "heading":0.00047, "vx":0.8155, "vy":-0.01765, "omega":0.00124, "ax":0.99825, "ay":-0.01437, "alpha":0.02023, "fx":[11.49109,10.86702,11.46094,11.46087], "fy":[-0.18327,-0.14269,-0.14262,-0.18323]}, + {"t":0.85907, "x":10.71876, "y":4.19172, "heading":0.00052, "vx":0.85838, "vy":-0.01827, "omega":0.0021, "ax":0.99758, "ay":-0.00971, "alpha":-0.01086, "fx":[11.32752,11.32759,11.29711,11.29705], "fy":[-0.14451,-0.04691,-0.10455,-0.14447]}, + {"t":0.90202, "x":10.75655, "y":4.19093, "heading":0.00061, "vx":0.90123, "vy":-0.01868, "omega":0.00164, "ax":0.99572, "ay":0.00239, "alpha":0.05677, "fx":[11.68284,10.19082,11.64571,11.64564], "fy":[0.00094,0.05317,0.05321,0.00099]}, + {"t":0.94498, "x":10.79618, "y":4.19013, "heading":0.00068, "vx":0.944, "vy":-0.01858, "omega":0.00408, "ax":0.97286, "ay":0.10854, "alpha":-0.0009, "fx":[11.04896,11.04903,11.01507,11.01503], "fy":[1.23969,1.16033,1.28347,1.23975]}, + {"t":0.98793, "x":10.83763, "y":4.18943, "heading":0.00085, "vx":0.98579, "vy":-0.01392, "omega":0.00404, "ax":-0.99027, "ay":0.0706, "alpha":-0.05805, "fx":[-11.51105,-10.33874,-11.53397,-11.53403], "fy":[0.78387,0.81677,0.81772,0.78388]}, + {"t":1.03088, "x":10.87905, "y":4.1889, "heading":0.00103, "vx":0.94325, "vy":-0.01089, "omega":0.00154, "ax":-0.99593, "ay":0.04442, "alpha":-0.0085, "fx":[-11.28418,-11.28417,-11.3032,-11.30324], "fy":[0.47503,0.56705,0.4979,0.47504]}, + {"t":1.07384, "x":10.91865, "y":4.18847, "heading":0.00109, "vx":0.90047, "vy":-0.00898, "omega":0.00118, "ax":-0.99735, "ay":0.0373, "alpha":-0.03804, "fx":[-11.5167,-10.68271,-11.51982,-11.5198], "fy":[0.4198,0.42619,0.42607,0.4198]}, + {"t":1.11679, "x":10.95641, "y":4.18812, "heading":0.00115, "vx":0.85763, "vy":-0.00738, "omega":-0.00045, "ax":-0.99799, "ay":0.03398, "alpha":-0.00313, "fx":[-11.31738,-11.31739,-11.31669,-11.31668], "fy":[0.37187,0.4283,0.36919,0.37185]}, + {"t":1.15975, "x":10.99233, "y":4.18783, "heading":0.00113, "vx":0.81477, "vy":-0.00592, "omega":-0.00059, "ax":-0.99836, "ay":0.03206, "alpha":-0.0198, "fx":[-11.44944,-10.95526,-11.44004,-11.43999], "fy":[0.369,0.35809,0.358,0.36898]}, + {"t":1.2027, "x":11.0264, "y":4.18761, "heading":0.0011, "vx":0.77188, "vy":-0.00454, "omega":-0.00144, "ax":-0.99859, "ay":0.0308, "alpha":-0.00031, "fx":[-11.32919,-11.32923,-11.31853,-11.3185], "fy":[0.34402,0.38084,0.32841,0.34399]}, + {"t":1.24565, "x":11.05864, "y":4.18744, "heading":0.00104, "vx":0.72899, "vy":-0.00322, "omega":-0.00145, "ax":-0.99876, "ay":0.02992, "alpha":-0.01242, "fx":[-11.42419,-11.06138,-11.40872,-11.40866], "fy":[0.34909,0.32962,0.32953,0.34906]}, + {"t":1.28861, "x":11.08903, "y":4.18733, "heading":0.00098, "vx":0.68609, "vy":-0.00193, "omega":-0.00199, "ax":-0.99888, "ay":0.02927, "alpha":0.00083, "fx":[-11.3347,-11.33474,-11.31954,-11.31949], "fy":[0.32973,0.35983,0.30839,0.32969]}, + {"t":1.33156, "x":11.11758, "y":4.18728, "heading":0.00089, "vx":0.64318, "vy":-0.00067, "omega":-0.00195, "ax":-0.99897, "ay":0.02877, "alpha":-0.00833, "fx":[-11.4079,-11.12431,-11.39029,-11.39022], "fy":[0.33757,0.31487,0.3148,0.33754]}, + {"t":1.37451, "x":11.14428, "y":4.18727, "heading":0.00081, "vx":0.60027, "vy":0.00056, "omega":-0.00231, "ax":-0.99905, "ay":0.02836, "alpha":0.00117, "fx":[-11.33728,-11.33733,-11.32076,-11.32072], "fy":[0.32042,0.34823,0.29756,0.32038]}, + {"t":1.41747, "x":11.16915, "y":4.18732, "heading":0.00071, "vx":0.55736, "vy":0.00178, "omega":-0.00226, "ax":-0.99911, "ay":0.02804, "alpha":-0.00596, "fx":[-11.39614,-11.16552,-11.37862,-11.37856], "fy":[0.32943,0.30652,0.30646,0.3294]}, + {"t":1.46042, "x":11.19216, "y":4.18743, "heading":0.00061, "vx":0.51445, "vy":0.00298, "omega":-0.00251, "ax":-0.99916, "ay":0.02777, "alpha":0.00113, "fx":[-11.33833,-11.33837,-11.32222,-11.32218], "fy":[0.31362,0.34071,0.29162,0.31359]}, + {"t":1.50337, "x":11.21334, "y":4.18758, "heading":0.0005, "vx":0.47153, "vy":0.00418, "omega":-0.00247, "ax":-0.9992, "ay":0.02754, "alpha":-0.00459, "fx":[-11.38706,-11.19418,-11.37091,-11.37086], "fy":[0.32304,0.3016,0.30155,0.32301]}, + {"t":1.54633, "x":11.23267, "y":4.18778, "heading":0.0004, "vx":0.42861, "vy":0.00536, "omega":-0.00266, "ax":-0.99924, "ay":0.02735, "alpha":0.00081, "fx":[-11.33853,-11.33856,-11.32379,-11.32376], "fy":[0.30796,0.33624,0.28822,0.30794]}, + {"t":1.58928, "x":11.25016, "y":4.18804, "heading":0.00028, "vx":0.38569, "vy":0.00653, "omega":-0.00263, "ax":-0.99927, "ay":0.02718, "alpha":-0.00383, "fx":[-11.37974,-11.21487,-11.36573,-11.3657], "fy":[0.31769,0.2987,0.29866,0.31767]}, + {"t":1.63223, "x":11.26581, "y":4.18835, "heading":0.00017, "vx":0.34277, "vy":0.0077, "omega":-0.00279, "ax":-0.9993, "ay":0.02703, "alpha":0.00027, "fx":[-11.33818,-11.3382,-11.32546,-11.32543], "fy":[0.30286,0.33402,0.28631,0.30285]}, + {"t":1.67519, "x":11.27961, "y":4.1887, "heading":0.00005, "vx":0.29984, "vy":0.00886, "omega":-0.00278, "ax":-0.10148, "ay":0.97251, "alpha":0.00009, "fx":[-1.17282,-1.10784,-1.16117,-1.16115], "fy":[11.03623,11.01983,11.01983,11.03622]}, + {"t":1.72023, "x":11.29301, "y":4.19009, "heading":-0.00008, "vx":0.29527, "vy":0.05267, "omega":-0.00278, "ax":-0.18435, "ay":0.76738, "alpha":0.0602, "fx":[-2.09319,-2.09305,-2.08799,-2.08797], "fy":[8.95396,7.95059,8.94921,8.95395]}, + {"t":1.76527, "x":11.30612, "y":4.19324, "heading":-0.0002, "vx":0.28697, "vy":0.08723, "omega":-0.00007, "ax":-0.01156, "ay":0.03764, "alpha":0.0184, "fx":[-0.03707,-0.42249,-0.03243,-0.03242], "fy":[0.43039,0.42318,0.42317,0.43038]}, + {"t":1.81032, "x":11.31904, "y":4.1972, "heading":-0.0002, "vx":0.28645, "vy":0.08893, "omega":0.00076, "ax":-0.0003, "ay":0.00097, "alpha":-0.02298, "fx":[-0.00626,-0.00628,-0.00054,-0.00052], "fy":[-0.08814,0.31499,-0.09491,-0.08815]}, + {"t":1.85536, "x":11.33194, "y":4.20121, "heading":-0.00017, "vx":0.28643, "vy":0.08897, "omega":-0.00027, "ax":0.00001, "ay":-0.00005, "alpha":0.00218, "fx":[0.00835,-0.02836,0.01033,0.01034], "fy":[0.00105,-0.0021,-0.00211,0.00104]}, + {"t":1.9004, "x":11.34484, "y":4.20522, "heading":-0.00018, "vx":0.28644, "vy":0.08897, "omega":-0.00017, "ax":0.00003, "ay":-0.00011, "alpha":0.00025, "fx":[-0.00078,-0.00079,0.00157,0.00158], "fy":[-0.00108,0.00096,-0.00386,-0.00109]}, + {"t":1.94544, "x":11.35774, "y":4.20923, "heading":-0.00019, "vx":0.28644, "vy":0.08896, "omega":-0.00016, "ax":-0.00002, "ay":0.00007, "alpha":0.00075, "fx":[-0.00024,-0.00437,0.0018,0.00181], "fy":[0.00245,-0.00081,-0.00082,0.00244]}, + {"t":1.99049, "x":11.37064, "y":4.21323, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":-0.00013, "ax":0.0, "ay":-0.00001, "alpha":0.00011, "fx":[-0.00135,-0.00136,0.00142,0.00143], "fy":[-0.00068,0.00474,-0.00381,-0.00069]}, + {"t":2.03553, "x":11.38355, "y":4.21724, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":-0.00012, "ax":0.0, "ay":0.00001, "alpha":0.00083, "fx":[-0.00034,-0.00385,0.00204,0.00205], "fy":[0.00203,-0.00185,-0.00186,0.00202]}, + {"t":2.08057, "x":11.39645, "y":4.22125, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00155,-0.00156,0.00158,0.00159], "fy":[-0.00135,0.00728,-0.00477,-0.00136]}, + {"t":2.12562, "x":11.40935, "y":4.22525, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00085, "fx":[-0.00047,-0.00359,0.002,0.00201], "fy":[0.00213,-0.00203,-0.00204,0.00212]}, + {"t":2.17066, "x":11.42225, "y":4.22926, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":-0.00005, "ax":0.0, "ay":-0.00001, "alpha":0.0002, "fx":[-0.00158,-0.00159,0.00162,0.00163], "fy":[-0.00042,0.0044,-0.00379,-0.00043]}, + {"t":2.2157, "x":11.43515, "y":4.23327, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":0.0008, "fx":[-0.00071,-0.00274,0.00169,0.0017], "fy":[0.00216,-0.00204,-0.00204,0.00215]}, + {"t":2.26074, "x":11.44806, "y":4.23728, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.0, "ax":0.0, "ay":-0.00001, "alpha":0.0004, "fx":[-0.00156,-0.00157,0.0016,0.00161], "fy":[0.00046,0.00161,-0.00274,0.00045]}, + {"t":2.30579, "x":11.46096, "y":4.24128, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.00001, "ax":0.0, "ay":0.00001, "alpha":0.00075, "fx":[-0.00088,-0.00204,0.00142,0.00143], "fy":[0.00214,-0.00202,-0.00203,0.00213]}, + {"t":2.35083, "x":11.47386, "y":4.24529, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00053, "fx":[-0.00155,-0.00155,0.00158,0.00159], "fy":[0.001,-0.00015,-0.00207,0.00099]}, + {"t":2.39587, "x":11.48676, "y":4.2493, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.00007, "ax":0.0, "ay":0.0, "alpha":0.00073, "fx":[-0.00098,-0.00165,0.00127,0.00128], "fy":[0.00215,-0.00203,-0.00204,0.00214]}, + {"t":2.44092, "x":11.49966, "y":4.25331, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":0.0001, "ax":0.0, "ay":0.0, "alpha":0.00061, "fx":[-0.00157,-0.00158,0.00161,0.00161], "fy":[0.00134,-0.00117,-0.00173,0.00133]}, + {"t":2.48596, "x":11.51256, "y":4.25731, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":0.00074, "fx":[-0.00107,-0.00153,0.00126,0.00127], "fy":[0.00223,-0.00211,-0.00212,0.00222]}, + {"t":2.531, "x":11.52547, "y":4.26132, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":0.00072, "fx":[-0.00167,-0.00168,0.0017,0.00171], "fy":[0.00171,-0.00203,-0.00159,0.0017]}, + {"t":2.57604, "x":11.53837, "y":4.26533, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":0.0002, "ax":0.0, "ay":0.0, "alpha":0.00087, "fx":[-0.00091,-0.00261,0.00172,0.00173], "fy":[0.00242,-0.00231,-0.00231,0.00242]}, + {"t":2.62109, "x":11.55127, "y":4.26933, "heading":-0.00019, "vx":0.28644, "vy":0.08897, "omega":0.00024, "ax":0.0, "ay":-0.00001, "alpha":0.00101, "fx":[-0.00189,-0.00189,0.00192,0.00193], "fy":[0.00278,-0.00468,-0.00111,0.00277]}, + {"t":2.66613, "x":11.56417, "y":4.27334, "heading":-0.00018, "vx":0.28644, "vy":0.08897, "omega":0.00028, "ax":0.0, "ay":0.0, "alpha":0.00245, "fx":[0.00675,-0.02691,0.01004,0.01005], "fy":[0.00287,-0.00275,-0.00275,0.00286]}, + {"t":2.71117, "x":11.57707, "y":4.27735, "heading":-0.00016, "vx":0.28644, "vy":0.08897, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":0.00345, "fx":[-0.00258,-0.00259,0.00262,0.00262], "fy":[0.01254,-0.03211,0.00681,0.01254]}, + {"t":2.75621, "x":11.58998, "y":4.28136, "heading":-0.00015, "vx":0.28644, "vy":0.08897, "omega":0.00055, "ax":-0.00001, "ay":0.0, "alpha":0.01019, "fx":[0.045,-0.14746,0.0511,0.0511], "fy":[0.00461,-0.00459,-0.00459,0.00461]}, + {"t":2.80126, "x":11.60288, "y":4.28536, "heading":-0.00012, "vx":0.28644, "vy":0.08897, "omega":0.00101, "ax":-0.63264, "ay":-0.1965, "alpha":-0.00115, "fx":[-7.17969,-7.17981,-7.16839,-7.1684], "fy":[-2.23736,-2.1871,-2.25111,-2.23737]}, + {"t":2.8463, "x":11.61514, "y":4.28917, "heading":-0.00008, "vx":0.25794, "vy":0.08011, "omega":0.00096, "ax":-0.9536, "ay":-0.29618, "alpha":-0.01516, "fx":[-10.92691,-10.50238,-10.91254,-10.91256], "fy":[-3.34838,-3.36908,-3.36879,-3.3484]}, + {"t":2.89134, "x":11.62579, "y":4.29248, "heading":-0.00003, "vx":0.21499, "vy":0.06677, "omega":0.00027, "ax":-0.9543, "ay":-0.2964, "alpha":-0.0004, "fx":[-10.82921,-10.8293,-10.81378,-10.81381], "fy":[-3.36859,-3.31942,-3.38788,-3.36861]}, + {"t":2.93639, "x":11.6345, "y":4.29519, "heading":-0.00002, "vx":0.172, "vy":0.05342, "omega":0.00026, "ax":-0.95453, "ay":-0.29647, "alpha":-0.00524, "fx":[-10.88422,-10.67532,-10.86856,-10.86859], "fy":[-3.35074,-3.37326,-3.37303,-3.35076]}, + {"t":2.98143, "x":11.64128, "y":4.29729, "heading":-0.00001, "vx":0.12901, "vy":0.04007, "omega":0.00002, "ax":-0.95465, "ay":-0.29651, "alpha":0.00229, "fx":[-10.83365,-10.8337,-10.8173,-10.81733], "fy":[-3.35883,-3.35251,-3.37924,-3.35885]}, + {"t":3.02647, "x":11.64613, "y":4.2988, "heading":-0.00001, "vx":0.08601, "vy":0.02671, "omega":0.00012, "ax":-0.95472, "ay":-0.29653, "alpha":-0.00182, "fx":[-10.86744,-10.73445,-10.85163,-10.85166], "fy":[-3.35126,-3.37401,-3.37387,-3.35128]}, + {"t":3.07151, "x":11.64903, "y":4.2997, "heading":0.0, "vx":0.04301, "vy":0.01336, "omega":0.00004, "ax":-0.95476, "ay":-0.29655, "alpha":-0.00089, "fx":[-10.86269,-10.75072,-10.84693,-10.84696], "fy":[-3.35144,-3.37415,-3.37403,-3.35146]}, + {"t":3.11656, "x":11.65, "y":4.3, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] From 2053a6a51bbfafdd93f5442f16c30ae2bfe3ee53 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sat, 22 Mar 2025 15:39:30 -0400 Subject: [PATCH 15/24] Widen tolerance for pose seek display --- src/main/java/frc/robot/LEDs/LEDs.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/LEDs/LEDs.java b/src/main/java/frc/robot/LEDs/LEDs.java index cc700d95..3cd83ab9 100644 --- a/src/main/java/frc/robot/LEDs/LEDs.java +++ b/src/main/java/frc/robot/LEDs/LEDs.java @@ -326,10 +326,10 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) { Segments.MIDDLE); var x = delta.getTranslation().getMeasureX().in(Centimeters); - fill(Math.abs(x) < 3 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP); + fill(Math.abs(x) < 5 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP); var y = delta.getTranslation().getMeasureY().in(Centimeters); - if (Math.abs(y) < 3) { + if (Math.abs(y) < 6) { fill(Color.kWhite, Segments.BOTTOM); } else { fill(Color.kGreen, y > 0 ? Segments.leftBottom : Segments.rightBottom); From 5475164ec216cb3f10b0504e37072efecd9a40cb Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sat, 22 Mar 2025 16:24:28 -0400 Subject: [PATCH 16/24] Remove init from lifter joystick command --- src/main/java/frc/robot/elevator/Lifter.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/elevator/Lifter.java b/src/main/java/frc/robot/elevator/Lifter.java index 61d5acf4..56e039ef 100644 --- a/src/main/java/frc/robot/elevator/Lifter.java +++ b/src/main/java/frc/robot/elevator/Lifter.java @@ -216,8 +216,8 @@ public Command createJoystickControlCommand(XboxController gamepad) { return new FunctionalCommand( // initialize () -> { - matchHeight(); - resetController(); + // matchHeight(); + // resetController(); }, // execute () -> { From d4c64c845d20537b929f3c3c14c7438361bd8491 Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sat, 22 Mar 2025 16:31:25 -0400 Subject: [PATCH 17/24] Tweaked the redCenterToL4G choreo --- src/main/deploy/choreo/redCenterToL4G.traj | 163 +++++++++++---------- 1 file changed, 82 insertions(+), 81 deletions(-) diff --git a/src/main/deploy/choreo/redCenterToL4G.traj b/src/main/deploy/choreo/redCenterToL4G.traj index 452f0178..513576a6 100644 --- a/src/main/deploy/choreo/redCenterToL4G.traj +++ b/src/main/deploy/choreo/redCenterToL4G.traj @@ -3,30 +3,32 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":10.35, "y":4.2, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":11.279606819152832, "y":4.188701152801514, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":11.65, "y":4.3, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":10.35, "y":4.4, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":11.279606819152832, "y":4.4, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.65, "y":4.4, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.3}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.3}}, "enabled":true}, + {"from":2, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"redStart.y", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.188701152801514 m", "val":4.188701152801514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.3 m", "val":4.3}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.4 m", "val":4.4}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.4 m", "val":4.4}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.4 m", "val":4.4}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.3 m / s", "val":0.3}}}, "enabled":true}], + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.3 m / s", "val":0.3}}}, "enabled":true}, + {"from":2, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -34,80 +36,79 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.67519,3.11656], + "waypoints":[0.0,1.67487,3.06049], "samples":[ - {"t":0.0, "x":10.35, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99925, "ay":-0.02332, "alpha":0.00472, "fx":[11.38806,11.19691,11.37008,11.37008], "fy":[-0.27359,-0.25524,-0.25523,-0.27359]}, - {"t":0.04295, "x":10.35092, "y":4.19998, "heading":0.0, "vx":0.04292, "vy":-0.001, "omega":0.0002, "ax":0.99948, "ay":-0.02325, "alpha":0.00143, "fx":[11.36861,11.26175,11.35261,11.35261], "fy":[-0.27166,-0.25567,-0.25566,-0.27166]}, - {"t":0.08591, "x":10.35369, "y":4.19991, "heading":0.00001, "vx":0.08585, "vy":-0.002, "omega":0.00026, "ax":0.99947, "ay":-0.02317, "alpha":-0.00203, "fx":[11.33892,11.33893,11.32863,11.32862], "fy":[-0.26747,-0.26764,-0.24848,-0.26747]}, - {"t":0.12886, "x":10.3583, "y":4.19981, "heading":0.00002, "vx":0.12878, "vy":-0.003, "omega":0.00018, "ax":0.99946, "ay":-0.02308, "alpha":0.00256, "fx":[11.37023,11.25082,11.35676,11.35676], "fy":[-0.26833,-0.25522,-0.25521,-0.26833]}, - {"t":0.17181, "x":10.36475, "y":4.19966, "heading":0.00003, "vx":0.17171, "vy":-0.00399, "omega":0.00029, "ax":0.99944, "ay":-0.02299, "alpha":-0.0017, "fx":[11.3379,11.33791,11.32908,11.32908], "fy":[-0.26454,-0.26559,-0.24802,-0.26454]}, - {"t":0.21477, "x":10.37305, "y":4.19946, "heading":0.00004, "vx":0.21464, "vy":-0.00497, "omega":0.00021, "ax":0.99943, "ay":-0.02288, "alpha":0.00347, "fx":[11.37321,11.23831,11.3609,11.3609], "fy":[-0.26547,-0.25344,-0.25342,-0.26547]}, - {"t":0.25772, "x":10.38319, "y":4.19923, "heading":0.00005, "vx":0.25757, "vy":-0.00596, "omega":0.00036, "ax":0.99941, "ay":-0.02276, "alpha":-0.00167, "fx":[11.33738,11.33738,11.32891,11.32891], "fy":[-0.26196,-0.262,-0.24638,-0.26196]}, - {"t":0.30067, "x":10.39518, "y":4.19895, "heading":0.00006, "vx":0.3005, "vy":-0.00693, "omega":0.00029, "ax":0.99939, "ay":-0.02262, "alpha":0.00432, "fx":[11.3776,11.22326,11.36544,11.36544], "fy":[-0.26266,-0.25038,-0.25036,-0.26266]}, - {"t":0.34363, "x":10.40901, "y":4.19863, "heading":0.00008, "vx":0.34343, "vy":-0.00791, "omega":0.00048, "ax":0.99937, "ay":-0.02246, "alpha":-0.00188, "fx":[11.33718,11.3372,11.32821,11.3282], "fy":[-0.25931,-0.25684,-0.24348,-0.25931]}, - {"t":0.38658, "x":10.42468, "y":4.19827, "heading":0.0001, "vx":0.38635, "vy":-0.00887, "omega":0.0004, "ax":0.99935, "ay":-0.02228, "alpha":0.00521, "fx":[11.3837,11.20431,11.37085,11.37083], "fy":[-0.25953,-0.24587,-0.24585,-0.25952]}, - {"t":0.42954, "x":10.4422, "y":4.19787, "heading":0.00011, "vx":0.42928, "vy":-0.00983, "omega":0.00062, "ax":0.99932, "ay":-0.02207, "alpha":-0.00231, "fx":[11.33724,11.33725,11.32697,11.32695], "fy":[-0.25621,-0.24976,-0.23907,-0.2562]}, - {"t":0.47249, "x":10.46156, "y":4.19743, "heading":0.00014, "vx":0.4722, "vy":-0.01078, "omega":0.00052, "ax":0.99929, "ay":-0.02183, "alpha":0.00626, "fx":[11.39208,11.17939,11.37772,11.3777], "fy":[-0.25559,-0.23945,-0.23942,-0.25558]}, - {"t":0.51544, "x":10.48276, "y":4.19695, "heading":0.00016, "vx":0.51513, "vy":-0.01171, "omega":0.00079, "ax":0.99925, "ay":-0.02153, "alpha":-0.00302, "fx":[11.33746,11.33749,11.32507,11.32505], "fy":[-0.25227,-0.23938,-0.23274,-0.25225]}, - {"t":0.5584, "x":10.50581, "y":4.19642, "heading":0.0002, "vx":0.55805, "vy":-0.01264, "omega":0.00066, "ax":0.9992, "ay":-0.02117, "alpha":0.00765, "fx":[11.4038,11.14503,11.38704,11.387], "fy":[-0.25004,-0.23016,-0.23012,-0.25003]}, - {"t":0.60135, "x":10.5307, "y":4.19586, "heading":0.00023, "vx":0.60097, "vy":-0.01355, "omega":0.00099, "ax":0.99914, "ay":-0.02073, "alpha":-0.00404, "fx":[11.33775,11.33779,11.32231,11.32227], "fy":[-0.24628,-0.22435,-0.22321,-0.24626]}, - {"t":0.6443, "x":10.55744, "y":4.19526, "heading":0.00027, "vx":0.64388, "vy":-0.01444, "omega":0.00081, "ax":0.99906, "ay":-0.02016, "alpha":0.00969, "fx":[11.42059,11.09521,11.40042,11.40037], "fy":[-0.24113,-0.21604,-0.21599,-0.2411]}, - {"t":0.68726, "x":10.58602, "y":4.19462, "heading":0.0003, "vx":0.6868, "vy":-0.0153, "omega":0.00123, "ax":0.99896, "ay":-0.0194, "alpha":-0.00547, "fx":[11.33775,11.3378,11.3182,11.31816], "fy":[-0.23578,-0.20069,-0.20792,-0.23575]}, - {"t":0.73021, "x":10.61644, "y":4.19395, "heading":0.00036, "vx":0.72971, "vy":-0.01614, "omega":0.001, "ax":0.99881, "ay":-0.01837, "alpha":0.01306, "fx":[11.44583,11.01727,11.42118,11.42112], "fy":[-0.22426,-0.19231,-0.19224,-0.22423]}, - {"t":0.77316, "x":10.6487, "y":4.19324, "heading":0.0004, "vx":0.77261, "vy":-0.01693, "omega":0.00156, "ax":0.9986, "ay":-0.01684, "alpha":-0.00747, "fx":[11.33628,11.33633,11.31159,11.31154], "fy":[-0.21317,-0.15802,-0.17943,-0.21313]}, - {"t":0.81612, "x":10.68281, "y":4.19249, "heading":0.00047, "vx":0.8155, "vy":-0.01765, "omega":0.00124, "ax":0.99825, "ay":-0.01437, "alpha":0.02023, "fx":[11.49109,10.86702,11.46094,11.46087], "fy":[-0.18327,-0.14269,-0.14262,-0.18323]}, - {"t":0.85907, "x":10.71876, "y":4.19172, "heading":0.00052, "vx":0.85838, "vy":-0.01827, "omega":0.0021, "ax":0.99758, "ay":-0.00971, "alpha":-0.01086, "fx":[11.32752,11.32759,11.29711,11.29705], "fy":[-0.14451,-0.04691,-0.10455,-0.14447]}, - {"t":0.90202, "x":10.75655, "y":4.19093, "heading":0.00061, "vx":0.90123, "vy":-0.01868, "omega":0.00164, "ax":0.99572, "ay":0.00239, "alpha":0.05677, "fx":[11.68284,10.19082,11.64571,11.64564], "fy":[0.00094,0.05317,0.05321,0.00099]}, - {"t":0.94498, "x":10.79618, "y":4.19013, "heading":0.00068, "vx":0.944, "vy":-0.01858, "omega":0.00408, "ax":0.97286, "ay":0.10854, "alpha":-0.0009, "fx":[11.04896,11.04903,11.01507,11.01503], "fy":[1.23969,1.16033,1.28347,1.23975]}, - {"t":0.98793, "x":10.83763, "y":4.18943, "heading":0.00085, "vx":0.98579, "vy":-0.01392, "omega":0.00404, "ax":-0.99027, "ay":0.0706, "alpha":-0.05805, "fx":[-11.51105,-10.33874,-11.53397,-11.53403], "fy":[0.78387,0.81677,0.81772,0.78388]}, - {"t":1.03088, "x":10.87905, "y":4.1889, "heading":0.00103, "vx":0.94325, "vy":-0.01089, "omega":0.00154, "ax":-0.99593, "ay":0.04442, "alpha":-0.0085, "fx":[-11.28418,-11.28417,-11.3032,-11.30324], "fy":[0.47503,0.56705,0.4979,0.47504]}, - {"t":1.07384, "x":10.91865, "y":4.18847, "heading":0.00109, "vx":0.90047, "vy":-0.00898, "omega":0.00118, "ax":-0.99735, "ay":0.0373, "alpha":-0.03804, "fx":[-11.5167,-10.68271,-11.51982,-11.5198], "fy":[0.4198,0.42619,0.42607,0.4198]}, - {"t":1.11679, "x":10.95641, "y":4.18812, "heading":0.00115, "vx":0.85763, "vy":-0.00738, "omega":-0.00045, "ax":-0.99799, "ay":0.03398, "alpha":-0.00313, "fx":[-11.31738,-11.31739,-11.31669,-11.31668], "fy":[0.37187,0.4283,0.36919,0.37185]}, - {"t":1.15975, "x":10.99233, "y":4.18783, "heading":0.00113, "vx":0.81477, "vy":-0.00592, "omega":-0.00059, "ax":-0.99836, "ay":0.03206, "alpha":-0.0198, "fx":[-11.44944,-10.95526,-11.44004,-11.43999], "fy":[0.369,0.35809,0.358,0.36898]}, - {"t":1.2027, "x":11.0264, "y":4.18761, "heading":0.0011, "vx":0.77188, "vy":-0.00454, "omega":-0.00144, "ax":-0.99859, "ay":0.0308, "alpha":-0.00031, "fx":[-11.32919,-11.32923,-11.31853,-11.3185], "fy":[0.34402,0.38084,0.32841,0.34399]}, - {"t":1.24565, "x":11.05864, "y":4.18744, "heading":0.00104, "vx":0.72899, "vy":-0.00322, "omega":-0.00145, "ax":-0.99876, "ay":0.02992, "alpha":-0.01242, "fx":[-11.42419,-11.06138,-11.40872,-11.40866], "fy":[0.34909,0.32962,0.32953,0.34906]}, - {"t":1.28861, "x":11.08903, "y":4.18733, "heading":0.00098, "vx":0.68609, "vy":-0.00193, "omega":-0.00199, "ax":-0.99888, "ay":0.02927, "alpha":0.00083, "fx":[-11.3347,-11.33474,-11.31954,-11.31949], "fy":[0.32973,0.35983,0.30839,0.32969]}, - {"t":1.33156, "x":11.11758, "y":4.18728, "heading":0.00089, "vx":0.64318, "vy":-0.00067, "omega":-0.00195, "ax":-0.99897, "ay":0.02877, "alpha":-0.00833, "fx":[-11.4079,-11.12431,-11.39029,-11.39022], "fy":[0.33757,0.31487,0.3148,0.33754]}, - {"t":1.37451, "x":11.14428, "y":4.18727, "heading":0.00081, "vx":0.60027, "vy":0.00056, "omega":-0.00231, "ax":-0.99905, "ay":0.02836, "alpha":0.00117, "fx":[-11.33728,-11.33733,-11.32076,-11.32072], "fy":[0.32042,0.34823,0.29756,0.32038]}, - {"t":1.41747, "x":11.16915, "y":4.18732, "heading":0.00071, "vx":0.55736, "vy":0.00178, "omega":-0.00226, "ax":-0.99911, "ay":0.02804, "alpha":-0.00596, "fx":[-11.39614,-11.16552,-11.37862,-11.37856], "fy":[0.32943,0.30652,0.30646,0.3294]}, - {"t":1.46042, "x":11.19216, "y":4.18743, "heading":0.00061, "vx":0.51445, "vy":0.00298, "omega":-0.00251, "ax":-0.99916, "ay":0.02777, "alpha":0.00113, "fx":[-11.33833,-11.33837,-11.32222,-11.32218], "fy":[0.31362,0.34071,0.29162,0.31359]}, - {"t":1.50337, "x":11.21334, "y":4.18758, "heading":0.0005, "vx":0.47153, "vy":0.00418, "omega":-0.00247, "ax":-0.9992, "ay":0.02754, "alpha":-0.00459, "fx":[-11.38706,-11.19418,-11.37091,-11.37086], "fy":[0.32304,0.3016,0.30155,0.32301]}, - {"t":1.54633, "x":11.23267, "y":4.18778, "heading":0.0004, "vx":0.42861, "vy":0.00536, "omega":-0.00266, "ax":-0.99924, "ay":0.02735, "alpha":0.00081, "fx":[-11.33853,-11.33856,-11.32379,-11.32376], "fy":[0.30796,0.33624,0.28822,0.30794]}, - {"t":1.58928, "x":11.25016, "y":4.18804, "heading":0.00028, "vx":0.38569, "vy":0.00653, "omega":-0.00263, "ax":-0.99927, "ay":0.02718, "alpha":-0.00383, "fx":[-11.37974,-11.21487,-11.36573,-11.3657], "fy":[0.31769,0.2987,0.29866,0.31767]}, - {"t":1.63223, "x":11.26581, "y":4.18835, "heading":0.00017, "vx":0.34277, "vy":0.0077, "omega":-0.00279, "ax":-0.9993, "ay":0.02703, "alpha":0.00027, "fx":[-11.33818,-11.3382,-11.32546,-11.32543], "fy":[0.30286,0.33402,0.28631,0.30285]}, - {"t":1.67519, "x":11.27961, "y":4.1887, "heading":0.00005, "vx":0.29984, "vy":0.00886, "omega":-0.00278, "ax":-0.10148, "ay":0.97251, "alpha":0.00009, "fx":[-1.17282,-1.10784,-1.16117,-1.16115], "fy":[11.03623,11.01983,11.01983,11.03622]}, - {"t":1.72023, "x":11.29301, "y":4.19009, "heading":-0.00008, "vx":0.29527, "vy":0.05267, "omega":-0.00278, "ax":-0.18435, "ay":0.76738, "alpha":0.0602, "fx":[-2.09319,-2.09305,-2.08799,-2.08797], "fy":[8.95396,7.95059,8.94921,8.95395]}, - {"t":1.76527, "x":11.30612, "y":4.19324, "heading":-0.0002, "vx":0.28697, "vy":0.08723, "omega":-0.00007, "ax":-0.01156, "ay":0.03764, "alpha":0.0184, "fx":[-0.03707,-0.42249,-0.03243,-0.03242], "fy":[0.43039,0.42318,0.42317,0.43038]}, - {"t":1.81032, "x":11.31904, "y":4.1972, "heading":-0.0002, "vx":0.28645, "vy":0.08893, "omega":0.00076, "ax":-0.0003, "ay":0.00097, "alpha":-0.02298, "fx":[-0.00626,-0.00628,-0.00054,-0.00052], "fy":[-0.08814,0.31499,-0.09491,-0.08815]}, - {"t":1.85536, "x":11.33194, "y":4.20121, "heading":-0.00017, "vx":0.28643, "vy":0.08897, "omega":-0.00027, "ax":0.00001, "ay":-0.00005, "alpha":0.00218, "fx":[0.00835,-0.02836,0.01033,0.01034], "fy":[0.00105,-0.0021,-0.00211,0.00104]}, - {"t":1.9004, "x":11.34484, "y":4.20522, "heading":-0.00018, "vx":0.28644, "vy":0.08897, "omega":-0.00017, "ax":0.00003, "ay":-0.00011, "alpha":0.00025, "fx":[-0.00078,-0.00079,0.00157,0.00158], "fy":[-0.00108,0.00096,-0.00386,-0.00109]}, - {"t":1.94544, "x":11.35774, "y":4.20923, "heading":-0.00019, "vx":0.28644, "vy":0.08896, "omega":-0.00016, "ax":-0.00002, "ay":0.00007, "alpha":0.00075, "fx":[-0.00024,-0.00437,0.0018,0.00181], "fy":[0.00245,-0.00081,-0.00082,0.00244]}, - {"t":1.99049, "x":11.37064, "y":4.21323, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":-0.00013, "ax":0.0, "ay":-0.00001, "alpha":0.00011, "fx":[-0.00135,-0.00136,0.00142,0.00143], "fy":[-0.00068,0.00474,-0.00381,-0.00069]}, - {"t":2.03553, "x":11.38355, "y":4.21724, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":-0.00012, "ax":0.0, "ay":0.00001, "alpha":0.00083, "fx":[-0.00034,-0.00385,0.00204,0.00205], "fy":[0.00203,-0.00185,-0.00186,0.00202]}, - {"t":2.08057, "x":11.39645, "y":4.22125, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00155,-0.00156,0.00158,0.00159], "fy":[-0.00135,0.00728,-0.00477,-0.00136]}, - {"t":2.12562, "x":11.40935, "y":4.22525, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00085, "fx":[-0.00047,-0.00359,0.002,0.00201], "fy":[0.00213,-0.00203,-0.00204,0.00212]}, - {"t":2.17066, "x":11.42225, "y":4.22926, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":-0.00005, "ax":0.0, "ay":-0.00001, "alpha":0.0002, "fx":[-0.00158,-0.00159,0.00162,0.00163], "fy":[-0.00042,0.0044,-0.00379,-0.00043]}, - {"t":2.2157, "x":11.43515, "y":4.23327, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":0.0008, "fx":[-0.00071,-0.00274,0.00169,0.0017], "fy":[0.00216,-0.00204,-0.00204,0.00215]}, - {"t":2.26074, "x":11.44806, "y":4.23728, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.0, "ax":0.0, "ay":-0.00001, "alpha":0.0004, "fx":[-0.00156,-0.00157,0.0016,0.00161], "fy":[0.00046,0.00161,-0.00274,0.00045]}, - {"t":2.30579, "x":11.46096, "y":4.24128, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.00001, "ax":0.0, "ay":0.00001, "alpha":0.00075, "fx":[-0.00088,-0.00204,0.00142,0.00143], "fy":[0.00214,-0.00202,-0.00203,0.00213]}, - {"t":2.35083, "x":11.47386, "y":4.24529, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00053, "fx":[-0.00155,-0.00155,0.00158,0.00159], "fy":[0.001,-0.00015,-0.00207,0.00099]}, - {"t":2.39587, "x":11.48676, "y":4.2493, "heading":-0.00022, "vx":0.28644, "vy":0.08897, "omega":0.00007, "ax":0.0, "ay":0.0, "alpha":0.00073, "fx":[-0.00098,-0.00165,0.00127,0.00128], "fy":[0.00215,-0.00203,-0.00204,0.00214]}, - {"t":2.44092, "x":11.49966, "y":4.25331, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":0.0001, "ax":0.0, "ay":0.0, "alpha":0.00061, "fx":[-0.00157,-0.00158,0.00161,0.00161], "fy":[0.00134,-0.00117,-0.00173,0.00133]}, - {"t":2.48596, "x":11.51256, "y":4.25731, "heading":-0.00021, "vx":0.28644, "vy":0.08897, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":0.00074, "fx":[-0.00107,-0.00153,0.00126,0.00127], "fy":[0.00223,-0.00211,-0.00212,0.00222]}, - {"t":2.531, "x":11.52547, "y":4.26132, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":0.00072, "fx":[-0.00167,-0.00168,0.0017,0.00171], "fy":[0.00171,-0.00203,-0.00159,0.0017]}, - {"t":2.57604, "x":11.53837, "y":4.26533, "heading":-0.0002, "vx":0.28644, "vy":0.08897, "omega":0.0002, "ax":0.0, "ay":0.0, "alpha":0.00087, "fx":[-0.00091,-0.00261,0.00172,0.00173], "fy":[0.00242,-0.00231,-0.00231,0.00242]}, - {"t":2.62109, "x":11.55127, "y":4.26933, "heading":-0.00019, "vx":0.28644, "vy":0.08897, "omega":0.00024, "ax":0.0, "ay":-0.00001, "alpha":0.00101, "fx":[-0.00189,-0.00189,0.00192,0.00193], "fy":[0.00278,-0.00468,-0.00111,0.00277]}, - {"t":2.66613, "x":11.56417, "y":4.27334, "heading":-0.00018, "vx":0.28644, "vy":0.08897, "omega":0.00028, "ax":0.0, "ay":0.0, "alpha":0.00245, "fx":[0.00675,-0.02691,0.01004,0.01005], "fy":[0.00287,-0.00275,-0.00275,0.00286]}, - {"t":2.71117, "x":11.57707, "y":4.27735, "heading":-0.00016, "vx":0.28644, "vy":0.08897, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":0.00345, "fx":[-0.00258,-0.00259,0.00262,0.00262], "fy":[0.01254,-0.03211,0.00681,0.01254]}, - {"t":2.75621, "x":11.58998, "y":4.28136, "heading":-0.00015, "vx":0.28644, "vy":0.08897, "omega":0.00055, "ax":-0.00001, "ay":0.0, "alpha":0.01019, "fx":[0.045,-0.14746,0.0511,0.0511], "fy":[0.00461,-0.00459,-0.00459,0.00461]}, - {"t":2.80126, "x":11.60288, "y":4.28536, "heading":-0.00012, "vx":0.28644, "vy":0.08897, "omega":0.00101, "ax":-0.63264, "ay":-0.1965, "alpha":-0.00115, "fx":[-7.17969,-7.17981,-7.16839,-7.1684], "fy":[-2.23736,-2.1871,-2.25111,-2.23737]}, - {"t":2.8463, "x":11.61514, "y":4.28917, "heading":-0.00008, "vx":0.25794, "vy":0.08011, "omega":0.00096, "ax":-0.9536, "ay":-0.29618, "alpha":-0.01516, "fx":[-10.92691,-10.50238,-10.91254,-10.91256], "fy":[-3.34838,-3.36908,-3.36879,-3.3484]}, - {"t":2.89134, "x":11.62579, "y":4.29248, "heading":-0.00003, "vx":0.21499, "vy":0.06677, "omega":0.00027, "ax":-0.9543, "ay":-0.2964, "alpha":-0.0004, "fx":[-10.82921,-10.8293,-10.81378,-10.81381], "fy":[-3.36859,-3.31942,-3.38788,-3.36861]}, - {"t":2.93639, "x":11.6345, "y":4.29519, "heading":-0.00002, "vx":0.172, "vy":0.05342, "omega":0.00026, "ax":-0.95453, "ay":-0.29647, "alpha":-0.00524, "fx":[-10.88422,-10.67532,-10.86856,-10.86859], "fy":[-3.35074,-3.37326,-3.37303,-3.35076]}, - {"t":2.98143, "x":11.64128, "y":4.29729, "heading":-0.00001, "vx":0.12901, "vy":0.04007, "omega":0.00002, "ax":-0.95465, "ay":-0.29651, "alpha":0.00229, "fx":[-10.83365,-10.8337,-10.8173,-10.81733], "fy":[-3.35883,-3.35251,-3.37924,-3.35885]}, - {"t":3.02647, "x":11.64613, "y":4.2988, "heading":-0.00001, "vx":0.08601, "vy":0.02671, "omega":0.00012, "ax":-0.95472, "ay":-0.29653, "alpha":-0.00182, "fx":[-10.86744,-10.73445,-10.85163,-10.85166], "fy":[-3.35126,-3.37401,-3.37387,-3.35128]}, - {"t":3.07151, "x":11.64903, "y":4.2997, "heading":0.0, "vx":0.04301, "vy":0.01336, "omega":0.00004, "ax":-0.95476, "ay":-0.29655, "alpha":-0.00089, "fx":[-10.86269,-10.75072,-10.84693,-10.84696], "fy":[-3.35144,-3.37415,-3.37403,-3.35146]}, - {"t":3.11656, "x":11.65, "y":4.3, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":10.35, "y":4.4, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99952, "ay":0.0, "alpha":0.00407, "fx":[11.35864,11.26366,11.3576,11.3576], "fy":[-0.00025,0.00025,0.00025,-0.00025]}, + {"t":0.04295, "x":10.35092, "y":4.4, "heading":0.0, "vx":0.04292, "vy":0.0, "omega":0.00017, "ax":0.99975, "ay":0.0, "alpha":0.00197, "fx":[11.34962,11.30129,11.34847,11.34847], "fy":[-0.00032,0.00032,0.00032,-0.00032]}, + {"t":0.08589, "x":10.35369, "y":4.4, "heading":0.00001, "vx":0.08586, "vy":0.0, "omega":0.00026, "ax":0.99974, "ay":0.0, "alpha":-0.00023, "fx":[11.33708,11.33708,11.33656,11.33656], "fy":[-0.00078,0.00078,0.00077,-0.00078]}, + {"t":0.12884, "x":10.3583, "y":4.4, "heading":0.00002, "vx":0.12879, "vy":0.0, "omega":0.00025, "ax":0.99972, "ay":0.0, "alpha":0.0021, "fx":[11.35075,11.29748,11.34923,11.34923], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, + {"t":0.17178, "x":10.36475, "y":4.4, "heading":0.00003, "vx":0.17173, "vy":0.0, "omega":0.00034, "ax":0.99971, "ay":0.0, "alpha":-0.00027, "fx":[11.33682,11.33682,11.33618,11.33618], "fy":[-0.00088,0.00089,0.00088,-0.00088]}, + {"t":0.21473, "x":10.37305, "y":4.4, "heading":0.00004, "vx":0.21466, "vy":0.0, "omega":0.00033, "ax":0.99969, "ay":0.0, "alpha":0.00237, "fx":[11.35193,11.29253,11.35038,11.35038], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, + {"t":0.25767, "x":10.38319, "y":4.4, "heading":0.00006, "vx":0.25759, "vy":0.0, "omega":0.00043, "ax":0.99967, "ay":0.0, "alpha":-0.00024, "fx":[11.33634,11.33634,11.33583,11.33583], "fy":[-0.00082,0.00082,0.00081,-0.00082]}, + {"t":0.30062, "x":10.39517, "y":4.4, "heading":0.00008, "vx":0.30052, "vy":0.0, "omega":0.00042, "ax":0.99965, "ay":0.0, "alpha":0.00276, "fx":[11.35336,11.28612,11.35195,11.35195], "fy":[-0.00044,0.00044,0.00044,-0.00044]}, + {"t":0.34356, "x":10.409, "y":4.4, "heading":0.00009, "vx":0.34345, "vy":0.0, "omega":0.00054, "ax":0.99963, "ay":0.0, "alpha":-0.00019, "fx":[11.33572,11.33572,11.33542,11.33542], "fy":[-0.00068,0.00068,0.00067,-0.00068]}, + {"t":0.38651, "x":10.42467, "y":4.4, "heading":0.00012, "vx":0.38638, "vy":0.0, "omega":0.00053, "ax":0.9996, "ay":0.0, "alpha":0.00326, "fx":[11.35528,11.27762,11.35405,11.35405], "fy":[-0.00033,0.00033,0.00033,-0.00033]}, + {"t":0.42945, "x":10.44219, "y":4.4, "heading":0.00014, "vx":0.42931, "vy":0.0, "omega":0.00067, "ax":0.99957, "ay":0.0, "alpha":-0.00015, "fx":[11.33495,11.33495,11.3348,11.3348], "fy":[-0.00057,0.00057,0.00056,-0.00057]}, + {"t":0.4724, "x":10.46154, "y":4.4, "heading":0.00017, "vx":0.47224, "vy":0.0, "omega":0.00066, "ax":0.99953, "ay":0.0, "alpha":0.00391, "fx":[11.35812,11.26582,11.35692,11.35691], "fy":[-0.00035,0.00035,0.00035,-0.00035]}, + {"t":0.51534, "x":10.48275, "y":4.4, "heading":0.0002, "vx":0.51516, "vy":0.0, "omega":0.00083, "ax":0.99948, "ay":0.0, "alpha":-0.00017, "fx":[11.33406,11.33406,11.33378,11.33378], "fy":[-0.0006,0.0006,0.00059,-0.0006]}, + {"t":0.55829, "x":10.50579, "y":4.4, "heading":0.00023, "vx":0.55808, "vy":0.0, "omega":0.00083, "ax":0.99942, "ay":0.0, "alpha":0.0047, "fx":[11.36209,11.24992,11.36057,11.36056], "fy":[-0.00064,0.00064,0.00064,-0.00064]}, + {"t":0.60123, "x":10.53068, "y":4.4, "heading":0.00027, "vx":0.60101, "vy":0.0, "omega":0.00103, "ax":0.99935, "ay":0.0, "alpha":-0.00032, "fx":[11.33298,11.33299,11.33201,11.332], "fy":[-0.00096,0.00097,0.00096,-0.00096]}, + {"t":0.64418, "x":10.55741, "y":4.4, "heading":0.00031, "vx":0.64392, "vy":0.0, "omega":0.00101, "ax":0.99927, "ay":0.0, "alpha":0.00602, "fx":[11.36998,11.2212,11.36739,11.36739], "fy":[-0.00151,0.00151,0.00151,-0.00151]}, + {"t":0.68712, "x":10.58599, "y":4.4, "heading":0.00036, "vx":0.68684, "vy":0.0, "omega":0.00127, "ax":0.99915, "ay":0.0, "alpha":-0.00069, "fx":[11.33151,11.33151,11.32882,11.32882], "fy":[-0.00192,0.00192,0.00191,-0.00192]}, + {"t":0.73007, "x":10.6164, "y":4.4, "heading":0.00041, "vx":0.72975, "vy":0.0, "omega":0.00124, "ax":0.99899, "ay":0.0, "alpha":0.00885, "fx":[11.38745,11.16084,11.38254,11.38253], "fy":[-0.00331,0.00331,0.00331,-0.00331]}, + {"t":0.77302, "x":10.64866, "y":4.4, "heading":0.00046, "vx":0.77265, "vy":0.0, "omega":0.00162, "ax":0.99875, "ay":0.0, "alpha":-0.00141, "fx":[11.3286,11.3286,11.32273,11.32273], "fy":[-0.00374,0.00374,0.00373,-0.00373]}, + {"t":0.81596, "x":10.68277, "y":4.4, "heading":0.00053, "vx":0.81554, "vy":0.0, "omega":0.00156, "ax":0.99837, "ay":0.0, "alpha":0.01922, "fx":[11.44747,10.96147,11.43825,11.43825], "fy":[-0.00654,0.00654,0.00654,-0.00654]}, + {"t":0.85891, "x":10.71871, "y":4.4, "heading":0.0006, "vx":0.85841, "vy":0.0, "omega":0.00239, "ax":0.99766, "ay":0.0, "alpha":-0.00229, "fx":[11.3182,11.3182,11.30838,11.30838], "fy":[-0.00596,0.00596,0.00596,-0.00596]}, + {"t":0.90185, "x":10.7565, "y":4.4, "heading":0.0007, "vx":0.90126, "vy":0.0, "omega":0.00229, "ax":0.99585, "ay":0.0, "alpha":0.10599, "fx":[11.92148,9.43985,11.90474,11.90473], "fy":[-0.0115,0.0115,0.01149,-0.01149]}, + {"t":0.9448, "x":10.79612, "y":4.4, "heading":0.0008, "vx":0.94403, "vy":0.0, "omega":0.00684, "ax":0.98155, "ay":0.0, "alpha":0.00128, "fx":[11.12811,11.1281,11.13297,11.13297], "fy":[0.00357,-0.0038,-0.00341,0.00356]}, + {"t":0.98774, "x":10.83757, "y":4.4, "heading":0.0011, "vx":0.98618, "vy":0.0, "omega":0.0069, "ax":-0.99236, "ay":0.0, "alpha":-0.16455, "fx":[-12.24271,-8.35081,-12.20963,-12.20962], "fy":[0.02215,-0.02215,-0.02215,0.02214]}, + {"t":1.03069, "x":10.879, "y":4.4, "heading":0.00139, "vx":0.94356, "vy":0.0, "omega":-0.00017, "ax":-0.99685, "ay":0.0, "alpha":0.00544, "fx":[-11.31527,-11.31527,-11.29292,-11.29292], "fy":[0.01455,-0.01467,-0.01442,0.01455]}, + {"t":1.07363, "x":10.91861, "y":4.4, "heading":0.00138, "vx":0.90075, "vy":0.0, "omega":0.00006, "ax":-0.99802, "ay":0.0, "alpha":-0.03316, "fx":[-11.5521,-10.66316,-11.52703,-11.52702], "fy":[0.0171,-0.0171,-0.0171,0.0171]}, + {"t":1.11658, "x":10.95637, "y":4.4, "heading":0.00139, "vx":0.85789, "vy":0.0, "omega":-0.00136, "ax":-0.99855, "ay":0.0, "alpha":0.0049, "fx":[-11.3335,-11.3335,-11.31332,-11.31331], "fy":[0.01309,-0.01313,-0.01304,0.01309]}, + {"t":1.15952, "x":10.99229, "y":4.4, "heading":0.00133, "vx":0.81501, "vy":0.0, "omega":-0.00115, "ax":-0.99886, "ay":0.0, "alpha":-0.00871, "fx":[-11.41102,-11.11118,-11.39271,-11.3927], "fy":[0.01252,-0.01252,-0.01252,0.01252]}, + {"t":1.20247, "x":11.02637, "y":4.4, "heading":0.00128, "vx":0.77211, "vy":0.0, "omega":-0.00153, "ax":-0.99906, "ay":0.0, "alpha":0.00373, "fx":[-11.33684,-11.33684,-11.3215,-11.32149], "fy":[0.01,-0.01002,-0.00998,0.01]}, + {"t":1.24541, "x":11.05861, "y":4.4, "heading":0.00121, "vx":0.72921, "vy":0.0, "omega":-0.00136, "ax":-0.9992, "ay":0.0, "alpha":-0.00426, "fx":[-11.3811,-11.20736,-11.36729,-11.36728], "fy":[0.00939,-0.00938,-0.00939,0.00938]}, + {"t":1.28836, "x":11.089, "y":4.4, "heading":0.00116, "vx":0.6863, "vy":0.0, "omega":-0.00155, "ax":-0.99931, "ay":0.0, "alpha":0.00289, "fx":[-11.33784,-11.33784,-11.32603,-11.32603], "fy":[0.00774,-0.00775,-0.00773,0.00774]}, + {"t":1.3313, "x":11.11755, "y":4.4, "heading":0.00109, "vx":0.64338, "vy":0.0, "omega":-0.00142, "ax":-0.99939, "ay":0.0, "alpha":-0.00286, "fx":[-11.36955,-11.24438,-11.35872,-11.35871], "fy":[0.00732,-0.00732,-0.00732,0.00732]}, + {"t":1.37425, "x":11.14426, "y":4.4, "heading":0.00103, "vx":0.60046, "vy":0.0, "omega":-0.00155, "ax":-0.99945, "ay":0.0, "alpha":0.00231, "fx":[-11.33826,-11.33827,-11.32885,-11.32885], "fy":[0.0062,-0.0062,-0.00619,0.0062]}, + {"t":1.4172, "x":11.16913, "y":4.4, "heading":0.00096, "vx":0.55754, "vy":0.0, "omega":-0.00145, "ax":-0.9995, "ay":0.0, "alpha":-0.00226, "fx":[-11.36362,-11.26337,-11.3548,-11.35479], "fy":[0.00594,-0.00593,-0.00594,0.00593]}, + {"t":1.46014, "x":11.19215, "y":4.4, "heading":0.0009, "vx":0.51462, "vy":0.0, "omega":-0.00154, "ax":-0.99954, "ay":0.0, "alpha":0.00191, "fx":[-11.33851,-11.33852,-11.33075,-11.33074], "fy":[0.00513,-0.00513,-0.00513,0.00513]}, + {"t":1.50309, "x":11.21333, "y":4.4, "heading":0.00083, "vx":0.47169, "vy":0.0, "omega":-0.00146, "ax":-0.99958, "ay":0.0, "alpha":-0.00185, "fx":[-11.35954,-11.27638,-11.35212,-11.35211], "fy":[0.00498,-0.00497,-0.00498,0.00497]}, + {"t":1.54603, "x":11.23266, "y":4.4, "heading":0.00077, "vx":0.42876, "vy":0.0, "omega":-0.00154, "ax":-0.99961, "ay":0.0, "alpha":0.00163, "fx":[-11.3387,-11.3387,-11.33207,-11.33207], "fy":[0.00439,-0.00439,-0.00439,0.00439]}, + {"t":1.58898, "x":11.25015, "y":4.4, "heading":0.0007, "vx":0.38583, "vy":0.0, "omega":-0.00147, "ax":-0.99964, "ay":0.0, "alpha":-0.00156, "fx":[-11.35671,-11.28554,-11.35025,-11.35025], "fy":[0.00433,-0.00433,-0.00433,0.00433]}, + {"t":1.63192, "x":11.2658, "y":4.4, "heading":0.00064, "vx":0.34291, "vy":0.0, "omega":-0.00154, "ax":-0.99966, "ay":0.0, "alpha":0.00145, "fx":[-11.33891,-11.33891,-11.33299,-11.33299], "fy":[0.00391,-0.00391,-0.00391,0.00391]}, + {"t":1.67487, "x":11.27961, "y":4.4, "heading":0.00057, "vx":0.29997, "vy":0.0, "omega":-0.00148, "ax":-0.00092, "ay":0.0, "alpha":0.01124, "fx":[0.04336,-0.18163,0.04835,0.04835], "fy":[0.00338,-0.00335,-0.00336,0.00338]}, + {"t":1.71956, "x":11.29301, "y":4.4, "heading":0.00051, "vx":0.29993, "vy":0.0, "omega":-0.00097, "ax":0.0, "ay":0.0, "alpha":0.00142, "fx":[-0.00287,-0.00287,0.00287,0.00287], "fy":[0.00383,-0.00399,-0.00377,0.00383]}, + {"t":1.76426, "x":11.30642, "y":4.4, "heading":0.00047, "vx":0.29993, "vy":0.0, "omega":-0.00091, "ax":0.0, "ay":0.0, "alpha":0.00059, "fx":[-0.00727,0.01063,-0.00168,-0.00168], "fy":[0.00377,-0.00373,-0.00373,0.00376]}, + {"t":1.80896, "x":11.31983, "y":4.4, "heading":0.00042, "vx":0.29993, "vy":0.0, "omega":-0.00088, "ax":0.0, "ay":0.0, "alpha":0.00131, "fx":[-0.00263,-0.00263,0.00263,0.00263], "fy":[0.00353,-0.00367,-0.00346,0.00353]}, + {"t":1.85366, "x":11.33323, "y":4.4, "heading":0.00038, "vx":0.29993, "vy":0.0, "omega":-0.00083, "ax":0.0, "ay":0.0, "alpha":0.00117, "fx":[-0.00272,-0.00173,0.00222,0.00223], "fy":[0.00331,-0.00327,-0.00327,0.00331]}, + {"t":1.89836, "x":11.34664, "y":4.4, "heading":0.00035, "vx":0.29993, "vy":0.0, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.00116, "fx":[-0.00231,-0.00231,0.00231,0.00231], "fy":[0.00314,-0.00326,-0.00308,0.00313]}, + {"t":1.94305, "x":11.36005, "y":4.4, "heading":0.00031, "vx":0.29993, "vy":0.0, "omega":-0.00072, "ax":0.0, "ay":0.0, "alpha":0.00108, "fx":[-0.00218,-0.00226,0.00222,0.00222], "fy":[0.00292,-0.00288,-0.00288,0.00291]}, + {"t":1.98775, "x":11.37345, "y":4.4, "heading":0.00028, "vx":0.29993, "vy":0.0, "omega":-0.00067, "ax":0.0, "ay":0.0, "alpha":0.00104, "fx":[-0.00205,-0.00205,0.00205,0.00205], "fy":[0.00281,-0.00293,-0.00277,0.00281]}, + {"t":2.03245, "x":11.38686, "y":4.4, "heading":0.00025, "vx":0.29993, "vy":0.0, "omega":-0.00063, "ax":0.0, "ay":0.0, "alpha":0.00097, "fx":[-0.00199,-0.00202,0.00201,0.00201], "fy":[0.00261,-0.00257,-0.00257,0.00261]}, + {"t":2.07715, "x":11.40026, "y":4.4, "heading":0.00022, "vx":0.29993, "vy":0.0, "omega":-0.00058, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00185,-0.00185,0.00185,0.00185], "fy":[0.00258,-0.00269,-0.00255,0.00258]}, + {"t":2.12184, "x":11.41367, "y":4.4, "heading":0.0002, "vx":0.29993, "vy":0.0, "omega":-0.00054, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00186,-0.00187,0.00187,0.00187], "fy":[0.00239,-0.00235,-0.00235,0.00239]}, + {"t":2.16654, "x":11.42708, "y":4.4, "heading":0.00017, "vx":0.29993, "vy":0.0, "omega":-0.0005, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00173,-0.00173,0.00173,0.00173], "fy":[0.00245,-0.00256,-0.00243,0.00245]}, + {"t":2.21124, "x":11.44048, "y":4.4, "heading":0.00015, "vx":0.29993, "vy":0.0, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.00086, "fx":[-0.00181,-0.00183,0.00182,0.00182], "fy":[0.00229,-0.00225,-0.00225,0.00229]}, + {"t":2.25594, "x":11.45389, "y":4.4, "heading":0.00013, "vx":0.29993, "vy":0.0, "omega":-0.00042, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.0017,-0.0017,0.0017,0.0017], "fy":[0.00245,-0.00255,-0.00244,0.00245]}, + {"t":2.30063, "x":11.4673, "y":4.4, "heading":0.00011, "vx":0.29993, "vy":0.0, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00187,-0.00191,0.00189,0.00189], "fy":[0.00234,-0.00229,-0.00229,0.00234]}, + {"t":2.34533, "x":11.4807, "y":4.4, "heading":0.00009, "vx":0.29993, "vy":0.0, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00181,-0.00181,0.00181,0.00181], "fy":[0.00262,-0.00272,-0.00261,0.00262]}, + {"t":2.39003, "x":11.49411, "y":4.4, "heading":0.00008, "vx":0.29993, "vy":0.0, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00098, "fx":[-0.00205,-0.00217,0.00211,0.00211], "fy":[0.00259,-0.00254,-0.00254,0.00258]}, + {"t":2.43473, "x":11.50752, "y":4.4, "heading":0.00006, "vx":0.29993, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00109, "fx":[-0.00209,-0.00209,0.00209,0.00209], "fy":[0.00301,-0.00311,-0.00301,0.00301]}, + {"t":2.47942, "x":11.52092, "y":4.4, "heading":0.00005, "vx":0.29993, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00118, "fx":[-0.00247,-0.00255,0.00251,0.00251], "fy":[0.00311,-0.00307,-0.00307,0.00311]}, + {"t":2.52412, "x":11.53433, "y":4.4, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00262,-0.00262,0.00262,0.00262], "fy":[0.00373,-0.00383,-0.00372,0.00373]}, + {"t":2.56882, "x":11.54773, "y":4.4, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00408,-0.00049,0.00228,0.00229], "fy":[0.00405,-0.004,-0.004,0.00405]}, + {"t":2.61352, "x":11.56114, "y":4.4, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00178, "fx":[-0.00349,-0.00349,0.00349,0.00349], "fy":[0.00487,-0.00498,-0.00486,0.00487]}, + {"t":2.65821, "x":11.57455, "y":4.4, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00158, "fx":[-0.00685,0.00365,0.0016,0.0016], "fy":[0.00549,-0.00544,-0.00544,0.00549]}, + {"t":2.70291, "x":11.58795, "y":4.4, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00012, "ax":-0.00001, "ay":0.0, "alpha":0.00239, "fx":[-0.00484,-0.00484,0.00469,0.00469], "fy":[0.00647,-0.00659,-0.00648,0.00647]}, + {"t":2.74761, "x":11.60136, "y":4.4, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":0.00022, "ax":-0.71391, "ay":0.0, "alpha":-0.00874, "fx":[-8.16629,-7.90667,-8.15479,-8.15479], "fy":[0.0075,-0.00747,-0.00747,0.0075]}, + {"t":2.79231, "x":11.61405, "y":4.4, "heading":0.00005, "vx":0.26802, "vy":0.0, "omega":-0.00017, "ax":-0.99852, "ay":0.0, "alpha":0.00234, "fx":[-11.32759,-11.32759,-11.31835,-11.31835], "fy":[0.00639,-0.0064,-0.00638,0.00639]}, + {"t":2.83701, "x":11.62503, "y":4.4, "heading":0.00004, "vx":0.22339, "vy":0.0, "omega":-0.00006, "ax":-0.99926, "ay":0.0, "alpha":-0.00436, "fx":[-11.37216,-11.22667,-11.36333,-11.36333], "fy":[0.00558,-0.00558,-0.00558,0.00558]}, + {"t":2.8817, "x":11.63402, "y":4.4, "heading":0.00004, "vx":0.17873, "vy":0.0, "omega":-0.00026, "ax":-0.9995, "ay":0.0, "alpha":0.00179, "fx":[-11.3376,-11.3376,-11.33076,-11.33076], "fy":[0.00499,-0.00499,-0.00499,0.00499]}, + {"t":2.9264, "x":11.64101, "y":4.4, "heading":0.00003, "vx":0.13405, "vy":0.0, "omega":-0.00018, "ax":-0.99963, "ay":0.0, "alpha":-0.0015, "fx":[-11.35726,-11.28522,-11.34994,-11.34994], "fy":[0.00442,-0.00442,-0.00442,0.00442]}, + {"t":2.9711, "x":11.64601, "y":4.4, "heading":0.00002, "vx":0.08937, "vy":0.0, "omega":-0.00025, "ax":-0.9997, "ay":0.0, "alpha":0.00165, "fx":[-11.33951,-11.33951,-11.33335,-11.33335], "fy":[0.00466,-0.00466,-0.00465,0.00466]}, + {"t":3.0158, "x":11.649, "y":4.4, "heading":0.00001, "vx":0.04469, "vy":0.0, "omega":-0.00017, "ax":-0.99975, "ay":0.0, "alpha":0.00385, "fx":[-11.35244,-11.35244,-11.34552,-11.29757], "fy":[0.00464,-0.00463,-0.00464,0.00463]}, + {"t":3.06049, "x":11.65, "y":4.4, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] From 76bd0c03a9ea9bd3abeaacb94c28a1659ed4cd54 Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sat, 22 Mar 2025 17:30:31 -0400 Subject: [PATCH 18/24] commented out the createjoystickcontrolcommand and tweaked choreo. --- src/main/deploy/choreo/redCenterToL4G.traj | 154 +++++++++---------- src/main/java/frc/robot/elevator/Lifter.java | 4 +- 2 files changed, 79 insertions(+), 79 deletions(-) diff --git a/src/main/deploy/choreo/redCenterToL4G.traj b/src/main/deploy/choreo/redCenterToL4G.traj index 513576a6..496a6a0d 100644 --- a/src/main/deploy/choreo/redCenterToL4G.traj +++ b/src/main/deploy/choreo/redCenterToL4G.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":10.35, "y":4.4, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":11.279606819152832, "y":4.4, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":11.65, "y":4.4, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":10.35, "y":4.25, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":11.279606819152832, "y":4.25, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.65, "y":4.25, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,9 +18,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.4 m", "val":4.4}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.4 m", "val":4.4}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.4 m", "val":4.4}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.25 m", "val":4.25}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.25 m", "val":4.25}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.25 m", "val":4.25}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -38,77 +38,77 @@ "sampleType":"Swerve", "waypoints":[0.0,1.67487,3.06049], "samples":[ - {"t":0.0, "x":10.35, "y":4.4, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99952, "ay":0.0, "alpha":0.00407, "fx":[11.35864,11.26366,11.3576,11.3576], "fy":[-0.00025,0.00025,0.00025,-0.00025]}, - {"t":0.04295, "x":10.35092, "y":4.4, "heading":0.0, "vx":0.04292, "vy":0.0, "omega":0.00017, "ax":0.99975, "ay":0.0, "alpha":0.00197, "fx":[11.34962,11.30129,11.34847,11.34847], "fy":[-0.00032,0.00032,0.00032,-0.00032]}, - {"t":0.08589, "x":10.35369, "y":4.4, "heading":0.00001, "vx":0.08586, "vy":0.0, "omega":0.00026, "ax":0.99974, "ay":0.0, "alpha":-0.00023, "fx":[11.33708,11.33708,11.33656,11.33656], "fy":[-0.00078,0.00078,0.00077,-0.00078]}, - {"t":0.12884, "x":10.3583, "y":4.4, "heading":0.00002, "vx":0.12879, "vy":0.0, "omega":0.00025, "ax":0.99972, "ay":0.0, "alpha":0.0021, "fx":[11.35075,11.29748,11.34923,11.34923], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, - {"t":0.17178, "x":10.36475, "y":4.4, "heading":0.00003, "vx":0.17173, "vy":0.0, "omega":0.00034, "ax":0.99971, "ay":0.0, "alpha":-0.00027, "fx":[11.33682,11.33682,11.33618,11.33618], "fy":[-0.00088,0.00089,0.00088,-0.00088]}, - {"t":0.21473, "x":10.37305, "y":4.4, "heading":0.00004, "vx":0.21466, "vy":0.0, "omega":0.00033, "ax":0.99969, "ay":0.0, "alpha":0.00237, "fx":[11.35193,11.29253,11.35038,11.35038], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, - {"t":0.25767, "x":10.38319, "y":4.4, "heading":0.00006, "vx":0.25759, "vy":0.0, "omega":0.00043, "ax":0.99967, "ay":0.0, "alpha":-0.00024, "fx":[11.33634,11.33634,11.33583,11.33583], "fy":[-0.00082,0.00082,0.00081,-0.00082]}, - {"t":0.30062, "x":10.39517, "y":4.4, "heading":0.00008, "vx":0.30052, "vy":0.0, "omega":0.00042, "ax":0.99965, "ay":0.0, "alpha":0.00276, "fx":[11.35336,11.28612,11.35195,11.35195], "fy":[-0.00044,0.00044,0.00044,-0.00044]}, - {"t":0.34356, "x":10.409, "y":4.4, "heading":0.00009, "vx":0.34345, "vy":0.0, "omega":0.00054, "ax":0.99963, "ay":0.0, "alpha":-0.00019, "fx":[11.33572,11.33572,11.33542,11.33542], "fy":[-0.00068,0.00068,0.00067,-0.00068]}, - {"t":0.38651, "x":10.42467, "y":4.4, "heading":0.00012, "vx":0.38638, "vy":0.0, "omega":0.00053, "ax":0.9996, "ay":0.0, "alpha":0.00326, "fx":[11.35528,11.27762,11.35405,11.35405], "fy":[-0.00033,0.00033,0.00033,-0.00033]}, - {"t":0.42945, "x":10.44219, "y":4.4, "heading":0.00014, "vx":0.42931, "vy":0.0, "omega":0.00067, "ax":0.99957, "ay":0.0, "alpha":-0.00015, "fx":[11.33495,11.33495,11.3348,11.3348], "fy":[-0.00057,0.00057,0.00056,-0.00057]}, - {"t":0.4724, "x":10.46154, "y":4.4, "heading":0.00017, "vx":0.47224, "vy":0.0, "omega":0.00066, "ax":0.99953, "ay":0.0, "alpha":0.00391, "fx":[11.35812,11.26582,11.35692,11.35691], "fy":[-0.00035,0.00035,0.00035,-0.00035]}, - {"t":0.51534, "x":10.48275, "y":4.4, "heading":0.0002, "vx":0.51516, "vy":0.0, "omega":0.00083, "ax":0.99948, "ay":0.0, "alpha":-0.00017, "fx":[11.33406,11.33406,11.33378,11.33378], "fy":[-0.0006,0.0006,0.00059,-0.0006]}, - {"t":0.55829, "x":10.50579, "y":4.4, "heading":0.00023, "vx":0.55808, "vy":0.0, "omega":0.00083, "ax":0.99942, "ay":0.0, "alpha":0.0047, "fx":[11.36209,11.24992,11.36057,11.36056], "fy":[-0.00064,0.00064,0.00064,-0.00064]}, - {"t":0.60123, "x":10.53068, "y":4.4, "heading":0.00027, "vx":0.60101, "vy":0.0, "omega":0.00103, "ax":0.99935, "ay":0.0, "alpha":-0.00032, "fx":[11.33298,11.33299,11.33201,11.332], "fy":[-0.00096,0.00097,0.00096,-0.00096]}, - {"t":0.64418, "x":10.55741, "y":4.4, "heading":0.00031, "vx":0.64392, "vy":0.0, "omega":0.00101, "ax":0.99927, "ay":0.0, "alpha":0.00602, "fx":[11.36998,11.2212,11.36739,11.36739], "fy":[-0.00151,0.00151,0.00151,-0.00151]}, - {"t":0.68712, "x":10.58599, "y":4.4, "heading":0.00036, "vx":0.68684, "vy":0.0, "omega":0.00127, "ax":0.99915, "ay":0.0, "alpha":-0.00069, "fx":[11.33151,11.33151,11.32882,11.32882], "fy":[-0.00192,0.00192,0.00191,-0.00192]}, - {"t":0.73007, "x":10.6164, "y":4.4, "heading":0.00041, "vx":0.72975, "vy":0.0, "omega":0.00124, "ax":0.99899, "ay":0.0, "alpha":0.00885, "fx":[11.38745,11.16084,11.38254,11.38253], "fy":[-0.00331,0.00331,0.00331,-0.00331]}, - {"t":0.77302, "x":10.64866, "y":4.4, "heading":0.00046, "vx":0.77265, "vy":0.0, "omega":0.00162, "ax":0.99875, "ay":0.0, "alpha":-0.00141, "fx":[11.3286,11.3286,11.32273,11.32273], "fy":[-0.00374,0.00374,0.00373,-0.00373]}, - {"t":0.81596, "x":10.68277, "y":4.4, "heading":0.00053, "vx":0.81554, "vy":0.0, "omega":0.00156, "ax":0.99837, "ay":0.0, "alpha":0.01922, "fx":[11.44747,10.96147,11.43825,11.43825], "fy":[-0.00654,0.00654,0.00654,-0.00654]}, - {"t":0.85891, "x":10.71871, "y":4.4, "heading":0.0006, "vx":0.85841, "vy":0.0, "omega":0.00239, "ax":0.99766, "ay":0.0, "alpha":-0.00229, "fx":[11.3182,11.3182,11.30838,11.30838], "fy":[-0.00596,0.00596,0.00596,-0.00596]}, - {"t":0.90185, "x":10.7565, "y":4.4, "heading":0.0007, "vx":0.90126, "vy":0.0, "omega":0.00229, "ax":0.99585, "ay":0.0, "alpha":0.10599, "fx":[11.92148,9.43985,11.90474,11.90473], "fy":[-0.0115,0.0115,0.01149,-0.01149]}, - {"t":0.9448, "x":10.79612, "y":4.4, "heading":0.0008, "vx":0.94403, "vy":0.0, "omega":0.00684, "ax":0.98155, "ay":0.0, "alpha":0.00128, "fx":[11.12811,11.1281,11.13297,11.13297], "fy":[0.00357,-0.0038,-0.00341,0.00356]}, - {"t":0.98774, "x":10.83757, "y":4.4, "heading":0.0011, "vx":0.98618, "vy":0.0, "omega":0.0069, "ax":-0.99236, "ay":0.0, "alpha":-0.16455, "fx":[-12.24271,-8.35081,-12.20963,-12.20962], "fy":[0.02215,-0.02215,-0.02215,0.02214]}, - {"t":1.03069, "x":10.879, "y":4.4, "heading":0.00139, "vx":0.94356, "vy":0.0, "omega":-0.00017, "ax":-0.99685, "ay":0.0, "alpha":0.00544, "fx":[-11.31527,-11.31527,-11.29292,-11.29292], "fy":[0.01455,-0.01467,-0.01442,0.01455]}, - {"t":1.07363, "x":10.91861, "y":4.4, "heading":0.00138, "vx":0.90075, "vy":0.0, "omega":0.00006, "ax":-0.99802, "ay":0.0, "alpha":-0.03316, "fx":[-11.5521,-10.66316,-11.52703,-11.52702], "fy":[0.0171,-0.0171,-0.0171,0.0171]}, - {"t":1.11658, "x":10.95637, "y":4.4, "heading":0.00139, "vx":0.85789, "vy":0.0, "omega":-0.00136, "ax":-0.99855, "ay":0.0, "alpha":0.0049, "fx":[-11.3335,-11.3335,-11.31332,-11.31331], "fy":[0.01309,-0.01313,-0.01304,0.01309]}, - {"t":1.15952, "x":10.99229, "y":4.4, "heading":0.00133, "vx":0.81501, "vy":0.0, "omega":-0.00115, "ax":-0.99886, "ay":0.0, "alpha":-0.00871, "fx":[-11.41102,-11.11118,-11.39271,-11.3927], "fy":[0.01252,-0.01252,-0.01252,0.01252]}, - {"t":1.20247, "x":11.02637, "y":4.4, "heading":0.00128, "vx":0.77211, "vy":0.0, "omega":-0.00153, "ax":-0.99906, "ay":0.0, "alpha":0.00373, "fx":[-11.33684,-11.33684,-11.3215,-11.32149], "fy":[0.01,-0.01002,-0.00998,0.01]}, - {"t":1.24541, "x":11.05861, "y":4.4, "heading":0.00121, "vx":0.72921, "vy":0.0, "omega":-0.00136, "ax":-0.9992, "ay":0.0, "alpha":-0.00426, "fx":[-11.3811,-11.20736,-11.36729,-11.36728], "fy":[0.00939,-0.00938,-0.00939,0.00938]}, - {"t":1.28836, "x":11.089, "y":4.4, "heading":0.00116, "vx":0.6863, "vy":0.0, "omega":-0.00155, "ax":-0.99931, "ay":0.0, "alpha":0.00289, "fx":[-11.33784,-11.33784,-11.32603,-11.32603], "fy":[0.00774,-0.00775,-0.00773,0.00774]}, - {"t":1.3313, "x":11.11755, "y":4.4, "heading":0.00109, "vx":0.64338, "vy":0.0, "omega":-0.00142, "ax":-0.99939, "ay":0.0, "alpha":-0.00286, "fx":[-11.36955,-11.24438,-11.35872,-11.35871], "fy":[0.00732,-0.00732,-0.00732,0.00732]}, - {"t":1.37425, "x":11.14426, "y":4.4, "heading":0.00103, "vx":0.60046, "vy":0.0, "omega":-0.00155, "ax":-0.99945, "ay":0.0, "alpha":0.00231, "fx":[-11.33826,-11.33827,-11.32885,-11.32885], "fy":[0.0062,-0.0062,-0.00619,0.0062]}, - {"t":1.4172, "x":11.16913, "y":4.4, "heading":0.00096, "vx":0.55754, "vy":0.0, "omega":-0.00145, "ax":-0.9995, "ay":0.0, "alpha":-0.00226, "fx":[-11.36362,-11.26337,-11.3548,-11.35479], "fy":[0.00594,-0.00593,-0.00594,0.00593]}, - {"t":1.46014, "x":11.19215, "y":4.4, "heading":0.0009, "vx":0.51462, "vy":0.0, "omega":-0.00154, "ax":-0.99954, "ay":0.0, "alpha":0.00191, "fx":[-11.33851,-11.33852,-11.33075,-11.33074], "fy":[0.00513,-0.00513,-0.00513,0.00513]}, - {"t":1.50309, "x":11.21333, "y":4.4, "heading":0.00083, "vx":0.47169, "vy":0.0, "omega":-0.00146, "ax":-0.99958, "ay":0.0, "alpha":-0.00185, "fx":[-11.35954,-11.27638,-11.35212,-11.35211], "fy":[0.00498,-0.00497,-0.00498,0.00497]}, - {"t":1.54603, "x":11.23266, "y":4.4, "heading":0.00077, "vx":0.42876, "vy":0.0, "omega":-0.00154, "ax":-0.99961, "ay":0.0, "alpha":0.00163, "fx":[-11.3387,-11.3387,-11.33207,-11.33207], "fy":[0.00439,-0.00439,-0.00439,0.00439]}, - {"t":1.58898, "x":11.25015, "y":4.4, "heading":0.0007, "vx":0.38583, "vy":0.0, "omega":-0.00147, "ax":-0.99964, "ay":0.0, "alpha":-0.00156, "fx":[-11.35671,-11.28554,-11.35025,-11.35025], "fy":[0.00433,-0.00433,-0.00433,0.00433]}, - {"t":1.63192, "x":11.2658, "y":4.4, "heading":0.00064, "vx":0.34291, "vy":0.0, "omega":-0.00154, "ax":-0.99966, "ay":0.0, "alpha":0.00145, "fx":[-11.33891,-11.33891,-11.33299,-11.33299], "fy":[0.00391,-0.00391,-0.00391,0.00391]}, - {"t":1.67487, "x":11.27961, "y":4.4, "heading":0.00057, "vx":0.29997, "vy":0.0, "omega":-0.00148, "ax":-0.00092, "ay":0.0, "alpha":0.01124, "fx":[0.04336,-0.18163,0.04835,0.04835], "fy":[0.00338,-0.00335,-0.00336,0.00338]}, - {"t":1.71956, "x":11.29301, "y":4.4, "heading":0.00051, "vx":0.29993, "vy":0.0, "omega":-0.00097, "ax":0.0, "ay":0.0, "alpha":0.00142, "fx":[-0.00287,-0.00287,0.00287,0.00287], "fy":[0.00383,-0.00399,-0.00377,0.00383]}, - {"t":1.76426, "x":11.30642, "y":4.4, "heading":0.00047, "vx":0.29993, "vy":0.0, "omega":-0.00091, "ax":0.0, "ay":0.0, "alpha":0.00059, "fx":[-0.00727,0.01063,-0.00168,-0.00168], "fy":[0.00377,-0.00373,-0.00373,0.00376]}, - {"t":1.80896, "x":11.31983, "y":4.4, "heading":0.00042, "vx":0.29993, "vy":0.0, "omega":-0.00088, "ax":0.0, "ay":0.0, "alpha":0.00131, "fx":[-0.00263,-0.00263,0.00263,0.00263], "fy":[0.00353,-0.00367,-0.00346,0.00353]}, - {"t":1.85366, "x":11.33323, "y":4.4, "heading":0.00038, "vx":0.29993, "vy":0.0, "omega":-0.00083, "ax":0.0, "ay":0.0, "alpha":0.00117, "fx":[-0.00272,-0.00173,0.00222,0.00223], "fy":[0.00331,-0.00327,-0.00327,0.00331]}, - {"t":1.89836, "x":11.34664, "y":4.4, "heading":0.00035, "vx":0.29993, "vy":0.0, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.00116, "fx":[-0.00231,-0.00231,0.00231,0.00231], "fy":[0.00314,-0.00326,-0.00308,0.00313]}, - {"t":1.94305, "x":11.36005, "y":4.4, "heading":0.00031, "vx":0.29993, "vy":0.0, "omega":-0.00072, "ax":0.0, "ay":0.0, "alpha":0.00108, "fx":[-0.00218,-0.00226,0.00222,0.00222], "fy":[0.00292,-0.00288,-0.00288,0.00291]}, - {"t":1.98775, "x":11.37345, "y":4.4, "heading":0.00028, "vx":0.29993, "vy":0.0, "omega":-0.00067, "ax":0.0, "ay":0.0, "alpha":0.00104, "fx":[-0.00205,-0.00205,0.00205,0.00205], "fy":[0.00281,-0.00293,-0.00277,0.00281]}, - {"t":2.03245, "x":11.38686, "y":4.4, "heading":0.00025, "vx":0.29993, "vy":0.0, "omega":-0.00063, "ax":0.0, "ay":0.0, "alpha":0.00097, "fx":[-0.00199,-0.00202,0.00201,0.00201], "fy":[0.00261,-0.00257,-0.00257,0.00261]}, - {"t":2.07715, "x":11.40026, "y":4.4, "heading":0.00022, "vx":0.29993, "vy":0.0, "omega":-0.00058, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00185,-0.00185,0.00185,0.00185], "fy":[0.00258,-0.00269,-0.00255,0.00258]}, - {"t":2.12184, "x":11.41367, "y":4.4, "heading":0.0002, "vx":0.29993, "vy":0.0, "omega":-0.00054, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00186,-0.00187,0.00187,0.00187], "fy":[0.00239,-0.00235,-0.00235,0.00239]}, - {"t":2.16654, "x":11.42708, "y":4.4, "heading":0.00017, "vx":0.29993, "vy":0.0, "omega":-0.0005, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00173,-0.00173,0.00173,0.00173], "fy":[0.00245,-0.00256,-0.00243,0.00245]}, - {"t":2.21124, "x":11.44048, "y":4.4, "heading":0.00015, "vx":0.29993, "vy":0.0, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.00086, "fx":[-0.00181,-0.00183,0.00182,0.00182], "fy":[0.00229,-0.00225,-0.00225,0.00229]}, - {"t":2.25594, "x":11.45389, "y":4.4, "heading":0.00013, "vx":0.29993, "vy":0.0, "omega":-0.00042, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.0017,-0.0017,0.0017,0.0017], "fy":[0.00245,-0.00255,-0.00244,0.00245]}, - {"t":2.30063, "x":11.4673, "y":4.4, "heading":0.00011, "vx":0.29993, "vy":0.0, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00187,-0.00191,0.00189,0.00189], "fy":[0.00234,-0.00229,-0.00229,0.00234]}, - {"t":2.34533, "x":11.4807, "y":4.4, "heading":0.00009, "vx":0.29993, "vy":0.0, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00181,-0.00181,0.00181,0.00181], "fy":[0.00262,-0.00272,-0.00261,0.00262]}, - {"t":2.39003, "x":11.49411, "y":4.4, "heading":0.00008, "vx":0.29993, "vy":0.0, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00098, "fx":[-0.00205,-0.00217,0.00211,0.00211], "fy":[0.00259,-0.00254,-0.00254,0.00258]}, - {"t":2.43473, "x":11.50752, "y":4.4, "heading":0.00006, "vx":0.29993, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00109, "fx":[-0.00209,-0.00209,0.00209,0.00209], "fy":[0.00301,-0.00311,-0.00301,0.00301]}, - {"t":2.47942, "x":11.52092, "y":4.4, "heading":0.00005, "vx":0.29993, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00118, "fx":[-0.00247,-0.00255,0.00251,0.00251], "fy":[0.00311,-0.00307,-0.00307,0.00311]}, - {"t":2.52412, "x":11.53433, "y":4.4, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00262,-0.00262,0.00262,0.00262], "fy":[0.00373,-0.00383,-0.00372,0.00373]}, - {"t":2.56882, "x":11.54773, "y":4.4, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00408,-0.00049,0.00228,0.00229], "fy":[0.00405,-0.004,-0.004,0.00405]}, - {"t":2.61352, "x":11.56114, "y":4.4, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00178, "fx":[-0.00349,-0.00349,0.00349,0.00349], "fy":[0.00487,-0.00498,-0.00486,0.00487]}, - {"t":2.65821, "x":11.57455, "y":4.4, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00158, "fx":[-0.00685,0.00365,0.0016,0.0016], "fy":[0.00549,-0.00544,-0.00544,0.00549]}, - {"t":2.70291, "x":11.58795, "y":4.4, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00012, "ax":-0.00001, "ay":0.0, "alpha":0.00239, "fx":[-0.00484,-0.00484,0.00469,0.00469], "fy":[0.00647,-0.00659,-0.00648,0.00647]}, - {"t":2.74761, "x":11.60136, "y":4.4, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":0.00022, "ax":-0.71391, "ay":0.0, "alpha":-0.00874, "fx":[-8.16629,-7.90667,-8.15479,-8.15479], "fy":[0.0075,-0.00747,-0.00747,0.0075]}, - {"t":2.79231, "x":11.61405, "y":4.4, "heading":0.00005, "vx":0.26802, "vy":0.0, "omega":-0.00017, "ax":-0.99852, "ay":0.0, "alpha":0.00234, "fx":[-11.32759,-11.32759,-11.31835,-11.31835], "fy":[0.00639,-0.0064,-0.00638,0.00639]}, - {"t":2.83701, "x":11.62503, "y":4.4, "heading":0.00004, "vx":0.22339, "vy":0.0, "omega":-0.00006, "ax":-0.99926, "ay":0.0, "alpha":-0.00436, "fx":[-11.37216,-11.22667,-11.36333,-11.36333], "fy":[0.00558,-0.00558,-0.00558,0.00558]}, - {"t":2.8817, "x":11.63402, "y":4.4, "heading":0.00004, "vx":0.17873, "vy":0.0, "omega":-0.00026, "ax":-0.9995, "ay":0.0, "alpha":0.00179, "fx":[-11.3376,-11.3376,-11.33076,-11.33076], "fy":[0.00499,-0.00499,-0.00499,0.00499]}, - {"t":2.9264, "x":11.64101, "y":4.4, "heading":0.00003, "vx":0.13405, "vy":0.0, "omega":-0.00018, "ax":-0.99963, "ay":0.0, "alpha":-0.0015, "fx":[-11.35726,-11.28522,-11.34994,-11.34994], "fy":[0.00442,-0.00442,-0.00442,0.00442]}, - {"t":2.9711, "x":11.64601, "y":4.4, "heading":0.00002, "vx":0.08937, "vy":0.0, "omega":-0.00025, "ax":-0.9997, "ay":0.0, "alpha":0.00165, "fx":[-11.33951,-11.33951,-11.33335,-11.33335], "fy":[0.00466,-0.00466,-0.00465,0.00466]}, - {"t":3.0158, "x":11.649, "y":4.4, "heading":0.00001, "vx":0.04469, "vy":0.0, "omega":-0.00017, "ax":-0.99975, "ay":0.0, "alpha":0.00385, "fx":[-11.35244,-11.35244,-11.34552,-11.29757], "fy":[0.00464,-0.00463,-0.00464,0.00463]}, - {"t":3.06049, "x":11.65, "y":4.4, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":10.35, "y":4.25, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99952, "ay":0.0, "alpha":0.00407, "fx":[11.35864,11.26366,11.3576,11.3576], "fy":[-0.00025,0.00025,0.00025,-0.00025]}, + {"t":0.04295, "x":10.35092, "y":4.25, "heading":0.0, "vx":0.04292, "vy":0.0, "omega":0.00017, "ax":0.99975, "ay":0.0, "alpha":0.00197, "fx":[11.34962,11.30129,11.34847,11.34847], "fy":[-0.00032,0.00032,0.00032,-0.00032]}, + {"t":0.08589, "x":10.35369, "y":4.25, "heading":0.00001, "vx":0.08586, "vy":0.0, "omega":0.00026, "ax":0.99974, "ay":0.0, "alpha":-0.00023, "fx":[11.33708,11.33708,11.33656,11.33656], "fy":[-0.00078,0.00078,0.00077,-0.00078]}, + {"t":0.12884, "x":10.3583, "y":4.25, "heading":0.00002, "vx":0.12879, "vy":0.0, "omega":0.00025, "ax":0.99972, "ay":0.0, "alpha":0.0021, "fx":[11.35075,11.29748,11.34923,11.34923], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, + {"t":0.17178, "x":10.36475, "y":4.25, "heading":0.00003, "vx":0.17173, "vy":0.0, "omega":0.00034, "ax":0.99971, "ay":0.0, "alpha":-0.00027, "fx":[11.33682,11.33682,11.33618,11.33618], "fy":[-0.00088,0.00089,0.00088,-0.00088]}, + {"t":0.21473, "x":10.37305, "y":4.25, "heading":0.00004, "vx":0.21466, "vy":0.0, "omega":0.00033, "ax":0.99969, "ay":0.0, "alpha":0.00237, "fx":[11.35193,11.29253,11.35038,11.35038], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, + {"t":0.25767, "x":10.38319, "y":4.25, "heading":0.00006, "vx":0.25759, "vy":0.0, "omega":0.00043, "ax":0.99967, "ay":0.0, "alpha":-0.00024, "fx":[11.33634,11.33634,11.33583,11.33583], "fy":[-0.00082,0.00082,0.00081,-0.00082]}, + {"t":0.30062, "x":10.39517, "y":4.25, "heading":0.00008, "vx":0.30052, "vy":0.0, "omega":0.00042, "ax":0.99965, "ay":0.0, "alpha":0.00276, "fx":[11.35336,11.28612,11.35195,11.35195], "fy":[-0.00044,0.00044,0.00044,-0.00044]}, + {"t":0.34356, "x":10.409, "y":4.25, "heading":0.00009, "vx":0.34345, "vy":0.0, "omega":0.00054, "ax":0.99963, "ay":0.0, "alpha":-0.00019, "fx":[11.33572,11.33572,11.33542,11.33542], "fy":[-0.00068,0.00068,0.00067,-0.00068]}, + {"t":0.38651, "x":10.42467, "y":4.25, "heading":0.00012, "vx":0.38638, "vy":0.0, "omega":0.00053, "ax":0.9996, "ay":0.0, "alpha":0.00326, "fx":[11.35528,11.27762,11.35405,11.35405], "fy":[-0.00033,0.00033,0.00033,-0.00033]}, + {"t":0.42945, "x":10.44219, "y":4.25, "heading":0.00014, "vx":0.42931, "vy":0.0, "omega":0.00067, "ax":0.99957, "ay":0.0, "alpha":-0.00015, "fx":[11.33495,11.33495,11.3348,11.3348], "fy":[-0.00057,0.00057,0.00056,-0.00057]}, + {"t":0.4724, "x":10.46154, "y":4.25, "heading":0.00017, "vx":0.47224, "vy":0.0, "omega":0.00066, "ax":0.99953, "ay":0.0, "alpha":0.00391, "fx":[11.35812,11.26582,11.35692,11.35691], "fy":[-0.00035,0.00035,0.00035,-0.00035]}, + {"t":0.51534, "x":10.48275, "y":4.25, "heading":0.0002, "vx":0.51516, "vy":0.0, "omega":0.00083, "ax":0.99948, "ay":0.0, "alpha":-0.00017, "fx":[11.33406,11.33406,11.33378,11.33378], "fy":[-0.0006,0.0006,0.00059,-0.0006]}, + {"t":0.55829, "x":10.50579, "y":4.25, "heading":0.00023, "vx":0.55808, "vy":0.0, "omega":0.00083, "ax":0.99942, "ay":0.0, "alpha":0.0047, "fx":[11.36209,11.24992,11.36057,11.36056], "fy":[-0.00064,0.00064,0.00064,-0.00064]}, + {"t":0.60123, "x":10.53068, "y":4.25, "heading":0.00027, "vx":0.60101, "vy":0.0, "omega":0.00103, "ax":0.99935, "ay":0.0, "alpha":-0.00032, "fx":[11.33298,11.33299,11.33201,11.332], "fy":[-0.00096,0.00097,0.00096,-0.00096]}, + {"t":0.64418, "x":10.55741, "y":4.25, "heading":0.00031, "vx":0.64392, "vy":0.0, "omega":0.00101, "ax":0.99927, "ay":0.0, "alpha":0.00602, "fx":[11.36998,11.2212,11.36739,11.36739], "fy":[-0.00151,0.00151,0.00151,-0.00151]}, + {"t":0.68712, "x":10.58599, "y":4.25, "heading":0.00036, "vx":0.68684, "vy":0.0, "omega":0.00127, "ax":0.99915, "ay":0.0, "alpha":-0.00069, "fx":[11.33151,11.33151,11.32882,11.32882], "fy":[-0.00192,0.00192,0.00191,-0.00192]}, + {"t":0.73007, "x":10.6164, "y":4.25, "heading":0.00041, "vx":0.72975, "vy":0.0, "omega":0.00124, "ax":0.99899, "ay":0.0, "alpha":0.00885, "fx":[11.38745,11.16084,11.38254,11.38253], "fy":[-0.00331,0.00331,0.00331,-0.00331]}, + {"t":0.77302, "x":10.64866, "y":4.25, "heading":0.00046, "vx":0.77265, "vy":0.0, "omega":0.00162, "ax":0.99875, "ay":0.0, "alpha":-0.00141, "fx":[11.3286,11.3286,11.32273,11.32273], "fy":[-0.00374,0.00374,0.00373,-0.00373]}, + {"t":0.81596, "x":10.68277, "y":4.25, "heading":0.00053, "vx":0.81554, "vy":0.0, "omega":0.00156, "ax":0.99837, "ay":0.0, "alpha":0.01922, "fx":[11.44747,10.96147,11.43825,11.43825], "fy":[-0.00654,0.00654,0.00654,-0.00654]}, + {"t":0.85891, "x":10.71871, "y":4.25, "heading":0.0006, "vx":0.85841, "vy":0.0, "omega":0.00239, "ax":0.99766, "ay":0.0, "alpha":-0.00229, "fx":[11.3182,11.3182,11.30838,11.30838], "fy":[-0.00596,0.00596,0.00596,-0.00596]}, + {"t":0.90185, "x":10.7565, "y":4.25, "heading":0.0007, "vx":0.90126, "vy":0.0, "omega":0.00229, "ax":0.99585, "ay":0.0, "alpha":0.10599, "fx":[11.92148,9.43985,11.90474,11.90473], "fy":[-0.0115,0.0115,0.01149,-0.01149]}, + {"t":0.9448, "x":10.79612, "y":4.25, "heading":0.0008, "vx":0.94403, "vy":0.0, "omega":0.00684, "ax":0.98155, "ay":0.0, "alpha":0.00128, "fx":[11.12811,11.1281,11.13297,11.13297], "fy":[0.00357,-0.0038,-0.00341,0.00356]}, + {"t":0.98774, "x":10.83757, "y":4.25, "heading":0.0011, "vx":0.98618, "vy":0.0, "omega":0.0069, "ax":-0.99236, "ay":0.0, "alpha":-0.16455, "fx":[-12.24271,-8.35081,-12.20963,-12.20962], "fy":[0.02215,-0.02215,-0.02215,0.02214]}, + {"t":1.03069, "x":10.879, "y":4.25, "heading":0.00139, "vx":0.94356, "vy":0.0, "omega":-0.00017, "ax":-0.99685, "ay":0.0, "alpha":0.00544, "fx":[-11.31527,-11.31527,-11.29292,-11.29292], "fy":[0.01455,-0.01467,-0.01442,0.01455]}, + {"t":1.07363, "x":10.91861, "y":4.25, "heading":0.00138, "vx":0.90075, "vy":0.0, "omega":0.00006, "ax":-0.99802, "ay":0.0, "alpha":-0.03316, "fx":[-11.5521,-10.66316,-11.52703,-11.52702], "fy":[0.0171,-0.0171,-0.0171,0.0171]}, + {"t":1.11658, "x":10.95637, "y":4.25, "heading":0.00139, "vx":0.85789, "vy":0.0, "omega":-0.00136, "ax":-0.99855, "ay":0.0, "alpha":0.0049, "fx":[-11.3335,-11.3335,-11.31332,-11.31331], "fy":[0.01309,-0.01313,-0.01304,0.01309]}, + {"t":1.15952, "x":10.99229, "y":4.25, "heading":0.00133, "vx":0.81501, "vy":0.0, "omega":-0.00115, "ax":-0.99886, "ay":0.0, "alpha":-0.00871, "fx":[-11.41102,-11.11118,-11.39271,-11.3927], "fy":[0.01252,-0.01252,-0.01252,0.01252]}, + {"t":1.20247, "x":11.02637, "y":4.25, "heading":0.00128, "vx":0.77211, "vy":0.0, "omega":-0.00153, "ax":-0.99906, "ay":0.0, "alpha":0.00373, "fx":[-11.33684,-11.33684,-11.3215,-11.32149], "fy":[0.01,-0.01002,-0.00998,0.01]}, + {"t":1.24541, "x":11.05861, "y":4.25, "heading":0.00121, "vx":0.72921, "vy":0.0, "omega":-0.00136, "ax":-0.9992, "ay":0.0, "alpha":-0.00426, "fx":[-11.3811,-11.20736,-11.36729,-11.36728], "fy":[0.00939,-0.00938,-0.00939,0.00938]}, + {"t":1.28836, "x":11.089, "y":4.25, "heading":0.00116, "vx":0.6863, "vy":0.0, "omega":-0.00155, "ax":-0.99931, "ay":0.0, "alpha":0.00289, "fx":[-11.33784,-11.33784,-11.32603,-11.32603], "fy":[0.00774,-0.00775,-0.00773,0.00774]}, + {"t":1.3313, "x":11.11755, "y":4.25, "heading":0.00109, "vx":0.64338, "vy":0.0, "omega":-0.00142, "ax":-0.99939, "ay":0.0, "alpha":-0.00286, "fx":[-11.36955,-11.24438,-11.35872,-11.35871], "fy":[0.00732,-0.00732,-0.00732,0.00732]}, + {"t":1.37425, "x":11.14426, "y":4.25, "heading":0.00103, "vx":0.60046, "vy":0.0, "omega":-0.00155, "ax":-0.99945, "ay":0.0, "alpha":0.00231, "fx":[-11.33826,-11.33827,-11.32885,-11.32885], "fy":[0.0062,-0.0062,-0.00619,0.0062]}, + {"t":1.4172, "x":11.16913, "y":4.25, "heading":0.00096, "vx":0.55754, "vy":0.0, "omega":-0.00145, "ax":-0.9995, "ay":0.0, "alpha":-0.00226, "fx":[-11.36362,-11.26337,-11.3548,-11.35479], "fy":[0.00594,-0.00593,-0.00594,0.00593]}, + {"t":1.46014, "x":11.19215, "y":4.25, "heading":0.0009, "vx":0.51462, "vy":0.0, "omega":-0.00154, "ax":-0.99954, "ay":0.0, "alpha":0.00191, "fx":[-11.33851,-11.33852,-11.33075,-11.33074], "fy":[0.00513,-0.00513,-0.00513,0.00513]}, + {"t":1.50309, "x":11.21333, "y":4.25, "heading":0.00083, "vx":0.47169, "vy":0.0, "omega":-0.00146, "ax":-0.99958, "ay":0.0, "alpha":-0.00185, "fx":[-11.35954,-11.27638,-11.35212,-11.35211], "fy":[0.00498,-0.00497,-0.00498,0.00497]}, + {"t":1.54603, "x":11.23266, "y":4.25, "heading":0.00077, "vx":0.42876, "vy":0.0, "omega":-0.00154, "ax":-0.99961, "ay":0.0, "alpha":0.00163, "fx":[-11.3387,-11.3387,-11.33207,-11.33207], "fy":[0.00439,-0.00439,-0.00439,0.00439]}, + {"t":1.58898, "x":11.25015, "y":4.25, "heading":0.0007, "vx":0.38583, "vy":0.0, "omega":-0.00147, "ax":-0.99964, "ay":0.0, "alpha":-0.00156, "fx":[-11.35671,-11.28554,-11.35025,-11.35025], "fy":[0.00433,-0.00433,-0.00433,0.00433]}, + {"t":1.63192, "x":11.2658, "y":4.25, "heading":0.00064, "vx":0.34291, "vy":0.0, "omega":-0.00154, "ax":-0.99966, "ay":0.0, "alpha":0.00145, "fx":[-11.33891,-11.33891,-11.33299,-11.33299], "fy":[0.00391,-0.00391,-0.00391,0.00391]}, + {"t":1.67487, "x":11.27961, "y":4.25, "heading":0.00057, "vx":0.29997, "vy":0.0, "omega":-0.00148, "ax":-0.00092, "ay":0.0, "alpha":0.01124, "fx":[0.04336,-0.18163,0.04835,0.04835], "fy":[0.00338,-0.00335,-0.00336,0.00338]}, + {"t":1.71956, "x":11.29301, "y":4.25, "heading":0.00051, "vx":0.29993, "vy":0.0, "omega":-0.00097, "ax":0.0, "ay":0.0, "alpha":0.00142, "fx":[-0.00287,-0.00287,0.00287,0.00287], "fy":[0.00383,-0.00399,-0.00377,0.00383]}, + {"t":1.76426, "x":11.30642, "y":4.25, "heading":0.00047, "vx":0.29993, "vy":0.0, "omega":-0.00091, "ax":0.0, "ay":0.0, "alpha":0.00059, "fx":[-0.00727,0.01063,-0.00168,-0.00168], "fy":[0.00377,-0.00373,-0.00373,0.00376]}, + {"t":1.80896, "x":11.31983, "y":4.25, "heading":0.00042, "vx":0.29993, "vy":0.0, "omega":-0.00088, "ax":0.0, "ay":0.0, "alpha":0.00131, "fx":[-0.00263,-0.00263,0.00263,0.00263], "fy":[0.00353,-0.00367,-0.00346,0.00353]}, + {"t":1.85366, "x":11.33323, "y":4.25, "heading":0.00038, "vx":0.29993, "vy":0.0, "omega":-0.00083, "ax":0.0, "ay":0.0, "alpha":0.00117, "fx":[-0.00272,-0.00173,0.00222,0.00223], "fy":[0.00331,-0.00327,-0.00327,0.00331]}, + {"t":1.89836, "x":11.34664, "y":4.25, "heading":0.00035, "vx":0.29993, "vy":0.0, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.00116, "fx":[-0.00231,-0.00231,0.00231,0.00231], "fy":[0.00314,-0.00326,-0.00308,0.00313]}, + {"t":1.94305, "x":11.36005, "y":4.25, "heading":0.00031, "vx":0.29993, "vy":0.0, "omega":-0.00072, "ax":0.0, "ay":0.0, "alpha":0.00108, "fx":[-0.00218,-0.00226,0.00222,0.00222], "fy":[0.00292,-0.00288,-0.00288,0.00291]}, + {"t":1.98775, "x":11.37345, "y":4.25, "heading":0.00028, "vx":0.29993, "vy":0.0, "omega":-0.00067, "ax":0.0, "ay":0.0, "alpha":0.00104, "fx":[-0.00205,-0.00205,0.00205,0.00205], "fy":[0.00281,-0.00293,-0.00277,0.00281]}, + {"t":2.03245, "x":11.38686, "y":4.25, "heading":0.00025, "vx":0.29993, "vy":0.0, "omega":-0.00063, "ax":0.0, "ay":0.0, "alpha":0.00097, "fx":[-0.00199,-0.00202,0.00201,0.00201], "fy":[0.00261,-0.00257,-0.00257,0.00261]}, + {"t":2.07715, "x":11.40026, "y":4.25, "heading":0.00022, "vx":0.29993, "vy":0.0, "omega":-0.00058, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00185,-0.00185,0.00185,0.00185], "fy":[0.00258,-0.00269,-0.00255,0.00258]}, + {"t":2.12184, "x":11.41367, "y":4.25, "heading":0.0002, "vx":0.29993, "vy":0.0, "omega":-0.00054, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00186,-0.00187,0.00187,0.00187], "fy":[0.00239,-0.00235,-0.00235,0.00239]}, + {"t":2.16654, "x":11.42708, "y":4.25, "heading":0.00017, "vx":0.29993, "vy":0.0, "omega":-0.0005, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00173,-0.00173,0.00173,0.00173], "fy":[0.00245,-0.00256,-0.00243,0.00245]}, + {"t":2.21124, "x":11.44048, "y":4.25, "heading":0.00015, "vx":0.29993, "vy":0.0, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.00086, "fx":[-0.00181,-0.00183,0.00182,0.00182], "fy":[0.00229,-0.00225,-0.00225,0.00229]}, + {"t":2.25594, "x":11.45389, "y":4.25, "heading":0.00013, "vx":0.29993, "vy":0.0, "omega":-0.00042, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.0017,-0.0017,0.0017,0.0017], "fy":[0.00245,-0.00255,-0.00244,0.00245]}, + {"t":2.30063, "x":11.4673, "y":4.25, "heading":0.00011, "vx":0.29993, "vy":0.0, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00187,-0.00191,0.00189,0.00189], "fy":[0.00234,-0.00229,-0.00229,0.00234]}, + {"t":2.34533, "x":11.4807, "y":4.25, "heading":0.00009, "vx":0.29993, "vy":0.0, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00181,-0.00181,0.00181,0.00181], "fy":[0.00262,-0.00272,-0.00261,0.00262]}, + {"t":2.39003, "x":11.49411, "y":4.25, "heading":0.00008, "vx":0.29993, "vy":0.0, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00098, "fx":[-0.00205,-0.00217,0.00211,0.00211], "fy":[0.00259,-0.00254,-0.00254,0.00258]}, + {"t":2.43473, "x":11.50752, "y":4.25, "heading":0.00006, "vx":0.29993, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00109, "fx":[-0.00209,-0.00209,0.00209,0.00209], "fy":[0.00301,-0.00311,-0.00301,0.00301]}, + {"t":2.47942, "x":11.52092, "y":4.25, "heading":0.00005, "vx":0.29993, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00118, "fx":[-0.00247,-0.00255,0.00251,0.00251], "fy":[0.00311,-0.00307,-0.00307,0.00311]}, + {"t":2.52412, "x":11.53433, "y":4.25, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00262,-0.00262,0.00262,0.00262], "fy":[0.00373,-0.00383,-0.00372,0.00373]}, + {"t":2.56882, "x":11.54773, "y":4.25, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00408,-0.00049,0.00228,0.00229], "fy":[0.00405,-0.004,-0.004,0.00405]}, + {"t":2.61352, "x":11.56114, "y":4.25, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00178, "fx":[-0.00349,-0.00349,0.00349,0.00349], "fy":[0.00487,-0.00498,-0.00486,0.00487]}, + {"t":2.65821, "x":11.57455, "y":4.25, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00158, "fx":[-0.00685,0.00365,0.0016,0.0016], "fy":[0.00549,-0.00544,-0.00544,0.00549]}, + {"t":2.70291, "x":11.58795, "y":4.25, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00012, "ax":-0.00001, "ay":0.0, "alpha":0.00239, "fx":[-0.00484,-0.00484,0.00469,0.00469], "fy":[0.00647,-0.00659,-0.00648,0.00647]}, + {"t":2.74761, "x":11.60136, "y":4.25, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":0.00022, "ax":-0.71391, "ay":0.0, "alpha":-0.00874, "fx":[-8.16629,-7.90667,-8.15479,-8.15479], "fy":[0.0075,-0.00747,-0.00747,0.0075]}, + {"t":2.79231, "x":11.61405, "y":4.25, "heading":0.00005, "vx":0.26802, "vy":0.0, "omega":-0.00017, "ax":-0.99852, "ay":0.0, "alpha":0.00234, "fx":[-11.32759,-11.32759,-11.31835,-11.31835], "fy":[0.00639,-0.0064,-0.00638,0.00639]}, + {"t":2.83701, "x":11.62503, "y":4.25, "heading":0.00004, "vx":0.22339, "vy":0.0, "omega":-0.00006, "ax":-0.99926, "ay":0.0, "alpha":-0.00436, "fx":[-11.37216,-11.22667,-11.36333,-11.36333], "fy":[0.00558,-0.00558,-0.00558,0.00558]}, + {"t":2.8817, "x":11.63402, "y":4.25, "heading":0.00004, "vx":0.17873, "vy":0.0, "omega":-0.00026, "ax":-0.9995, "ay":0.0, "alpha":0.00179, "fx":[-11.3376,-11.3376,-11.33076,-11.33076], "fy":[0.00499,-0.00499,-0.00499,0.00499]}, + {"t":2.9264, "x":11.64101, "y":4.25, "heading":0.00003, "vx":0.13405, "vy":0.0, "omega":-0.00018, "ax":-0.99963, "ay":0.0, "alpha":-0.0015, "fx":[-11.35726,-11.28522,-11.34994,-11.34994], "fy":[0.00442,-0.00442,-0.00442,0.00442]}, + {"t":2.9711, "x":11.64601, "y":4.25, "heading":0.00002, "vx":0.08937, "vy":0.0, "omega":-0.00025, "ax":-0.9997, "ay":0.0, "alpha":0.00165, "fx":[-11.33951,-11.33951,-11.33335,-11.33335], "fy":[0.00466,-0.00466,-0.00465,0.00466]}, + {"t":3.0158, "x":11.649, "y":4.25, "heading":0.00001, "vx":0.04469, "vy":0.0, "omega":-0.00017, "ax":-0.99975, "ay":0.0, "alpha":0.00385, "fx":[-11.35244,-11.35244,-11.34552,-11.29757], "fy":[0.00464,-0.00463,-0.00464,0.00463]}, + {"t":3.06049, "x":11.65, "y":4.25, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/elevator/Lifter.java b/src/main/java/frc/robot/elevator/Lifter.java index 61d5acf4..d7bc3481 100644 --- a/src/main/java/frc/robot/elevator/Lifter.java +++ b/src/main/java/frc/robot/elevator/Lifter.java @@ -216,8 +216,8 @@ public Command createJoystickControlCommand(XboxController gamepad) { return new FunctionalCommand( // initialize () -> { - matchHeight(); - resetController(); + // matchHeight(); + // resetController(); }, // execute () -> { From 9be8f767edd839e0c58d912b64bfb818580dc672 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sun, 23 Mar 2025 09:39:02 -0400 Subject: [PATCH 19/24] Another attempt to prevent skyrocket on teleop init --- src/main/java/frc/robot/Robot.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9e03e045..a55146d6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -211,6 +211,8 @@ public void teleopInit() { swerve.setDefaultCommand( new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID())); + lifter.matchHeight(); + lifter.resetController(); lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID())); // lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID())); leds.replaceDefaultCommandImmediately( From 7dbda8c740bf8dc7a7f78d788bd3a23f205eac4d Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sun, 23 Mar 2025 10:04:33 -0400 Subject: [PATCH 20/24] Adjusted path for 1 piece auto --- src/main/deploy/choreo/redCenterToL4G.traj | 154 ++++++++++----------- 1 file changed, 77 insertions(+), 77 deletions(-) diff --git a/src/main/deploy/choreo/redCenterToL4G.traj b/src/main/deploy/choreo/redCenterToL4G.traj index 496a6a0d..b2e743bf 100644 --- a/src/main/deploy/choreo/redCenterToL4G.traj +++ b/src/main/deploy/choreo/redCenterToL4G.traj @@ -3,9 +3,9 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":10.35, "y":4.25, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":11.279606819152832, "y":4.25, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":11.65, "y":4.25, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":10.35, "y":4.22, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":11.279606819152832, "y":4.22, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":11.65, "y":4.22, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -18,9 +18,9 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.25 m", "val":4.25}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.25 m", "val":4.25}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.25 m", "val":4.25}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, @@ -38,77 +38,77 @@ "sampleType":"Swerve", "waypoints":[0.0,1.67487,3.06049], "samples":[ - {"t":0.0, "x":10.35, "y":4.25, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99952, "ay":0.0, "alpha":0.00407, "fx":[11.35864,11.26366,11.3576,11.3576], "fy":[-0.00025,0.00025,0.00025,-0.00025]}, - {"t":0.04295, "x":10.35092, "y":4.25, "heading":0.0, "vx":0.04292, "vy":0.0, "omega":0.00017, "ax":0.99975, "ay":0.0, "alpha":0.00197, "fx":[11.34962,11.30129,11.34847,11.34847], "fy":[-0.00032,0.00032,0.00032,-0.00032]}, - {"t":0.08589, "x":10.35369, "y":4.25, "heading":0.00001, "vx":0.08586, "vy":0.0, "omega":0.00026, "ax":0.99974, "ay":0.0, "alpha":-0.00023, "fx":[11.33708,11.33708,11.33656,11.33656], "fy":[-0.00078,0.00078,0.00077,-0.00078]}, - {"t":0.12884, "x":10.3583, "y":4.25, "heading":0.00002, "vx":0.12879, "vy":0.0, "omega":0.00025, "ax":0.99972, "ay":0.0, "alpha":0.0021, "fx":[11.35075,11.29748,11.34923,11.34923], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, - {"t":0.17178, "x":10.36475, "y":4.25, "heading":0.00003, "vx":0.17173, "vy":0.0, "omega":0.00034, "ax":0.99971, "ay":0.0, "alpha":-0.00027, "fx":[11.33682,11.33682,11.33618,11.33618], "fy":[-0.00088,0.00089,0.00088,-0.00088]}, - {"t":0.21473, "x":10.37305, "y":4.25, "heading":0.00004, "vx":0.21466, "vy":0.0, "omega":0.00033, "ax":0.99969, "ay":0.0, "alpha":0.00237, "fx":[11.35193,11.29253,11.35038,11.35038], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, - {"t":0.25767, "x":10.38319, "y":4.25, "heading":0.00006, "vx":0.25759, "vy":0.0, "omega":0.00043, "ax":0.99967, "ay":0.0, "alpha":-0.00024, "fx":[11.33634,11.33634,11.33583,11.33583], "fy":[-0.00082,0.00082,0.00081,-0.00082]}, - {"t":0.30062, "x":10.39517, "y":4.25, "heading":0.00008, "vx":0.30052, "vy":0.0, "omega":0.00042, "ax":0.99965, "ay":0.0, "alpha":0.00276, "fx":[11.35336,11.28612,11.35195,11.35195], "fy":[-0.00044,0.00044,0.00044,-0.00044]}, - {"t":0.34356, "x":10.409, "y":4.25, "heading":0.00009, "vx":0.34345, "vy":0.0, "omega":0.00054, "ax":0.99963, "ay":0.0, "alpha":-0.00019, "fx":[11.33572,11.33572,11.33542,11.33542], "fy":[-0.00068,0.00068,0.00067,-0.00068]}, - {"t":0.38651, "x":10.42467, "y":4.25, "heading":0.00012, "vx":0.38638, "vy":0.0, "omega":0.00053, "ax":0.9996, "ay":0.0, "alpha":0.00326, "fx":[11.35528,11.27762,11.35405,11.35405], "fy":[-0.00033,0.00033,0.00033,-0.00033]}, - {"t":0.42945, "x":10.44219, "y":4.25, "heading":0.00014, "vx":0.42931, "vy":0.0, "omega":0.00067, "ax":0.99957, "ay":0.0, "alpha":-0.00015, "fx":[11.33495,11.33495,11.3348,11.3348], "fy":[-0.00057,0.00057,0.00056,-0.00057]}, - {"t":0.4724, "x":10.46154, "y":4.25, "heading":0.00017, "vx":0.47224, "vy":0.0, "omega":0.00066, "ax":0.99953, "ay":0.0, "alpha":0.00391, "fx":[11.35812,11.26582,11.35692,11.35691], "fy":[-0.00035,0.00035,0.00035,-0.00035]}, - {"t":0.51534, "x":10.48275, "y":4.25, "heading":0.0002, "vx":0.51516, "vy":0.0, "omega":0.00083, "ax":0.99948, "ay":0.0, "alpha":-0.00017, "fx":[11.33406,11.33406,11.33378,11.33378], "fy":[-0.0006,0.0006,0.00059,-0.0006]}, - {"t":0.55829, "x":10.50579, "y":4.25, "heading":0.00023, "vx":0.55808, "vy":0.0, "omega":0.00083, "ax":0.99942, "ay":0.0, "alpha":0.0047, "fx":[11.36209,11.24992,11.36057,11.36056], "fy":[-0.00064,0.00064,0.00064,-0.00064]}, - {"t":0.60123, "x":10.53068, "y":4.25, "heading":0.00027, "vx":0.60101, "vy":0.0, "omega":0.00103, "ax":0.99935, "ay":0.0, "alpha":-0.00032, "fx":[11.33298,11.33299,11.33201,11.332], "fy":[-0.00096,0.00097,0.00096,-0.00096]}, - {"t":0.64418, "x":10.55741, "y":4.25, "heading":0.00031, "vx":0.64392, "vy":0.0, "omega":0.00101, "ax":0.99927, "ay":0.0, "alpha":0.00602, "fx":[11.36998,11.2212,11.36739,11.36739], "fy":[-0.00151,0.00151,0.00151,-0.00151]}, - {"t":0.68712, "x":10.58599, "y":4.25, "heading":0.00036, "vx":0.68684, "vy":0.0, "omega":0.00127, "ax":0.99915, "ay":0.0, "alpha":-0.00069, "fx":[11.33151,11.33151,11.32882,11.32882], "fy":[-0.00192,0.00192,0.00191,-0.00192]}, - {"t":0.73007, "x":10.6164, "y":4.25, "heading":0.00041, "vx":0.72975, "vy":0.0, "omega":0.00124, "ax":0.99899, "ay":0.0, "alpha":0.00885, "fx":[11.38745,11.16084,11.38254,11.38253], "fy":[-0.00331,0.00331,0.00331,-0.00331]}, - {"t":0.77302, "x":10.64866, "y":4.25, "heading":0.00046, "vx":0.77265, "vy":0.0, "omega":0.00162, "ax":0.99875, "ay":0.0, "alpha":-0.00141, "fx":[11.3286,11.3286,11.32273,11.32273], "fy":[-0.00374,0.00374,0.00373,-0.00373]}, - {"t":0.81596, "x":10.68277, "y":4.25, "heading":0.00053, "vx":0.81554, "vy":0.0, "omega":0.00156, "ax":0.99837, "ay":0.0, "alpha":0.01922, "fx":[11.44747,10.96147,11.43825,11.43825], "fy":[-0.00654,0.00654,0.00654,-0.00654]}, - {"t":0.85891, "x":10.71871, "y":4.25, "heading":0.0006, "vx":0.85841, "vy":0.0, "omega":0.00239, "ax":0.99766, "ay":0.0, "alpha":-0.00229, "fx":[11.3182,11.3182,11.30838,11.30838], "fy":[-0.00596,0.00596,0.00596,-0.00596]}, - {"t":0.90185, "x":10.7565, "y":4.25, "heading":0.0007, "vx":0.90126, "vy":0.0, "omega":0.00229, "ax":0.99585, "ay":0.0, "alpha":0.10599, "fx":[11.92148,9.43985,11.90474,11.90473], "fy":[-0.0115,0.0115,0.01149,-0.01149]}, - {"t":0.9448, "x":10.79612, "y":4.25, "heading":0.0008, "vx":0.94403, "vy":0.0, "omega":0.00684, "ax":0.98155, "ay":0.0, "alpha":0.00128, "fx":[11.12811,11.1281,11.13297,11.13297], "fy":[0.00357,-0.0038,-0.00341,0.00356]}, - {"t":0.98774, "x":10.83757, "y":4.25, "heading":0.0011, "vx":0.98618, "vy":0.0, "omega":0.0069, "ax":-0.99236, "ay":0.0, "alpha":-0.16455, "fx":[-12.24271,-8.35081,-12.20963,-12.20962], "fy":[0.02215,-0.02215,-0.02215,0.02214]}, - {"t":1.03069, "x":10.879, "y":4.25, "heading":0.00139, "vx":0.94356, "vy":0.0, "omega":-0.00017, "ax":-0.99685, "ay":0.0, "alpha":0.00544, "fx":[-11.31527,-11.31527,-11.29292,-11.29292], "fy":[0.01455,-0.01467,-0.01442,0.01455]}, - {"t":1.07363, "x":10.91861, "y":4.25, "heading":0.00138, "vx":0.90075, "vy":0.0, "omega":0.00006, "ax":-0.99802, "ay":0.0, "alpha":-0.03316, "fx":[-11.5521,-10.66316,-11.52703,-11.52702], "fy":[0.0171,-0.0171,-0.0171,0.0171]}, - {"t":1.11658, "x":10.95637, "y":4.25, "heading":0.00139, "vx":0.85789, "vy":0.0, "omega":-0.00136, "ax":-0.99855, "ay":0.0, "alpha":0.0049, "fx":[-11.3335,-11.3335,-11.31332,-11.31331], "fy":[0.01309,-0.01313,-0.01304,0.01309]}, - {"t":1.15952, "x":10.99229, "y":4.25, "heading":0.00133, "vx":0.81501, "vy":0.0, "omega":-0.00115, "ax":-0.99886, "ay":0.0, "alpha":-0.00871, "fx":[-11.41102,-11.11118,-11.39271,-11.3927], "fy":[0.01252,-0.01252,-0.01252,0.01252]}, - {"t":1.20247, "x":11.02637, "y":4.25, "heading":0.00128, "vx":0.77211, "vy":0.0, "omega":-0.00153, "ax":-0.99906, "ay":0.0, "alpha":0.00373, "fx":[-11.33684,-11.33684,-11.3215,-11.32149], "fy":[0.01,-0.01002,-0.00998,0.01]}, - {"t":1.24541, "x":11.05861, "y":4.25, "heading":0.00121, "vx":0.72921, "vy":0.0, "omega":-0.00136, "ax":-0.9992, "ay":0.0, "alpha":-0.00426, "fx":[-11.3811,-11.20736,-11.36729,-11.36728], "fy":[0.00939,-0.00938,-0.00939,0.00938]}, - {"t":1.28836, "x":11.089, "y":4.25, "heading":0.00116, "vx":0.6863, "vy":0.0, "omega":-0.00155, "ax":-0.99931, "ay":0.0, "alpha":0.00289, "fx":[-11.33784,-11.33784,-11.32603,-11.32603], "fy":[0.00774,-0.00775,-0.00773,0.00774]}, - {"t":1.3313, "x":11.11755, "y":4.25, "heading":0.00109, "vx":0.64338, "vy":0.0, "omega":-0.00142, "ax":-0.99939, "ay":0.0, "alpha":-0.00286, "fx":[-11.36955,-11.24438,-11.35872,-11.35871], "fy":[0.00732,-0.00732,-0.00732,0.00732]}, - {"t":1.37425, "x":11.14426, "y":4.25, "heading":0.00103, "vx":0.60046, "vy":0.0, "omega":-0.00155, "ax":-0.99945, "ay":0.0, "alpha":0.00231, "fx":[-11.33826,-11.33827,-11.32885,-11.32885], "fy":[0.0062,-0.0062,-0.00619,0.0062]}, - {"t":1.4172, "x":11.16913, "y":4.25, "heading":0.00096, "vx":0.55754, "vy":0.0, "omega":-0.00145, "ax":-0.9995, "ay":0.0, "alpha":-0.00226, "fx":[-11.36362,-11.26337,-11.3548,-11.35479], "fy":[0.00594,-0.00593,-0.00594,0.00593]}, - {"t":1.46014, "x":11.19215, "y":4.25, "heading":0.0009, "vx":0.51462, "vy":0.0, "omega":-0.00154, "ax":-0.99954, "ay":0.0, "alpha":0.00191, "fx":[-11.33851,-11.33852,-11.33075,-11.33074], "fy":[0.00513,-0.00513,-0.00513,0.00513]}, - {"t":1.50309, "x":11.21333, "y":4.25, "heading":0.00083, "vx":0.47169, "vy":0.0, "omega":-0.00146, "ax":-0.99958, "ay":0.0, "alpha":-0.00185, "fx":[-11.35954,-11.27638,-11.35212,-11.35211], "fy":[0.00498,-0.00497,-0.00498,0.00497]}, - {"t":1.54603, "x":11.23266, "y":4.25, "heading":0.00077, "vx":0.42876, "vy":0.0, "omega":-0.00154, "ax":-0.99961, "ay":0.0, "alpha":0.00163, "fx":[-11.3387,-11.3387,-11.33207,-11.33207], "fy":[0.00439,-0.00439,-0.00439,0.00439]}, - {"t":1.58898, "x":11.25015, "y":4.25, "heading":0.0007, "vx":0.38583, "vy":0.0, "omega":-0.00147, "ax":-0.99964, "ay":0.0, "alpha":-0.00156, "fx":[-11.35671,-11.28554,-11.35025,-11.35025], "fy":[0.00433,-0.00433,-0.00433,0.00433]}, - {"t":1.63192, "x":11.2658, "y":4.25, "heading":0.00064, "vx":0.34291, "vy":0.0, "omega":-0.00154, "ax":-0.99966, "ay":0.0, "alpha":0.00145, "fx":[-11.33891,-11.33891,-11.33299,-11.33299], "fy":[0.00391,-0.00391,-0.00391,0.00391]}, - {"t":1.67487, "x":11.27961, "y":4.25, "heading":0.00057, "vx":0.29997, "vy":0.0, "omega":-0.00148, "ax":-0.00092, "ay":0.0, "alpha":0.01124, "fx":[0.04336,-0.18163,0.04835,0.04835], "fy":[0.00338,-0.00335,-0.00336,0.00338]}, - {"t":1.71956, "x":11.29301, "y":4.25, "heading":0.00051, "vx":0.29993, "vy":0.0, "omega":-0.00097, "ax":0.0, "ay":0.0, "alpha":0.00142, "fx":[-0.00287,-0.00287,0.00287,0.00287], "fy":[0.00383,-0.00399,-0.00377,0.00383]}, - {"t":1.76426, "x":11.30642, "y":4.25, "heading":0.00047, "vx":0.29993, "vy":0.0, "omega":-0.00091, "ax":0.0, "ay":0.0, "alpha":0.00059, "fx":[-0.00727,0.01063,-0.00168,-0.00168], "fy":[0.00377,-0.00373,-0.00373,0.00376]}, - {"t":1.80896, "x":11.31983, "y":4.25, "heading":0.00042, "vx":0.29993, "vy":0.0, "omega":-0.00088, "ax":0.0, "ay":0.0, "alpha":0.00131, "fx":[-0.00263,-0.00263,0.00263,0.00263], "fy":[0.00353,-0.00367,-0.00346,0.00353]}, - {"t":1.85366, "x":11.33323, "y":4.25, "heading":0.00038, "vx":0.29993, "vy":0.0, "omega":-0.00083, "ax":0.0, "ay":0.0, "alpha":0.00117, "fx":[-0.00272,-0.00173,0.00222,0.00223], "fy":[0.00331,-0.00327,-0.00327,0.00331]}, - {"t":1.89836, "x":11.34664, "y":4.25, "heading":0.00035, "vx":0.29993, "vy":0.0, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.00116, "fx":[-0.00231,-0.00231,0.00231,0.00231], "fy":[0.00314,-0.00326,-0.00308,0.00313]}, - {"t":1.94305, "x":11.36005, "y":4.25, "heading":0.00031, "vx":0.29993, "vy":0.0, "omega":-0.00072, "ax":0.0, "ay":0.0, "alpha":0.00108, "fx":[-0.00218,-0.00226,0.00222,0.00222], "fy":[0.00292,-0.00288,-0.00288,0.00291]}, - {"t":1.98775, "x":11.37345, "y":4.25, "heading":0.00028, "vx":0.29993, "vy":0.0, "omega":-0.00067, "ax":0.0, "ay":0.0, "alpha":0.00104, "fx":[-0.00205,-0.00205,0.00205,0.00205], "fy":[0.00281,-0.00293,-0.00277,0.00281]}, - {"t":2.03245, "x":11.38686, "y":4.25, "heading":0.00025, "vx":0.29993, "vy":0.0, "omega":-0.00063, "ax":0.0, "ay":0.0, "alpha":0.00097, "fx":[-0.00199,-0.00202,0.00201,0.00201], "fy":[0.00261,-0.00257,-0.00257,0.00261]}, - {"t":2.07715, "x":11.40026, "y":4.25, "heading":0.00022, "vx":0.29993, "vy":0.0, "omega":-0.00058, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00185,-0.00185,0.00185,0.00185], "fy":[0.00258,-0.00269,-0.00255,0.00258]}, - {"t":2.12184, "x":11.41367, "y":4.25, "heading":0.0002, "vx":0.29993, "vy":0.0, "omega":-0.00054, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00186,-0.00187,0.00187,0.00187], "fy":[0.00239,-0.00235,-0.00235,0.00239]}, - {"t":2.16654, "x":11.42708, "y":4.25, "heading":0.00017, "vx":0.29993, "vy":0.0, "omega":-0.0005, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00173,-0.00173,0.00173,0.00173], "fy":[0.00245,-0.00256,-0.00243,0.00245]}, - {"t":2.21124, "x":11.44048, "y":4.25, "heading":0.00015, "vx":0.29993, "vy":0.0, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.00086, "fx":[-0.00181,-0.00183,0.00182,0.00182], "fy":[0.00229,-0.00225,-0.00225,0.00229]}, - {"t":2.25594, "x":11.45389, "y":4.25, "heading":0.00013, "vx":0.29993, "vy":0.0, "omega":-0.00042, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.0017,-0.0017,0.0017,0.0017], "fy":[0.00245,-0.00255,-0.00244,0.00245]}, - {"t":2.30063, "x":11.4673, "y":4.25, "heading":0.00011, "vx":0.29993, "vy":0.0, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00187,-0.00191,0.00189,0.00189], "fy":[0.00234,-0.00229,-0.00229,0.00234]}, - {"t":2.34533, "x":11.4807, "y":4.25, "heading":0.00009, "vx":0.29993, "vy":0.0, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00181,-0.00181,0.00181,0.00181], "fy":[0.00262,-0.00272,-0.00261,0.00262]}, - {"t":2.39003, "x":11.49411, "y":4.25, "heading":0.00008, "vx":0.29993, "vy":0.0, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00098, "fx":[-0.00205,-0.00217,0.00211,0.00211], "fy":[0.00259,-0.00254,-0.00254,0.00258]}, - {"t":2.43473, "x":11.50752, "y":4.25, "heading":0.00006, "vx":0.29993, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00109, "fx":[-0.00209,-0.00209,0.00209,0.00209], "fy":[0.00301,-0.00311,-0.00301,0.00301]}, - {"t":2.47942, "x":11.52092, "y":4.25, "heading":0.00005, "vx":0.29993, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00118, "fx":[-0.00247,-0.00255,0.00251,0.00251], "fy":[0.00311,-0.00307,-0.00307,0.00311]}, - {"t":2.52412, "x":11.53433, "y":4.25, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00262,-0.00262,0.00262,0.00262], "fy":[0.00373,-0.00383,-0.00372,0.00373]}, - {"t":2.56882, "x":11.54773, "y":4.25, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00408,-0.00049,0.00228,0.00229], "fy":[0.00405,-0.004,-0.004,0.00405]}, - {"t":2.61352, "x":11.56114, "y":4.25, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00178, "fx":[-0.00349,-0.00349,0.00349,0.00349], "fy":[0.00487,-0.00498,-0.00486,0.00487]}, - {"t":2.65821, "x":11.57455, "y":4.25, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00158, "fx":[-0.00685,0.00365,0.0016,0.0016], "fy":[0.00549,-0.00544,-0.00544,0.00549]}, - {"t":2.70291, "x":11.58795, "y":4.25, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00012, "ax":-0.00001, "ay":0.0, "alpha":0.00239, "fx":[-0.00484,-0.00484,0.00469,0.00469], "fy":[0.00647,-0.00659,-0.00648,0.00647]}, - {"t":2.74761, "x":11.60136, "y":4.25, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":0.00022, "ax":-0.71391, "ay":0.0, "alpha":-0.00874, "fx":[-8.16629,-7.90667,-8.15479,-8.15479], "fy":[0.0075,-0.00747,-0.00747,0.0075]}, - {"t":2.79231, "x":11.61405, "y":4.25, "heading":0.00005, "vx":0.26802, "vy":0.0, "omega":-0.00017, "ax":-0.99852, "ay":0.0, "alpha":0.00234, "fx":[-11.32759,-11.32759,-11.31835,-11.31835], "fy":[0.00639,-0.0064,-0.00638,0.00639]}, - {"t":2.83701, "x":11.62503, "y":4.25, "heading":0.00004, "vx":0.22339, "vy":0.0, "omega":-0.00006, "ax":-0.99926, "ay":0.0, "alpha":-0.00436, "fx":[-11.37216,-11.22667,-11.36333,-11.36333], "fy":[0.00558,-0.00558,-0.00558,0.00558]}, - {"t":2.8817, "x":11.63402, "y":4.25, "heading":0.00004, "vx":0.17873, "vy":0.0, "omega":-0.00026, "ax":-0.9995, "ay":0.0, "alpha":0.00179, "fx":[-11.3376,-11.3376,-11.33076,-11.33076], "fy":[0.00499,-0.00499,-0.00499,0.00499]}, - {"t":2.9264, "x":11.64101, "y":4.25, "heading":0.00003, "vx":0.13405, "vy":0.0, "omega":-0.00018, "ax":-0.99963, "ay":0.0, "alpha":-0.0015, "fx":[-11.35726,-11.28522,-11.34994,-11.34994], "fy":[0.00442,-0.00442,-0.00442,0.00442]}, - {"t":2.9711, "x":11.64601, "y":4.25, "heading":0.00002, "vx":0.08937, "vy":0.0, "omega":-0.00025, "ax":-0.9997, "ay":0.0, "alpha":0.00165, "fx":[-11.33951,-11.33951,-11.33335,-11.33335], "fy":[0.00466,-0.00466,-0.00465,0.00466]}, - {"t":3.0158, "x":11.649, "y":4.25, "heading":0.00001, "vx":0.04469, "vy":0.0, "omega":-0.00017, "ax":-0.99975, "ay":0.0, "alpha":0.00385, "fx":[-11.35244,-11.35244,-11.34552,-11.29757], "fy":[0.00464,-0.00463,-0.00464,0.00463]}, - {"t":3.06049, "x":11.65, "y":4.25, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":10.35, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99952, "ay":0.0, "alpha":0.00407, "fx":[11.35863,11.26365,11.35761,11.35761], "fy":[-0.00024,0.00024,0.00024,-0.00024]}, + {"t":0.04295, "x":10.35092, "y":4.22, "heading":0.0, "vx":0.04292, "vy":0.0, "omega":0.00017, "ax":0.99975, "ay":0.0, "alpha":0.00197, "fx":[11.34962,11.30129,11.34848,11.34848], "fy":[-0.00031,0.00031,0.00031,-0.00031]}, + {"t":0.08589, "x":10.35369, "y":4.22, "heading":0.00001, "vx":0.08586, "vy":0.0, "omega":0.00026, "ax":0.99974, "ay":0.0, "alpha":-0.00023, "fx":[11.33708,11.33708,11.33657,11.33657], "fy":[-0.00077,0.00077,0.00077,-0.00077]}, + {"t":0.12884, "x":10.3583, "y":4.22, "heading":0.00002, "vx":0.12879, "vy":0.0, "omega":0.00025, "ax":0.99972, "ay":0.0, "alpha":0.00211, "fx":[11.35074,11.29747,11.34923,11.34923], "fy":[-0.00053,0.00053,0.00053,-0.00053]}, + {"t":0.17178, "x":10.36475, "y":4.22, "heading":0.00003, "vx":0.17173, "vy":0.0, "omega":0.00034, "ax":0.99971, "ay":0.0, "alpha":-0.00026, "fx":[11.33681,11.33681,11.33618,11.33618], "fy":[-0.00088,0.00088,0.00087,-0.00088]}, + {"t":0.21473, "x":10.37305, "y":4.22, "heading":0.00004, "vx":0.21466, "vy":0.0, "omega":0.00033, "ax":0.99969, "ay":0.0, "alpha":0.00238, "fx":[11.35193,11.29253,11.35038,11.35038], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, + {"t":0.25767, "x":10.38319, "y":4.22, "heading":0.00006, "vx":0.25759, "vy":0.0, "omega":0.00043, "ax":0.99967, "ay":0.0, "alpha":-0.00024, "fx":[11.33634,11.33634,11.33584,11.33584], "fy":[-0.00081,0.00082,0.00081,-0.00081]}, + {"t":0.30062, "x":10.39517, "y":4.22, "heading":0.00008, "vx":0.30052, "vy":0.0, "omega":0.00042, "ax":0.99965, "ay":0.0, "alpha":0.00276, "fx":[11.35335,11.28612,11.35195,11.35195], "fy":[-0.00043,0.00043,0.00043,-0.00043]}, + {"t":0.34356, "x":10.409, "y":4.22, "heading":0.00009, "vx":0.34345, "vy":0.0, "omega":0.00054, "ax":0.99963, "ay":0.0, "alpha":-0.00019, "fx":[11.33571,11.33571,11.33542,11.33542], "fy":[-0.00067,0.00068,0.00067,-0.00067]}, + {"t":0.38651, "x":10.42467, "y":4.22, "heading":0.00012, "vx":0.38638, "vy":0.0, "omega":0.00053, "ax":0.9996, "ay":0.0, "alpha":0.00326, "fx":[11.35528,11.27762,11.35405,11.35405], "fy":[-0.00033,0.00033,0.00033,-0.00033]}, + {"t":0.42945, "x":10.44219, "y":4.22, "heading":0.00014, "vx":0.42931, "vy":0.0, "omega":0.00067, "ax":0.99957, "ay":0.0, "alpha":-0.00015, "fx":[11.33495,11.33495,11.3348,11.3348], "fy":[-0.00056,0.00057,0.00056,-0.00056]}, + {"t":0.4724, "x":10.46154, "y":4.22, "heading":0.00017, "vx":0.47224, "vy":0.0, "omega":0.00067, "ax":0.99953, "ay":0.0, "alpha":0.00391, "fx":[11.35811,11.26581,11.35692,11.35692], "fy":[-0.00034,0.00034,0.00034,-0.00034]}, + {"t":0.51534, "x":10.48275, "y":4.22, "heading":0.0002, "vx":0.51516, "vy":0.0, "omega":0.00083, "ax":0.99948, "ay":0.0, "alpha":-0.00017, "fx":[11.33405,11.33406,11.33378,11.33378], "fy":[-0.0006,0.0006,0.00059,-0.0006]}, + {"t":0.55829, "x":10.50579, "y":4.22, "heading":0.00023, "vx":0.55808, "vy":0.0, "omega":0.00083, "ax":0.99942, "ay":0.0, "alpha":0.0047, "fx":[11.36209,11.24991,11.36057,11.36056], "fy":[-0.00064,0.00064,0.00064,-0.00064]}, + {"t":0.60123, "x":10.53068, "y":4.22, "heading":0.00027, "vx":0.60101, "vy":0.0, "omega":0.00103, "ax":0.99935, "ay":0.0, "alpha":-0.00031, "fx":[11.33298,11.33299,11.33201,11.332], "fy":[-0.00096,0.00097,0.00096,-0.00096]}, + {"t":0.64418, "x":10.55741, "y":4.22, "heading":0.00031, "vx":0.64392, "vy":0.0, "omega":0.00101, "ax":0.99927, "ay":0.0, "alpha":0.00602, "fx":[11.36998,11.22121,11.36739,11.36739], "fy":[-0.00151,0.00151,0.00151,-0.00151]}, + {"t":0.68712, "x":10.58599, "y":4.22, "heading":0.00036, "vx":0.68684, "vy":0.0, "omega":0.00127, "ax":0.99915, "ay":0.0, "alpha":-0.00069, "fx":[11.33151,11.33151,11.32882,11.32882], "fy":[-0.00192,0.00192,0.00191,-0.00192]}, + {"t":0.73007, "x":10.6164, "y":4.22, "heading":0.00041, "vx":0.72975, "vy":0.0, "omega":0.00124, "ax":0.99899, "ay":0.0, "alpha":0.00884, "fx":[11.38745,11.16084,11.38253,11.38253], "fy":[-0.00331,0.00331,0.00331,-0.00331]}, + {"t":0.77302, "x":10.64866, "y":4.22, "heading":0.00047, "vx":0.77265, "vy":0.0, "omega":0.00162, "ax":0.99875, "ay":0.0, "alpha":-0.00141, "fx":[11.3286,11.3286,11.32273,11.32273], "fy":[-0.00374,0.00374,0.00373,-0.00374]}, + {"t":0.81596, "x":10.68277, "y":4.22, "heading":0.00053, "vx":0.81554, "vy":0.0, "omega":0.00156, "ax":0.99837, "ay":0.0, "alpha":0.01922, "fx":[11.44747,10.96147,11.43825,11.43825], "fy":[-0.00654,0.00654,0.00654,-0.00654]}, + {"t":0.85891, "x":10.71871, "y":4.22, "heading":0.0006, "vx":0.85841, "vy":0.0, "omega":0.00239, "ax":0.99766, "ay":0.0, "alpha":-0.00229, "fx":[11.3182,11.3182,11.30838,11.30838], "fy":[-0.00596,0.00597,0.00596,-0.00596]}, + {"t":0.90185, "x":10.7565, "y":4.22, "heading":0.0007, "vx":0.90126, "vy":0.0, "omega":0.00229, "ax":0.99585, "ay":0.0, "alpha":0.10599, "fx":[11.92148,9.43985,11.90474,11.90473], "fy":[-0.0115,0.0115,0.01149,-0.01149]}, + {"t":0.9448, "x":10.79612, "y":4.22, "heading":0.0008, "vx":0.94403, "vy":0.0, "omega":0.00684, "ax":0.98155, "ay":0.0, "alpha":0.00128, "fx":[11.12811,11.12811,11.13297,11.13297], "fy":[0.00356,-0.00379,-0.0034,0.00356]}, + {"t":0.98774, "x":10.83757, "y":4.22, "heading":0.0011, "vx":0.98618, "vy":0.0, "omega":0.0069, "ax":-0.99236, "ay":0.0, "alpha":-0.16455, "fx":[-12.24271,-8.35081,-12.20963,-12.20962], "fy":[0.02215,-0.02214,-0.02215,0.02214]}, + {"t":1.03069, "x":10.879, "y":4.22, "heading":0.00139, "vx":0.94356, "vy":0.0, "omega":-0.00017, "ax":-0.99685, "ay":0.0, "alpha":0.00543, "fx":[-11.31526,-11.31527,-11.29292,-11.29292], "fy":[0.01455,-0.01466,-0.01441,0.01455]}, + {"t":1.07363, "x":10.91861, "y":4.22, "heading":0.00139, "vx":0.90075, "vy":0.0, "omega":0.00006, "ax":-0.99802, "ay":0.0, "alpha":-0.03316, "fx":[-11.5521,-10.66315,-11.52703,-11.52702], "fy":[0.0171,-0.0171,-0.0171,0.0171]}, + {"t":1.11658, "x":10.95637, "y":4.22, "heading":0.00139, "vx":0.85789, "vy":0.0, "omega":-0.00136, "ax":-0.99855, "ay":0.0, "alpha":0.0049, "fx":[-11.3335,-11.3335,-11.31332,-11.31332], "fy":[0.01309,-0.01313,-0.01304,0.01309]}, + {"t":1.15952, "x":10.99229, "y":4.22, "heading":0.00133, "vx":0.81501, "vy":0.0, "omega":-0.00115, "ax":-0.99886, "ay":0.0, "alpha":-0.00871, "fx":[-11.41101,-11.11117,-11.39271,-11.39271], "fy":[0.01252,-0.01251,-0.01252,0.01251]}, + {"t":1.20247, "x":11.02637, "y":4.22, "heading":0.00128, "vx":0.77211, "vy":0.0, "omega":-0.00152, "ax":-0.99906, "ay":0.0, "alpha":0.00373, "fx":[-11.33683,-11.33684,-11.3215,-11.3215], "fy":[0.01,-0.01001,-0.00997,0.00999]}, + {"t":1.24541, "x":11.05861, "y":4.22, "heading":0.00122, "vx":0.72921, "vy":0.0, "omega":-0.00136, "ax":-0.9992, "ay":0.0, "alpha":-0.00426, "fx":[-11.38109,-11.20735,-11.3673,-11.36729], "fy":[0.00938,-0.00938,-0.00938,0.00938]}, + {"t":1.28836, "x":11.089, "y":4.22, "heading":0.00116, "vx":0.6863, "vy":0.0, "omega":-0.00155, "ax":-0.99931, "ay":0.0, "alpha":0.00288, "fx":[-11.33783,-11.33784,-11.32604,-11.32603], "fy":[0.00774,-0.00774,-0.00772,0.00773]}, + {"t":1.3313, "x":11.11755, "y":4.22, "heading":0.00109, "vx":0.64338, "vy":0.0, "omega":-0.00142, "ax":-0.99939, "ay":0.0, "alpha":-0.00286, "fx":[-11.36954,-11.24438,-11.35872,-11.35872], "fy":[0.00731,-0.00731,-0.00731,0.00731]}, + {"t":1.37425, "x":11.14426, "y":4.22, "heading":0.00103, "vx":0.60046, "vy":0.0, "omega":-0.00155, "ax":-0.99945, "ay":0.0, "alpha":0.0023, "fx":[-11.33826,-11.33826,-11.32886,-11.32885], "fy":[0.0062,-0.0062,-0.00619,0.00619]}, + {"t":1.4172, "x":11.16913, "y":4.22, "heading":0.00096, "vx":0.55754, "vy":0.0, "omega":-0.00145, "ax":-0.9995, "ay":0.0, "alpha":-0.00226, "fx":[-11.36361,-11.26337,-11.3548,-11.35479], "fy":[0.00593,-0.00593,-0.00593,0.00593]}, + {"t":1.46014, "x":11.19215, "y":4.22, "heading":0.0009, "vx":0.51462, "vy":0.0, "omega":-0.00154, "ax":-0.99954, "ay":0.0, "alpha":0.0019, "fx":[-11.33851,-11.33851,-11.33075,-11.33075], "fy":[0.00513,-0.00513,-0.00512,0.00512]}, + {"t":1.50309, "x":11.21333, "y":4.22, "heading":0.00083, "vx":0.47169, "vy":0.0, "omega":-0.00146, "ax":-0.99958, "ay":0.0, "alpha":-0.00186, "fx":[-11.35953,-11.27637,-11.35212,-11.35212], "fy":[0.00497,-0.00497,-0.00497,0.00497]}, + {"t":1.54603, "x":11.23266, "y":4.22, "heading":0.00077, "vx":0.42876, "vy":0.0, "omega":-0.00154, "ax":-0.99961, "ay":0.0, "alpha":0.00163, "fx":[-11.3387,-11.3387,-11.33208,-11.33207], "fy":[0.00438,-0.00438,-0.00438,0.00438]}, + {"t":1.58898, "x":11.25015, "y":4.22, "heading":0.00071, "vx":0.38583, "vy":0.0, "omega":-0.00147, "ax":-0.99964, "ay":0.0, "alpha":-0.00156, "fx":[-11.3567,-11.28553,-11.35026,-11.35025], "fy":[0.00433,-0.00432,-0.00433,0.00432]}, + {"t":1.63192, "x":11.2658, "y":4.22, "heading":0.00064, "vx":0.34291, "vy":0.0, "omega":-0.00154, "ax":-0.99966, "ay":0.0, "alpha":0.00145, "fx":[-11.3389,-11.33891,-11.333,-11.33299], "fy":[0.00391,-0.0039,-0.00391,0.0039]}, + {"t":1.67487, "x":11.27961, "y":4.22, "heading":0.00058, "vx":0.29997, "vy":0.0, "omega":-0.00148, "ax":-0.00092, "ay":0.0, "alpha":0.01123, "fx":[0.04337,-0.18163,0.04834,0.04835], "fy":[0.00338,-0.00335,-0.00335,0.00338]}, + {"t":1.71956, "x":11.29301, "y":4.22, "heading":0.00051, "vx":0.29993, "vy":0.0, "omega":-0.00097, "ax":0.0, "ay":0.0, "alpha":0.00142, "fx":[-0.00287,-0.00287,0.00287,0.00287], "fy":[0.00383,-0.00399,-0.00376,0.00382]}, + {"t":1.76426, "x":11.30642, "y":4.22, "heading":0.00047, "vx":0.29993, "vy":0.0, "omega":-0.00091, "ax":0.0, "ay":0.0, "alpha":0.00059, "fx":[-0.00727,0.01063,-0.00168,-0.00168], "fy":[0.00376,-0.00372,-0.00373,0.00376]}, + {"t":1.80896, "x":11.31983, "y":4.22, "heading":0.00043, "vx":0.29993, "vy":0.0, "omega":-0.00088, "ax":0.0, "ay":0.0, "alpha":0.00131, "fx":[-0.00262,-0.00263,0.00262,0.00263], "fy":[0.00353,-0.00367,-0.00346,0.00353]}, + {"t":1.85366, "x":11.33323, "y":4.22, "heading":0.00039, "vx":0.29993, "vy":0.0, "omega":-0.00083, "ax":0.0, "ay":0.0, "alpha":0.00117, "fx":[-0.00272,-0.00173,0.00222,0.00222], "fy":[0.0033,-0.00327,-0.00327,0.0033]}, + {"t":1.89836, "x":11.34664, "y":4.22, "heading":0.00035, "vx":0.29993, "vy":0.0, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.00116, "fx":[-0.00231,-0.00231,0.00231,0.00231], "fy":[0.00313,-0.00326,-0.00308,0.00313]}, + {"t":1.94305, "x":11.36005, "y":4.22, "heading":0.00031, "vx":0.29993, "vy":0.0, "omega":-0.00072, "ax":0.0, "ay":0.0, "alpha":0.00108, "fx":[-0.00218,-0.00226,0.00222,0.00222], "fy":[0.00291,-0.00287,-0.00287,0.00291]}, + {"t":1.98775, "x":11.37345, "y":4.22, "heading":0.00028, "vx":0.29993, "vy":0.0, "omega":-0.00067, "ax":0.0, "ay":0.0, "alpha":0.00103, "fx":[-0.00204,-0.00205,0.00204,0.00205], "fy":[0.00281,-0.00293,-0.00277,0.00281]}, + {"t":2.03245, "x":11.38686, "y":4.22, "heading":0.00025, "vx":0.29993, "vy":0.0, "omega":-0.00063, "ax":0.0, "ay":0.0, "alpha":0.00097, "fx":[-0.00199,-0.00202,0.002,0.00201], "fy":[0.00261,-0.00256,-0.00257,0.0026]}, + {"t":2.07715, "x":11.40026, "y":4.22, "heading":0.00022, "vx":0.29993, "vy":0.0, "omega":-0.00058, "ax":0.0, "ay":0.0, "alpha":0.00094, "fx":[-0.00185,-0.00185,0.00185,0.00185], "fy":[0.00258,-0.00269,-0.00255,0.00258]}, + {"t":2.12184, "x":11.41367, "y":4.22, "heading":0.0002, "vx":0.29993, "vy":0.0, "omega":-0.00054, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00186,-0.00187,0.00187,0.00187], "fy":[0.00239,-0.00235,-0.00235,0.00239]}, + {"t":2.16654, "x":11.42708, "y":4.22, "heading":0.00017, "vx":0.29993, "vy":0.0, "omega":-0.0005, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00173,-0.00173,0.00173,0.00173], "fy":[0.00245,-0.00256,-0.00243,0.00245]}, + {"t":2.21124, "x":11.44048, "y":4.22, "heading":0.00015, "vx":0.29993, "vy":0.0, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.00086, "fx":[-0.00181,-0.00183,0.00182,0.00182], "fy":[0.00229,-0.00225,-0.00225,0.00229]}, + {"t":2.25594, "x":11.45389, "y":4.22, "heading":0.00013, "vx":0.29993, "vy":0.0, "omega":-0.00042, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.0017,-0.0017,0.0017,0.0017], "fy":[0.00245,-0.00255,-0.00244,0.00245]}, + {"t":2.30063, "x":11.4673, "y":4.22, "heading":0.00011, "vx":0.29993, "vy":0.0, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00187,-0.00191,0.00189,0.00189], "fy":[0.00234,-0.00229,-0.0023,0.00234]}, + {"t":2.34533, "x":11.4807, "y":4.22, "heading":0.00009, "vx":0.29993, "vy":0.0, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00181,-0.00181,0.00181,0.00181], "fy":[0.00262,-0.00272,-0.00261,0.00262]}, + {"t":2.39003, "x":11.49411, "y":4.22, "heading":0.00008, "vx":0.29993, "vy":0.0, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00098, "fx":[-0.00205,-0.00217,0.00211,0.00212], "fy":[0.00259,-0.00254,-0.00254,0.00259]}, + {"t":2.43473, "x":11.50752, "y":4.22, "heading":0.00007, "vx":0.29993, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00109, "fx":[-0.00209,-0.00209,0.00209,0.00209], "fy":[0.00302,-0.00311,-0.00301,0.00301]}, + {"t":2.47942, "x":11.52092, "y":4.22, "heading":0.00005, "vx":0.29993, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00118, "fx":[-0.00247,-0.00255,0.00251,0.00251], "fy":[0.00312,-0.00307,-0.00307,0.00312]}, + {"t":2.52412, "x":11.53433, "y":4.22, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00262,-0.00262,0.00262,0.00262], "fy":[0.00373,-0.00383,-0.00373,0.00373]}, + {"t":2.56882, "x":11.54773, "y":4.22, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00409,-0.00049,0.00229,0.00229], "fy":[0.00405,-0.004,-0.004,0.00405]}, + {"t":2.61352, "x":11.56114, "y":4.22, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":0.00178, "fx":[-0.0035,-0.0035,0.0035,0.0035], "fy":[0.00487,-0.00498,-0.00486,0.00487]}, + {"t":2.65821, "x":11.57455, "y":4.22, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00158, "fx":[-0.00686,0.00365,0.0016,0.0016], "fy":[0.00549,-0.00544,-0.00544,0.00549]}, + {"t":2.70291, "x":11.58795, "y":4.22, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00012, "ax":-0.00001, "ay":0.0, "alpha":0.00239, "fx":[-0.00484,-0.00484,0.00469,0.00469], "fy":[0.00647,-0.00659,-0.00648,0.00647]}, + {"t":2.74761, "x":11.60136, "y":4.22, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":0.00022, "ax":-0.71391, "ay":0.0, "alpha":-0.00874, "fx":[-8.16629,-7.90667,-8.15478,-8.15478], "fy":[0.0075,-0.00747,-0.00747,0.0075]}, + {"t":2.79231, "x":11.61405, "y":4.22, "heading":0.00005, "vx":0.26802, "vy":0.0, "omega":-0.00017, "ax":-0.99852, "ay":0.0, "alpha":0.00234, "fx":[-11.32759,-11.32759,-11.31835,-11.31835], "fy":[0.00639,-0.0064,-0.00638,0.00639]}, + {"t":2.83701, "x":11.62503, "y":4.22, "heading":0.00004, "vx":0.22339, "vy":0.0, "omega":-0.00006, "ax":-0.99926, "ay":0.0, "alpha":-0.00436, "fx":[-11.37217,-11.22668,-11.36332,-11.36332], "fy":[0.00559,-0.00559,-0.00559,0.00559]}, + {"t":2.8817, "x":11.63402, "y":4.22, "heading":0.00004, "vx":0.17873, "vy":0.0, "omega":-0.00026, "ax":-0.9995, "ay":0.0, "alpha":0.00179, "fx":[-11.33761,-11.33761,-11.33076,-11.33076], "fy":[0.005,-0.005,-0.00499,0.005]}, + {"t":2.9264, "x":11.64101, "y":4.22, "heading":0.00003, "vx":0.13405, "vy":0.0, "omega":-0.00018, "ax":-0.99963, "ay":0.0, "alpha":-0.0015, "fx":[-11.35727,-11.28522,-11.34993,-11.34993], "fy":[0.00442,-0.00442,-0.00442,0.00442]}, + {"t":2.9711, "x":11.64601, "y":4.22, "heading":0.00002, "vx":0.08937, "vy":0.0, "omega":-0.00025, "ax":-0.9997, "ay":0.0, "alpha":0.00165, "fx":[-11.33952,-11.33952,-11.33335,-11.33335], "fy":[0.00466,-0.00467,-0.00466,0.00466]}, + {"t":3.0158, "x":11.649, "y":4.22, "heading":0.00001, "vx":0.04469, "vy":0.0, "omega":-0.00017, "ax":-0.99975, "ay":0.0, "alpha":0.00385, "fx":[-11.35245,-11.35245,-11.34552,-11.29756], "fy":[0.00465,-0.00464,-0.00464,0.00464]}, + {"t":3.06049, "x":11.65, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] From e86226a9921441788ee86fd761ed582fc925f0e5 Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Sun, 23 Mar 2025 10:34:10 -0400 Subject: [PATCH 21/24] Add a few metrics to smart dashboard --- src/main/java/frc/game/Reef.java | 8 ++++---- src/main/java/frc/robot/elevator/AlgaeRoller.java | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/game/Reef.java b/src/main/java/frc/game/Reef.java index 89be7d06..b28cd275 100644 --- a/src/main/java/frc/game/Reef.java +++ b/src/main/java/frc/game/Reef.java @@ -74,10 +74,10 @@ public int getSector(Pose2d atPose) { var delta = centerPose.getTranslation().minus(atPose.getTranslation()); int vectorAngle = (int) delta.getAngle().getDegrees(); int rayAngle = (vectorAngle + 30 + 360 + (int) centerPose.getRotation().getDegrees()) % 360; - SmartDashboard.putNumber("Sector delta X", delta.getX()); - SmartDashboard.putNumber("Sector delta Y", delta.getY()); - SmartDashboard.putNumber("Sector vector delta", vectorAngle); - SmartDashboard.putNumber("Sector ray angle", rayAngle); + // SmartDashboard.putNumber("Sector delta X", delta.getX()); + // SmartDashboard.putNumber("Sector delta Y", delta.getY()); + // SmartDashboard.putNumber("Sector vector delta", vectorAngle); + // SmartDashboard.putNumber("Sector ray angle", rayAngle); return rayAngle / 60; } diff --git a/src/main/java/frc/robot/elevator/AlgaeRoller.java b/src/main/java/frc/robot/elevator/AlgaeRoller.java index 7ada5c6a..be6204e7 100644 --- a/src/main/java/frc/robot/elevator/AlgaeRoller.java +++ b/src/main/java/frc/robot/elevator/AlgaeRoller.java @@ -67,8 +67,8 @@ public AlgaeRoller() { @Override public void periodic() { SmartDashboard.putNumber("Algae Roller/Velocity", encoder.getVelocity()); - // SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput()); - // SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent()); + SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput()); + SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent()); SmartDashboard.putBoolean("Algae Loaded", hasAlgae.getAsBoolean()); SmartDashboard.putBoolean("Algae isRolling", isRolling.getAsBoolean()); } From 1c268cf544cdc704e32d6aa547d82ce270787bc5 Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sun, 23 Mar 2025 12:32:30 -0400 Subject: [PATCH 22/24] Slowed down the red Auto in Choero --- src/main/deploy/choreo/redCenterToL4G.traj | 187 ++++++++++++--------- 1 file changed, 103 insertions(+), 84 deletions(-) diff --git a/src/main/deploy/choreo/redCenterToL4G.traj b/src/main/deploy/choreo/redCenterToL4G.traj index b2e743bf..11a141ec 100644 --- a/src/main/deploy/choreo/redCenterToL4G.traj +++ b/src/main/deploy/choreo/redCenterToL4G.traj @@ -3,32 +3,30 @@ "version":1, "snapshot":{ "waypoints":[ - {"x":10.35, "y":4.22, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":11.279606819152832, "y":4.22, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":10.35, "y":4.22, "heading":0.0, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":11.170971870422363, "y":4.225747585296631, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":11.65, "y":4.22, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.3}}, "enabled":true}, - {"from":2, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":0.8}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.25}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, + {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"11.170971870422363 m", "val":11.170971870422363}, "y":{"exp":"4.225747585296631 m", "val":4.225747585296631}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}, - {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true}, - {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}, - {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.3 m / s", "val":0.3}}}, "enabled":true}, - {"from":2, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true}, + {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"0.8 m / s ^ 2", "val":0.8}}}, "enabled":true}, + {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.25 m / s", "val":0.25}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -36,79 +34,100 @@ }, "trajectory":{ "sampleType":"Swerve", - "waypoints":[0.0,1.67487,3.06049], + "waypoints":[0.0,2.03338,4.1069], "samples":[ - {"t":0.0, "x":10.35, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99952, "ay":0.0, "alpha":0.00407, "fx":[11.35863,11.26365,11.35761,11.35761], "fy":[-0.00024,0.00024,0.00024,-0.00024]}, - {"t":0.04295, "x":10.35092, "y":4.22, "heading":0.0, "vx":0.04292, "vy":0.0, "omega":0.00017, "ax":0.99975, "ay":0.0, "alpha":0.00197, "fx":[11.34962,11.30129,11.34848,11.34848], "fy":[-0.00031,0.00031,0.00031,-0.00031]}, - {"t":0.08589, "x":10.35369, "y":4.22, "heading":0.00001, "vx":0.08586, "vy":0.0, "omega":0.00026, "ax":0.99974, "ay":0.0, "alpha":-0.00023, "fx":[11.33708,11.33708,11.33657,11.33657], "fy":[-0.00077,0.00077,0.00077,-0.00077]}, - {"t":0.12884, "x":10.3583, "y":4.22, "heading":0.00002, "vx":0.12879, "vy":0.0, "omega":0.00025, "ax":0.99972, "ay":0.0, "alpha":0.00211, "fx":[11.35074,11.29747,11.34923,11.34923], "fy":[-0.00053,0.00053,0.00053,-0.00053]}, - {"t":0.17178, "x":10.36475, "y":4.22, "heading":0.00003, "vx":0.17173, "vy":0.0, "omega":0.00034, "ax":0.99971, "ay":0.0, "alpha":-0.00026, "fx":[11.33681,11.33681,11.33618,11.33618], "fy":[-0.00088,0.00088,0.00087,-0.00088]}, - {"t":0.21473, "x":10.37305, "y":4.22, "heading":0.00004, "vx":0.21466, "vy":0.0, "omega":0.00033, "ax":0.99969, "ay":0.0, "alpha":0.00238, "fx":[11.35193,11.29253,11.35038,11.35038], "fy":[-0.00054,0.00054,0.00054,-0.00054]}, - {"t":0.25767, "x":10.38319, "y":4.22, "heading":0.00006, "vx":0.25759, "vy":0.0, "omega":0.00043, "ax":0.99967, "ay":0.0, "alpha":-0.00024, "fx":[11.33634,11.33634,11.33584,11.33584], "fy":[-0.00081,0.00082,0.00081,-0.00081]}, - {"t":0.30062, "x":10.39517, "y":4.22, "heading":0.00008, "vx":0.30052, "vy":0.0, "omega":0.00042, "ax":0.99965, "ay":0.0, "alpha":0.00276, "fx":[11.35335,11.28612,11.35195,11.35195], "fy":[-0.00043,0.00043,0.00043,-0.00043]}, - {"t":0.34356, "x":10.409, "y":4.22, "heading":0.00009, "vx":0.34345, "vy":0.0, "omega":0.00054, "ax":0.99963, "ay":0.0, "alpha":-0.00019, "fx":[11.33571,11.33571,11.33542,11.33542], "fy":[-0.00067,0.00068,0.00067,-0.00067]}, - {"t":0.38651, "x":10.42467, "y":4.22, "heading":0.00012, "vx":0.38638, "vy":0.0, "omega":0.00053, "ax":0.9996, "ay":0.0, "alpha":0.00326, "fx":[11.35528,11.27762,11.35405,11.35405], "fy":[-0.00033,0.00033,0.00033,-0.00033]}, - {"t":0.42945, "x":10.44219, "y":4.22, "heading":0.00014, "vx":0.42931, "vy":0.0, "omega":0.00067, "ax":0.99957, "ay":0.0, "alpha":-0.00015, "fx":[11.33495,11.33495,11.3348,11.3348], "fy":[-0.00056,0.00057,0.00056,-0.00056]}, - {"t":0.4724, "x":10.46154, "y":4.22, "heading":0.00017, "vx":0.47224, "vy":0.0, "omega":0.00067, "ax":0.99953, "ay":0.0, "alpha":0.00391, "fx":[11.35811,11.26581,11.35692,11.35692], "fy":[-0.00034,0.00034,0.00034,-0.00034]}, - {"t":0.51534, "x":10.48275, "y":4.22, "heading":0.0002, "vx":0.51516, "vy":0.0, "omega":0.00083, "ax":0.99948, "ay":0.0, "alpha":-0.00017, "fx":[11.33405,11.33406,11.33378,11.33378], "fy":[-0.0006,0.0006,0.00059,-0.0006]}, - {"t":0.55829, "x":10.50579, "y":4.22, "heading":0.00023, "vx":0.55808, "vy":0.0, "omega":0.00083, "ax":0.99942, "ay":0.0, "alpha":0.0047, "fx":[11.36209,11.24991,11.36057,11.36056], "fy":[-0.00064,0.00064,0.00064,-0.00064]}, - {"t":0.60123, "x":10.53068, "y":4.22, "heading":0.00027, "vx":0.60101, "vy":0.0, "omega":0.00103, "ax":0.99935, "ay":0.0, "alpha":-0.00031, "fx":[11.33298,11.33299,11.33201,11.332], "fy":[-0.00096,0.00097,0.00096,-0.00096]}, - {"t":0.64418, "x":10.55741, "y":4.22, "heading":0.00031, "vx":0.64392, "vy":0.0, "omega":0.00101, "ax":0.99927, "ay":0.0, "alpha":0.00602, "fx":[11.36998,11.22121,11.36739,11.36739], "fy":[-0.00151,0.00151,0.00151,-0.00151]}, - {"t":0.68712, "x":10.58599, "y":4.22, "heading":0.00036, "vx":0.68684, "vy":0.0, "omega":0.00127, "ax":0.99915, "ay":0.0, "alpha":-0.00069, "fx":[11.33151,11.33151,11.32882,11.32882], "fy":[-0.00192,0.00192,0.00191,-0.00192]}, - {"t":0.73007, "x":10.6164, "y":4.22, "heading":0.00041, "vx":0.72975, "vy":0.0, "omega":0.00124, "ax":0.99899, "ay":0.0, "alpha":0.00884, "fx":[11.38745,11.16084,11.38253,11.38253], "fy":[-0.00331,0.00331,0.00331,-0.00331]}, - {"t":0.77302, "x":10.64866, "y":4.22, "heading":0.00047, "vx":0.77265, "vy":0.0, "omega":0.00162, "ax":0.99875, "ay":0.0, "alpha":-0.00141, "fx":[11.3286,11.3286,11.32273,11.32273], "fy":[-0.00374,0.00374,0.00373,-0.00374]}, - {"t":0.81596, "x":10.68277, "y":4.22, "heading":0.00053, "vx":0.81554, "vy":0.0, "omega":0.00156, "ax":0.99837, "ay":0.0, "alpha":0.01922, "fx":[11.44747,10.96147,11.43825,11.43825], "fy":[-0.00654,0.00654,0.00654,-0.00654]}, - {"t":0.85891, "x":10.71871, "y":4.22, "heading":0.0006, "vx":0.85841, "vy":0.0, "omega":0.00239, "ax":0.99766, "ay":0.0, "alpha":-0.00229, "fx":[11.3182,11.3182,11.30838,11.30838], "fy":[-0.00596,0.00597,0.00596,-0.00596]}, - {"t":0.90185, "x":10.7565, "y":4.22, "heading":0.0007, "vx":0.90126, "vy":0.0, "omega":0.00229, "ax":0.99585, "ay":0.0, "alpha":0.10599, "fx":[11.92148,9.43985,11.90474,11.90473], "fy":[-0.0115,0.0115,0.01149,-0.01149]}, - {"t":0.9448, "x":10.79612, "y":4.22, "heading":0.0008, "vx":0.94403, "vy":0.0, "omega":0.00684, "ax":0.98155, "ay":0.0, "alpha":0.00128, "fx":[11.12811,11.12811,11.13297,11.13297], "fy":[0.00356,-0.00379,-0.0034,0.00356]}, - {"t":0.98774, "x":10.83757, "y":4.22, "heading":0.0011, "vx":0.98618, "vy":0.0, "omega":0.0069, "ax":-0.99236, "ay":0.0, "alpha":-0.16455, "fx":[-12.24271,-8.35081,-12.20963,-12.20962], "fy":[0.02215,-0.02214,-0.02215,0.02214]}, - {"t":1.03069, "x":10.879, "y":4.22, "heading":0.00139, "vx":0.94356, "vy":0.0, "omega":-0.00017, "ax":-0.99685, "ay":0.0, "alpha":0.00543, "fx":[-11.31526,-11.31527,-11.29292,-11.29292], "fy":[0.01455,-0.01466,-0.01441,0.01455]}, - {"t":1.07363, "x":10.91861, "y":4.22, "heading":0.00139, "vx":0.90075, "vy":0.0, "omega":0.00006, "ax":-0.99802, "ay":0.0, "alpha":-0.03316, "fx":[-11.5521,-10.66315,-11.52703,-11.52702], "fy":[0.0171,-0.0171,-0.0171,0.0171]}, - {"t":1.11658, "x":10.95637, "y":4.22, "heading":0.00139, "vx":0.85789, "vy":0.0, "omega":-0.00136, "ax":-0.99855, "ay":0.0, "alpha":0.0049, "fx":[-11.3335,-11.3335,-11.31332,-11.31332], "fy":[0.01309,-0.01313,-0.01304,0.01309]}, - {"t":1.15952, "x":10.99229, "y":4.22, "heading":0.00133, "vx":0.81501, "vy":0.0, "omega":-0.00115, "ax":-0.99886, "ay":0.0, "alpha":-0.00871, "fx":[-11.41101,-11.11117,-11.39271,-11.39271], "fy":[0.01252,-0.01251,-0.01252,0.01251]}, - {"t":1.20247, "x":11.02637, "y":4.22, "heading":0.00128, "vx":0.77211, "vy":0.0, "omega":-0.00152, "ax":-0.99906, "ay":0.0, "alpha":0.00373, "fx":[-11.33683,-11.33684,-11.3215,-11.3215], "fy":[0.01,-0.01001,-0.00997,0.00999]}, - {"t":1.24541, "x":11.05861, "y":4.22, "heading":0.00122, "vx":0.72921, "vy":0.0, "omega":-0.00136, "ax":-0.9992, "ay":0.0, "alpha":-0.00426, "fx":[-11.38109,-11.20735,-11.3673,-11.36729], "fy":[0.00938,-0.00938,-0.00938,0.00938]}, - {"t":1.28836, "x":11.089, "y":4.22, "heading":0.00116, "vx":0.6863, "vy":0.0, "omega":-0.00155, "ax":-0.99931, "ay":0.0, "alpha":0.00288, "fx":[-11.33783,-11.33784,-11.32604,-11.32603], "fy":[0.00774,-0.00774,-0.00772,0.00773]}, - {"t":1.3313, "x":11.11755, "y":4.22, "heading":0.00109, "vx":0.64338, "vy":0.0, "omega":-0.00142, "ax":-0.99939, "ay":0.0, "alpha":-0.00286, "fx":[-11.36954,-11.24438,-11.35872,-11.35872], "fy":[0.00731,-0.00731,-0.00731,0.00731]}, - {"t":1.37425, "x":11.14426, "y":4.22, "heading":0.00103, "vx":0.60046, "vy":0.0, "omega":-0.00155, "ax":-0.99945, "ay":0.0, "alpha":0.0023, "fx":[-11.33826,-11.33826,-11.32886,-11.32885], "fy":[0.0062,-0.0062,-0.00619,0.00619]}, - {"t":1.4172, "x":11.16913, "y":4.22, "heading":0.00096, "vx":0.55754, "vy":0.0, "omega":-0.00145, "ax":-0.9995, "ay":0.0, "alpha":-0.00226, "fx":[-11.36361,-11.26337,-11.3548,-11.35479], "fy":[0.00593,-0.00593,-0.00593,0.00593]}, - {"t":1.46014, "x":11.19215, "y":4.22, "heading":0.0009, "vx":0.51462, "vy":0.0, "omega":-0.00154, "ax":-0.99954, "ay":0.0, "alpha":0.0019, "fx":[-11.33851,-11.33851,-11.33075,-11.33075], "fy":[0.00513,-0.00513,-0.00512,0.00512]}, - {"t":1.50309, "x":11.21333, "y":4.22, "heading":0.00083, "vx":0.47169, "vy":0.0, "omega":-0.00146, "ax":-0.99958, "ay":0.0, "alpha":-0.00186, "fx":[-11.35953,-11.27637,-11.35212,-11.35212], "fy":[0.00497,-0.00497,-0.00497,0.00497]}, - {"t":1.54603, "x":11.23266, "y":4.22, "heading":0.00077, "vx":0.42876, "vy":0.0, "omega":-0.00154, "ax":-0.99961, "ay":0.0, "alpha":0.00163, "fx":[-11.3387,-11.3387,-11.33208,-11.33207], "fy":[0.00438,-0.00438,-0.00438,0.00438]}, - {"t":1.58898, "x":11.25015, "y":4.22, "heading":0.00071, "vx":0.38583, "vy":0.0, "omega":-0.00147, "ax":-0.99964, "ay":0.0, "alpha":-0.00156, "fx":[-11.3567,-11.28553,-11.35026,-11.35025], "fy":[0.00433,-0.00432,-0.00433,0.00432]}, - {"t":1.63192, "x":11.2658, "y":4.22, "heading":0.00064, "vx":0.34291, "vy":0.0, "omega":-0.00154, "ax":-0.99966, "ay":0.0, "alpha":0.00145, "fx":[-11.3389,-11.33891,-11.333,-11.33299], "fy":[0.00391,-0.0039,-0.00391,0.0039]}, - {"t":1.67487, "x":11.27961, "y":4.22, "heading":0.00058, "vx":0.29997, "vy":0.0, "omega":-0.00148, "ax":-0.00092, "ay":0.0, "alpha":0.01123, "fx":[0.04337,-0.18163,0.04834,0.04835], "fy":[0.00338,-0.00335,-0.00335,0.00338]}, - {"t":1.71956, "x":11.29301, "y":4.22, "heading":0.00051, "vx":0.29993, "vy":0.0, "omega":-0.00097, "ax":0.0, "ay":0.0, "alpha":0.00142, "fx":[-0.00287,-0.00287,0.00287,0.00287], "fy":[0.00383,-0.00399,-0.00376,0.00382]}, - {"t":1.76426, "x":11.30642, "y":4.22, "heading":0.00047, "vx":0.29993, "vy":0.0, "omega":-0.00091, "ax":0.0, "ay":0.0, "alpha":0.00059, "fx":[-0.00727,0.01063,-0.00168,-0.00168], "fy":[0.00376,-0.00372,-0.00373,0.00376]}, - {"t":1.80896, "x":11.31983, "y":4.22, "heading":0.00043, "vx":0.29993, "vy":0.0, "omega":-0.00088, "ax":0.0, "ay":0.0, "alpha":0.00131, "fx":[-0.00262,-0.00263,0.00262,0.00263], "fy":[0.00353,-0.00367,-0.00346,0.00353]}, - {"t":1.85366, "x":11.33323, "y":4.22, "heading":0.00039, "vx":0.29993, "vy":0.0, "omega":-0.00083, "ax":0.0, "ay":0.0, "alpha":0.00117, "fx":[-0.00272,-0.00173,0.00222,0.00222], "fy":[0.0033,-0.00327,-0.00327,0.0033]}, - {"t":1.89836, "x":11.34664, "y":4.22, "heading":0.00035, "vx":0.29993, "vy":0.0, "omega":-0.00077, "ax":0.0, "ay":0.0, "alpha":0.00116, "fx":[-0.00231,-0.00231,0.00231,0.00231], "fy":[0.00313,-0.00326,-0.00308,0.00313]}, - {"t":1.94305, "x":11.36005, "y":4.22, "heading":0.00031, "vx":0.29993, "vy":0.0, "omega":-0.00072, "ax":0.0, "ay":0.0, "alpha":0.00108, "fx":[-0.00218,-0.00226,0.00222,0.00222], "fy":[0.00291,-0.00287,-0.00287,0.00291]}, - {"t":1.98775, "x":11.37345, "y":4.22, "heading":0.00028, "vx":0.29993, "vy":0.0, "omega":-0.00067, "ax":0.0, "ay":0.0, "alpha":0.00103, "fx":[-0.00204,-0.00205,0.00204,0.00205], "fy":[0.00281,-0.00293,-0.00277,0.00281]}, - {"t":2.03245, "x":11.38686, "y":4.22, "heading":0.00025, "vx":0.29993, "vy":0.0, "omega":-0.00063, "ax":0.0, "ay":0.0, "alpha":0.00097, "fx":[-0.00199,-0.00202,0.002,0.00201], "fy":[0.00261,-0.00256,-0.00257,0.0026]}, - {"t":2.07715, "x":11.40026, "y":4.22, "heading":0.00022, "vx":0.29993, "vy":0.0, "omega":-0.00058, "ax":0.0, "ay":0.0, "alpha":0.00094, "fx":[-0.00185,-0.00185,0.00185,0.00185], "fy":[0.00258,-0.00269,-0.00255,0.00258]}, - {"t":2.12184, "x":11.41367, "y":4.22, "heading":0.0002, "vx":0.29993, "vy":0.0, "omega":-0.00054, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00186,-0.00187,0.00187,0.00187], "fy":[0.00239,-0.00235,-0.00235,0.00239]}, - {"t":2.16654, "x":11.42708, "y":4.22, "heading":0.00017, "vx":0.29993, "vy":0.0, "omega":-0.0005, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00173,-0.00173,0.00173,0.00173], "fy":[0.00245,-0.00256,-0.00243,0.00245]}, - {"t":2.21124, "x":11.44048, "y":4.22, "heading":0.00015, "vx":0.29993, "vy":0.0, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.00086, "fx":[-0.00181,-0.00183,0.00182,0.00182], "fy":[0.00229,-0.00225,-0.00225,0.00229]}, - {"t":2.25594, "x":11.45389, "y":4.22, "heading":0.00013, "vx":0.29993, "vy":0.0, "omega":-0.00042, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.0017,-0.0017,0.0017,0.0017], "fy":[0.00245,-0.00255,-0.00244,0.00245]}, - {"t":2.30063, "x":11.4673, "y":4.22, "heading":0.00011, "vx":0.29993, "vy":0.0, "omega":-0.00038, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00187,-0.00191,0.00189,0.00189], "fy":[0.00234,-0.00229,-0.0023,0.00234]}, - {"t":2.34533, "x":11.4807, "y":4.22, "heading":0.00009, "vx":0.29993, "vy":0.0, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00095, "fx":[-0.00181,-0.00181,0.00181,0.00181], "fy":[0.00262,-0.00272,-0.00261,0.00262]}, - {"t":2.39003, "x":11.49411, "y":4.22, "heading":0.00008, "vx":0.29993, "vy":0.0, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00098, "fx":[-0.00205,-0.00217,0.00211,0.00212], "fy":[0.00259,-0.00254,-0.00254,0.00259]}, - {"t":2.43473, "x":11.50752, "y":4.22, "heading":0.00007, "vx":0.29993, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00109, "fx":[-0.00209,-0.00209,0.00209,0.00209], "fy":[0.00302,-0.00311,-0.00301,0.00301]}, - {"t":2.47942, "x":11.52092, "y":4.22, "heading":0.00005, "vx":0.29993, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00118, "fx":[-0.00247,-0.00255,0.00251,0.00251], "fy":[0.00312,-0.00307,-0.00307,0.00312]}, - {"t":2.52412, "x":11.53433, "y":4.22, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00262,-0.00262,0.00262,0.00262], "fy":[0.00373,-0.00383,-0.00373,0.00373]}, - {"t":2.56882, "x":11.54773, "y":4.22, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00136, "fx":[-0.00409,-0.00049,0.00229,0.00229], "fy":[0.00405,-0.004,-0.004,0.00405]}, - {"t":2.61352, "x":11.56114, "y":4.22, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":0.00178, "fx":[-0.0035,-0.0035,0.0035,0.0035], "fy":[0.00487,-0.00498,-0.00486,0.00487]}, - {"t":2.65821, "x":11.57455, "y":4.22, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00158, "fx":[-0.00686,0.00365,0.0016,0.0016], "fy":[0.00549,-0.00544,-0.00544,0.00549]}, - {"t":2.70291, "x":11.58795, "y":4.22, "heading":0.00003, "vx":0.29993, "vy":0.0, "omega":0.00012, "ax":-0.00001, "ay":0.0, "alpha":0.00239, "fx":[-0.00484,-0.00484,0.00469,0.00469], "fy":[0.00647,-0.00659,-0.00648,0.00647]}, - {"t":2.74761, "x":11.60136, "y":4.22, "heading":0.00004, "vx":0.29993, "vy":0.0, "omega":0.00022, "ax":-0.71391, "ay":0.0, "alpha":-0.00874, "fx":[-8.16629,-7.90667,-8.15478,-8.15478], "fy":[0.0075,-0.00747,-0.00747,0.0075]}, - {"t":2.79231, "x":11.61405, "y":4.22, "heading":0.00005, "vx":0.26802, "vy":0.0, "omega":-0.00017, "ax":-0.99852, "ay":0.0, "alpha":0.00234, "fx":[-11.32759,-11.32759,-11.31835,-11.31835], "fy":[0.00639,-0.0064,-0.00638,0.00639]}, - {"t":2.83701, "x":11.62503, "y":4.22, "heading":0.00004, "vx":0.22339, "vy":0.0, "omega":-0.00006, "ax":-0.99926, "ay":0.0, "alpha":-0.00436, "fx":[-11.37217,-11.22668,-11.36332,-11.36332], "fy":[0.00559,-0.00559,-0.00559,0.00559]}, - {"t":2.8817, "x":11.63402, "y":4.22, "heading":0.00004, "vx":0.17873, "vy":0.0, "omega":-0.00026, "ax":-0.9995, "ay":0.0, "alpha":0.00179, "fx":[-11.33761,-11.33761,-11.33076,-11.33076], "fy":[0.005,-0.005,-0.00499,0.005]}, - {"t":2.9264, "x":11.64101, "y":4.22, "heading":0.00003, "vx":0.13405, "vy":0.0, "omega":-0.00018, "ax":-0.99963, "ay":0.0, "alpha":-0.0015, "fx":[-11.35727,-11.28522,-11.34993,-11.34993], "fy":[0.00442,-0.00442,-0.00442,0.00442]}, - {"t":2.9711, "x":11.64601, "y":4.22, "heading":0.00002, "vx":0.08937, "vy":0.0, "omega":-0.00025, "ax":-0.9997, "ay":0.0, "alpha":0.00165, "fx":[-11.33952,-11.33952,-11.33335,-11.33335], "fy":[0.00466,-0.00467,-0.00466,0.00466]}, - {"t":3.0158, "x":11.649, "y":4.22, "heading":0.00001, "vx":0.04469, "vy":0.0, "omega":-0.00017, "ax":-0.99975, "ay":0.0, "alpha":0.00385, "fx":[-11.35245,-11.35245,-11.34552,-11.29756], "fy":[0.00465,-0.00464,-0.00464,0.00464]}, - {"t":3.06049, "x":11.65, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":10.35, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.79961, "ay":0.00581, "alpha":0.00059, "fx":[9.09007,9.01384,9.08287,9.08287], "fy":[0.05675,0.07495,0.07495,0.05675]}, + {"t":0.0442, "x":10.35078, "y":4.22001, "heading":0.0, "vx":0.03535, "vy":0.00026, "omega":0.00003, "ax":0.79978, "ay":0.00581, "alpha":-0.00085, "fx":[9.08352,9.041,9.07644,9.07644], "fy":[0.05696,0.07477,0.07477,0.05696]}, + {"t":0.08841, "x":10.35313, "y":4.22002, "heading":0.0, "vx":0.0707, "vy":0.00051, "omega":-0.00001, "ax":0.79976, "ay":0.00581, "alpha":-0.00232, "fx":[9.0757,9.0757,9.06262,9.06262], "fy":[0.061,0.07115,0.0703,0.061]}, + {"t":0.13261, "x":10.35703, "y":4.22005, "heading":0.0, "vx":0.10605, "vy":0.00077, "omega":-0.00011, "ax":0.79974, "ay":0.00581, "alpha":-0.00045, "fx":[9.08512,9.03444,9.07809,9.07809], "fy":[0.05708,0.07463,0.07463,0.05708]}, + {"t":0.17682, "x":10.3625, "y":4.22009, "heading":0.0, "vx":0.1414, "vy":0.00103, "omega":-0.00013, "ax":0.79972, "ay":0.00581, "alpha":-0.0023, "fx":[9.07514,9.07514,9.06219,9.06219], "fy":[0.06103,0.07107,0.0703,0.06103]}, + {"t":0.22102, "x":10.36953, "y":4.22014, "heading":-0.00001, "vx":0.17675, "vy":0.00128, "omega":-0.00024, "ax":0.79969, "ay":0.00581, "alpha":0.00007, "fx":[9.08753,9.02492,9.08045,9.08045], "fy":[0.05708,0.07463,0.07463,0.05708]}, + {"t":0.26522, "x":10.37813, "y":4.2202, "heading":-0.00002, "vx":0.2121, "vy":0.00154, "omega":-0.00023, "ax":0.79965, "ay":0.00581, "alpha":-0.0023, "fx":[9.07441,9.07441,9.06145,9.06145], "fy":[0.061,0.07103,0.07037,0.061]}, + {"t":0.30943, "x":10.38828, "y":4.22028, "heading":-0.00003, "vx":0.24745, "vy":0.0018, "omega":-0.00033, "ax":0.79961, "ay":0.00581, "alpha":0.00078, "fx":[9.09082,9.01159,9.0836,9.0836], "fy":[0.05698,0.07472,0.07472,0.05698]}, + {"t":0.35363, "x":10.4, "y":4.22036, "heading":-0.00005, "vx":0.2828, "vy":0.00205, "omega":-0.0003, "ax":0.79955, "ay":0.00581, "alpha":-0.00233, "fx":[9.07325,9.07325,9.06014,9.06014], "fy":[0.06094,0.07097,0.07052,0.06094]}, + {"t":0.39784, "x":10.41329, "y":4.22046, "heading":-0.00006, "vx":0.31814, "vy":0.00231, "omega":-0.0004, "ax":0.79946, "ay":0.00581, "alpha":0.00216, "fx":[9.0974,8.98558,9.08994,9.08994], "fy":[0.05676,0.07492,0.07492,0.05676]}, + {"t":0.44204, "x":10.42813, "y":4.22057, "heading":-0.00008, "vx":0.35348, "vy":0.00257, "omega":-0.00031, "ax":0.79933, "ay":0.0058, "alpha":-0.00236, "fx":[9.07089,9.07089,9.05757,9.05757], "fy":[0.06087,0.07081,0.07075,0.06087]}, + {"t":0.48624, "x":10.44454, "y":4.22069, "heading":-0.00009, "vx":0.38881, "vy":0.00282, "omega":-0.00041, "ax":0.79911, "ay":0.0058, "alpha":0.00517, "fx":[9.11114,8.92919,9.10334,9.10334], "fy":[0.05642,0.0752,0.0752,0.05642]}, + {"t":0.53045, "x":10.4625, "y":4.22082, "heading":-0.00011, "vx":0.42414, "vy":0.00308, "omega":-0.00018, "ax":0.79867, "ay":0.0058, "alpha":-0.00234, "fx":[9.06351,9.06351,9.05006,9.05006], "fy":[0.06096,0.07005,0.07115,0.06096]}, + {"t":0.57465, "x":10.48203, "y":4.22096, "heading":-0.00012, "vx":0.45944, "vy":0.00334, "omega":-0.00029, "ax":0.79734, "ay":0.00579, "alpha":0.02742, "fx":[9.21904,8.52766,9.21007,9.21007], "fy":[0.05507,0.0763,0.07629,0.05507]}, + {"t":0.61886, "x":10.50312, "y":4.22111, "heading":-0.00013, "vx":0.49469, "vy":0.00359, "omega":0.00093, "ax":0.11729, "ay":0.00081, "alpha":-0.00312, "fx":[1.33635,1.33635,1.32378,1.32378], "fy":[0.00069,0.02457,0.01056,0.00069]}, + {"t":0.66306, "x":10.5251, "y":4.22127, "heading":-0.00009, "vx":0.49987, "vy":0.00363, "omega":0.00079, "ax":0.0, "ay":0.00005, "alpha":0.01007, "fx":[0.07628,-0.21229,0.06805,0.06805], "fy":[-0.00805,0.00907,0.00907,-0.00805]}, + {"t":0.70726, "x":10.5472, "y":4.22143, "heading":-0.00005, "vx":0.49987, "vy":0.00363, "omega":0.00123, "ax":0.0, "ay":0.0, "alpha":-0.00191, "fx":[0.00535,0.00535,-0.00535,-0.00535], "fy":[-0.004,0.0035,0.00462,-0.004]}, + {"t":0.75147, "x":10.5693, "y":4.22159, "heading":0.0, "vx":0.49987, "vy":0.00363, "omega":0.00115, "ax":0.0, "ay":0.0, "alpha":-0.00176, "fx":[0.00445,-0.00177,-0.00134,-0.00134], "fy":[-0.00645,0.00635,0.00636,-0.00644]}, + {"t":0.79567, "x":10.59139, "y":4.22175, "heading":0.00005, "vx":0.49987, "vy":0.00363, "omega":0.00107, "ax":0.0, "ay":-0.00001, "alpha":-0.00158, "fx":[0.00427,0.00427,-0.00427,-0.00427], "fy":[-0.00352,0.00349,0.00332,-0.00352]}, + {"t":0.83988, "x":10.61349, "y":4.22191, "heading":0.0001, "vx":0.49987, "vy":0.00363, "omega":0.001, "ax":0.0, "ay":0.0, "alpha":-0.00187, "fx":[0.00063,0.00705,-0.00384,-0.00384], "fy":[-0.00496,0.00505,0.00505,-0.00496]}, + {"t":0.88408, "x":10.63558, "y":4.22207, "heading":0.00014, "vx":0.49987, "vy":0.00363, "omega":0.00092, "ax":0.0, "ay":0.0, "alpha":-0.00122, "fx":[0.0033,0.0033,-0.0033,-0.0033], "fy":[-0.00273,0.00278,0.0025,-0.00273]}, + {"t":0.92828, "x":10.65768, "y":4.22223, "heading":0.00018, "vx":0.49987, "vy":0.00363, "omega":0.00086, "ax":0.0, "ay":0.0, "alpha":-0.00137, "fx":[0.00065,0.00471,-0.00268,-0.00268], "fy":[-0.00374,0.00383,0.00383,-0.00374]}, + {"t":0.97249, "x":10.67978, "y":4.22239, "heading":0.00022, "vx":0.49987, "vy":0.00363, "omega":0.0008, "ax":0.0, "ay":0.0, "alpha":-0.00089, "fx":[0.00243,0.00243,-0.00242,-0.00243], "fy":[-0.00199,0.00205,0.00178,-0.00199]}, + {"t":1.01669, "x":10.70187, "y":4.22256, "heading":0.00026, "vx":0.49987, "vy":0.00363, "omega":0.00076, "ax":0.0, "ay":0.0, "alpha":-0.00092, "fx":[0.00072,0.00244,-0.00158,-0.00158], "fy":[-0.00264,0.00271,0.00271,-0.00264]}, + {"t":1.0609, "x":10.72397, "y":4.22272, "heading":0.00029, "vx":0.49987, "vy":0.00363, "omega":0.00072, "ax":0.0, "ay":0.0, "alpha":-0.00059, "fx":[0.00162,0.00162,-0.00162,-0.00162], "fy":[-0.00131,0.00139,0.00112,-0.00131]}, + {"t":1.1051, "x":10.74607, "y":4.22288, "heading":0.00032, "vx":0.49987, "vy":0.00363, "omega":0.0007, "ax":0.0, "ay":0.0, "alpha":-0.00053, "fx":[0.00054,0.00104,-0.00079,-0.00079], "fy":[-0.00162,0.00166,0.00166,-0.00162]}, + {"t":1.1493, "x":10.76816, "y":4.22304, "heading":0.00035, "vx":0.49987, "vy":0.00363, "omega":0.00067, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00086,0.00086,-0.00086,-0.00086], "fy":[-0.00066,0.00078,0.00049,-0.00066]}, + {"t":1.19351, "x":10.79026, "y":4.2232, "heading":0.00038, "vx":0.49987, "vy":0.00363, "omega":0.00066, "ax":0.0, "ay":0.0, "alpha":-0.00017, "fx":[0.0003,-0.00011,-0.0001,-0.0001], "fy":[-0.00064,0.00066,0.00066,-0.00064]}, + {"t":1.23771, "x":10.81236, "y":4.22336, "heading":0.00041, "vx":0.49987, "vy":0.00363, "omega":0.00065, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00012,0.00012,-0.00012,-0.00012], "fy":[0.00001,0.00011,-0.00007,0.00001]}, + {"t":1.28191, "x":10.83445, "y":4.22352, "heading":0.00044, "vx":0.49987, "vy":0.00363, "omega":0.00065, "ax":0.0, "ay":0.0, "alpha":0.0002, "fx":[0.00016,-0.00149,0.00067,0.00067], "fy":[0.00031,-0.00034,-0.00034,0.00031]}, + {"t":1.32612, "x":10.85655, "y":4.22368, "heading":0.00047, "vx":0.49987, "vy":0.00363, "omega":0.00066, "ax":0.0, "ay":0.00001, "alpha":0.00029, "fx":[-0.00061,-0.00061,0.00061,0.00061], "fy":[0.00086,-0.00096,-0.00044,0.00086]}, + {"t":1.37032, "x":10.87864, "y":4.22384, "heading":0.0005, "vx":0.49987, "vy":0.00363, "omega":0.00067, "ax":0.0, "ay":-0.00001, "alpha":0.00061, "fx":[0.00029,-0.00374,0.00173,0.00173], "fy":[0.00113,-0.00146,-0.00146,0.00113]}, + {"t":1.41453, "x":10.90074, "y":4.224, "heading":0.00053, "vx":0.49987, "vy":0.00363, "omega":0.0007, "ax":0.0, "ay":0.00002, "alpha":0.00075, "fx":[-0.00135,-0.00135,0.00135,0.00135], "fy":[0.00239,-0.00365,-0.00017,0.00239]}, + {"t":1.45873, "x":10.92284, "y":4.22416, "heading":0.00056, "vx":0.49987, "vy":0.00363, "omega":0.00073, "ax":0.0, "ay":-0.00006, "alpha":0.00106, "fx":[0.00046,-0.00617,0.00287,0.00287], "fy":[0.00158,-0.00305,-0.00305,0.00158]}, + {"t":1.50293, "x":10.94493, "y":4.22432, "heading":0.00059, "vx":0.49987, "vy":0.00363, "omega":0.00078, "ax":0.0, "ay":0.00008, "alpha":0.00163, "fx":[-0.00213,-0.00213,0.00212,0.00211], "fy":[0.00625,-0.01101,0.00237,0.00625]}, + {"t":1.54714, "x":10.96703, "y":4.22448, "heading":0.00063, "vx":0.49987, "vy":0.00363, "omega":0.00085, "ax":0.0, "ay":-0.00005, "alpha":0.00091, "fx":[-0.00299,0.00196,0.00052,0.00052], "fy":[0.00291,-0.00398,-0.00398,0.00291]}, + {"t":1.59134, "x":10.98913, "y":4.22464, "heading":0.00067, "vx":0.49987, "vy":0.00363, "omega":0.00089, "ax":0.0, "ay":0.00005, "alpha":0.00303, "fx":[-0.00295,-0.00295,0.00294,0.00294], "fy":[0.01112,-0.02585,0.00582,0.01112]}, + {"t":1.63555, "x":11.01122, "y":4.2248, "heading":0.00071, "vx":0.49987, "vy":0.00363, "omega":0.00103, "ax":0.00001, "ay":-0.00141, "alpha":-0.01024, "fx":[-0.07029,0.19995,-0.06463,-0.06463], "fy":[-0.01067,-0.02126,-0.02126,-0.01067]}, + {"t":1.67975, "x":11.03332, "y":4.22496, "heading":0.00075, "vx":0.49987, "vy":0.00357, "omega":0.00057, "ax":-0.06041, "ay":-0.01326, "alpha":-0.00078, "fx":[-0.68898,-0.68898,-0.6812,-0.6812], "fy":[-0.15655,-0.12504,-0.16318,-0.15655]}, + {"t":1.72395, "x":11.05536, "y":4.22511, "heading":0.00078, "vx":0.4972, "vy":0.00298, "omega":0.00054, "ax":-0.79729, "ay":-0.00593, "alpha":-0.03145, "fx":[-9.23415,-8.47463,-9.22777,-9.22777], "fy":[-0.06007,-0.07448,-0.07447,-0.06007]}, + {"t":1.76816, "x":11.07656, "y":4.22523, "heading":0.0008, "vx":0.46196, "vy":0.00272, "omega":-0.00085, "ax":-0.79866, "ay":-0.00587, "alpha":0.00199, "fx":[-9.06041,-9.0604,-9.05284,-9.05284], "fy":[-0.06103,-0.07705,-0.06719,-0.06103]}, + {"t":1.81236, "x":11.0962, "y":4.22535, "heading":0.00076, "vx":0.42666, "vy":0.00246, "omega":-0.00076, "ax":-0.7991, "ay":-0.00585, "alpha":-0.0065, "fx":[-9.10974,-8.92663,-9.10518,-9.10518], "fy":[-0.06116,-0.07153,-0.07153,-0.06116]}, + {"t":1.85657, "x":11.11427, "y":4.22545, "heading":0.00073, "vx":0.39133, "vy":0.0022, "omega":-0.00105, "ax":-0.79932, "ay":-0.00584, "alpha":0.00148, "fx":[-9.06741,-9.06741,-9.06097,-9.06097], "fy":[-0.0624,-0.07254,-0.06757,-0.0624]}, + {"t":1.90077, "x":11.13079, "y":4.22554, "heading":0.00068, "vx":0.356, "vy":0.00194, "omega":-0.00098, "ax":-0.79946, "ay":-0.00583, "alpha":-0.00368, "fx":[-9.09564,-8.98318,-9.09196,-9.09196], "fy":[-0.06201,-0.07031,-0.07031,-0.06201]}, + {"t":1.94497, "x":11.14575, "y":4.22562, "heading":0.00064, "vx":0.32066, "vy":0.00169, "omega":-0.00115, "ax":-0.79954, "ay":-0.00583, "alpha":0.00116, "fx":[-9.06922,-9.06922,-9.06414,-9.06414], "fy":[-0.06315,-0.07093,-0.06723,-0.06315]}, + {"t":1.98918, "x":11.15914, "y":4.22569, "heading":0.00059, "vx":0.28532, "vy":0.00143, "omega":-0.0011, "ax":-0.79961, "ay":-0.00583, "alpha":-0.00256, "fx":[-9.08874,-9.0089,-9.08596,-9.08596], "fy":[-0.06297,-0.0692,-0.06919,-0.06297]}, + {"t":2.03338, "x":11.17097, "y":4.22575, "heading":0.00054, "vx":0.24997, "vy":0.00117, "omega":-0.00121, "ax":-0.00089, "ay":-0.08794, "alpha":0.00533, "fx":[0.01474,-0.08878,0.01691,0.01691], "fy":[-0.99496,-0.99949,-0.99949,-0.99496]}, + {"t":2.07946, "x":11.18249, "y":4.22571, "heading":0.00048, "vx":0.24993, "vy":-0.00288, "omega":-0.00096, "ax":-0.00005, "ay":-0.00393, "alpha":0.00783, "fx":[-0.00182,-0.00182,0.00076,0.00076], "fy":[-0.01254,-0.13857,-0.01476,-0.01254]}, + {"t":2.12554, "x":11.19401, "y":4.22557, "heading":0.00044, "vx":0.24993, "vy":-0.00306, "omega":-0.0006, "ax":0.0, "ay":0.00025, "alpha":0.00089, "fx":[0.0008,-0.00573,0.00254,0.00254], "fy":[0.00475,0.00097,0.00097,0.00475]}, + {"t":2.17162, "x":11.20552, "y":4.22543, "heading":0.00041, "vx":0.24993, "vy":-0.00305, "omega":-0.00056, "ax":0.0, "ay":-0.00005, "alpha":0.00228, "fx":[-0.00124,-0.00124,0.00123,0.00123], "fy":[0.00814,-0.02461,0.0061,0.00814]}, + {"t":2.2177, "x":11.21704, "y":4.22529, "heading":0.00039, "vx":0.24993, "vy":-0.00305, "omega":-0.00046, "ax":0.0, "ay":-0.00002, "alpha":0.00057, "fx":[-0.00045,-0.00166,0.00105,0.00105], "fy":[0.00134,-0.00189,-0.00189,0.00134]}, + {"t":2.26377, "x":11.22855, "y":4.22515, "heading":0.00036, "vx":0.24993, "vy":-0.00305, "omega":-0.00043, "ax":0.0, "ay":0.0, "alpha":0.00043, "fx":[-0.00114,-0.00114,0.00114,0.00114], "fy":[0.00099,-0.00106,-0.00088,0.00099]}, + {"t":2.30985, "x":11.24007, "y":4.22501, "heading":0.00035, "vx":0.24993, "vy":-0.00305, "omega":-0.00041, "ax":0.0, "ay":0.0, "alpha":0.00046, "fx":[-0.0006,-0.00081,0.0007,0.0007], "fy":[0.00142,-0.00141,-0.00141,0.00142]}, + {"t":2.35593, "x":11.25159, "y":4.22487, "heading":0.00033, "vx":0.24993, "vy":-0.00305, "omega":-0.00039, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[-0.001,-0.001,0.001,0.001], "fy":[0.00063,-0.00025,-0.001,0.00063]}, + {"t":2.40201, "x":11.2631, "y":4.22473, "heading":0.00031, "vx":0.24993, "vy":-0.00305, "omega":-0.00037, "ax":0.0, "ay":0.0, "alpha":0.0004, "fx":[-0.00056,-0.00058,0.00057,0.00057], "fy":[0.00124,-0.00125,-0.00125,0.00124]}, + {"t":2.44809, "x":11.27462, "y":4.22459, "heading":0.00029, "vx":0.24993, "vy":-0.00305, "omega":-0.00036, "ax":0.0, "ay":0.0, "alpha":0.00031, "fx":[-0.00087,-0.00087,0.00087,0.00087], "fy":[0.00064,-0.0005,-0.00077,0.00064]}, + {"t":2.49417, "x":11.28614, "y":4.22445, "heading":0.00027, "vx":0.24993, "vy":-0.00305, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00035, "fx":[-0.00049,-0.00048,0.00049,0.00049], "fy":[0.00109,-0.00109,-0.00109,0.00109]}, + {"t":2.54024, "x":11.29765, "y":4.2243, "heading":0.00026, "vx":0.24993, "vy":-0.00305, "omega":-0.00033, "ax":0.0, "ay":0.0, "alpha":0.00028, "fx":[-0.00077,-0.00077,0.00077,0.00077], "fy":[0.00059,-0.00057,-0.00061,0.00059]}, + {"t":2.58632, "x":11.30917, "y":4.22416, "heading":0.00024, "vx":0.24993, "vy":-0.00305, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":0.0003, "fx":[-0.00042,-0.00042,0.00042,0.00042], "fy":[0.00096,-0.00096,-0.00096,0.00096]}, + {"t":2.6324, "x":11.32068, "y":4.22402, "heading":0.00023, "vx":0.24993, "vy":-0.00305, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00025, "fx":[-0.00068,-0.00068,0.00068,0.00068], "fy":[0.00052,-0.00054,-0.00051,0.00052]}, + {"t":2.67848, "x":11.3322, "y":4.22388, "heading":0.00022, "vx":0.24993, "vy":-0.00305, "omega":-0.00029, "ax":0.0, "ay":0.0, "alpha":0.00027, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00086,-0.00086,-0.00086,0.00086]}, + {"t":2.72456, "x":11.34372, "y":4.22374, "heading":0.0002, "vx":0.24993, "vy":-0.00305, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[-0.00061,-0.00061,0.00061,0.00061], "fy":[0.00045,-0.00047,-0.00043,0.00045]}, + {"t":2.77063, "x":11.35523, "y":4.2236, "heading":0.00019, "vx":0.24993, "vy":-0.00305, "omega":-0.00027, "ax":0.0, "ay":0.0, "alpha":0.00024, "fx":[-0.0003,-0.0003,0.0003,0.0003], "fy":[0.00077,-0.00077,-0.00077,0.00077]}, + {"t":2.81671, "x":11.36675, "y":4.22346, "heading":0.00018, "vx":0.24993, "vy":-0.00305, "omega":-0.00025, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[-0.00055,-0.00055,0.00055,0.00055], "fy":[0.00037,-0.00039,-0.00035,0.00037]}, + {"t":2.86279, "x":11.37827, "y":4.22332, "heading":0.00017, "vx":0.24993, "vy":-0.00305, "omega":-0.00025, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[-0.00024,-0.00025,0.00024,0.00024], "fy":[0.00069,-0.00069,-0.00069,0.00069]}, + {"t":2.90887, "x":11.38978, "y":4.22318, "heading":0.00015, "vx":0.24993, "vy":-0.00305, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[-0.0005,-0.0005,0.0005,0.0005], "fy":[0.0003,-0.00032,-0.00028,0.0003]}, + {"t":2.95495, "x":11.4013, "y":4.22304, "heading":0.00014, "vx":0.24993, "vy":-0.00305, "omega":-0.00023, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[-0.00019,-0.0002,0.00019,0.00019], "fy":[0.00063,-0.00063,-0.00063,0.00063]}, + {"t":3.00103, "x":11.41281, "y":4.2229, "heading":0.00013, "vx":0.24993, "vy":-0.00305, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":0.00014, "fx":[-0.00045,-0.00045,0.00045,0.00045], "fy":[0.00023,-0.00025,-0.00021,0.00023]}, + {"t":3.0471, "x":11.42433, "y":4.22276, "heading":0.00012, "vx":0.24993, "vy":-0.00305, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[-0.00014,-0.00015,0.00014,0.00014], "fy":[0.00058,-0.00058,-0.00058,0.00058]}, + {"t":3.09318, "x":11.43585, "y":4.22262, "heading":0.00011, "vx":0.24993, "vy":-0.00305, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[-0.00042,-0.00042,0.00042,0.00042], "fy":[0.00016,-0.00018,-0.00014,0.00016]}, + {"t":3.13926, "x":11.44736, "y":4.22248, "heading":0.0001, "vx":0.24993, "vy":-0.00305, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00014, "fx":[-0.00008,-0.0001,0.00009,0.00009], "fy":[0.00054,-0.00054,-0.00054,0.00054]}, + {"t":3.18534, "x":11.45888, "y":4.22234, "heading":0.00009, "vx":0.24993, "vy":-0.00305, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[-0.00039,-0.00039,0.00039,0.00039], "fy":[0.00009,-0.0001,-0.00007,0.00009]}, + {"t":3.23142, "x":11.47039, "y":4.22219, "heading":0.00009, "vx":0.24993, "vy":-0.00305, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[-0.00002,-0.00004,0.00003,0.00003], "fy":[0.0005,-0.0005,-0.0005,0.0005]}, + {"t":3.2775, "x":11.48191, "y":4.22205, "heading":0.00008, "vx":0.24993, "vy":-0.00305, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[-0.00037,-0.00037,0.00037,0.00037], "fy":[0.00001,-0.00003,0.00001,0.00001]}, + {"t":3.32357, "x":11.49343, "y":4.22191, "heading":0.00007, "vx":0.24993, "vy":-0.00305, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00004,0.0,-0.00002,-0.00002], "fy":[0.00047,-0.00047,-0.00047,0.00047]}, + {"t":3.36965, "x":11.50494, "y":4.22177, "heading":0.00006, "vx":0.24993, "vy":-0.00305, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.00035,-0.00035,0.00035,0.00035], "fy":[-0.00007,0.00006,0.0001,-0.00007]}, + {"t":3.41573, "x":11.51646, "y":4.22163, "heading":0.00005, "vx":0.24993, "vy":-0.00305, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.00015,-0.00004,-0.00005,-0.00005], "fy":[0.00044,-0.00045,-0.00045,0.00044]}, + {"t":3.46181, "x":11.52798, "y":4.22149, "heading":0.00004, "vx":0.24993, "vy":-0.00305, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00033,-0.00033,0.00033,0.00033], "fy":[-0.00017,0.00015,0.00019,-0.00017]}, + {"t":3.50789, "x":11.53949, "y":4.22135, "heading":0.00004, "vx":0.24993, "vy":-0.00305, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00039,-0.00046,0.00003,0.00003], "fy":[0.00042,-0.00043,-0.00043,0.00042]}, + {"t":3.55397, "x":11.55101, "y":4.22121, "heading":0.00003, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00032,-0.00032,0.00032,0.00032], "fy":[-0.00028,0.00026,0.00031,-0.00028]}, + {"t":3.60004, "x":11.56252, "y":4.22107, "heading":0.00002, "vx":0.24993, "vy":-0.00305, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[0.0012,-0.00251,0.00065,0.00065], "fy":[0.0004,-0.0004,-0.0004,0.0004]}, + {"t":3.64612, "x":11.57404, "y":4.22093, "heading":0.00001, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[-0.00031,-0.00031,0.00031,0.00031], "fy":[-0.00039,0.00034,0.00044,-0.00039]}, + {"t":3.6922, "x":11.58556, "y":4.22079, "heading":0.00001, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[-0.00063,0.00332,-0.00135,-0.00135], "fy":[0.00041,-0.00041,-0.00041,0.00041]}, + {"t":3.73828, "x":11.59707, "y":4.22065, "heading":0.0, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00037,-0.00037,0.00021,0.00021], "fy":[-0.00022,-0.00046,0.00091,-0.00022]}, + {"t":3.78436, "x":11.60859, "y":4.22051, "heading":-0.00001, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":-0.62718, "ay":0.00766, "alpha":0.00887, "fx":[-7.06051,-7.26376,-7.06217,-7.06217], "fy":[0.08681,0.08699,0.08699,0.08681]}, + {"t":3.83043, "x":11.61944, "y":4.22037, "heading":-0.00002, "vx":0.22103, "vy":-0.0027, "omega":0.00024, "ax":-0.79878, "ay":0.00976, "alpha":0.00006, "fx":[-9.05844,-9.05844,-9.05752,-9.05752], "fy":[0.11057,0.10996,0.11158,0.11057]}, + {"t":3.87651, "x":11.62878, "y":4.22026, "heading":-0.00001, "vx":0.18422, "vy":-0.00225, "omega":0.00025, "ax":-0.79936, "ay":0.00977, "alpha":-0.00487, "fx":[-9.09306,-8.97831,-9.09342,-9.09342], "fy":[0.11187,0.10964,0.10963,0.11187]}, + {"t":3.92259, "x":11.63642, "y":4.22017, "heading":0.00001, "vx":0.14739, "vy":-0.0018, "omega":0.00002, "ax":-0.79955, "ay":0.00977, "alpha":0.00006, "fx":[-9.06749,-9.06749,-9.06602,-9.06602], "fy":[0.11048,0.11111,0.11104,0.11048]}, + {"t":3.96867, "x":11.64236, "y":4.22009, "heading":0.00001, "vx":0.11055, "vy":-0.00135, "omega":0.00003, "ax":-0.79965, "ay":0.00977, "alpha":-0.00244, "fx":[-9.08302,-9.02197,-9.08322,-9.08322], "fy":[0.11203,0.10955,0.10955,0.11203]}, + {"t":4.01475, "x":11.6466, "y":4.22004, "heading":0.00001, "vx":0.0737, "vy":-0.0009, "omega":-0.00009, "ax":-0.79971, "ay":0.00977, "alpha":0.00009, "fx":[-9.0694,-9.0694,-9.06764,-9.06764], "fy":[0.11053,0.1113,0.11084,0.11053]}, + {"t":4.06083, "x":11.64915, "y":4.22001, "heading":0.0, "vx":0.03685, "vy":-0.00045, "omega":-0.00008, "ax":-0.79975, "ay":0.00977, "alpha":0.00179, "fx":[-9.07904,-9.07904,-9.07928,-9.03848], "fy":[0.11108,0.11082,0.11082,0.1105]}, + {"t":4.1069, "x":11.65, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] From 31c728ab15067fb420a3eed3f687883bc2f9a5eb Mon Sep 17 00:00:00 2001 From: Pez357 Date: Sun, 23 Mar 2025 14:21:03 -0400 Subject: [PATCH 23/24] Adjust Al;gae L3 height, comment out algae wrist set angle on grab --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/elevator/ElevatorConstants.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a55146d6..fd9b4937 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -387,8 +387,8 @@ private void configureEventBindings() { algaeRoller.hasAlgae .whileTrue(algaeRoller.createHoldAlgaeCommand()); - algaeRoller.hasAlgae - .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge)); + // algaeRoller.hasAlgae + // .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge)); coralRoller.isRolling.or(algaeRoller.isRolling).whileTrue(createRollerAnimationCommand()); } diff --git a/src/main/java/frc/robot/elevator/ElevatorConstants.java b/src/main/java/frc/robot/elevator/ElevatorConstants.java index a2ddb688..b18238a0 100644 --- a/src/main/java/frc/robot/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/elevator/ElevatorConstants.java @@ -73,7 +73,7 @@ public static enum LifterState { CoralIntake(11.7), AlgaeProcessor(12.0), AlgaeL2(29.5), - AlgaeL3(45), + AlgaeL3(43), AlgaeBarge(67.8), Max(68.3); From d7e10bb7666ac90d3b2de210cf45fe20a4d4116a Mon Sep 17 00:00:00 2001 From: Christopher Larrieu Date: Tue, 25 Mar 2025 10:54:01 -0400 Subject: [PATCH 24/24] Added pure odometry tracking for comparison with vision --- src/main/java/frc/lib/AutoSelector.java | 4 +- src/main/java/frc/robot/Robot.java | 13 +++++- src/main/java/frc/robot/auto/AutoMode.java | 6 +-- .../java/frc/robot/drivetrain/Drivetrain.java | 42 ++++++++++++------- 4 files changed, 43 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/lib/AutoSelector.java b/src/main/java/frc/lib/AutoSelector.java index 3b9502db..b80c9b14 100644 --- a/src/main/java/frc/lib/AutoSelector.java +++ b/src/main/java/frc/lib/AutoSelector.java @@ -127,12 +127,12 @@ public void disabledPeriodic() { SmartDashboard.putString("SelectedAutonomousMode", ao.getName()); ao.getInitialPose() .ifPresentOrElse( - initialPosePublisher::set, () -> initialPosePublisher.set(new Pose2d())); + initialPosePublisher::set, () -> initialPosePublisher.set(Pose2d.kZero)); }, () -> { SmartDashboard.putString( "SelectedAutonomousMode", "None; no auto mode assigned to this slot"); - initialPosePublisher.set(new Pose2d()); + initialPosePublisher.set(Pose2d.kZero); }); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 556ee75e..6eeb24d6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,5 +1,7 @@ package frc.robot; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructArrayPublisher; @@ -106,7 +108,15 @@ public Gamepiece get() { "Align Encoders", new InstantCommand(() -> swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true)); - addPeriodic(() -> swerve.refreshRelativeTurningEncoder(), 0.1); + addPeriodic(() -> swerve.refreshRelativeTurningEncoder(), Seconds.of(0.1)); + // TODO: see what happens with and without this odometry update + // addPeriodic(() -> updateOdometry(), Seconds.of(1)); + } + + public void updateOdometry() { + vision + .getEstimatedGlobalPose() + .ifPresent(pose -> swerve.resetOdometry(pose.estimatedPose.toPose2d())); } @Override @@ -384,6 +394,7 @@ private synchronized StructPublisher getPose2dPublisher(String name) { } protected void checkVision() { + vision .getPoseEstimates() .forEach( diff --git a/src/main/java/frc/robot/auto/AutoMode.java b/src/main/java/frc/robot/auto/AutoMode.java index d44bb918..6e85198f 100644 --- a/src/main/java/frc/robot/auto/AutoMode.java +++ b/src/main/java/frc/robot/auto/AutoMode.java @@ -7,15 +7,13 @@ import java.util.Optional; public abstract class AutoMode { - private final Drivetrain drivetrain; private final AutoFactory autoFactory; - public AutoMode(Drivetrain dt) { - this.drivetrain = dt; + public AutoMode(Drivetrain drivetrain) { autoFactory = new AutoFactory( drivetrain::getPose, - drivetrain::setPose, + drivetrain::resetOdometry, drivetrain::followTrajectory, false, drivetrain); diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index b7f9feba..c802cc5f 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; @@ -47,6 +48,7 @@ public class Drivetrain extends SubsystemBase { // The robot pose estimator for tracking swerve odometry and applying vision corrections. private final SwerveDrivePoseEstimator poseEstimator; + private final SwerveDriveOdometry odometry; private final PIDController xController = new PIDController(TranslationControllerGains.kP, 0.0, 0.0); @@ -58,8 +60,10 @@ public class Drivetrain extends SubsystemBase { private final Canandgyro canandgyro = new Canandgyro(0); private Rotation2d headingOffset = new Rotation2d(); - private StructPublisher m_visionPosePublisher = + private StructPublisher visionPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Vision", Pose2d.struct).publish(); + private StructPublisher odometryPosePublisher = + NetworkTableInstance.getDefault().getStructTopic("Odometry", Pose2d.struct).publish(); public Drivetrain(BooleanSupplier fieldRotated, Supplier elevatorHeight) { this.fieldRotatedSupplier = fieldRotated; @@ -78,9 +82,14 @@ public Drivetrain(BooleanSupplier fieldRotated, Supplier elevator DriveConstants.kDriveKinematics, canandgyro.getRotation2d(), getSwerveModulePositions(), - new Pose2d(), + Pose2d.kZero, DriveConstants.kStateStdDevs, VisionConstants.kMultiTagStdDevs); + odometry = + new SwerveDriveOdometry( + DriveConstants.kDriveKinematics, + canandgyro.getRotation2d(), + getSwerveModulePositions()); thetaController.enableContinuousInput(-Math.PI, Math.PI); } @@ -88,7 +97,9 @@ public Drivetrain(BooleanSupplier fieldRotated, Supplier elevator @Override public void periodic() { poseEstimator.update(canandgyro.getRotation2d(), getSwerveModulePositions()); - m_visionPosePublisher.set(poseEstimator.getEstimatedPosition()); + odometry.update(canandgyro.getRotation2d(), getSwerveModulePositions()); + visionPosePublisher.set(poseEstimator.getEstimatedPosition()); + odometryPosePublisher.set(odometry.getPoseMeters()); for (SwerveModule module : SwerveModule.values()) { SmartDashboard.putNumber( @@ -169,8 +180,20 @@ public Pose2d getPose() { /** * @param pose The robot pose */ - public void setPose(Pose2d pose) { + public void resetOdometry(Pose2d pose) { + resetOdometry(pose, false); + } + + /** + * Reset the swerve drive's field pose. + * + * @param pose New robot pose. + * @param resetSimPose If the simulated robot pose should also be reset. This effectively + * teleports the robot and should only be used during the setup of the simulation world. + */ + public void resetOdometry(Pose2d pose, boolean resetSimPose) { poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose); + odometry.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose); } public void initializeRelativeTurningEncoder() { @@ -234,17 +257,6 @@ public void addVisionMeasurement( poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs); } - /** - * Reset the estimated pose of the swerve drive on the field. - * - * @param pose New robot pose. - * @param resetSimPose If the simulated robot pose should also be reset. This effectively - * teleports the robot and should only be used during the setup of the simulation world. - */ - public void resetPose(Pose2d pose, boolean resetSimPose) { - poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose); - } - public void followTrajectory(SwerveSample sample) { Pose2d pose = getPose();