From 16deab3f233fdc52504602c6a3d9f2c007db53cd Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Sat, 14 Mar 2026 10:07:41 -0500 Subject: [PATCH 01/32] Changes made to the auto shoot and dashboard --- src/main/java/frc/robot/Autos.java | 45 ++++++------------ src/main/java/frc/robot/RobotContainer.java | 3 ++ .../robot/subsystems/dashboard/Dashboard.java | 46 +++++++++++++++++++ .../frc/robot/subsystems/shooter/Turret.java | 5 ++ src/main/java/frc/robot/util/Utilities.java | 21 +++++++++ 5 files changed, 88 insertions(+), 32 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/dashboard/Dashboard.java diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 4afae24..d660f3a 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -1,13 +1,10 @@ package frc.robot; -import static edu.wpi.first.units.Units.RPM; - import java.util.stream.IntStream; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -25,22 +22,20 @@ public class Autos extends SubsystemBase private enum StartPosition { // @formatter:off - LeftTrench ("Left Trench", ChoreoVars.Poses.LeftTrench, RPM.of(4500)), - LeftBump ("Left Bump", ChoreoVars.Poses.LeftBump, RPM.of(3200)), - HubStart ("Hub", ChoreoVars.Poses.HubStart, RPM.zero()), - RightBump ("Right Bump", ChoreoVars.Poses.RightBump, RPM.of(3200)), - RightTrench("Right Trench", ChoreoVars.Poses.RightTrench, RPM.of(4500)); + LeftTrench ("Left Trench", ChoreoVars.Poses.LeftTrench), + LeftBump ("Left Bump", ChoreoVars.Poses.LeftBump), + HubStart ("Hub", ChoreoVars.Poses.HubStart), + RightBump ("Right Bump", ChoreoVars.Poses.RightBump), + RightTrench("Right Trench", ChoreoVars.Poses.RightTrench); // @formatter:on - public String displayName; - public Pose2d pose; - public AngularVelocity flywheelVelocity; + public String displayName; + public Pose2d pose; - private StartPosition(String name, Pose2d bluePose, AngularVelocity velocity) + private StartPosition(String name, Pose2d bluePose) { - displayName = name; - pose = bluePose; - flywheelVelocity = velocity; + displayName = name; + pose = bluePose; } } @@ -144,24 +139,10 @@ public Command buildAuto() // @formatter:off return Commands.sequence ( - Commands.runOnce(() -> - { - _driveSubsystem.resetPose(flip(start.pose)); - _shooterSubsystem._turret.setDisabled(true); - }), + Commands.runOnce(() -> _driveSubsystem.resetPose(flip(start.pose))), Commands.waitSeconds(_autoDelay.getSelected()), - Commands.sequence - ( - _shooterSubsystem.setFlywheelVelocity(start.flywheelVelocity), - Commands.waitUntil(() -> _shooterSubsystem._flywheel.atSpeed()), - _shooterSubsystem.runFeeder() - ) - .onlyIf(() -> SmartDashboard.getBoolean(_shootNTKey, false) && start != StartPosition.HubStart) - .finallyDo(() -> - { - _shooterSubsystem._turret.setDisabled(false); - _shooterSubsystem._flywheel.stop(); - }) + _shooterSubsystem.shoot() + .onlyIf(() -> SmartDashboard.getBoolean(_shootNTKey, false) && start != StartPosition.HubStart) ); // @formatter:on } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c74b95..670349e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.dashboard.Dashboard; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.TunerConstants; import frc.robot.subsystems.intake.Intake; @@ -42,6 +43,8 @@ public class RobotContainer private final Intake _intake = new Intake(); private final Shooter _shooter = new Shooter(_drive::getState); @NotLogged + private final Dashboard _dashboard = new Dashboard(_shooter); + @NotLogged private final Autos _autos = new Autos(_drive, _shooter); private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; diff --git a/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java b/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java new file mode 100644 index 0000000..ab6ffb4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.dashboard; + +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.Turret.TurretState; +import frc.robot.util.Utilities; + +public class Dashboard extends SubsystemBase +{ + private final Shooter _shooter; + private final DoublePublisher _matchTimePub; + private final BooleanPublisher _hubActivePub; + private final DoublePublisher _hubShiftTimerPub; + private final BooleanPublisher _shooterIsShootModePub; + private final BooleanPublisher _turretHasTargetPub; + private final BooleanPublisher _turretLinedUpPub; + + public Dashboard(Shooter shooter) + { + _shooter = shooter; + + var table = NetworkTableInstance.getDefault().getTable("Dashboard").getSubTable("Robot Values"); + + _matchTimePub = table.getDoubleTopic("Match Time").publish(); + _hubActivePub = table.getBooleanTopic("Hub Active").publish(); + _hubShiftTimerPub = table.getDoubleTopic("Hub State Time Remaining").publish(); + _shooterIsShootModePub = table.getBooleanTopic("Shooter Is Shoot Mode").publish(); + _turretHasTargetPub = table.getBooleanTopic("Turret Has Target").publish(); + _turretLinedUpPub = table.getBooleanTopic("Turret Lined Up").publish(); + } + + @Override + public void periodic() + { + _matchTimePub.set(DriverStation.getMatchTime()); + _hubActivePub.set(Utilities.isHubActive()); + _hubShiftTimerPub.set(Utilities.getTimeUntilNextShift()); + _shooterIsShootModePub.set(_shooter._turret.getTurretState() == TurretState.Track); + _turretHasTargetPub.set(_shooter._turret.hasLimelightTarget()); + _turretLinedUpPub.set(_shooter._turret.isLinedUp()); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index ff2c89e..65aecab 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -262,6 +262,11 @@ public boolean hasValidTarget() return _targetValid; } + public boolean hasLimelightTarget() + { + return _limelightHasTarget; + } + public boolean isReadyToShoot() { return hasValidTarget() && isLinedUp(); diff --git a/src/main/java/frc/robot/util/Utilities.java b/src/main/java/frc/robot/util/Utilities.java index cc3eb57..fda4d6f 100644 --- a/src/main/java/frc/robot/util/Utilities.java +++ b/src/main/java/frc/robot/util/Utilities.java @@ -123,4 +123,25 @@ public static Translation2d getHubCoordinates() { return isBlueAlliance() ? ShooterConstants.BLUE_HUB : ShooterConstants.RED_HUB; } + + /** + * Returns the number of seconds remaining until the next hub shift boundary. + * Works in practice mode (no FMS game data required) since shift timing is + * purely based on match time. Returns -1 when not in teleop. + */ + public static double getTimeUntilNextShift() + { + double timeRemaining = DriverStation.getMatchTime(); + if (timeRemaining < 0 || !DriverStation.isTeleop()) + { + return -1; + } + + if (timeRemaining > kTransitionShiftEndTimeSecs.in(Seconds)) return timeRemaining - kTransitionShiftEndTimeSecs.in(Seconds); + if (timeRemaining > kShift1EndTimeSecs.in(Seconds)) return timeRemaining - kShift1EndTimeSecs.in(Seconds); + if (timeRemaining > kShift2EndTimeSecs.in(Seconds)) return timeRemaining - kShift2EndTimeSecs.in(Seconds); + if (timeRemaining > kShift3EndTimeSecs.in(Seconds)) return timeRemaining - kShift3EndTimeSecs.in(Seconds); + if (timeRemaining > kShift4EndTimeSecs.in(Seconds)) return timeRemaining - kShift4EndTimeSecs.in(Seconds); + return timeRemaining; // endgame - time left in match + } } From 3ca1bf4328d2d1ade2fbfeb59a01dc13992d1f91 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Sat, 14 Mar 2026 14:38:37 -0500 Subject: [PATCH 02/32] Enhance shooter and turret functionality with new auto mode and manual controls; update dashboard for improved telemetry and match time handling. --- src/main/deploy/elastic-layout.json | 39 +++-- src/main/java/frc/robot/Autos.java | 149 ++++++++++-------- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 11 ++ .../robot/subsystems/dashboard/Dashboard.java | 62 +++++++- .../frc/robot/subsystems/shooter/Shooter.java | 33 +++- .../frc/robot/subsystems/shooter/Turret.java | 62 ++++++-- 7 files changed, 263 insertions(+), 94 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 9c7a411..9cd4eef 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -18,22 +18,22 @@ }, "children": [ { - "title": "Auto Delay", - "x": 1984.0, - "y": 416.0, + "title": "Auto Mode", + "x": 0.0, + "y": 0.0, "width": 288.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { - "topic": "/SmartDashboard/Auto Delay", + "topic": "/SmartDashboard/Auto Mode", "period": 0.06, "sort_options": false } }, { "title": "Start Position", - "x": 1984.0, - "y": 480.0, + "x": 0.0, + "y": 0.0, "width": 288.0, "height": 128.0, "type": "ComboBox Chooser", @@ -44,16 +44,29 @@ } }, { - "title": "Shoot", - "x": 1664.0, - "y": 320.0, - "width": 128.0, + "title": "Auto Trajectory", + "x": 0.0, + "y": 0.0, + "width": 288.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Trajectory", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Auto Delay", + "x": 0.0, + "y": 0.0, + "width": 288.0, "height": 128.0, - "type": "Toggle Switch", + "type": "ComboBox Chooser", "properties": { - "topic": "/SmartDashboard/Shoot", + "topic": "/SmartDashboard/Auto Delay", "period": 0.06, - "data_type": "boolean" + "sort_options": false } } ] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index d660f3a..df76471 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -2,6 +2,13 @@ import java.util.stream.IntStream; +import choreo.auto.AutoFactory; +import choreo.trajectory.SwerveSample; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; + +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -12,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.GeneralConstants; +import frc.robot.generated.ChoreoTraj; import frc.robot.generated.ChoreoVars; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.Shooter; @@ -19,6 +27,24 @@ public class Autos extends SubsystemBase { + private enum AutoMode + { + // @formatter:off + DoNothing ("Do Nothing"), + ShootOnly ("Shoot Only"), + ShootWithDelay ("Shoot with Delay"), + ShootThenDrive ("Shoot then Drive"), + ShootWithDelayThenDrive("Shoot with Delay then Drive"); + // @formatter:on + + public final String displayName; + + private AutoMode(String name) + { + displayName = name; + } + } + private enum StartPosition { // @formatter:off @@ -27,6 +53,7 @@ private enum StartPosition HubStart ("Hub", ChoreoVars.Poses.HubStart), RightBump ("Right Bump", ChoreoVars.Poses.RightBump), RightTrench("Right Trench", ChoreoVars.Poses.RightTrench); + // @formatter:on public String displayName; @@ -39,37 +66,44 @@ private StartPosition(String name, Pose2d bluePose) } } - // private final AutoFactory _autoFactory; - private final Drive _driveSubsystem; - private final Shooter _shooterSubsystem; - // private final SwerveRequest.FieldCentric _autoFollowingRequest = new - // SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - // private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); - // private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); - // private final PIDController _headingController = new PIDController(0.0, 0.0, - // 0.0); - private final SendableChooser _autoDelay = new SendableChooser(); - private final SendableChooser _startChooser = new SendableChooser(); - private final String _shootNTKey = "Shoot"; + private static final String NO_TRAJECTORY = "None (Stay)"; + private final AutoFactory _autoFactory; + private final Drive _driveSubsystem; + private final Shooter _shooterSubsystem; + private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); + private final SendableChooser _modeChooser = new SendableChooser(); + private final SendableChooser _autoDelay = new SendableChooser(); + private final SendableChooser _trajChooser = new SendableChooser(); + private final SendableChooser _startChooser = new SendableChooser(); private final Field2d _field; - private StartPosition _startPosition = null; + private StartPosition _startPosition = null; public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { _driveSubsystem = driveSubsystem; _shooterSubsystem = shooterSubsystem; + _headingController.enableContinuousInput(-Math.PI, Math.PI); + // @formatter:off - // _autoFactory = new AutoFactory - // ( - // () -> driveSubsystem.getState().Pose, - // driveSubsystem::resetPose, - // this::followTrajectory, - // true, - // driveSubsystem - // ); + _autoFactory = new AutoFactory( + () -> driveSubsystem.getState().Pose, + driveSubsystem::resetPose, + this::followTrajectory, + true, + driveSubsystem + ); // @formatter:on + _modeChooser.setDefaultOption(AutoMode.ShootOnly.displayName, AutoMode.ShootOnly); + _modeChooser.addOption(AutoMode.ShootWithDelay.displayName, AutoMode.ShootWithDelay); + _modeChooser.addOption(AutoMode.ShootThenDrive.displayName, AutoMode.ShootThenDrive); + _modeChooser.addOption(AutoMode.ShootWithDelayThenDrive.displayName, AutoMode.ShootWithDelayThenDrive); + _modeChooser.addOption(AutoMode.DoNothing.displayName, AutoMode.DoNothing); + _autoDelay.setDefaultOption("0", 0); IntStream.range(1, 6).forEach(n -> _autoDelay.addOption(String.valueOf(n), n)); @@ -79,16 +113,16 @@ public Autos(Drive driveSubsystem, Shooter shooterSubsystem) _startChooser.addOption(StartPosition.RightBump.displayName, StartPosition.RightBump); _startChooser.addOption(StartPosition.RightTrench.displayName, StartPosition.RightTrench); + _trajChooser.setDefaultOption(NO_TRAJECTORY, NO_TRAJECTORY); + ChoreoTraj.ALL_TRAJECTORIES.keySet().stream().sorted().forEach(name -> _trajChooser.addOption(name, name)); + _field = new Field2d(); + SmartDashboard.putData("Auto Mode", _modeChooser); + SmartDashboard.putData("Auto Trajectory", _trajChooser); SmartDashboard.putData("Auto Delay", _autoDelay); SmartDashboard.putData("Start Position", _startChooser); SmartDashboard.putData("Autonomous Mode", _field); - - if (!SmartDashboard.containsKey(_shootNTKey)) - { - SmartDashboard.putBoolean(_shootNTKey, true); - } } @Override @@ -103,46 +137,37 @@ public void periodic() } } - // private void followTrajectory(SwerveSample sample) - // { - // // Get the current pose of the robot - // Pose2d pose = _driveSubsystem.getState().Pose; - - // // Build up "request" based on "sample" - // // @formatter:off - // _autoFollowingRequest - // .withVelocityX(sample.vx + _xController.calculate(pose.getX(), sample.x)) - // .withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) - // .withRotationalRate(sample.omega + _headingController.calculate(pose.getRotation().getRadians(), sample.heading)) - // .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - // // @formatter:on - - // _driveSubsystem.setControl(_autoFollowingRequest); - // } - - // public Command followPath(String pathName) - // { - // // @formatter:off - // return Commands.sequence - // ( - // _autoFactory.resetOdometry(pathName), - // _autoFactory.trajectoryCmd(pathName), - // Commands.runOnce(() -> _driveSubsystem.setControl(new SwerveRequest.Idle()), _driveSubsystem) - // ); - // // @formatter:on - // } - public Command buildAuto() { - var start = _startChooser.getSelected(); + var start = _startChooser.getSelected(); + var mode = _modeChooser.getSelected(); + var trajName = _trajChooser.getSelected(); + var reset = Commands.runOnce(() -> _driveSubsystem.resetPose(flip(start.pose))); + var shoot = _shooterSubsystem.shoot(); + var delay = Commands.waitSeconds(_autoDelay.getSelected()); + var drive = NO_TRAJECTORY.equals(trajName) ? Commands.none() : _autoFactory.trajectoryCmd(trajName); + + // @formatter:off + return switch (mode) + { + case DoNothing -> reset; + case ShootOnly -> Commands.sequence(reset, shoot); + case ShootWithDelay -> Commands.sequence(reset, delay, shoot); + case ShootThenDrive -> Commands.sequence(reset, shoot, drive); + case ShootWithDelayThenDrive -> Commands.sequence(reset, delay, shoot, drive); + }; + // @formatter:on + } + + private void followTrajectory(SwerveSample sample) + { + var pose = _driveSubsystem.getState().Pose; // @formatter:off - return Commands.sequence - ( - Commands.runOnce(() -> _driveSubsystem.resetPose(flip(start.pose))), - Commands.waitSeconds(_autoDelay.getSelected()), - _shooterSubsystem.shoot() - .onlyIf(() -> SmartDashboard.getBoolean(_shootNTKey, false) && start != StartPosition.HubStart) + _driveSubsystem.setControl(_autoRequest + .withVelocityX(sample.vx + _xController.calculate(pose.getX(), sample.x)) + .withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) + .withRotationalRate(sample.omega + _headingController.calculate(pose.getRotation().getRadians(), sample.heading)) ); // @formatter:on } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7c2ca08..59356d1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -163,6 +163,7 @@ public static class ShooterConstants public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); // Forward-facing when no target public static final Angle TURRET_TOLERANCE = Degrees.of(2.0); // degrees public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(2); + public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); // Physical offset between turret rotation center and shooter exit point public static final String LIMELIGHT_NAME = "limelight-shooter"; public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 670349e..34d83db 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,8 @@ public class RobotContainer { private static final double MANUAL_FLYWHEEL_START_RPM = 3500.0; private static final double MANUAL_FLYWHEEL_STEP_RPM = 50.0; + private static final double MANUAL_TURRET_START_DEG = 0.0; + private static final double MANUAL_TURRET_STEP_DEG = 2.0; /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric _fieldCentric = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -80,6 +82,12 @@ private void setManualFlywheelRPM(double rpm) _shooter.setManualFlywheel(_manualFlywheelRPM); } + private void bumpManualTurretAngle(double deltaDeg) + { + _manualTurretDeg += deltaDeg; + _shooter.bumpManualTurretAngle(deltaDeg); + } + private void configureBindings() { _drive.setDefaultCommand(_drive.applyRequest(this::getFieldCentricRequest)); @@ -91,6 +99,7 @@ private void configureBindings() _driver.button(2).whileTrue(Commands.startEnd(() -> _driveMultiplier = DriveConstants.SLOW_MODE_SCALE, () -> _driveMultiplier = DriveConstants.FULL_SPEED_SCALE)); _driver.button(3).whileTrue(_drive.applyRequest(() -> _robotCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate()))); _driver.button(5).whileTrue(_shooter.pass()); + _driver.button(6).whileTrue(_shooter.trackOnly()); _driver.button(7).onTrue(_drive.runOnce(_drive::seedFieldCentric)); _drive.registerTelemetry(_logger::telemeterize); @@ -107,6 +116,8 @@ private void configureBindings() _operator.rightBumper().whileTrue(_shooter.runManualFeeder()); _operator.povDown().onTrue(_intake.getRetractCmd()); _operator.povUp().onTrue(_intake.getExtendCmd()); + _operator.povLeft().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(-MANUAL_TURRET_STEP_DEG)).onlyIf(_shooter::inManualMode)); + _operator.povRight().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(MANUAL_TURRET_STEP_DEG)).onlyIf(_shooter::inManualMode)); } public Command getAutonomousCommand() diff --git a/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java b/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java index ab6ffb4..f0b7f76 100644 --- a/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java @@ -4,6 +4,7 @@ import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.Turret.TurretState; @@ -11,6 +12,17 @@ public class Dashboard extends SubsystemBase { + // Assumed teleop length when FMS match time is unavailable + private static final double TELEOP_SECS = 135.0; + + // Elapsed-time shift boundaries (derived from Utilities constants, assuming + // 135s teleop) + // kTransitionShiftEnd = 130s remaining → 5s elapsed + // kShift1End = 105s remaining → 30s elapsed + // kShift2End = 80s remaining → 55s elapsed + // kShift3End = 55s remaining → 80s elapsed + // kShift4End = 30s remaining → 105s elapsed + private static final double[] SHIFT_ELAPSED_ENDS = { 5.0, 30.0, 55.0, 80.0, 105.0 }; private final Shooter _shooter; private final DoublePublisher _matchTimePub; private final BooleanPublisher _hubActivePub; @@ -18,6 +30,8 @@ public class Dashboard extends SubsystemBase private final BooleanPublisher _shooterIsShootModePub; private final BooleanPublisher _turretHasTargetPub; private final BooleanPublisher _turretLinedUpPub; + private final Timer _teleopTimer = new Timer(); + private boolean _wasInTeleop = false; public Dashboard(Shooter shooter) { @@ -36,11 +50,55 @@ public Dashboard(Shooter shooter) @Override public void periodic() { - _matchTimePub.set(DriverStation.getMatchTime()); + updateTeleopTimer(); + + _matchTimePub.set(getMatchTime()); _hubActivePub.set(Utilities.isHubActive()); - _hubShiftTimerPub.set(Utilities.getTimeUntilNextShift()); + _hubShiftTimerPub.set(getShiftTimer()); _shooterIsShootModePub.set(_shooter._turret.getTurretState() == TurretState.Track); _turretHasTargetPub.set(_shooter._turret.hasLimelightTarget()); _turretLinedUpPub.set(_shooter._turret.isLinedUp()); } + + private void updateTeleopTimer() + { + boolean inTeleop = DriverStation.isTeleop() && DriverStation.isEnabled(); + + if (inTeleop && !_wasInTeleop) + { + _teleopTimer.restart(); + } + else if (!DriverStation.isTeleop()) + { + _teleopTimer.stop(); + _teleopTimer.reset(); + } + + _wasInTeleop = inTeleop; + } + + private double getMatchTime() + { + double fmsTime = DriverStation.getMatchTime(); + if (fmsTime >= 0) return fmsTime; + if (!DriverStation.isTeleop()) return -1; + + // Fallback: count down from assumed teleop length + return Math.max(0, TELEOP_SECS - _teleopTimer.get()); + } + + private double getShiftTimer() + { + double fmsTime = DriverStation.getMatchTime(); + if (fmsTime >= 0) return Utilities.getTimeUntilNextShift(); + if (!DriverStation.isTeleop()) return -1; + + // Fallback: compute from local elapsed timer + double elapsed = _teleopTimer.get(); + for (double end : SHIFT_ELAPSED_ENDS) + { + if (elapsed < end) return end - elapsed; + } + return Math.max(0, TELEOP_SECS - elapsed); // endgame + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0009d91..7a1efff 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; import java.util.function.Supplier; @@ -19,7 +20,7 @@ public class Shooter extends SubsystemBase { public enum ShooterState { - Idle, Preparing, Ready, Firing, Manual; + Idle, Preparing, Ready, Firing, Manual, TrackingOnly; } public Command startShooter() @@ -151,6 +152,30 @@ public Command stop() return runOnce(this::stopShooter); } + public Command trackOnly() + { + return startEnd(() -> + { + _state = ShooterState.TrackingOnly; + _turret.setDisabled(false); + }, this::stopShooter); + } + + public void bumpManualTurretAngle(double deltaDeg) + { + if (inManualMode()) + { + _turret.setDisabled(false); + _turret.setTurretState(TurretState.ManualAngle); + _turret.bumpManualAngle(Degrees.of(deltaDeg)); + } + } + + public double getManualTurretAngleDeg() + { + return _turret.getManualAngle().in(Degrees); + } + public final Flywheel _flywheel; public final Feeder _feeder; public final Turret _turret; @@ -222,6 +247,12 @@ public void periodic() case Manual: break; + case TrackingOnly: + _feeder.set(false); + _flywheel.stop(); + _turret.setTurretState(TurretState.Track); + break; + case Idle: default: _flywheel.stop(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 65aecab..90e0053 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -44,7 +44,7 @@ public class Turret { public enum TurretState { - Idle, Track, Pass + Idle, Track, Pass, ManualAngle } private final TalonFX _turretMotor; @@ -54,6 +54,7 @@ public enum TurretState private final PIDController _pidController; private SwerveDriveState _currentSwerveState; private TurretState _turretState; + private Angle _manualAngleSetpoint; @Logged private Angle _turretAngle; @Logged @@ -86,21 +87,22 @@ public Turret(Supplier swerveStateSupplier) sensorOffset = sensorOffset.unaryMinus(); } - _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); - _turretSensor = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); - _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); - _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); - _currentSwerveState = new SwerveDriveState(); - _turretState = TurretState.Idle; - _turretAngle = Degrees.zero(); - _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _hasSetpoint = false; - _motorVoltage = Volts.zero(); - _targetDistance = Meters.zero(); - _targetPose = new Pose2d(); - _limelightHasTarget = false; - _targetValid = false; - _disabled = false; + _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); + _turretSensor = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); + _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); + _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); + _currentSwerveState = new SwerveDriveState(); + _turretState = TurretState.Idle; + _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _turretAngle = Degrees.zero(); + _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _hasSetpoint = false; + _motorVoltage = Volts.zero(); + _targetDistance = Meters.zero(); + _targetPose = new Pose2d(); + _limelightHasTarget = false; + _targetValid = false; + _disabled = false; var currentConfig = new CurrentLimitsConfigs(); currentConfig.StatorCurrentLimit = ShooterConstants.TURRET_CURRENT_LIMIT.in(Amps); @@ -186,6 +188,13 @@ public void periodic() } } + // The shooter exit point is forward of the turret rotation center. This creates + // a lateral miss that varies as sin(turretAngle): zero at 0°, max at ±90°. + // Guard distance from zero to avoid atan2 blowing up at startup. + var safeDistanceM = Math.max(_targetDistance.in(Meters), 0.5); + var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); + rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); + _targetValid = Utilities.isHubActive(); break; @@ -208,6 +217,12 @@ public void periodic() rawSetpoint = fieldTargetAngle.minus(_currentSwerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); break; + case ManualAngle: + _hasSetpoint = true; + _targetValid = false; + rawSetpoint = _manualAngleSetpoint; + break; + case Idle: default: _hasSetpoint = false; @@ -247,6 +262,21 @@ public void setTurretState(TurretState state) _turretState = state; } + public void setManualAngle(Angle angle) + { + _manualAngleSetpoint = angle; + } + + public void bumpManualAngle(Angle delta) + { + _manualAngleSetpoint = _manualAngleSetpoint.plus(delta); + } + + public Angle getManualAngle() + { + return _manualAngleSetpoint; + } + public TurretState getTurretState() { return _turretState; From 68d90e9a6d934e0312fae98dc5eb107aeae815a2 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Sun, 15 Mar 2026 12:33:55 -0500 Subject: [PATCH 03/32] Refactor shooter and turret logic for improved functionality, including manual turret angle adjustments and new tracking state --- src/main/deploy/elastic-layout.json | 823 ------------------ src/main/java/frc/robot/Autos.java | 126 +-- src/main/java/frc/robot/RobotContainer.java | 8 - src/main/java/frc/robot/Telemetry.java | 116 --- .../robot/subsystems/dashboard/Dashboard.java | 104 --- 5 files changed, 32 insertions(+), 1145 deletions(-) delete mode 100644 src/main/deploy/elastic-layout.json delete mode 100644 src/main/java/frc/robot/Telemetry.java delete mode 100644 src/main/java/frc/robot/subsystems/dashboard/Dashboard.java diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json deleted file mode 100644 index 9cd4eef..0000000 --- a/src/main/deploy/elastic-layout.json +++ /dev/null @@ -1,823 +0,0 @@ -{ - "version": 1.0, - "grid_size": 32, - "tabs": [ - { - "name": "Autonomous", - "grid_layout": { - "layouts": [ - { - "title": "List Layout", - "x": 1568.0, - "y": 0.0, - "width": 352.0, - "height": 448.0, - "type": "List Layout", - "properties": { - "label_position": "TOP" - }, - "children": [ - { - "title": "Auto Mode", - "x": 0.0, - "y": 0.0, - "width": 288.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Auto Mode", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Start Position", - "x": 0.0, - "y": 0.0, - "width": 288.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Start Position", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Auto Trajectory", - "x": 0.0, - "y": 0.0, - "width": 288.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Auto Trajectory", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Auto Delay", - "x": 0.0, - "y": 0.0, - "width": 288.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Auto Delay", - "period": 0.06, - "sort_options": false - } - } - ] - } - ], - "containers": [ - { - "title": "Alerts", - "x": 1568.0, - "y": 448.0, - "width": 352.0, - "height": 320.0, - "type": "Alerts", - "properties": { - "topic": "/SmartDashboard/Alerts", - "period": 0.02 - } - }, - { - "title": "Autonomous Mode", - "x": 0.0, - "y": 0.0, - "width": 1568.0, - "height": 768.0, - "type": "Field", - "properties": { - "topic": "/SmartDashboard/Autonomous Mode", - "period": 0.06, - "field_game": "Rebuilt", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295, - "show_robot_outside_widget": true - } - } - ] - } - }, - { - "name": "Teleoperated", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Alerts", - "x": 1664.0, - "y": 0.0, - "width": 256.0, - "height": 768.0, - "type": "Alerts", - "properties": { - "topic": "/SmartDashboard/Alerts", - "period": 0.02 - } - }, - { - "title": "Match Time", - "x": 0.0, - "y": 0.0, - "width": 1152.0, - "height": 768.0, - "type": "Match Time", - "properties": { - "topic": "Dashboard/Robot Values/Match Time", - "period": 0.02, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, - { - "title": "Hub Active", - "x": 1152.0, - "y": 0.0, - "width": 512.0, - "height": 192.0, - "type": "Boolean Box", - "properties": { - "topic": "Dashboard/Robot Values/Hub Active", - "period": 0.02, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Turret Has Target", - "x": 1152.0, - "y": 576.0, - "width": 256.0, - "height": 192.0, - "type": "Boolean Box", - "properties": { - "topic": "Dashboard/Robot Values/Turret Has Target", - "period": 0.02, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Hub State Timer", - "x": 1152.0, - "y": 192.0, - "width": 512.0, - "height": 192.0, - "type": "Text Display", - "properties": { - "topic": "Dashboard/Robot Values/Hub State Time Remaining", - "period": 0.02, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Shooter Mode", - "x": 1152.0, - "y": 384.0, - "width": 512.0, - "height": 192.0, - "type": "Boolean Box", - "properties": { - "topic": "Dashboard/Robot Values/Shooter Is Shoot Mode", - "period": 0.02, - "data_type": "boolean", - "true_color": 4278190335, - "false_color": 4294934272, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Turret Lined Up", - "x": 1408.0, - "y": 576.0, - "width": 256.0, - "height": 192.0, - "type": "Boolean Box", - "properties": { - "topic": "Dashboard/Robot Values/Turret Lined Up", - "period": 0.02, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - } - ] - } - }, - { - "name": "Programmer", - "grid_layout": { - "layouts": [ - { - "title": "Hood & Turret", - "x": 0.0, - "y": 0.0, - "width": 256.0, - "height": 736.0, - "type": "List Layout", - "properties": { - "label_position": "TOP" - }, - "children": [ - { - "title": "Hood PID", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 128.0, - "type": "PIDController", - "properties": { - "topic": "/SmartDashboard/Hood PID", - "period": 0.05 - } - }, - { - "title": "Turret PID", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 128.0, - "type": "PIDController", - "properties": { - "topic": "/SmartDashboard/Turret PID", - "period": 0.05 - } - } - ] - }, - { - "title": "Auto Driving", - "x": 256.0, - "y": 0.0, - "width": 256.0, - "height": 736.0, - "type": "List Layout", - "properties": { - "label_position": "TOP" - }, - "children": [ - { - "title": "Auto X PID", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 128.0, - "type": "PIDController", - "properties": { - "topic": "/SmartDashboard/Auto X PID", - "period": 0.05 - } - }, - { - "title": "Auto Y PID", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 128.0, - "type": "PIDController", - "properties": { - "topic": "/SmartDashboard/Auto Y PID", - "period": 0.05 - } - }, - { - "title": "Auto Heading PID", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 128.0, - "type": "PIDController", - "properties": { - "topic": "/SmartDashboard/Auto Heading PID", - "period": 0.05 - } - } - ] - }, - { - "title": "Intake", - "x": 512.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "List Layout", - "properties": { - "label_position": "RIGHT" - }, - "children": [ - { - "title": "Forward Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Forward Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Reverse Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Reverse Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Extension Max Position", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Extension Max Position", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Extend Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Extend Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Retract Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Retract Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - } - ] - }, - { - "title": "Hood", - "x": 768.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "List Layout", - "properties": { - "label_position": "RIGHT" - }, - "children": [ - { - "title": "Min Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Min Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Max Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Max Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Shoot Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Shoot Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Pass Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Pass Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Tolerance", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Tolerance", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - } - ] - }, - { - "title": "Turret", - "x": 1024.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "List Layout", - "properties": { - "label_position": "RIGHT" - }, - "children": [ - { - "title": "Min Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Min Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Max Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Max Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Home Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Home Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Tolerance", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Tolerance", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Pass Angle", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Pass Angle", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - } - ] - }, - { - "title": "Flywheel", - "x": 1280.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "List Layout", - "properties": { - "label_position": "RIGHT" - }, - "children": [ - { - "title": "Tolerance", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel Tolerance", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Pass Velocity", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel Pass Velocity", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - } - ] - }, - { - "title": "Feeder", - "x": 1280.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "List Layout", - "properties": { - "label_position": "RIGHT" - }, - "children": [ - { - "title": "Feed Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Feeder Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - } - ] - }, - { - "title": "Vision", - "x": 512.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "List Layout", - "properties": { - "label_position": "RIGHT" - }, - "children": [ - { - "title": "Max Angular Rate", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Vision Max Angular Rate", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Max Tilt", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Vision Max Tilt", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - } - ] - }, - { - "title": "Climber", - "x": 1536.0, - "y": 0.0, - "width": 256.0, - "height": 736.0, - "type": "List Layout", - "properties": { - "label_position": "RIGHT" - }, - "children": [ - { - "title": "L1 Rotation", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber L1 Rotation", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "L3 Rotation", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber L3 Rotation", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Extension Threshold", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Extension Threshold", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Retraction Threshold", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Retraction Threshold", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Extend Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Extend Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Retract Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Retract Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Rotate Voltage", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Rotate Voltage", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - }, - { - "title": "Tolerance", - "x": 0.0, - "y": 0.0, - "width": 121.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Tolerance", - "period": 0.02, - "data_type": "double", - "show_submit_button": true - } - } - ] - } - ], - "containers": [ - { - "title": "Turret Test Mode", - "x": 768.0, - "y": 256.0, - "width": 256.0, - "height": 192.0, - "type": "Command", - "properties": { - "topic": "/SmartDashboard/Turret Test Mode", - "period": 0.05, - "show_type": true, - "maximize_button_space": true - } - } - ] - } - } - ] -} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index df76471..1ea7861 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -1,10 +1,7 @@ package frc.robot; -import java.util.stream.IntStream; - import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; - import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -12,74 +9,53 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.GeneralConstants; -import frc.robot.generated.ChoreoTraj; import frc.robot.generated.ChoreoVars; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Utilities; -public class Autos extends SubsystemBase +public class Autos { private enum AutoMode { - // @formatter:off - DoNothing ("Do Nothing"), - ShootOnly ("Shoot Only"), - ShootWithDelay ("Shoot with Delay"), - ShootThenDrive ("Shoot then Drive"), - ShootWithDelayThenDrive("Shoot with Delay then Drive"); - // @formatter:on - - public final String displayName; - - private AutoMode(String name) - { - displayName = name; - } + DoNothing, + ShootOnly, + ShootWithDelay, + ShootThenDrive, + ShootWithDelayThenDrive } private enum StartPosition { - // @formatter:off - LeftTrench ("Left Trench", ChoreoVars.Poses.LeftTrench), - LeftBump ("Left Bump", ChoreoVars.Poses.LeftBump), - HubStart ("Hub", ChoreoVars.Poses.HubStart), - RightBump ("Right Bump", ChoreoVars.Poses.RightBump), - RightTrench("Right Trench", ChoreoVars.Poses.RightTrench); + LeftTrench(ChoreoVars.Poses.LeftTrench), + LeftBump(ChoreoVars.Poses.LeftBump), + HubStart(ChoreoVars.Poses.HubStart), + RightBump(ChoreoVars.Poses.RightBump), + RightTrench(ChoreoVars.Poses.RightTrench); - // @formatter:on - - public String displayName; - public Pose2d pose; + public final Pose2d pose; - private StartPosition(String name, Pose2d bluePose) + private StartPosition(Pose2d bluePose) { - displayName = name; - pose = bluePose; + pose = bluePose; } } - private static final String NO_TRAJECTORY = "None (Stay)"; - private final AutoFactory _autoFactory; - private final Drive _driveSubsystem; - private final Shooter _shooterSubsystem; - private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); - private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); - private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); - private final SendableChooser _modeChooser = new SendableChooser(); - private final SendableChooser _autoDelay = new SendableChooser(); - private final SendableChooser _trajChooser = new SendableChooser(); - private final SendableChooser _startChooser = new SendableChooser(); - private final Field2d _field; - private StartPosition _startPosition = null; + private static final String NO_TRAJECTORY = "None (Stay)"; + private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; + private static final int DEFAULT_AUTO_DELAY_SECS = 0; + private static final StartPosition DEFAULT_START_POSITION = StartPosition.LeftTrench; + private static final String DEFAULT_AUTO_TRAJECTORY = NO_TRAJECTORY; + private final AutoFactory _autoFactory; + private final Drive _driveSubsystem; + private final Shooter _shooterSubsystem; + private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { @@ -97,54 +73,16 @@ public Autos(Drive driveSubsystem, Shooter shooterSubsystem) driveSubsystem ); // @formatter:on - - _modeChooser.setDefaultOption(AutoMode.ShootOnly.displayName, AutoMode.ShootOnly); - _modeChooser.addOption(AutoMode.ShootWithDelay.displayName, AutoMode.ShootWithDelay); - _modeChooser.addOption(AutoMode.ShootThenDrive.displayName, AutoMode.ShootThenDrive); - _modeChooser.addOption(AutoMode.ShootWithDelayThenDrive.displayName, AutoMode.ShootWithDelayThenDrive); - _modeChooser.addOption(AutoMode.DoNothing.displayName, AutoMode.DoNothing); - - _autoDelay.setDefaultOption("0", 0); - IntStream.range(1, 6).forEach(n -> _autoDelay.addOption(String.valueOf(n), n)); - - _startChooser.setDefaultOption(StartPosition.LeftTrench.displayName, StartPosition.LeftTrench); - _startChooser.addOption(StartPosition.LeftBump.displayName, StartPosition.LeftBump); - _startChooser.addOption(StartPosition.HubStart.displayName, StartPosition.HubStart); - _startChooser.addOption(StartPosition.RightBump.displayName, StartPosition.RightBump); - _startChooser.addOption(StartPosition.RightTrench.displayName, StartPosition.RightTrench); - - _trajChooser.setDefaultOption(NO_TRAJECTORY, NO_TRAJECTORY); - ChoreoTraj.ALL_TRAJECTORIES.keySet().stream().sorted().forEach(name -> _trajChooser.addOption(name, name)); - - _field = new Field2d(); - - SmartDashboard.putData("Auto Mode", _modeChooser); - SmartDashboard.putData("Auto Trajectory", _trajChooser); - SmartDashboard.putData("Auto Delay", _autoDelay); - SmartDashboard.putData("Start Position", _startChooser); - SmartDashboard.putData("Autonomous Mode", _field); - } - - @Override - public void periodic() - { - var currentStart = _startChooser.getSelected(); - - if (_startPosition != currentStart) - { - _startPosition = currentStart; - _field.setRobotPose(flip(_startPosition.pose)); - } } public Command buildAuto() { - var start = _startChooser.getSelected(); - var mode = _modeChooser.getSelected(); - var trajName = _trajChooser.getSelected(); + var start = DEFAULT_START_POSITION; + var mode = DEFAULT_AUTO_MODE; + var trajName = DEFAULT_AUTO_TRAJECTORY; var reset = Commands.runOnce(() -> _driveSubsystem.resetPose(flip(start.pose))); - var shoot = _shooterSubsystem.shoot(); - var delay = Commands.waitSeconds(_autoDelay.getSelected()); + var shoot = _shooterSubsystem.shoot().withTimeout(4.0); + var delay = Commands.waitSeconds(DEFAULT_AUTO_DELAY_SECS); var drive = NO_TRAJECTORY.equals(trajName) ? Commands.none() : _autoFactory.trajectoryCmd(trajName); // @formatter:off @@ -166,7 +104,7 @@ private void followTrajectory(SwerveSample sample) // @formatter:off _driveSubsystem.setControl(_autoRequest .withVelocityX(sample.vx + _xController.calculate(pose.getX(), sample.x)) - .withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) + .withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) .withRotationalRate(sample.omega + _headingController.calculate(pose.getRotation().getRadians(), sample.heading)) ); // @formatter:on diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34d83db..18900c8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.Constants.DriveConstants; -import frc.robot.subsystems.dashboard.Dashboard; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.TunerConstants; import frc.robot.subsystems.intake.Intake; @@ -32,21 +31,17 @@ public class RobotContainer { private static final double MANUAL_FLYWHEEL_START_RPM = 3500.0; private static final double MANUAL_FLYWHEEL_STEP_RPM = 50.0; - private static final double MANUAL_TURRET_START_DEG = 0.0; private static final double MANUAL_TURRET_STEP_DEG = 2.0; /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric _fieldCentric = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); private final SwerveRequest.RobotCentric _robotCentric = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final Telemetry _logger = new Telemetry(DriveConstants.MAX_SPEED.in(MetersPerSecond)); private final CommandJoystick _driver = new CommandJoystick(0); private final CommandXboxController _operator = new CommandXboxController(1); private final Drive _drive = TunerConstants.createDrivetrain(); private final Intake _intake = new Intake(); private final Shooter _shooter = new Shooter(_drive::getState); @NotLogged - private final Dashboard _dashboard = new Dashboard(_shooter); - @NotLogged private final Autos _autos = new Autos(_drive, _shooter); private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; @@ -84,7 +79,6 @@ private void setManualFlywheelRPM(double rpm) private void bumpManualTurretAngle(double deltaDeg) { - _manualTurretDeg += deltaDeg; _shooter.bumpManualTurretAngle(deltaDeg); } @@ -102,8 +96,6 @@ private void configureBindings() _driver.button(6).whileTrue(_shooter.trackOnly()); _driver.button(7).onTrue(_drive.runOnce(_drive::seedFieldCentric)); - _drive.registerTelemetry(_logger::telemeterize); - _operator.leftTrigger().whileTrue(_intake.runRollersForward()); _operator.leftBumper().whileTrue(_intake.runRollersReverse()); _operator.rightTrigger().whileTrue(_intake.jiggle()); diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java deleted file mode 100644 index 3f4ec89..0000000 --- a/src/main/java/frc/robot/Telemetry.java +++ /dev/null @@ -1,116 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.Constants.LoggingConstants; - -public class Telemetry -{ - private final double MaxSpeed; - - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public Telemetry(double maxSpeed) - { - MaxSpeed = maxSpeed; - - /* Set up the module state Mechanism2d telemetry */ - for (int i = 0; i < 4; ++i) - { - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - } - - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5).append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5).append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5).append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5).append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), }; - private final double[] m_poseArray = new double[3]; - - /** - * Accept the swerve drive state and telemeterize it to NT/SmartDashboard, with - * optional Phoenix signal logging when enabled. - */ - public void telemeterize(SwerveDriveState state) - { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - if (LoggingConstants.ENABLE_CTRE_SIGNAL_LOG) - { - SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); - SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); - SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); - SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); - SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - } - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - fieldPub.set(m_poseArray); - - /* Telemeterize each module state to a Mechanism2d */ - for (int i = 0; i < 4; ++i) - { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java b/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java deleted file mode 100644 index f0b7f76..0000000 --- a/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java +++ /dev/null @@ -1,104 +0,0 @@ -package frc.robot.subsystems.dashboard; - -import edu.wpi.first.networktables.BooleanPublisher; -import edu.wpi.first.networktables.DoublePublisher; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.Turret.TurretState; -import frc.robot.util.Utilities; - -public class Dashboard extends SubsystemBase -{ - // Assumed teleop length when FMS match time is unavailable - private static final double TELEOP_SECS = 135.0; - - // Elapsed-time shift boundaries (derived from Utilities constants, assuming - // 135s teleop) - // kTransitionShiftEnd = 130s remaining → 5s elapsed - // kShift1End = 105s remaining → 30s elapsed - // kShift2End = 80s remaining → 55s elapsed - // kShift3End = 55s remaining → 80s elapsed - // kShift4End = 30s remaining → 105s elapsed - private static final double[] SHIFT_ELAPSED_ENDS = { 5.0, 30.0, 55.0, 80.0, 105.0 }; - private final Shooter _shooter; - private final DoublePublisher _matchTimePub; - private final BooleanPublisher _hubActivePub; - private final DoublePublisher _hubShiftTimerPub; - private final BooleanPublisher _shooterIsShootModePub; - private final BooleanPublisher _turretHasTargetPub; - private final BooleanPublisher _turretLinedUpPub; - private final Timer _teleopTimer = new Timer(); - private boolean _wasInTeleop = false; - - public Dashboard(Shooter shooter) - { - _shooter = shooter; - - var table = NetworkTableInstance.getDefault().getTable("Dashboard").getSubTable("Robot Values"); - - _matchTimePub = table.getDoubleTopic("Match Time").publish(); - _hubActivePub = table.getBooleanTopic("Hub Active").publish(); - _hubShiftTimerPub = table.getDoubleTopic("Hub State Time Remaining").publish(); - _shooterIsShootModePub = table.getBooleanTopic("Shooter Is Shoot Mode").publish(); - _turretHasTargetPub = table.getBooleanTopic("Turret Has Target").publish(); - _turretLinedUpPub = table.getBooleanTopic("Turret Lined Up").publish(); - } - - @Override - public void periodic() - { - updateTeleopTimer(); - - _matchTimePub.set(getMatchTime()); - _hubActivePub.set(Utilities.isHubActive()); - _hubShiftTimerPub.set(getShiftTimer()); - _shooterIsShootModePub.set(_shooter._turret.getTurretState() == TurretState.Track); - _turretHasTargetPub.set(_shooter._turret.hasLimelightTarget()); - _turretLinedUpPub.set(_shooter._turret.isLinedUp()); - } - - private void updateTeleopTimer() - { - boolean inTeleop = DriverStation.isTeleop() && DriverStation.isEnabled(); - - if (inTeleop && !_wasInTeleop) - { - _teleopTimer.restart(); - } - else if (!DriverStation.isTeleop()) - { - _teleopTimer.stop(); - _teleopTimer.reset(); - } - - _wasInTeleop = inTeleop; - } - - private double getMatchTime() - { - double fmsTime = DriverStation.getMatchTime(); - if (fmsTime >= 0) return fmsTime; - if (!DriverStation.isTeleop()) return -1; - - // Fallback: count down from assumed teleop length - return Math.max(0, TELEOP_SECS - _teleopTimer.get()); - } - - private double getShiftTimer() - { - double fmsTime = DriverStation.getMatchTime(); - if (fmsTime >= 0) return Utilities.getTimeUntilNextShift(); - if (!DriverStation.isTeleop()) return -1; - - // Fallback: compute from local elapsed timer - double elapsed = _teleopTimer.get(); - for (double end : SHIFT_ELAPSED_ENDS) - { - if (elapsed < end) return end - elapsed; - } - return Math.max(0, TELEOP_SECS - elapsed); // endgame - } -} From baf5f0c1ee0c86a91b02f6b44313c1a18ef2e5bc Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Sun, 15 Mar 2026 16:37:12 -0500 Subject: [PATCH 04/32] Add shooter telemetry baseline --- .../frc/robot/subsystems/drive/Drive.java | 25 +++++++++++++++++-- .../frc/robot/subsystems/shooter/Feeder.java | 3 +++ .../frc/robot/subsystems/shooter/Turret.java | 6 ++++- 3 files changed, 31 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e30f237..9a23508 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; @@ -47,10 +48,20 @@ @Logged public class Drive extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.004; // 4 ms + private static final double kSimLoopPeriod = 0.004; // 4 ms private final DriveVision _vision; - private Notifier m_simNotifier = null; + private Notifier m_simNotifier = null; private double m_lastSimTime; + @Logged + private Pose2d _robotPoseNow = new Pose2d(); + @Logged + private double _robotSpeedVxMetersPerSecond = 0.0; + @Logged + private double _robotSpeedVyMetersPerSecond = 0.0; + @Logged + private double _robotSpeedOmegaRadiansPerSecond = 0.0; + @Logged + private Translation2d _fieldRelativeTranslationalVelocity = new Translation2d(); /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; @@ -235,6 +246,16 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) @Override public void periodic() { + var driveState = getState(); + var robotPose = driveState.Pose; + var robotSpeed = driveState.Speeds; + + _robotPoseNow = robotPose; + _robotSpeedVxMetersPerSecond = robotSpeed.vxMetersPerSecond; + _robotSpeedVyMetersPerSecond = robotSpeed.vyMetersPerSecond; + _robotSpeedOmegaRadiansPerSecond = robotSpeed.omegaRadiansPerSecond; + _fieldRelativeTranslationalVelocity = new Translation2d(robotSpeed.vxMetersPerSecond, robotSpeed.vyMetersPerSecond).rotateBy(robotPose.getRotation()); + /* * Periodically try to apply the operator perspective. If we haven't applied the * operator perspective before, then we should apply it regardless of DS state. diff --git a/src/main/java/frc/robot/subsystems/shooter/Feeder.java b/src/main/java/frc/robot/subsystems/shooter/Feeder.java index f0c5b70..f2c237f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Feeder.java +++ b/src/main/java/frc/robot/subsystems/shooter/Feeder.java @@ -23,6 +23,8 @@ public class Feeder private final SparkFlex _feederMotor; @Logged private Voltage _feederMotorVoltage = Volts.of(0.0); + @Logged + private boolean _enabled = false; public Feeder() { @@ -41,6 +43,7 @@ public void periodic() public void set(boolean on) { + _enabled = on; Voltage targetVoltage = on ? ShooterConstants.FEEDER_VOLTAGE : Volts.zero(); _feederMotor.setVoltage(targetVoltage.in(Volts)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 90e0053..9c9ed43 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -70,6 +70,8 @@ public enum TurretState @Logged private boolean _limelightHasTarget; @Logged + private boolean _hubActive; + @Logged private boolean _targetValid; @Logged private boolean _disabled; @@ -101,6 +103,7 @@ public Turret(Supplier swerveStateSupplier) _targetDistance = Meters.zero(); _targetPose = new Pose2d(); _limelightHasTarget = false; + _hubActive = false; _targetValid = false; _disabled = false; @@ -135,6 +138,7 @@ public void periodic() _turretAngle = Degrees.of(_turretSensor.get()); _motorVoltage = _turretMotor.getMotorVoltage().getValue(); _limelightHasTarget = false; + _hubActive = Utilities.isHubActive(); _targetValid = false; var fiducials = new AprilTagFiducial[0]; @@ -195,7 +199,7 @@ public void periodic() var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); - _targetValid = Utilities.isHubActive(); + _targetValid = _hubActive; break; case Pass: From 9966cbf901d88617ec71e888b9fe7d267928fd77 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Sun, 15 Mar 2026 16:52:49 -0500 Subject: [PATCH 05/32] Add shot calculator skeleton --- .../subsystems/shooter/ShotSolution.java | 10 + .../frc/robot/subsystems/shooter/Turret.java | 105 ++-------- .../subsystems/shooter/TurretDirector.java | 186 ++++++++++-------- .../shooter/TurretDirectorTest.java | 171 ++++++++++++++++ 4 files changed, 307 insertions(+), 165 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShotSolution.java create mode 100644 src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotSolution.java b/src/main/java/frc/robot/subsystems/shooter/ShotSolution.java new file mode 100644 index 0000000..3258fc7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShotSolution.java @@ -0,0 +1,10 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import frc.robot.subsystems.shooter.Turret.TurretState; + +public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, TurretState mode, Pose2d targetPose, boolean hasVisionTarget) +{ +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 9c9ed43..6e0f299 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -20,9 +20,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; @@ -32,7 +29,6 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AIOConstants; import frc.robot.Constants.CANConstants; -import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.util.MeasureUtil; import frc.robot.util.Utilities; @@ -50,6 +46,7 @@ public enum TurretState private final TalonFX _turretMotor; private final AnalogPotentiometer _turretSensor; private final Supplier _swerveStateSupplier; + private final TurretDirector _turretDirector; private final Limelight _limelight; private final PIDController _pidController; private SwerveDriveState _currentSwerveState; @@ -79,6 +76,7 @@ public enum TurretState public Turret(Supplier swerveStateSupplier) { _swerveStateSupplier = swerveStateSupplier; + _turretDirector = new TurretDirector(); var sensorRange = ShooterConstants.TURRET_HARD_MAX_ANGLE.minus(ShooterConstants.TURRET_HARD_MIN_ANGLE); var sensorOffset = ShooterConstants.TURRET_HARD_MIN_ANGLE; @@ -143,106 +141,51 @@ public void periodic() var fiducials = new AprilTagFiducial[0]; var motorOutput = Volts.zero(); - var rawSetpoint = Degrees.zero(); switch (_turretState) { case Track: - _hasSetpoint = true; var results = _limelight.getLatestResults(); if (results.isPresent()) { fiducials = results.get().targets_Fiducials; } - - var localHubTranslation = getHubTranslationInRobotFrame(); - var turretToHubTranslation = localHubTranslation.minus(ShooterConstants.TURRET_POSITION); - - _targetDistance = Meters.of(turretToHubTranslation.getNorm()); - rawSetpoint = getTurretSetpointForRobotFrameTarget(turretToHubTranslation); - - AprilTagFiducial bestTag = null; - - for (AprilTagFiducial tag : fiducials) - { - // Limelight reports the tag ID as a double, but our tag lists are ints. - var tagId = (int)Math.round(tag.fiducialID); - - if (!Utilities.getOurHubTagIds().contains(tagId)) - { - continue; - } - - if (bestTag == null || tag.ta > bestTag.ta || tag.ta == bestTag.ta && Math.abs(tag.tx) < Math.abs(bestTag.tx)) - { - bestTag = tag; - } - } - - if (bestTag != null) - { - _limelightHasTarget = true; - var txCorrection = Degrees.of(-bestTag.tx); - - // Ignore tiny Limelight yaw error so we don't keep hunting on vision noise. - if (!MathUtil.isNear(0.0, txCorrection.in(Degrees), ShooterConstants.TURRET_TRACK_TX_DEADBAND.in(Degrees))) - { - rawSetpoint = rawSetpoint.minus(txCorrection); - } - } - - // The shooter exit point is forward of the turret rotation center. This creates - // a lateral miss that varies as sin(turretAngle): zero at 0°, max at ±90°. - // Guard distance from zero to avoid atan2 blowing up at startup. - var safeDistanceM = Math.max(_targetDistance.in(Meters), 0.5); - var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); - rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); - - _targetValid = _hubActive; + var trackSolution = _turretDirector.calculate(_turretState, _currentSwerveState, Utilities.getHubCoordinates(), Utilities.getOurHubTagIds(), Utilities.isBlueAlliance(), _hubActive, fiducials); + _hasSetpoint = true; + _limelightHasTarget = trackSolution.hasVisionTarget(); + _targetValid = trackSolution.valid(); + _targetDistance = trackSolution.distance(); + _targetPose = trackSolution.targetPose(); + _turretSetpoint = trackSolution.turretAngle(); break; case Pass: + var passSolution = _turretDirector.calculate(_turretState, _currentSwerveState, Utilities.getHubCoordinates(), Utilities.getOurHubTagIds(), Utilities.isBlueAlliance(), _hubActive); _hasSetpoint = true; - _targetValid = true; - Angle fieldTargetAngle; - - if (Utilities.isBlueAlliance()) - { - _targetDistance = _currentSwerveState.Pose.getMeasureX(); - fieldTargetAngle = Degrees.of(180); - } - else - { - _targetDistance = GeneralConstants.FIELD_SIZE_X.minus(_currentSwerveState.Pose.getMeasureX()); - fieldTargetAngle = Degrees.zero(); - } - - rawSetpoint = fieldTargetAngle.minus(_currentSwerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); + _limelightHasTarget = passSolution.hasVisionTarget(); + _targetValid = passSolution.valid(); + _targetDistance = passSolution.distance(); + _targetPose = passSolution.targetPose(); + _turretSetpoint = passSolution.turretAngle(); break; case ManualAngle: _hasSetpoint = true; _targetValid = false; - rawSetpoint = _manualAngleSetpoint; + _turretSetpoint = MeasureUtil.clamp(moduloAngle(_manualAngleSetpoint), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); break; case Idle: default: _hasSetpoint = false; _targetValid = false; - rawSetpoint = ShooterConstants.TURRET_HOME_ANGLE; _targetDistance = Meters.zero(); + _targetPose = _currentSwerveState.Pose; + _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; break; } - var forwardTranslation = new Translation2d(_targetDistance, Meters.zero()); - var rotatedByTurretOffset = forwardTranslation.rotateBy(new Rotation2d(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD)); - var rotatedBySetpoint = rotatedByTurretOffset.rotateBy(new Rotation2d(rawSetpoint)); - - _targetPose = _currentSwerveState.Pose.plus(new Transform2d(rotatedBySetpoint, new Rotation2d())); - _turretSetpoint = MeasureUtil.clamp(moduloAngle(rawSetpoint), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); - if (_hasSetpoint && !_disabled) { motorOutput = Volts.of(_pidController.calculate(_turretAngle.in(Degrees), _turretSetpoint.in(Degrees))); @@ -340,16 +283,4 @@ private Voltage applySoftLimit(Voltage requestedVoltage) return requestedVoltage; } - - private Translation2d getHubTranslationInRobotFrame() - { - var robotToHubField = Utilities.getHubCoordinates().minus(_currentSwerveState.Pose.getTranslation()); - - return robotToHubField.rotateBy(_currentSwerveState.Pose.getRotation().unaryMinus()); - } - - private Angle getTurretSetpointForRobotFrameTarget(Translation2d targetTranslation) - { - return targetTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index f72588f..12bd9eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -1,107 +1,137 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Radians; + +import java.util.List; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.shooter.Turret.TurretState; -import frc.robot.util.Utilities; +import frc.robot.util.MeasureUtil; import limelight.networktables.target.AprilTagFiducial; public class TurretDirector { - private TurretDirector() - { - } - - public static Translation2d calculate(TurretState turretState, Rotation2d localTurretAngle, SwerveDriveState swerveState, AprilTagFiducial... fiducials) + public ShotSolution calculate(TurretState turretState, SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean isBlueAlliance, boolean hubActive, AprilTagFiducial... fiducials) { - Translation2d ret; - switch (turretState) { case Track: - // No tags, get angle from current pose to hub - if (fiducials.length <= 0) - { - Translation2d hub = Utilities.getHubCoordinates(); - Translation2d robot = swerveState.Pose.getTranslation(); - Translation2d robotToHub = hub.minus(robot); - Rotation2d robotAngleCorrection = swerveState.Pose.getRotation().unaryMinus(); - - ret = robotToHub.rotateBy(robotAngleCorrection); - } - else - { - double invertedYawSumDegrees = 0.0; - double invertedPitchSumDegrees = 0.0; - - for (AprilTagFiducial tag : fiducials) - { - // Limelight is mounted upside down, so invert tx/ty before averaging. - invertedYawSumDegrees -= tag.tx; - invertedPitchSumDegrees -= tag.ty; - } - - int sampleCount = fiducials.length; - - double averageInvertedYawDegrees = invertedYawSumDegrees / sampleCount; - double averageInvertedPitchDegrees = invertedPitchSumDegrees / sampleCount; - - // Tx and Ty are now the average pitch and yaw to the target. - // Now calculate distance from pitch - var tyMeasure = Degrees.of(averageInvertedPitchDegrees); - var distance = ShooterConstants.TURRET_TO_HUB_HEIGHT_DELTA.div(Math.tan(tyMeasure.plus(ShooterConstants.TURRET_LIMELIGHT_PITCH).in(Radians))); - - // We have a distance and an angle. We can now describe the camera to target - // translation as a forward vector with magnitude "distance" rotated by angle - // "averageInvertedYawDegrees" - - var localCameraToTarget = new Translation2d(distance, Inches.zero()).rotateBy(Rotation2d.fromDegrees(-averageInvertedYawDegrees)); - - // now, calculate ret and account for the turret being offset and at an angle - // get translation from the center of the robot to the camera - var cameraPosition = ShooterConstants.TURRET_POSITION.plus(ShooterConstants.TURRET_CAMERA_POSITION.rotateBy(localTurretAngle)); - ret = cameraPosition.plus(localCameraToTarget.rotateBy(localTurretAngle).rotateBy(Rotation2d.fromDegrees(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD.in(Degrees)))); - } - break; + return calculateTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials); case Pass: - Rotation2d robotAngle = swerveState.Pose.getRotation(); - Rotation2d rotation; - Translation2d translation; - - // The distance we need to aim for is determined by the robot's pose and the - // alliance we're on. - // The direction we need to aim is based on the alliance. - // Describe the translation as a forward vector rotated by the robot's rotation - if (Utilities.isBlueAlliance()) - { - translation = new Translation2d(Meters.of(swerveState.Pose.getX()), Meters.zero()); - rotation = Rotation2d.k180deg.minus(robotAngle); - } - else - { - translation = new Translation2d(GeneralConstants.FIELD_SIZE_X.minus(swerveState.Pose.getMeasureX()), Meters.zero()); - rotation = robotAngle.unaryMinus(); - } - - ret = translation.rotateBy(rotation); - break; + return calculatePass(swerveState, isBlueAlliance); case Idle: default: - ret = new Translation2d(); - break; + return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), turretState, swerveState.Pose, false); + } + } + + private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) + { + var localHubTranslation = getHubTranslationInRobotFrame(swerveState, hubCoordinates); + var turretToHubTranslation = localHubTranslation.minus(ShooterConstants.TURRET_POSITION); + var distance = Meters.of(turretToHubTranslation.getNorm()); + var rawSetpoint = getTurretSetpointForRobotFrameTarget(turretToHubTranslation); + var bestTag = selectBestTag(targetTagIds, fiducials); + + if (bestTag != null) + { + var txCorrection = Degrees.of(-bestTag.tx); + + if (!MathUtil.isNear(0.0, txCorrection.in(Degrees), ShooterConstants.TURRET_TRACK_TX_DEADBAND.in(Degrees))) + { + rawSetpoint = rawSetpoint.minus(txCorrection); + } + } + + // Preserve the current static shooter-offset correction so the calculator + // reproduces today's behavior before SWM math is introduced. + var safeDistanceM = Math.max(distance.in(Meters), 0.5); + var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); + rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); + + return new ShotSolution(hubActive, clampTurretAngle(rawSetpoint), distance, TurretState.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); + } + + private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueAlliance) + { + Angle fieldTargetAngle; + Distance distance; + + if (isBlueAlliance) + { + distance = swerveState.Pose.getMeasureX(); + fieldTargetAngle = Degrees.of(180); + } + else + { + distance = GeneralConstants.FIELD_SIZE_X.minus(swerveState.Pose.getMeasureX()); + fieldTargetAngle = Degrees.zero(); + } + + var rawSetpoint = fieldTargetAngle.minus(swerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); + + return new ShotSolution(true, clampTurretAngle(rawSetpoint), distance, TurretState.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); + } + + private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSetpoint) + { + var forwardTranslation = new Translation2d(distance, Meters.zero()); + var rotatedByTurretOffset = forwardTranslation.rotateBy(new Rotation2d(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD)); + var rotatedBySetpoint = rotatedByTurretOffset.rotateBy(new Rotation2d(rawSetpoint)); + + return robotPose.plus(new Transform2d(rotatedBySetpoint, new Rotation2d())); + } + + private Translation2d getHubTranslationInRobotFrame(SwerveDriveState swerveState, Translation2d hubCoordinates) + { + var robotToHubField = hubCoordinates.minus(swerveState.Pose.getTranslation()); + + return robotToHubField.rotateBy(swerveState.Pose.getRotation().unaryMinus()); + } + + private Angle getTurretSetpointForRobotFrameTarget(Translation2d targetTranslation) + { + return targetTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); + } + + private Angle clampTurretAngle(Angle rawSetpoint) + { + var wrappedDegrees = MathUtil.inputModulus(rawSetpoint.in(Degrees), -180, 180); + return MeasureUtil.clamp(Degrees.of(wrappedDegrees), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); + } + + private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiducial... fiducials) + { + AprilTagFiducial bestTag = null; + + for (AprilTagFiducial tag : fiducials) + { + var tagId = (int)Math.round(tag.fiducialID); + + if (!targetTagIds.contains(tagId)) + { + continue; + } + + if (bestTag == null || tag.ta > bestTag.ta || tag.ta == bestTag.ta && Math.abs(tag.tx) < Math.abs(bestTag.tx)) + { + bestTag = tag; + } } - return ret; + return bestTag; } } diff --git a/src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java b/src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java new file mode 100644 index 0000000..50099a5 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java @@ -0,0 +1,171 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import frc.robot.Constants.GeneralConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.subsystems.shooter.Turret.TurretState; +import frc.robot.util.MeasureUtil; +import limelight.networktables.target.AprilTagFiducial; + +class TurretDirectorTest +{ + private static final double kAngleToleranceDeg = 1e-9; + private static final double kDistanceToleranceM = 1e-9; + private static final double kTranslationTolerance = 1e-9; + private final TurretDirector _turretDirector = new TurretDirector(); + + @Test + void trackStaticShotMatchesLegacyGeometryWithoutVision() + { + var state = createState(new Pose2d(3.1, 4.2, Rotation2d.fromDegrees(32.0))); + var actual = _turretDirector.calculate(TurretState.Track, state, ShooterConstants.BLUE_HUB, ShooterConstants.BLUE_HUB_TAG_IDS, true, true); + var expected = legacyTrackSolution(state, ShooterConstants.BLUE_HUB, true); + + assertSolutionMatches(expected, actual); + } + + @Test + void trackStaticShotPreservesHubValidityWithoutLookahead() + { + var state = createState(new Pose2d(5.0, 2.0, Rotation2d.fromDegrees(-18.0))); + var actual = _turretDirector.calculate(TurretState.Track, state, ShooterConstants.RED_HUB, ShooterConstants.RED_HUB_TAG_IDS, false, false); + var expected = legacyTrackSolution(state, ShooterConstants.RED_HUB, false); + + assertSolutionMatches(expected, actual); + } + + @Test + void passShotMatchesLegacyBlueAllianceBehavior() + { + var state = createState(new Pose2d(2.4, 5.5, Rotation2d.fromDegrees(47.0))); + var actual = _turretDirector.calculate(TurretState.Pass, state, ShooterConstants.BLUE_HUB, ShooterConstants.BLUE_HUB_TAG_IDS, true, true); + var expected = legacyPassSolution(state, true); + + assertSolutionMatches(expected, actual); + } + + @Test + void passShotMatchesLegacyRedAllianceBehavior() + { + var state = createState(new Pose2d(10.2, 3.3, Rotation2d.fromDegrees(-21.0))); + var actual = _turretDirector.calculate(TurretState.Pass, state, ShooterConstants.RED_HUB, ShooterConstants.RED_HUB_TAG_IDS, false, true); + var expected = legacyPassSolution(state, false); + + assertSolutionMatches(expected, actual); + } + + @Test + void trackShotUsesBestAllowedVisionTagForTrim() + { + var state = createState(new Pose2d(3.6, 4.8, Rotation2d.fromDegrees(14.0))); + var actual = _turretDirector.calculate(TurretState.Track, state, ShooterConstants.BLUE_HUB, ShooterConstants.BLUE_HUB_TAG_IDS, true, true, createTag(999, 50.0, -20.0), createTag(20, 4.0, 6.0), createTag(18, 8.0, 3.0)); + var expected = legacyTrackSolution(state, ShooterConstants.BLUE_HUB, true, 3.0, true); + + assertSolutionMatches(expected, actual); + } + + private ShotSolution legacyTrackSolution(SwerveDriveState swerveState, Translation2d hubCoordinates, boolean hubActive) + { + return legacyTrackSolution(swerveState, hubCoordinates, hubActive, 0.0, false); + } + + private ShotSolution legacyTrackSolution(SwerveDriveState swerveState, Translation2d hubCoordinates, boolean hubActive, double txDegrees, boolean hasVisionTarget) + { + var robotToHubField = hubCoordinates.minus(swerveState.Pose.getTranslation()); + var localHubTranslation = robotToHubField.rotateBy(swerveState.Pose.getRotation().unaryMinus()); + var turretToHubTranslation = localHubTranslation.minus(ShooterConstants.TURRET_POSITION); + var distance = Meters.of(turretToHubTranslation.getNorm()); + var rawSetpoint = turretToHubTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); + + if (hasVisionTarget && !MathUtil.isNear(0.0, txDegrees, ShooterConstants.TURRET_TRACK_TX_DEADBAND.in(Degrees))) + { + rawSetpoint = rawSetpoint.plus(Degrees.of(txDegrees)); + } + + var safeDistanceM = Math.max(distance.in(Meters), 0.5); + var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); + rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); + + return new ShotSolution(hubActive, clampTurretAngle(rawSetpoint), distance, TurretState.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), hasVisionTarget); + } + + private ShotSolution legacyPassSolution(SwerveDriveState swerveState, boolean isBlueAlliance) + { + Angle fieldTargetAngle; + Distance distance; + + if (isBlueAlliance) + { + distance = swerveState.Pose.getMeasureX(); + fieldTargetAngle = Degrees.of(180); + } + else + { + distance = GeneralConstants.FIELD_SIZE_X.minus(swerveState.Pose.getMeasureX()); + fieldTargetAngle = Degrees.zero(); + } + + var rawSetpoint = fieldTargetAngle.minus(swerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); + + return new ShotSolution(true, clampTurretAngle(rawSetpoint), distance, TurretState.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); + } + + private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSetpoint) + { + var forwardTranslation = new Translation2d(distance, Meters.zero()); + var rotatedByTurretOffset = forwardTranslation.rotateBy(new Rotation2d(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD)); + var rotatedBySetpoint = rotatedByTurretOffset.rotateBy(new Rotation2d(rawSetpoint)); + + return robotPose.plus(new Transform2d(rotatedBySetpoint, new Rotation2d())); + } + + private Angle clampTurretAngle(Angle rawSetpoint) + { + var wrappedDegrees = MathUtil.inputModulus(rawSetpoint.in(Degrees), -180, 180); + return MeasureUtil.clamp(Degrees.of(wrappedDegrees), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); + } + + private SwerveDriveState createState(Pose2d pose) + { + var state = new SwerveDriveState(); + state.Pose = pose; + state.Speeds = new ChassisSpeeds(0.0, 0.0, 0.0); + return state; + } + + private AprilTagFiducial createTag(int fiducialId, double ta, double tx) + { + var tag = new AprilTagFiducial(); + tag.fiducialID = fiducialId; + tag.ta = ta; + tag.tx = tx; + return tag; + } + + private void assertSolutionMatches(ShotSolution expected, ShotSolution actual) + { + assertEquals(expected.valid(), actual.valid()); + assertEquals(expected.hasVisionTarget(), actual.hasVisionTarget()); + assertEquals(expected.mode(), actual.mode()); + assertEquals(expected.turretAngle().in(Degrees), actual.turretAngle().in(Degrees), kAngleToleranceDeg); + assertEquals(expected.distance().in(Meters), actual.distance().in(Meters), kDistanceToleranceM); + assertEquals(expected.targetPose().getX(), actual.targetPose().getX(), kTranslationTolerance); + assertEquals(expected.targetPose().getY(), actual.targetPose().getY(), kTranslationTolerance); + assertEquals(expected.targetPose().getRotation().getDegrees(), actual.targetPose().getRotation().getDegrees(), kAngleToleranceDeg); + } +} From f6ea82272742fda0000713d801270f0ed3506b26 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 00:11:55 -0500 Subject: [PATCH 06/32] Refactor shooter targeting ownership --- .../frc/robot/subsystems/shooter/Shooter.java | 133 +++++++++----- .../subsystems/shooter/ShotSolution.java | 10 -- .../frc/robot/subsystems/shooter/Turret.java | 164 ++++-------------- .../subsystems/shooter/TurretDirector.java | 103 +++++++++-- 4 files changed, 209 insertions(+), 201 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/shooter/ShotSolution.java diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 7a1efff..fc176ab 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RPM; import java.util.function.Supplier; @@ -8,12 +9,14 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; -import frc.robot.subsystems.shooter.Turret.TurretState; +import frc.robot.subsystems.shooter.TurretDirector.ShotMode; +import frc.robot.subsystems.shooter.TurretDirector.ShotSolution; @Logged public class Shooter extends SubsystemBase @@ -23,6 +26,38 @@ public enum ShooterState Idle, Preparing, Ready, Firing, Manual, TrackingOnly; } + public final Flywheel _flywheel; + public final Feeder _feeder; + public final Turret _turret; + public final Rotor _rotor; + public final TurretDirector _turretDirector; + @Logged + private ShooterState _state; + @Logged + private boolean _autoShootEnabled; + @Logged + private ShotMode _requestedShotMode; + private ShotSolution _currentShotSolution; + + public Shooter(Supplier swerveStateSupplier) + { + _flywheel = new Flywheel(); + _feeder = new Feeder(); + _turret = new Turret(); + _rotor = new Rotor(); + _turretDirector = new TurretDirector(swerveStateSupplier); + _state = ShooterState.Idle; + _autoShootEnabled = false; + _requestedShotMode = ShotMode.Idle; + _currentShotSolution = createIdleSolution(); + + _feeder.set(false); + _rotor.set(false); + _flywheel.stop(); + _turret.clearTargetAngle(); + _turret.setDisabled(false); + } + public Command startShooter() { return runOnce(() -> startPreparedShoot(false)); @@ -52,10 +87,12 @@ public Command setManualMode(boolean manual) { return runOnce(() -> { - _state = manual ? ShooterState.Manual : ShooterState.Idle; - _autoShootEnabled = false; + _state = manual ? ShooterState.Manual : ShooterState.Idle; + _autoShootEnabled = false; + _requestedShotMode = ShotMode.Idle; + _currentShotSolution = createIdleSolution(); _feeder.set(false); - _turret.setTurretState(TurretState.Idle); + _turret.clearTargetAngle(); _turret.setDisabled(manual); if (manual) @@ -156,7 +193,8 @@ public Command trackOnly() { return startEnd(() -> { - _state = ShooterState.TrackingOnly; + _state = ShooterState.TrackingOnly; + _requestedShotMode = ShotMode.Track; _turret.setDisabled(false); }, this::stopShooter); } @@ -166,7 +204,6 @@ public void bumpManualTurretAngle(double deltaDeg) if (inManualMode()) { _turret.setDisabled(false); - _turret.setTurretState(TurretState.ManualAngle); _turret.bumpManualAngle(Degrees.of(deltaDeg)); } } @@ -176,31 +213,6 @@ public double getManualTurretAngleDeg() return _turret.getManualAngle().in(Degrees); } - public final Flywheel _flywheel; - public final Feeder _feeder; - public final Turret _turret; - public final Rotor _rotor; - @Logged - private ShooterState _state; - @Logged - private boolean _autoShootEnabled; - - public Shooter(Supplier swerveStateSupplier) - { - _flywheel = new Flywheel(); - _feeder = new Feeder(); - _turret = new Turret(swerveStateSupplier); - _rotor = new Rotor(); - _state = ShooterState.Idle; - _autoShootEnabled = false; - - _feeder.set(false); - _rotor.set(false); - _flywheel.stop(); - _turret.setTurretState(TurretState.Idle); - _turret.setDisabled(false); - } - @Override public void periodic() { @@ -210,7 +222,7 @@ public void periodic() case Ready: _feeder.set(false); _rotor.set(false); - primeShot(); + runRequestedShot(true); if (isReadyToFeed()) { @@ -230,27 +242,32 @@ public void periodic() break; case Firing: - _feeder.set(true); - _rotor.set(true); - primeShot(); + runRequestedShot(true); if (isReadyToFeed()) { _feeder.set(true); + _rotor.set(true); } else { _feeder.set(false); + _rotor.set(false); _state = ShooterState.Preparing; } break; case Manual: + if (_requestedShotMode != ShotMode.Idle) + { + runRequestedShot(false); + } break; case TrackingOnly: _feeder.set(false); _flywheel.stop(); - _turret.setTurretState(TurretState.Track); + _requestedShotMode = ShotMode.Track; + runRequestedShot(false); break; case Idle: @@ -258,7 +275,7 @@ public void periodic() _flywheel.stop(); _feeder.set(false); _rotor.set(false); - _turret.setTurretState(TurretState.Idle); + clearShotRequest(); break; } @@ -276,21 +293,21 @@ public void simulationPeriodic() public void startPreparedShoot(boolean autoShoot) { - beginShootingSequence(TurretState.Track, autoShoot); + beginShootingSequence(ShotMode.Track, autoShoot); } public void startPass(boolean autoShoot) { - beginShootingSequence(TurretState.Pass, autoShoot); + beginShootingSequence(ShotMode.Pass, autoShoot); } - private void beginShootingSequence(TurretState turretState, boolean autoShoot) + private void beginShootingSequence(ShotMode shotMode, boolean autoShoot) { _feeder.set(false); _turret.setDisabled(false); - _turret.setTurretState(turretState); - _autoShootEnabled = autoShoot; - _state = ShooterState.Preparing; + _requestedShotMode = shotMode; + _autoShootEnabled = autoShoot; + _state = ShooterState.Preparing; } public void stopShooter() @@ -298,6 +315,7 @@ public void stopShooter() _state = ShooterState.Idle; _autoShootEnabled = false; _turret.setDisabled(false); + clearShotRequest(); } public void commenceFiring() @@ -308,13 +326,34 @@ public void commenceFiring() } } - private void primeShot() + private void runRequestedShot(boolean spinFlywheel) { - if (_state != ShooterState.Idle) + if (_requestedShotMode == ShotMode.Idle) { - var distance = _turret.getTargetDistance(); - _flywheel.setVelocity(ShooterConstants.getFlywheelSpeedForDistance(distance)); + _turret.clearTargetAngle(); + _currentShotSolution = createIdleSolution(); + return; } + + _currentShotSolution = _turretDirector.calculate(_requestedShotMode); + _turret.setTargetAngle(_currentShotSolution.turretAngle()); + + if (spinFlywheel) + { + _flywheel.setVelocity(ShooterConstants.getFlywheelSpeedForDistance(_currentShotSolution.distance())); + } + } + + private void clearShotRequest() + { + _requestedShotMode = ShotMode.Idle; + _currentShotSolution = createIdleSolution(); + _turret.clearTargetAngle(); + } + + private ShotSolution createIdleSolution() + { + return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false); } private boolean isReadyToFeed() diff --git a/src/main/java/frc/robot/subsystems/shooter/ShotSolution.java b/src/main/java/frc/robot/subsystems/shooter/ShotSolution.java deleted file mode 100644 index 3258fc7..0000000 --- a/src/main/java/frc/robot/subsystems/shooter/ShotSolution.java +++ /dev/null @@ -1,10 +0,0 @@ -package frc.robot.subsystems.shooter; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import frc.robot.subsystems.shooter.Turret.TurretState; - -public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, TurretState mode, Pose2d targetPose, boolean hasVisionTarget) -{ -} diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 6e0f299..14dc245 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -2,82 +2,53 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Volts; -import java.util.List; -import java.util.function.Supplier; - import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AIOConstants; import frc.robot.Constants.CANConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.util.MeasureUtil; -import frc.robot.util.Utilities; -import limelight.Limelight; -import limelight.networktables.target.AprilTagFiducial; @Logged public class Turret { - public enum TurretState + enum ControlMode { - Idle, Track, Pass, ManualAngle + Idle, TargetAngle, ManualAngle } - private final TalonFX _turretMotor; - private final AnalogPotentiometer _turretSensor; - private final Supplier _swerveStateSupplier; - private final TurretDirector _turretDirector; - private final Limelight _limelight; - private final PIDController _pidController; - private SwerveDriveState _currentSwerveState; - private TurretState _turretState; - private Angle _manualAngleSetpoint; - @Logged - private Angle _turretAngle; - @Logged - private Angle _turretSetpoint; - @Logged - private boolean _hasSetpoint; + private final TalonFX _turretMotor; + private final AnalogPotentiometer _turretSensor; + private final PIDController _pidController; + private ControlMode _controlMode; + private Angle _manualAngleSetpoint; + private Angle _targetAngleSetpoint; @Logged - private Voltage _motorVoltage; + private Angle _turretAngle; @Logged - private Distance _targetDistance; + private Angle _turretSetpoint; @Logged - private Pose2d _targetPose; + private boolean _hasSetpoint; @Logged - private boolean _limelightHasTarget; + private Voltage _motorVoltage; @Logged - private boolean _hubActive; - @Logged - private boolean _targetValid; - @Logged - private boolean _disabled; + private boolean _disabled; - public Turret(Supplier swerveStateSupplier) + public Turret() { - _swerveStateSupplier = swerveStateSupplier; - _turretDirector = new TurretDirector(); - var sensorRange = ShooterConstants.TURRET_HARD_MAX_ANGLE.minus(ShooterConstants.TURRET_HARD_MIN_ANGLE); var sensorOffset = ShooterConstants.TURRET_HARD_MIN_ANGLE; @@ -89,20 +60,14 @@ public Turret(Supplier swerveStateSupplier) _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); _turretSensor = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); - _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); - _currentSwerveState = new SwerveDriveState(); - _turretState = TurretState.Idle; + _controlMode = ControlMode.Idle; _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; _turretAngle = Degrees.zero(); _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; _hasSetpoint = false; _motorVoltage = Volts.zero(); - _targetDistance = Meters.zero(); - _targetPose = new Pose2d(); - _limelightHasTarget = false; - _hubActive = false; - _targetValid = false; _disabled = false; var currentConfig = new CurrentLimitsConfigs(); @@ -117,71 +82,30 @@ public Turret(Supplier swerveStateSupplier) _pidController.setTolerance(ShooterConstants.TURRET_TOLERANCE.in(Degrees)); _pidController.setSetpoint(ShooterConstants.TURRET_HOME_ANGLE.in(Degrees)); - - _limelight.getSettings().withAprilTagOffset(ShooterConstants.CENTER_TAG_TO_HUB_CENTER_OFFSET).withAprilTagIdFilter(ShooterConstants.RED_HUB_TAG_IDS).save(); - - var blueTrigger = new Trigger(Utilities::isBlueAlliance); - var redTrigger = new Trigger(Utilities::isRedAlliance); - var emptyTrigger = new Trigger(() -> DriverStation.getAlliance().isEmpty()); - - blueTrigger.onTrue(Commands.runOnce(() -> updateFilter(ShooterConstants.BLUE_HUB_TAG_IDS))); - redTrigger.onTrue(Commands.runOnce(() -> updateFilter(ShooterConstants.RED_HUB_TAG_IDS))); - emptyTrigger.onTrue(Commands.runOnce(() -> updateFilter(ShooterConstants.ALL_HUB_TAG_IDS))); } public void periodic() { - _currentSwerveState = _swerveStateSupplier.get(); + _turretAngle = Degrees.of(_turretSensor.get()); + _motorVoltage = _turretMotor.getMotorVoltage().getValue(); - _turretAngle = Degrees.of(_turretSensor.get()); - _motorVoltage = _turretMotor.getMotorVoltage().getValue(); - _limelightHasTarget = false; - _hubActive = Utilities.isHubActive(); - _targetValid = false; - - var fiducials = new AprilTagFiducial[0]; var motorOutput = Volts.zero(); - switch (_turretState) + switch (_controlMode) { - case Track: - var results = _limelight.getLatestResults(); - - if (results.isPresent()) - { - fiducials = results.get().targets_Fiducials; - } - var trackSolution = _turretDirector.calculate(_turretState, _currentSwerveState, Utilities.getHubCoordinates(), Utilities.getOurHubTagIds(), Utilities.isBlueAlliance(), _hubActive, fiducials); - _hasSetpoint = true; - _limelightHasTarget = trackSolution.hasVisionTarget(); - _targetValid = trackSolution.valid(); - _targetDistance = trackSolution.distance(); - _targetPose = trackSolution.targetPose(); - _turretSetpoint = trackSolution.turretAngle(); - break; - - case Pass: - var passSolution = _turretDirector.calculate(_turretState, _currentSwerveState, Utilities.getHubCoordinates(), Utilities.getOurHubTagIds(), Utilities.isBlueAlliance(), _hubActive); + case TargetAngle: _hasSetpoint = true; - _limelightHasTarget = passSolution.hasVisionTarget(); - _targetValid = passSolution.valid(); - _targetDistance = passSolution.distance(); - _targetPose = passSolution.targetPose(); - _turretSetpoint = passSolution.turretAngle(); + _turretSetpoint = clampToSoftLimits(_targetAngleSetpoint); break; case ManualAngle: _hasSetpoint = true; - _targetValid = false; - _turretSetpoint = MeasureUtil.clamp(moduloAngle(_manualAngleSetpoint), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); + _turretSetpoint = clampToSoftLimits(_manualAngleSetpoint); break; case Idle: default: _hasSetpoint = false; - _targetValid = false; - _targetDistance = Meters.zero(); - _targetPose = _currentSwerveState.Pose; _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; break; } @@ -199,24 +123,27 @@ public void simulationPeriodic() { } - public Distance getTargetDistance() + public void setTargetAngle(Angle angle) { - return _targetDistance; + _targetAngleSetpoint = angle; + _controlMode = ControlMode.TargetAngle; } - public void setTurretState(TurretState state) + public void clearTargetAngle() { - _turretState = state; + _controlMode = ControlMode.Idle; } public void setManualAngle(Angle angle) { _manualAngleSetpoint = angle; + _controlMode = ControlMode.ManualAngle; } public void bumpManualAngle(Angle delta) { _manualAngleSetpoint = _manualAngleSetpoint.plus(delta); + _controlMode = ControlMode.ManualAngle; } public Angle getManualAngle() @@ -224,49 +151,24 @@ public Angle getManualAngle() return _manualAngleSetpoint; } - public TurretState getTurretState() - { - return _turretState; - } - public boolean isLinedUp() { return _hasSetpoint && _pidController.atSetpoint(); } - public boolean hasValidTarget() - { - return _targetValid; - } - - public boolean hasLimelightTarget() - { - return _limelightHasTarget; - } - - public boolean isReadyToShoot() - { - return hasValidTarget() && isLinedUp(); - } - public void setDisabled(boolean disabled) { _disabled = disabled; } - private void updateFilter(List filters) + private Angle moduloAngle(Angle angle) { - if (DriverStation.isDisabled()) - { - _limelight.getSettings().withAprilTagIdFilter(filters).save(); - } + return Degrees.of(MathUtil.inputModulus(angle.in(Degrees), -180, 180)); } - private Angle moduloAngle(Angle angle) + private Angle clampToSoftLimits(Angle requestedAngle) { - var degrees = angle.in(Degrees); - - return Degrees.of(MathUtil.inputModulus(degrees, -180, 180)); + return MeasureUtil.clamp(moduloAngle(requestedAngle), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); } private Voltage applySoftLimit(Voltage requestedVoltage) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index 12bd9eb..5d499ea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -4,9 +4,11 @@ import static edu.wpi.first.units.Units.Meters; import java.util.List; +import java.util.function.Supplier; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -14,28 +16,92 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.subsystems.shooter.Turret.TurretState; import frc.robot.util.MeasureUtil; +import frc.robot.util.Utilities; +import limelight.Limelight; import limelight.networktables.target.AprilTagFiducial; +@Logged public class TurretDirector { - public ShotSolution calculate(TurretState turretState, SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean isBlueAlliance, boolean hubActive, AprilTagFiducial... fiducials) + public enum ShotMode { - switch (turretState) - { - case Track: - return calculateTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials); + Idle, Track, Pass + } + + public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, ShotMode mode, Pose2d targetPose, boolean hasVisionTarget) + { + } + + private final Supplier _swerveStateSupplier; + private final Limelight _limelight; + private List _targetTagFilter; + private ShotSolution _currentSolution; + @Logged + private Distance _targetDistance; + @Logged + private Pose2d _targetPose; + @Logged + private boolean _limelightHasTarget; + @Logged + private boolean _hubActive; + @Logged + private boolean _targetValid; + + public TurretDirector(Supplier swerveStateSupplier) + { + _swerveStateSupplier = swerveStateSupplier; + _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); + _targetTagFilter = List.of(); + _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false); + _targetDistance = Meters.zero(); + _targetPose = new Pose2d(); + _limelightHasTarget = false; + _hubActive = false; + _targetValid = false; + + _limelight.getSettings().withAprilTagOffset(ShooterConstants.CENTER_TAG_TO_HUB_CENTER_OFFSET).save(); + updateFilter(Utilities.getOurHubTagIds(), true); + } + + public ShotSolution calculate(ShotMode shotMode) + { + var swerveState = _swerveStateSupplier.get(); + var targetTagIds = Utilities.getOurHubTagIds(); + var isBlueAlliance = Utilities.isBlueAlliance(); + + updateFilter(targetTagIds, false); + + _hubActive = Utilities.isHubActive(); - case Pass: - return calculatePass(swerveState, isBlueAlliance); + var fiducials = new AprilTagFiducial[0]; + var results = _limelight.getLatestResults(); - case Idle: - default: - return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), turretState, swerveState.Pose, false); + if (results.isPresent()) + { + fiducials = results.get().targets_Fiducials; } + + var solution = calculate(shotMode, swerveState, Utilities.getHubCoordinates(), targetTagIds, isBlueAlliance, _hubActive, fiducials); + _currentSolution = solution; + _targetDistance = solution.distance(); + _targetPose = solution.targetPose(); + _limelightHasTarget = solution.hasVisionTarget(); + _targetValid = solution.valid(); + return solution; + } + + ShotSolution calculate(ShotMode shotMode, SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean isBlueAlliance, boolean hubActive, AprilTagFiducial... fiducials) + { + return switch (shotMode) + { + case Track -> calculateTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials); + case Pass -> calculatePass(swerveState, isBlueAlliance); + case Idle -> new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, swerveState.Pose, false); + }; } private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) @@ -62,7 +128,7 @@ private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); - return new ShotSolution(hubActive, clampTurretAngle(rawSetpoint), distance, TurretState.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); + return new ShotSolution(hubActive, clampTurretAngle(rawSetpoint), distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); } private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueAlliance) @@ -83,7 +149,7 @@ private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueA var rawSetpoint = fieldTargetAngle.minus(swerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - return new ShotSolution(true, clampTurretAngle(rawSetpoint), distance, TurretState.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); + return new ShotSolution(true, clampTurretAngle(rawSetpoint), distance, ShotMode.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); } private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSetpoint) @@ -134,4 +200,15 @@ private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiduc return bestTag; } + + private void updateFilter(List filters, boolean force) + { + if (!force && (!DriverStation.isDisabled() || filters.equals(_targetTagFilter))) + { + return; + } + + _limelight.getSettings().withAprilTagIdFilter(filters).save(); + _targetTagFilter = List.copyOf(filters); + } } From 7b1b85d4289be09107f52f7572710dce39c613f2 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 00:56:24 -0500 Subject: [PATCH 07/32] Fix shooter readiness gate --- .../frc/robot/subsystems/shooter/Shooter.java | 43 ++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index fc176ab..ff1cb70 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -38,6 +38,16 @@ public enum ShooterState @Logged private ShotMode _requestedShotMode; private ShotSolution _currentShotSolution; + @Logged + private boolean _solutionReady; + @Logged + private boolean _turretReady; + @Logged + private boolean _flywheelReady; + @Logged + private boolean _targetReady; + @Logged + private boolean _feedReady; public Shooter(Supplier swerveStateSupplier) { @@ -50,6 +60,11 @@ public Shooter(Supplier swerveStateSupplier) _autoShootEnabled = false; _requestedShotMode = ShotMode.Idle; _currentShotSolution = createIdleSolution(); + _solutionReady = false; + _turretReady = false; + _flywheelReady = false; + _targetReady = false; + _feedReady = false; _feeder.set(false); _rotor.set(false); @@ -332,6 +347,7 @@ private void runRequestedShot(boolean spinFlywheel) { _turret.clearTargetAngle(); _currentShotSolution = createIdleSolution(); + clearReadinessGate(); return; } @@ -348,6 +364,7 @@ private void clearShotRequest() { _requestedShotMode = ShotMode.Idle; _currentShotSolution = createIdleSolution(); + clearReadinessGate(); _turret.clearTargetAngle(); } @@ -358,6 +375,30 @@ private ShotSolution createIdleSolution() private boolean isReadyToFeed() { - return _flywheel.atSpeed(); + _solutionReady = _currentShotSolution.valid(); + _turretReady = _turret.isLinedUp(); + _flywheelReady = _flywheel.atSpeed(); + _targetReady = hasValidTargetForCurrentShot(); + _feedReady = _solutionReady && _turretReady && _flywheelReady && _targetReady; + return _feedReady; + } + + private boolean hasValidTargetForCurrentShot() + { + return switch (_currentShotSolution.mode()) + { + case Track -> _currentShotSolution.valid(); + case Pass -> true; + case Idle -> false; + }; + } + + private void clearReadinessGate() + { + _solutionReady = false; + _turretReady = false; + _flywheelReady = false; + _targetReady = false; + _feedReady = false; } } From e0b009113187b5283cc9a2f4bf163acbe3a2cfaf Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 09:24:19 -0500 Subject: [PATCH 08/32] Clean up turret geometry and wrap handling --- src/main/java/frc/robot/Constants.java | 22 +++++--- .../frc/robot/subsystems/shooter/Turret.java | 50 ++++++++++++++++--- .../subsystems/shooter/TurretDirector.java | 31 ++++-------- .../shooter/TurretAngleUtilTest.java | 48 ++++++++++++++++++ 4 files changed, 117 insertions(+), 34 deletions(-) create mode 100644 src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 59356d1..5c6b4ad 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -163,7 +163,6 @@ public static class ShooterConstants public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); // Forward-facing when no target public static final Angle TURRET_TOLERANCE = Degrees.of(2.0); // degrees public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(2); - public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); // Physical offset between turret rotation center and shooter exit point public static final String LIMELIGHT_NAME = "limelight-shooter"; public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); @@ -175,12 +174,21 @@ public static class ShooterConstants // left of robot forward public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); - public static final Translation2d TURRET_POSITION = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_CAMERA_POSITION = new Translation2d(Inches.zero(), Inches.of(8.0)); - public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" - public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); - public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); - public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); + public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.zero(), Inches.of(1.0)); // Placeholder delta used to preserve today's static shot geometry until the + // measured release transform replaces it + public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); + public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); + @Deprecated + public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); + @Deprecated + public static final Translation2d TURRET_POSITION = ROBOT_TO_TURRET_PIVOT; + @Deprecated + public static final Translation2d TURRET_CAMERA_POSITION = ROBOT_TO_TURRET_CAMERA; + public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" + public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); + public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); + public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); // @formatter:off private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 14dc245..77382f8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -20,11 +20,13 @@ import frc.robot.Constants.AIOConstants; import frc.robot.Constants.CANConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.util.MeasureUtil; @Logged public class Turret { + private static final int kCandidateWrapCount = 2; + private static final double kComparisonToleranceDeg = 1e-9; + enum ControlMode { Idle, TargetAngle, ManualAngle @@ -95,12 +97,12 @@ public void periodic() { case TargetAngle: _hasSetpoint = true; - _turretSetpoint = clampToSoftLimits(_targetAngleSetpoint); + _turretSetpoint = selectLegalSetpoint(_targetAngleSetpoint); break; case ManualAngle: _hasSetpoint = true; - _turretSetpoint = clampToSoftLimits(_manualAngleSetpoint); + _turretSetpoint = selectLegalSetpoint(_manualAngleSetpoint); break; case Idle: @@ -161,14 +163,48 @@ public void setDisabled(boolean disabled) _disabled = disabled; } - private Angle moduloAngle(Angle angle) + private Angle selectLegalSetpoint(Angle requestedAngle) { - return Degrees.of(MathUtil.inputModulus(angle.in(Degrees), -180, 180)); + return chooseNearestLegalAngle(_turretAngle, requestedAngle); } - private Angle clampToSoftLimits(Angle requestedAngle) + static Angle chooseNearestLegalAngle(Angle currentAngle, Angle requestedAngle) { - return MeasureUtil.clamp(moduloAngle(requestedAngle), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); + var currentDegrees = currentAngle.in(Degrees); + var requestedDegrees = requestedAngle.in(Degrees); + var minDegrees = ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Degrees); + var maxDegrees = ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Degrees); + var wrapCenter = (int)Math.round((currentDegrees - requestedDegrees) / 360.0); + var bestAngle = MathUtil.clamp(requestedDegrees, minDegrees, maxDegrees); + var bestTravel = Double.POSITIVE_INFINITY; + var bestClampAmount = Double.POSITIVE_INFINITY; + var bestWrapDistance = Integer.MAX_VALUE; + + for (int wrapOffset = -kCandidateWrapCount; wrapOffset <= kCandidateWrapCount; wrapOffset++) + { + var wrapIndex = wrapCenter + wrapOffset; + var wrappedDegrees = requestedDegrees + 360.0 * wrapIndex; + var legalDegrees = MathUtil.clamp(wrappedDegrees, minDegrees, maxDegrees); + var travelDegrees = Math.abs(legalDegrees - currentDegrees); + var clampAmount = Math.abs(legalDegrees - wrappedDegrees); + var wrapDistance = Math.abs(wrapIndex); + + // Inspired by Team 5000's TurretCalculator and 6328's unwrap-style helpers: + // choose a legal branch relative to the current azimuth instead of moduloing + // first, so requests near +/- soft limits stay on the local side. + var isBetterCandidate = travelDegrees < bestTravel || MathUtil.isNear(travelDegrees, bestTravel, kComparisonToleranceDeg) + && (clampAmount < bestClampAmount || MathUtil.isNear(clampAmount, bestClampAmount, kComparisonToleranceDeg) && (wrapDistance < bestWrapDistance || wrapDistance == bestWrapDistance && legalDegrees > bestAngle)); + + if (isBetterCandidate) + { + bestAngle = legalDegrees; + bestTravel = travelDegrees; + bestClampAmount = clampAmount; + bestWrapDistance = wrapDistance; + } + } + + return Degrees.of(bestAngle); } private Voltage applySoftLimit(Voltage requestedVoltage) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index 5d499ea..5d7c7c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -19,7 +19,6 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.util.MeasureUtil; import frc.robot.util.Utilities; import limelight.Limelight; import limelight.networktables.target.AprilTagFiducial; @@ -106,11 +105,15 @@ ShotSolution calculate(ShotMode shotMode, SwerveDriveState swerveState, Translat private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) { - var localHubTranslation = getHubTranslationInRobotFrame(swerveState, hubCoordinates); - var turretToHubTranslation = localHubTranslation.minus(ShooterConstants.TURRET_POSITION); - var distance = Meters.of(turretToHubTranslation.getNorm()); - var rawSetpoint = getTurretSetpointForRobotFrameTarget(turretToHubTranslation); - var bestTag = selectBestTag(targetTagIds, fiducials); + // Use explicit robot-frame geometry points here instead of the old + // lateral-offset + // trig fudge, in the same "named transforms first" spirit used by 6328. + var hubTranslationInRobotFrame = getHubTranslationInRobotFrame(swerveState, hubCoordinates); + var pivotToHubTranslation = hubTranslationInRobotFrame.minus(ShooterConstants.ROBOT_TO_TURRET_PIVOT); + var releaseToHubTranslation = hubTranslationInRobotFrame.minus(ShooterConstants.ROBOT_TO_TURRET_RELEASE); + var distance = Meters.of(pivotToHubTranslation.getNorm()); + var rawSetpoint = getTurretSetpointForRobotFrameTarget(releaseToHubTranslation); + var bestTag = selectBestTag(targetTagIds, fiducials); if (bestTag != null) { @@ -122,13 +125,7 @@ private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d } } - // Preserve the current static shooter-offset correction so the calculator - // reproduces today's behavior before SWM math is introduced. - var safeDistanceM = Math.max(distance.in(Meters), 0.5); - var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); - rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); - - return new ShotSolution(hubActive, clampTurretAngle(rawSetpoint), distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); + return new ShotSolution(hubActive, rawSetpoint, distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); } private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueAlliance) @@ -149,7 +146,7 @@ private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueA var rawSetpoint = fieldTargetAngle.minus(swerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - return new ShotSolution(true, clampTurretAngle(rawSetpoint), distance, ShotMode.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); + return new ShotSolution(true, rawSetpoint, distance, ShotMode.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); } private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSetpoint) @@ -173,12 +170,6 @@ private Angle getTurretSetpointForRobotFrameTarget(Translation2d targetTranslati return targetTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); } - private Angle clampTurretAngle(Angle rawSetpoint) - { - var wrappedDegrees = MathUtil.inputModulus(rawSetpoint.in(Degrees), -180, 180); - return MeasureUtil.clamp(Degrees.of(wrappedDegrees), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); - } - private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiducial... fiducials) { AprilTagFiducial bestTag = null; diff --git a/src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java b/src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java new file mode 100644 index 0000000..c5cd2c0 --- /dev/null +++ b/src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; + +class TurretAngleUtilTest +{ + private static final double kAngleToleranceDeg = 1e-9; + + @Test + void keepsAlreadyLegalTarget() + { + assertNearestLegalAngle(12.0, 35.0, 35.0); + } + + @Test + void wrapsToPositiveEquivalentNearUpperSoftLimit() + { + assertNearestLegalAngle(150.0, -200.0, 160.0); + } + + @Test + void clampsTowardCurrentBranchWhenPositiveRequestIsPastLimit() + { + assertNearestLegalAngle(150.0, 190.0, 160.0); + } + + @Test + void clampsTowardCurrentBranchWhenNegativeRequestIsPastLimit() + { + assertNearestLegalAngle(-150.0, -190.0, -160.0); + } + + @Test + void breaksCenterTieDeterministically() + { + assertNearestLegalAngle(0.0, 180.0, 160.0); + } + + private void assertNearestLegalAngle(double currentDegrees, double requestedDegrees, double expectedDegrees) + { + var actual = Turret.chooseNearestLegalAngle(Degrees.of(currentDegrees), Degrees.of(requestedDegrees)); + + assertEquals(expectedDegrees, actual.in(Degrees), kAngleToleranceDeg); + } +} From 71b7badce4174a2dd802e3190402117864115f0c Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 09:26:16 -0500 Subject: [PATCH 09/32] Remove turret wrap test file --- .../shooter/TurretAngleUtilTest.java | 48 ------------------- 1 file changed, 48 deletions(-) delete mode 100644 src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java diff --git a/src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java b/src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java deleted file mode 100644 index c5cd2c0..0000000 --- a/src/test/java/frc/robot/subsystems/shooter/TurretAngleUtilTest.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Degrees; -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.Test; - -class TurretAngleUtilTest -{ - private static final double kAngleToleranceDeg = 1e-9; - - @Test - void keepsAlreadyLegalTarget() - { - assertNearestLegalAngle(12.0, 35.0, 35.0); - } - - @Test - void wrapsToPositiveEquivalentNearUpperSoftLimit() - { - assertNearestLegalAngle(150.0, -200.0, 160.0); - } - - @Test - void clampsTowardCurrentBranchWhenPositiveRequestIsPastLimit() - { - assertNearestLegalAngle(150.0, 190.0, 160.0); - } - - @Test - void clampsTowardCurrentBranchWhenNegativeRequestIsPastLimit() - { - assertNearestLegalAngle(-150.0, -190.0, -160.0); - } - - @Test - void breaksCenterTieDeterministically() - { - assertNearestLegalAngle(0.0, 180.0, 160.0); - } - - private void assertNearestLegalAngle(double currentDegrees, double requestedDegrees, double expectedDegrees) - { - var actual = Turret.chooseNearestLegalAngle(Degrees.of(currentDegrees), Degrees.of(requestedDegrees)); - - assertEquals(expectedDegrees, actual.in(Degrees), kAngleToleranceDeg); - } -} From c7684ea566a6f4bdf6ed41ea924e878584a4788e Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 09:53:19 -0500 Subject: [PATCH 10/32] Restore track parity and drop local turret test --- .../subsystems/shooter/TurretDirector.java | 20 +- .../shooter/TurretDirectorTest.java | 171 ------------------ 2 files changed, 15 insertions(+), 176 deletions(-) delete mode 100644 src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index 5d7c7c3..fd8eb56 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -105,14 +105,13 @@ ShotSolution calculate(ShotMode shotMode, SwerveDriveState swerveState, Translat private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) { - // Use explicit robot-frame geometry points here instead of the old - // lateral-offset - // trig fudge, in the same "named transforms first" spirit used by 6328. + // Keep the named geometry points here, but preserve the old static shot + // behavior for now: distance is still pivot-to-hub, and the release point + // only contributes its lateral component as the legacy angle trim. var hubTranslationInRobotFrame = getHubTranslationInRobotFrame(swerveState, hubCoordinates); var pivotToHubTranslation = hubTranslationInRobotFrame.minus(ShooterConstants.ROBOT_TO_TURRET_PIVOT); - var releaseToHubTranslation = hubTranslationInRobotFrame.minus(ShooterConstants.ROBOT_TO_TURRET_RELEASE); var distance = Meters.of(pivotToHubTranslation.getNorm()); - var rawSetpoint = getTurretSetpointForRobotFrameTarget(releaseToHubTranslation); + var rawSetpoint = getTurretSetpointForRobotFrameTarget(pivotToHubTranslation); var bestTag = selectBestTag(targetTagIds, fiducials); if (bestTag != null) @@ -125,6 +124,8 @@ private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d } } + rawSetpoint = rawSetpoint.plus(getLegacyReleaseLateralCorrection(rawSetpoint, distance)); + return new ShotSolution(hubActive, rawSetpoint, distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); } @@ -170,6 +171,15 @@ private Angle getTurretSetpointForRobotFrameTarget(Translation2d targetTranslati return targetTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); } + private Angle getLegacyReleaseLateralCorrection(Angle rawSetpoint, Distance distance) + { + var safeDistanceM = Math.max(distance.in(Meters), 0.5); + var releaseLateralOffset = ShooterConstants.TURRET_PIVOT_TO_RELEASE.getY(); + var lateralErrorM = releaseLateralOffset * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); + + return Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM))); + } + private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiducial... fiducials) { AprilTagFiducial bestTag = null; diff --git a/src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java b/src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java deleted file mode 100644 index 50099a5..0000000 --- a/src/test/java/frc/robot/subsystems/shooter/TurretDirectorTest.java +++ /dev/null @@ -1,171 +0,0 @@ -package frc.robot.subsystems.shooter; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Meters; -import static org.junit.jupiter.api.Assertions.assertEquals; - -import org.junit.jupiter.api.Test; - -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; -import frc.robot.Constants.GeneralConstants; -import frc.robot.Constants.ShooterConstants; -import frc.robot.subsystems.shooter.Turret.TurretState; -import frc.robot.util.MeasureUtil; -import limelight.networktables.target.AprilTagFiducial; - -class TurretDirectorTest -{ - private static final double kAngleToleranceDeg = 1e-9; - private static final double kDistanceToleranceM = 1e-9; - private static final double kTranslationTolerance = 1e-9; - private final TurretDirector _turretDirector = new TurretDirector(); - - @Test - void trackStaticShotMatchesLegacyGeometryWithoutVision() - { - var state = createState(new Pose2d(3.1, 4.2, Rotation2d.fromDegrees(32.0))); - var actual = _turretDirector.calculate(TurretState.Track, state, ShooterConstants.BLUE_HUB, ShooterConstants.BLUE_HUB_TAG_IDS, true, true); - var expected = legacyTrackSolution(state, ShooterConstants.BLUE_HUB, true); - - assertSolutionMatches(expected, actual); - } - - @Test - void trackStaticShotPreservesHubValidityWithoutLookahead() - { - var state = createState(new Pose2d(5.0, 2.0, Rotation2d.fromDegrees(-18.0))); - var actual = _turretDirector.calculate(TurretState.Track, state, ShooterConstants.RED_HUB, ShooterConstants.RED_HUB_TAG_IDS, false, false); - var expected = legacyTrackSolution(state, ShooterConstants.RED_HUB, false); - - assertSolutionMatches(expected, actual); - } - - @Test - void passShotMatchesLegacyBlueAllianceBehavior() - { - var state = createState(new Pose2d(2.4, 5.5, Rotation2d.fromDegrees(47.0))); - var actual = _turretDirector.calculate(TurretState.Pass, state, ShooterConstants.BLUE_HUB, ShooterConstants.BLUE_HUB_TAG_IDS, true, true); - var expected = legacyPassSolution(state, true); - - assertSolutionMatches(expected, actual); - } - - @Test - void passShotMatchesLegacyRedAllianceBehavior() - { - var state = createState(new Pose2d(10.2, 3.3, Rotation2d.fromDegrees(-21.0))); - var actual = _turretDirector.calculate(TurretState.Pass, state, ShooterConstants.RED_HUB, ShooterConstants.RED_HUB_TAG_IDS, false, true); - var expected = legacyPassSolution(state, false); - - assertSolutionMatches(expected, actual); - } - - @Test - void trackShotUsesBestAllowedVisionTagForTrim() - { - var state = createState(new Pose2d(3.6, 4.8, Rotation2d.fromDegrees(14.0))); - var actual = _turretDirector.calculate(TurretState.Track, state, ShooterConstants.BLUE_HUB, ShooterConstants.BLUE_HUB_TAG_IDS, true, true, createTag(999, 50.0, -20.0), createTag(20, 4.0, 6.0), createTag(18, 8.0, 3.0)); - var expected = legacyTrackSolution(state, ShooterConstants.BLUE_HUB, true, 3.0, true); - - assertSolutionMatches(expected, actual); - } - - private ShotSolution legacyTrackSolution(SwerveDriveState swerveState, Translation2d hubCoordinates, boolean hubActive) - { - return legacyTrackSolution(swerveState, hubCoordinates, hubActive, 0.0, false); - } - - private ShotSolution legacyTrackSolution(SwerveDriveState swerveState, Translation2d hubCoordinates, boolean hubActive, double txDegrees, boolean hasVisionTarget) - { - var robotToHubField = hubCoordinates.minus(swerveState.Pose.getTranslation()); - var localHubTranslation = robotToHubField.rotateBy(swerveState.Pose.getRotation().unaryMinus()); - var turretToHubTranslation = localHubTranslation.minus(ShooterConstants.TURRET_POSITION); - var distance = Meters.of(turretToHubTranslation.getNorm()); - var rawSetpoint = turretToHubTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - - if (hasVisionTarget && !MathUtil.isNear(0.0, txDegrees, ShooterConstants.TURRET_TRACK_TX_DEADBAND.in(Degrees))) - { - rawSetpoint = rawSetpoint.plus(Degrees.of(txDegrees)); - } - - var safeDistanceM = Math.max(distance.in(Meters), 0.5); - var lateralErrorM = ShooterConstants.SHOOTER_LATERAL_OFFSET.in(Meters) * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); - rawSetpoint = rawSetpoint.plus(Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM)))); - - return new ShotSolution(hubActive, clampTurretAngle(rawSetpoint), distance, TurretState.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), hasVisionTarget); - } - - private ShotSolution legacyPassSolution(SwerveDriveState swerveState, boolean isBlueAlliance) - { - Angle fieldTargetAngle; - Distance distance; - - if (isBlueAlliance) - { - distance = swerveState.Pose.getMeasureX(); - fieldTargetAngle = Degrees.of(180); - } - else - { - distance = GeneralConstants.FIELD_SIZE_X.minus(swerveState.Pose.getMeasureX()); - fieldTargetAngle = Degrees.zero(); - } - - var rawSetpoint = fieldTargetAngle.minus(swerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - - return new ShotSolution(true, clampTurretAngle(rawSetpoint), distance, TurretState.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); - } - - private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSetpoint) - { - var forwardTranslation = new Translation2d(distance, Meters.zero()); - var rotatedByTurretOffset = forwardTranslation.rotateBy(new Rotation2d(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD)); - var rotatedBySetpoint = rotatedByTurretOffset.rotateBy(new Rotation2d(rawSetpoint)); - - return robotPose.plus(new Transform2d(rotatedBySetpoint, new Rotation2d())); - } - - private Angle clampTurretAngle(Angle rawSetpoint) - { - var wrappedDegrees = MathUtil.inputModulus(rawSetpoint.in(Degrees), -180, 180); - return MeasureUtil.clamp(Degrees.of(wrappedDegrees), ShooterConstants.TURRET_SOFT_MIN_ANGLE, ShooterConstants.TURRET_SOFT_MAX_ANGLE); - } - - private SwerveDriveState createState(Pose2d pose) - { - var state = new SwerveDriveState(); - state.Pose = pose; - state.Speeds = new ChassisSpeeds(0.0, 0.0, 0.0); - return state; - } - - private AprilTagFiducial createTag(int fiducialId, double ta, double tx) - { - var tag = new AprilTagFiducial(); - tag.fiducialID = fiducialId; - tag.ta = ta; - tag.tx = tx; - return tag; - } - - private void assertSolutionMatches(ShotSolution expected, ShotSolution actual) - { - assertEquals(expected.valid(), actual.valid()); - assertEquals(expected.hasVisionTarget(), actual.hasVisionTarget()); - assertEquals(expected.mode(), actual.mode()); - assertEquals(expected.turretAngle().in(Degrees), actual.turretAngle().in(Degrees), kAngleToleranceDeg); - assertEquals(expected.distance().in(Meters), actual.distance().in(Meters), kDistanceToleranceM); - assertEquals(expected.targetPose().getX(), actual.targetPose().getX(), kTranslationTolerance); - assertEquals(expected.targetPose().getY(), actual.targetPose().getY(), kTranslationTolerance); - assertEquals(expected.targetPose().getRotation().getDegrees(), actual.targetPose().getRotation().getDegrees(), kAngleToleranceDeg); - } -} From fde46a053041810ef538dccfe73d2eaabe0a0cd5 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 10:34:00 -0500 Subject: [PATCH 11/32] Add SWM tracking-only turret solve --- src/main/java/frc/robot/Constants.java | 51 +++++-- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../subsystems/shooter/TurretDirector.java | 134 ++++++++++++++++-- 3 files changed, 162 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5c6b4ad..e9433ed 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,6 +13,7 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Value; import static edu.wpi.first.units.Units.Volts; @@ -174,21 +175,25 @@ public static class ShooterConstants // left of robot forward public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); - public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.zero(), Inches.of(1.0)); // Placeholder delta used to preserve today's static shot geometry until the - // measured release transform replaces it - public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); - public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); + public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.zero(), Inches.of(1.0)); // Placeholder delta used to preserve today's static shot geometry until the + // measured release transform replaces it + public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); + public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); @Deprecated - public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); + public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); @Deprecated - public static final Translation2d TURRET_POSITION = ROBOT_TO_TURRET_PIVOT; + public static final Translation2d TURRET_POSITION = ROBOT_TO_TURRET_PIVOT; @Deprecated - public static final Translation2d TURRET_CAMERA_POSITION = ROBOT_TO_TURRET_CAMERA; - public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" - public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); - public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); - public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); + public static final Translation2d TURRET_CAMERA_POSITION = ROBOT_TO_TURRET_CAMERA; + public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" + public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); + public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); + public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); + public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); + public static final int SWM_LOOKAHEAD_ITERATIONS = 8; + private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; + private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; // @formatter:off private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries @@ -202,6 +207,17 @@ public static class ShooterConstants Map.entry(183.0, 5100.0), Map.entry(204.0, 5850.0) ); + + // Keep TOF as its own lookup so the moving-shot solve matches the empirical + // map-based pattern used by teams like 5000, 6328, and Eeshwar's writeup. + private static final InterpolatingDoubleTreeMap SHOT_TOF_TABLE = InterpolatingDoubleTreeMap.ofEntries + ( + Map.entry(64.0, 0.542), + Map.entry(85.0, 1.06), + Map.entry(87.0, 1.02), + Map.entry(134.0, 1.22), + Map.entry(159.0, 1.74) + ); // @formatter:on // Feeder @@ -215,6 +231,17 @@ public static AngularVelocity getFlywheelSpeedForDistance(Distance distance) { return RPM.of(FLYWHEEL_SPEED_TABLE.get(distance.in(Inches))).plus(SMART_SHOOT_RPM_BIAS); } + + public static Time getShotTimeOfFlight(Distance distance) + { + return Seconds.of(SHOT_TOF_TABLE.get(distance.in(Inches))); + } + + public static boolean isMovingShotDistanceValid(Distance distance) + { + var distanceInches = distance.in(Inches); + return distanceInches >= SWM_MIN_TOF_DISTANCE_IN && distanceInches <= SWM_MAX_TOF_DISTANCE_IN; + } } public static class VisionConstants diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ff1cb70..2663845 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -351,7 +351,7 @@ private void runRequestedShot(boolean spinFlywheel) return; } - _currentShotSolution = _turretDirector.calculate(_requestedShotMode); + _currentShotSolution = _turretDirector.calculate(_requestedShotMode, _state == ShooterState.TrackingOnly); _turret.setTargetAngle(_currentShotSolution.turretAngle()); if (spinFlywheel) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index fd8eb56..5c0f85b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; import java.util.List; import java.util.function.Supplier; @@ -14,8 +15,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; @@ -35,6 +39,10 @@ public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, { } + private record MovingTrackState(Pose2d releasePose, Translation2d lookaheadTurretOrigin, Distance lookaheadDistance, Time timeOfFlight) + { + } + private final Supplier _swerveStateSupplier; private final Limelight _limelight; private List _targetTagFilter; @@ -49,24 +57,38 @@ public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, private boolean _hubActive; @Logged private boolean _targetValid; + @Logged + private Pose2d _predictedReleasePose; + @Logged + private Pose2d _lookaheadTurretOriginPose; + @Logged + private Time _shotTimeOfFlight; public TurretDirector(Supplier swerveStateSupplier) { - _swerveStateSupplier = swerveStateSupplier; - _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); - _targetTagFilter = List.of(); - _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false); - _targetDistance = Meters.zero(); - _targetPose = new Pose2d(); - _limelightHasTarget = false; - _hubActive = false; - _targetValid = false; + _swerveStateSupplier = swerveStateSupplier; + _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); + _targetTagFilter = List.of(); + _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false); + _targetDistance = Meters.zero(); + _targetPose = new Pose2d(); + _limelightHasTarget = false; + _hubActive = false; + _targetValid = false; + _predictedReleasePose = new Pose2d(); + _lookaheadTurretOriginPose = new Pose2d(); + _shotTimeOfFlight = Seconds.zero(); _limelight.getSettings().withAprilTagOffset(ShooterConstants.CENTER_TAG_TO_HUB_CENTER_OFFSET).save(); updateFilter(Utilities.getOurHubTagIds(), true); } public ShotSolution calculate(ShotMode shotMode) + { + return calculate(shotMode, false); + } + + public ShotSolution calculate(ShotMode shotMode, boolean useMovingShotMath) { var swerveState = _swerveStateSupplier.get(); var targetTagIds = Utilities.getOurHubTagIds(); @@ -84,7 +106,7 @@ public ShotSolution calculate(ShotMode shotMode) fiducials = results.get().targets_Fiducials; } - var solution = calculate(shotMode, swerveState, Utilities.getHubCoordinates(), targetTagIds, isBlueAlliance, _hubActive, fiducials); + var solution = calculate(shotMode, useMovingShotMath, swerveState, Utilities.getHubCoordinates(), targetTagIds, isBlueAlliance, _hubActive, fiducials); _currentSolution = solution; _targetDistance = solution.distance(); _targetPose = solution.targetPose(); @@ -94,12 +116,17 @@ public ShotSolution calculate(ShotMode shotMode) } ShotSolution calculate(ShotMode shotMode, SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean isBlueAlliance, boolean hubActive, AprilTagFiducial... fiducials) + { + return calculate(shotMode, false, swerveState, hubCoordinates, targetTagIds, isBlueAlliance, hubActive, fiducials); + } + + ShotSolution calculate(ShotMode shotMode, boolean useMovingShotMath, SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean isBlueAlliance, boolean hubActive, AprilTagFiducial... fiducials) { return switch (shotMode) { - case Track -> calculateTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials); + case Track -> useMovingShotMath ? calculateMovingTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials) : calculateTrack(swerveState, hubCoordinates, targetTagIds, hubActive, fiducials); case Pass -> calculatePass(swerveState, isBlueAlliance); - case Idle -> new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, swerveState.Pose, false); + case Idle -> createIdleSolution(swerveState.Pose); }; } @@ -126,6 +153,8 @@ private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d rawSetpoint = rawSetpoint.plus(getLegacyReleaseLateralCorrection(rawSetpoint, distance)); + logShotKinematics(swerveState.Pose, getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), distance, ShooterConstants.getShotTimeOfFlight(distance)); + return new ShotSolution(hubActive, rawSetpoint, distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); } @@ -147,9 +176,28 @@ private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueA var rawSetpoint = fieldTargetAngle.minus(swerveState.Pose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); + logShotKinematics(swerveState.Pose, getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), distance, ShooterConstants.getShotTimeOfFlight(distance)); + return new ShotSolution(true, rawSetpoint, distance, ShotMode.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); } + private ShotSolution calculateMovingTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) + { + var movingTrackState = calculateMovingTrackState(swerveState, hubCoordinates); + var lookaheadToHub = hubCoordinates.minus(movingTrackState.lookaheadTurretOrigin()); + var rawSetpoint = getTurretSetpointForFieldTarget(movingTrackState.releasePose(), lookaheadToHub.getAngle().getMeasure()); + var bestTag = selectBestTag(targetTagIds, fiducials); + + rawSetpoint = rawSetpoint.plus(getLegacyReleaseLateralCorrection(rawSetpoint, movingTrackState.lookaheadDistance())); + + logShotKinematics(movingTrackState.releasePose(), movingTrackState.lookaheadTurretOrigin(), movingTrackState.lookaheadDistance(), movingTrackState.timeOfFlight()); + + return new ShotSolution( + hubActive && ShooterConstants.isMovingShotDistanceValid(movingTrackState.lookaheadDistance()), rawSetpoint, movingTrackState.lookaheadDistance(), ShotMode.Track, + buildTargetPose(movingTrackState.releasePose(), movingTrackState.lookaheadDistance(), rawSetpoint), bestTag != null + ); + } + private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSetpoint) { var forwardTranslation = new Translation2d(distance, Meters.zero()); @@ -171,6 +219,11 @@ private Angle getTurretSetpointForRobotFrameTarget(Translation2d targetTranslati return targetTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); } + private Angle getTurretSetpointForFieldTarget(Pose2d robotPose, Angle fieldTargetAngle) + { + return fieldTargetAngle.minus(robotPose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); + } + private Angle getLegacyReleaseLateralCorrection(Angle rawSetpoint, Distance distance) { var safeDistanceM = Math.max(distance.in(Meters), 0.5); @@ -180,6 +233,63 @@ private Angle getLegacyReleaseLateralCorrection(Angle rawSetpoint, Distance dist return Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM))); } + private MovingTrackState calculateMovingTrackState(SwerveDriveState swerveState, Translation2d hubCoordinates) + { + // This follows the turret-origin solve pattern described publicly by 5000 + // and 6328: predict the release pose, compute turret-point velocity as + // v + omega x r, then iterate TOF lookahead from the turret pivot. + var releasePose = predictReleasePose(swerveState.Pose, swerveState.Speeds); + var turretOriginField = getPointInField(releasePose, ShooterConstants.ROBOT_TO_TURRET_PIVOT); + var turretPointVelocity = getTurretPointVelocityField(releasePose, swerveState.Speeds, ShooterConstants.ROBOT_TO_TURRET_PIVOT); + var lookaheadOrigin = turretOriginField; + var lookaheadDistance = Meters.of(hubCoordinates.getDistance(lookaheadOrigin)); + var timeOfFlight = ShooterConstants.getShotTimeOfFlight(lookaheadDistance); + + for (int i = 0; i < ShooterConstants.SWM_LOOKAHEAD_ITERATIONS; i++) + { + lookaheadOrigin = turretOriginField.plus(turretPointVelocity.times(timeOfFlight.in(Seconds))); + lookaheadDistance = Meters.of(hubCoordinates.getDistance(lookaheadOrigin)); + timeOfFlight = ShooterConstants.getShotTimeOfFlight(lookaheadDistance); + } + + return new MovingTrackState(releasePose, lookaheadOrigin, lookaheadDistance, timeOfFlight); + } + + private Pose2d predictReleasePose(Pose2d robotPose, ChassisSpeeds robotRelativeSpeeds) + { + var phaseDelaySeconds = ShooterConstants.SWM_RELEASE_PHASE_DELAY.in(Seconds); + + return robotPose.exp(new Twist2d(robotRelativeSpeeds.vxMetersPerSecond * phaseDelaySeconds, robotRelativeSpeeds.vyMetersPerSecond * phaseDelaySeconds, robotRelativeSpeeds.omegaRadiansPerSecond * phaseDelaySeconds)); + } + + private Translation2d getPointInField(Pose2d robotPose, Translation2d robotRelativePoint) + { + return robotPose.getTranslation().plus(robotRelativePoint.rotateBy(robotPose.getRotation())); + } + + private Translation2d getTurretPointVelocityField(Pose2d robotPose, ChassisSpeeds robotRelativeSpeeds, Translation2d robotRelativeOffset) + { + var fieldRelativeRobotVelocity = new Translation2d(robotRelativeSpeeds.vxMetersPerSecond, robotRelativeSpeeds.vyMetersPerSecond).rotateBy(robotPose.getRotation()); + var fieldRelativeOffset = robotRelativeOffset.rotateBy(robotPose.getRotation()); + var omegaCrossR = new Translation2d(-robotRelativeSpeeds.omegaRadiansPerSecond * fieldRelativeOffset.getY(), robotRelativeSpeeds.omegaRadiansPerSecond * fieldRelativeOffset.getX()); + + return fieldRelativeRobotVelocity.plus(omegaCrossR); + } + + private ShotSolution createIdleSolution(Pose2d robotPose) + { + logShotKinematics(robotPose, getPointInField(robotPose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), Meters.zero(), Seconds.zero()); + return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, robotPose, false); + } + + private void logShotKinematics(Pose2d releasePose, Translation2d lookaheadTurretOrigin, Distance distance, Time timeOfFlight) + { + _predictedReleasePose = releasePose; + _lookaheadTurretOriginPose = new Pose2d(lookaheadTurretOrigin, new Rotation2d()); + _targetDistance = distance; + _shotTimeOfFlight = timeOfFlight; + } + private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiducial... fiducials) { AprilTagFiducial bestTag = null; From 32a7700bf09f99130db6115bafe391862e6d1cd2 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 12:27:56 -0500 Subject: [PATCH 12/32] Add rotor subsystem and integrate with shooter system --- src/main/java/frc/robot/Constants.java | 87 +++++---- .../frc/robot/subsystems/shooter/Rotor.java | 35 +--- .../frc/robot/subsystems/shooter/Shooter.java | 182 ++++++++++++++---- .../subsystems/shooter/TurretDirector.java | 15 +- 4 files changed, 205 insertions(+), 114 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e9433ed..775dae7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -86,7 +86,7 @@ public static class CANConstants public static final int FLYWHEEL_FOLLOW = 19; // Vortex public static final int CLIMBER_EXTEND = 21; public static final int CLIMBER_ROTATE = 22; - public static final int ROTOR_MOTOR = 23; + public static final int ROTOR_MOTOR = 23; // Vortex } public static class AIOConstants @@ -151,49 +151,54 @@ public static class ShooterConstants private static final AngularVelocity SMART_SHOOT_RPM_BIAS = RPM.of(150.0); // Turret - public static final Current TURRET_CURRENT_LIMIT = Amps.of(40.0); - public static final double TURRET_KP = 0.04; // TODO: Tune (onboard TalonFX PID) - public static final double TURRET_KI = 0.0; - public static final double TURRET_KD = 0.0115; - public static final Dimensionless TURRET_GEAR_RATIO = Value.of(13.0 / 1.0); // 13 : 1 - public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); // degrees (full 360 rotation) - public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); // degrees - public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); // degrees (full 360 rotation) - public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); // degrees - public static final boolean TURRET_SENSOR_INVERTED = true; - public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); // Forward-facing when no target - public static final Angle TURRET_TOLERANCE = Degrees.of(2.0); // degrees - public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(2); - public static final String LIMELIGHT_NAME = "limelight-shooter"; - public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice - public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); - public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); - public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); - public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); - public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); - public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Turret "zero" is 90 degrees - // left of robot forward - public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); - public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); - public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.zero(), Inches.of(1.0)); // Placeholder delta used to preserve today's static shot geometry until the - // measured release transform replaces it - public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); - public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); + public static final Current TURRET_CURRENT_LIMIT = Amps.of(40.0); + public static final double TURRET_KP = 0.04; // TODO: Tune (onboard TalonFX PID) + public static final double TURRET_KI = 0.0; + public static final double TURRET_KD = 0.0115; + public static final Dimensionless TURRET_GEAR_RATIO = Value.of(13.0 / 1.0); // 13 : 1 + public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); // degrees (full 360 rotation) + public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); // degrees + public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); // degrees (full 360 rotation) + public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); // degrees + public static final boolean TURRET_SENSOR_INVERTED = true; + public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); // Forward-facing when no target + public static final Angle TURRET_TOLERANCE = Degrees.of(2.0); // degrees + public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(2); + public static final String LIMELIGHT_NAME = "limelight-shooter"; + public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice + public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); + public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); + public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); + public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); + public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); + public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees + // left of robot forward + public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); + public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); + public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.zero(), Inches.of(1.0)); // Placeholder delta used to preserve today's static shot geometry until the + // measured release transform replaces it + public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); + public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); @Deprecated - public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); + public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); @Deprecated - public static final Translation2d TURRET_POSITION = ROBOT_TO_TURRET_PIVOT; + public static final Translation2d TURRET_POSITION = ROBOT_TO_TURRET_PIVOT; @Deprecated - public static final Translation2d TURRET_CAMERA_POSITION = ROBOT_TO_TURRET_CAMERA; - public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" - public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); - public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); - public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); - public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); - public static final int SWM_LOOKAHEAD_ITERATIONS = 8; - private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; - private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; + public static final Translation2d TURRET_CAMERA_POSITION = ROBOT_TO_TURRET_CAMERA; + public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" + public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); + public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); + public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); + public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); + public static final int SWM_LOOKAHEAD_ITERATIONS = 8; + public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); + public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); + public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.0); + public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(20.0); + public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(125.0); + private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; + private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; // @formatter:off private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries diff --git a/src/main/java/frc/robot/subsystems/shooter/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/Rotor.java index 58c46af..01b7f1f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Rotor.java +++ b/src/main/java/frc/robot/subsystems/shooter/Rotor.java @@ -7,12 +7,12 @@ import com.revrobotics.ResetMode; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; + import edu.wpi.first.epilogue.Logged; import edu.wpi.first.units.measure.Voltage; -import frc.robot.subsystems.test.MotorHook; -import frc.robot.subsystems.test.TestHook; + import frc.robot.Constants.CANConstants; import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; @@ -22,13 +22,15 @@ public class Rotor { private final SparkFlex _rotorMotor; @Logged - private Voltage _rotorMotorVoltage = Volts.of(0.0); + private Voltage _rotorMotorVoltage = Volts.zero(); + @Logged + private boolean _enabled = false; public Rotor() { _rotorMotor = new SparkFlex(CANConstants.ROTOR_MOTOR, MotorType.kBrushless); - SparkFlexConfig config = new SparkFlexConfig(); + var config = new SparkFlexConfig(); config.inverted(false).idleMode(IdleMode.kBrake).smartCurrentLimit((int)ShooterConstants.ROTOR_CURRENT_LIMIT.in(Amps)).voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts)); _rotorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); @@ -41,27 +43,8 @@ public void periodic() public void set(boolean on) { - Voltage targetVoltage = on ? ShooterConstants.ROTOR_VOLTAGE : Volts.zero(); + _enabled = on; + var targetVoltage = on ? ShooterConstants.ROTOR_VOLTAGE : Volts.zero(); _rotorMotor.setVoltage(targetVoltage.in(Volts)); } - - private class FeederHook extends MotorHook - { - @Override - public void stop() - { - set(false); - } - - @Override - public void setRate(double rate) - { - _rotorMotor.setVoltage(GeneralConstants.MOTOR_VOLTAGE.times(rate * _polarity)); - } - } - - public TestHook getHook() - { - return new FeederHook(); - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2663845..ca0be8c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,13 +2,17 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Seconds; import java.util.function.Supplier; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; @@ -26,45 +30,67 @@ public enum ShooterState Idle, Preparing, Ready, Firing, Manual, TrackingOnly; } - public final Flywheel _flywheel; - public final Feeder _feeder; - public final Turret _turret; - public final Rotor _rotor; - public final TurretDirector _turretDirector; + public final Flywheel _flywheel; + public final Feeder _feeder; + public final Rotor _rotor; + public final Turret _turret; + public final TurretDirector _turretDirector; + private final Supplier _swerveStateSupplier; + private final Debouncer _movingFeedDebouncer; @Logged - private ShooterState _state; + private ShooterState _state; @Logged - private boolean _autoShootEnabled; + private boolean _autoShootEnabled; @Logged - private ShotMode _requestedShotMode; - private ShotSolution _currentShotSolution; + private ShotMode _requestedShotMode; + private ShotSolution _currentShotSolution; @Logged - private boolean _solutionReady; + private boolean _solutionReady; @Logged - private boolean _turretReady; + private boolean _turretReady; @Logged - private boolean _flywheelReady; + private boolean _flywheelReady; @Logged - private boolean _targetReady; + private boolean _hubReady; @Logged - private boolean _feedReady; + private boolean _inRangeReady; + @Logged + private boolean _rolloutReady; + @Logged + private boolean _stabilityReady; + @Logged + private boolean _usingMovingShotMath; + @Logged + private double _robotTranslationSpeedMetersPerSecond; + @Logged + private double _robotAngularSpeedRadiansPerSecond; + @Logged + private boolean _feedReady; public Shooter(Supplier swerveStateSupplier) { - _flywheel = new Flywheel(); - _feeder = new Feeder(); - _turret = new Turret(); - _rotor = new Rotor(); - _turretDirector = new TurretDirector(swerveStateSupplier); - _state = ShooterState.Idle; - _autoShootEnabled = false; - _requestedShotMode = ShotMode.Idle; - _currentShotSolution = createIdleSolution(); - _solutionReady = false; - _turretReady = false; - _flywheelReady = false; - _targetReady = false; - _feedReady = false; + _swerveStateSupplier = swerveStateSupplier; + _flywheel = new Flywheel(); + _feeder = new Feeder(); + _rotor = new Rotor(); + _turret = new Turret(); + _turretDirector = new TurretDirector(swerveStateSupplier); + _movingFeedDebouncer = new Debouncer(ShooterConstants.SWM_FEED_STABILITY_WINDOW.in(Seconds)); + _state = ShooterState.Idle; + _autoShootEnabled = false; + _requestedShotMode = ShotMode.Idle; + _currentShotSolution = createIdleSolution(); + _solutionReady = false; + _turretReady = false; + _flywheelReady = false; + _hubReady = false; + _inRangeReady = false; + _rolloutReady = false; + _stabilityReady = false; + _usingMovingShotMath = false; + _robotTranslationSpeedMetersPerSecond = 0.0; + _robotAngularSpeedRadiansPerSecond = 0.0; + _feedReady = false; _feeder.set(false); _rotor.set(false); @@ -107,6 +133,7 @@ public Command setManualMode(boolean manual) _requestedShotMode = ShotMode.Idle; _currentShotSolution = createIdleSolution(); _feeder.set(false); + _rotor.set(false); _turret.clearTargetAngle(); _turret.setDisabled(manual); @@ -182,7 +209,15 @@ public Command setFlywheelVelocity(AngularVelocity velocity) public Command runManualFeeder() { - return startEnd(() -> _feeder.set(true), () -> _feeder.set(false)).onlyIf(this::inManualMode); + return startEnd(() -> + { + _feeder.set(true); + _rotor.set(true); + }, () -> + { + _feeder.set(false); + _rotor.set(false); + }).onlyIf(this::inManualMode); } public Command runFeeder() @@ -191,9 +226,11 @@ public Command runFeeder() { _state = ShooterState.Manual; _feeder.set(true); + _rotor.set(true); }, () -> { _feeder.set(false); + _rotor.set(false); _flywheel.stop(); _state = ShooterState.Idle; }); @@ -280,6 +317,7 @@ public void periodic() case TrackingOnly: _feeder.set(false); + _rotor.set(false); _flywheel.stop(); _requestedShotMode = ShotMode.Track; runRequestedShot(false); @@ -351,7 +389,9 @@ private void runRequestedShot(boolean spinFlywheel) return; } - _currentShotSolution = _turretDirector.calculate(_requestedShotMode, _state == ShooterState.TrackingOnly); + updateRobotMotionState(); + _usingMovingShotMath = shouldUseMovingShotMath(); + _currentShotSolution = _turretDirector.calculate(_requestedShotMode, _usingMovingShotMath); _turret.setTargetAngle(_currentShotSolution.turretAngle()); if (spinFlywheel) @@ -366,11 +406,12 @@ private void clearShotRequest() _currentShotSolution = createIdleSolution(); clearReadinessGate(); _turret.clearTargetAngle(); + _rotor.set(false); } private ShotSolution createIdleSolution() { - return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false); + return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false, false, false); } private boolean isReadyToFeed() @@ -378,27 +419,88 @@ private boolean isReadyToFeed() _solutionReady = _currentShotSolution.valid(); _turretReady = _turret.isLinedUp(); _flywheelReady = _flywheel.atSpeed(); - _targetReady = hasValidTargetForCurrentShot(); - _feedReady = _solutionReady && _turretReady && _flywheelReady && _targetReady; + _hubReady = hasActiveHubForCurrentShot(); + _inRangeReady = hasValidDistanceForCurrentShot(); + _rolloutReady = isWithinMovingFeedRolloutEnvelope(); + + // Temporary rollout override: keep logging hub-active state, but do not block + // feed on it while we validate the rest of the moving-shot gate. + var rawFeedReady = _solutionReady && _turretReady && _flywheelReady && _inRangeReady && _rolloutReady; + + // Keep moving feed behind an explicit gate and short stability window so we + // can roll it out conservatively and see which bit is blocking launch. + _stabilityReady = !_usingMovingShotMath || _movingFeedDebouncer.calculate(rawFeedReady); + _feedReady = rawFeedReady && _stabilityReady; return _feedReady; } - private boolean hasValidTargetForCurrentShot() + private boolean hasActiveHubForCurrentShot() { return switch (_currentShotSolution.mode()) { - case Track -> _currentShotSolution.valid(); + case Track -> _currentShotSolution.hubActive(); case Pass -> true; case Idle -> false; }; } + private boolean hasValidDistanceForCurrentShot() + { + return switch (_currentShotSolution.mode()) + { + case Track -> !_usingMovingShotMath || _currentShotSolution.inRange(); + case Pass -> true; + case Idle -> false; + }; + } + + private boolean shouldUseMovingShotMath() + { + if (_requestedShotMode != ShotMode.Track) + { + return false; + } + + if (_state == ShooterState.TrackingOnly) + { + return true; + } + + return _robotTranslationSpeedMetersPerSecond >= ShooterConstants.SWM_ENABLE_TRANSLATIONAL_SPEED.in(MetersPerSecond) || Math.abs(_robotAngularSpeedRadiansPerSecond) >= ShooterConstants.SWM_ENABLE_ANGULAR_SPEED.in(RadiansPerSecond); + } + + private boolean isWithinMovingFeedRolloutEnvelope() + { + if (!_usingMovingShotMath || _currentShotSolution.mode() != ShotMode.Track) + { + return true; + } + + return _robotTranslationSpeedMetersPerSecond <= ShooterConstants.SWM_FEED_MAX_TRANSLATIONAL_SPEED.in(MetersPerSecond) + && Math.abs(_robotAngularSpeedRadiansPerSecond) <= ShooterConstants.SWM_FEED_MAX_ANGULAR_SPEED.in(RadiansPerSecond); + } + + private void updateRobotMotionState() + { + var speeds = _swerveStateSupplier.get().Speeds; + + _robotTranslationSpeedMetersPerSecond = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + _robotAngularSpeedRadiansPerSecond = speeds.omegaRadiansPerSecond; + } + private void clearReadinessGate() { - _solutionReady = false; - _turretReady = false; - _flywheelReady = false; - _targetReady = false; - _feedReady = false; + _solutionReady = false; + _turretReady = false; + _flywheelReady = false; + _hubReady = false; + _inRangeReady = false; + _rolloutReady = false; + _stabilityReady = false; + _usingMovingShotMath = false; + _robotTranslationSpeedMetersPerSecond = 0.0; + _robotAngularSpeedRadiansPerSecond = 0.0; + _movingFeedDebouncer.calculate(false); + _feedReady = false; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index 5c0f85b..6aa09e8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -35,7 +35,7 @@ public enum ShotMode Idle, Track, Pass } - public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, ShotMode mode, Pose2d targetPose, boolean hasVisionTarget) + public record ShotSolution(boolean valid, Angle turretAngle, Distance distance, ShotMode mode, Pose2d targetPose, boolean hasVisionTarget, boolean inRange, boolean hubActive) { } @@ -69,7 +69,7 @@ public TurretDirector(Supplier swerveStateSupplier) _swerveStateSupplier = swerveStateSupplier; _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); _targetTagFilter = List.of(); - _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false); + _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false, false, false); _targetDistance = Meters.zero(); _targetPose = new Pose2d(); _limelightHasTarget = false; @@ -155,7 +155,7 @@ private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d logShotKinematics(swerveState.Pose, getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), distance, ShooterConstants.getShotTimeOfFlight(distance)); - return new ShotSolution(hubActive, rawSetpoint, distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null); + return new ShotSolution(true, rawSetpoint, distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null, true, hubActive); } private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueAlliance) @@ -178,7 +178,7 @@ private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueA logShotKinematics(swerveState.Pose, getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), distance, ShooterConstants.getShotTimeOfFlight(distance)); - return new ShotSolution(true, rawSetpoint, distance, ShotMode.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false); + return new ShotSolution(true, rawSetpoint, distance, ShotMode.Pass, buildTargetPose(swerveState.Pose, distance, rawSetpoint), false, true, true); } private ShotSolution calculateMovingTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) @@ -192,9 +192,10 @@ private ShotSolution calculateMovingTrack(SwerveDriveState swerveState, Translat logShotKinematics(movingTrackState.releasePose(), movingTrackState.lookaheadTurretOrigin(), movingTrackState.lookaheadDistance(), movingTrackState.timeOfFlight()); + var inRange = ShooterConstants.isMovingShotDistanceValid(movingTrackState.lookaheadDistance()); + return new ShotSolution( - hubActive && ShooterConstants.isMovingShotDistanceValid(movingTrackState.lookaheadDistance()), rawSetpoint, movingTrackState.lookaheadDistance(), ShotMode.Track, - buildTargetPose(movingTrackState.releasePose(), movingTrackState.lookaheadDistance(), rawSetpoint), bestTag != null + inRange, rawSetpoint, movingTrackState.lookaheadDistance(), ShotMode.Track, buildTargetPose(movingTrackState.releasePose(), movingTrackState.lookaheadDistance(), rawSetpoint), bestTag != null, inRange, hubActive ); } @@ -279,7 +280,7 @@ private Translation2d getTurretPointVelocityField(Pose2d robotPose, ChassisSpeed private ShotSolution createIdleSolution(Pose2d robotPose) { logShotKinematics(robotPose, getPointInField(robotPose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), Meters.zero(), Seconds.zero()); - return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, robotPose, false); + return new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, robotPose, false, false, false); } private void logShotKinematics(Pose2d releasePose, Translation2d lookaheadTurretOrigin, Distance distance, Time timeOfFlight) From e88c390ff7e486252138b28e5593a3505dc22b9b Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Mon, 16 Mar 2026 18:56:24 -0500 Subject: [PATCH 13/32] Refactor shooter and turret subsystems for improved control and performance --- src/main/java/frc/robot/Autos.java | 12 +-- src/main/java/frc/robot/Constants.java | 17 ++-- src/main/java/frc/robot/RobotContainer.java | 14 ++- .../frc/robot/subsystems/shooter/Rotor.java | 37 +++++--- .../frc/robot/subsystems/shooter/Shooter.java | 91 +++++++++---------- .../frc/robot/subsystems/shooter/Turret.java | 47 +++++++++- .../subsystems/shooter/TurretDirector.java | 14 ++- 7 files changed, 135 insertions(+), 97 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 1ea7861..72e70b4 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -21,20 +21,12 @@ public class Autos { private enum AutoMode { - DoNothing, - ShootOnly, - ShootWithDelay, - ShootThenDrive, - ShootWithDelayThenDrive + DoNothing, ShootOnly, ShootWithDelay, ShootThenDrive, ShootWithDelayThenDrive } private enum StartPosition { - LeftTrench(ChoreoVars.Poses.LeftTrench), - LeftBump(ChoreoVars.Poses.LeftBump), - HubStart(ChoreoVars.Poses.HubStart), - RightBump(ChoreoVars.Poses.RightBump), - RightTrench(ChoreoVars.Poses.RightTrench); + LeftTrench(ChoreoVars.Poses.LeftTrench), LeftBump(ChoreoVars.Poses.LeftBump), HubStart(ChoreoVars.Poses.HubStart), RightBump(ChoreoVars.Poses.RightBump), RightTrench(ChoreoVars.Poses.RightTrench); public final Pose2d pose; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 775dae7..af767c0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -112,7 +112,7 @@ public static class IntakeConstants public static final Voltage INTAKE_VOLTS = Volts.of(9.0); public static final Voltage REVERSE_VOLTS = Volts.of(-9.0); public static final Current ROLLER_CURRENT_LIMIT = Amps.of(80); - public static final Current EXTENSION_CURRENT_LIMIT = Amps.of(40); + public static final Current EXTENSION_CURRENT_LIMIT = Amps.of(60); public static final int CAMERA_DEVICE_INDEX = 0; public static final String CAMERA_NAME = "IntakeCam"; public static final int CAMERA_WIDTH = 320; @@ -148,11 +148,11 @@ public static class ShooterConstants public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(60); public static final AngularVelocity MANUAL_SHOOT_RPM = RPM.of(3500.0); public static final AngularVelocity PASS_FLYWHEEL_VELOCITY = RPM.of(3000.0); // TODO: Tune - private static final AngularVelocity SMART_SHOOT_RPM_BIAS = RPM.of(150.0); + private static final AngularVelocity SMART_SHOOT_RPM_BIAS = RPM.of(-150.0); // Turret public static final Current TURRET_CURRENT_LIMIT = Amps.of(40.0); - public static final double TURRET_KP = 0.04; // TODO: Tune (onboard TalonFX PID) + public static final double TURRET_KP = 0.12; // TODO: Tune (onboard TalonFX PID) public static final double TURRET_KI = 0.0; public static final double TURRET_KD = 0.0115; public static final Dimensionless TURRET_GEAR_RATIO = Value.of(13.0 / 1.0); // 13 : 1 @@ -161,9 +161,12 @@ public static class ShooterConstants public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); // degrees (full 360 rotation) public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); // degrees public static final boolean TURRET_SENSOR_INVERTED = true; + public static final Angle TURRET_SENSOR_ZERO_OFFSET = Degrees.of(-4.67); public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); // Forward-facing when no target - public static final Angle TURRET_TOLERANCE = Degrees.of(2.0); // degrees - public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(2); + public static final Angle TURRET_TOLERANCE = Degrees.of(3.5); // degrees + public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(5.0); // degrees + public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(1.0); // degrees + public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(3.5); public static final String LIMELIGHT_NAME = "limelight-shooter"; public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); @@ -176,8 +179,8 @@ public static class ShooterConstants public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.zero(), Inches.of(1.0)); // Placeholder delta used to preserve today's static shot geometry until the - // measured release transform replaces it + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not + // lateral public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); @Deprecated diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18900c8..9247937 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -99,17 +99,15 @@ private void configureBindings() _operator.leftTrigger().whileTrue(_intake.runRollersForward()); _operator.leftBumper().whileTrue(_intake.runRollersReverse()); _operator.rightTrigger().whileTrue(_intake.jiggle()); - _operator.start().and(_operator.back()).onTrue(_shooter.setManualMode(true)); - _operator.start().and(_operator.back().negate()).onTrue(_shooter.setManualMode(false)); - _operator.y().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(MANUAL_FLYWHEEL_START_RPM)).onlyIf(_shooter::inManualMode)); - _operator.x().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM - MANUAL_FLYWHEEL_STEP_RPM)).onlyIf(_shooter::inManualMode)); - _operator.b().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM + MANUAL_FLYWHEEL_STEP_RPM)).onlyIf(_shooter::inManualMode)); - _operator.a().onTrue(Commands.runOnce(_shooter::stopManualFlywheel).onlyIf(_shooter::inManualMode)); + _operator.y().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(MANUAL_FLYWHEEL_START_RPM))); + _operator.x().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM - MANUAL_FLYWHEEL_STEP_RPM))); + _operator.b().onTrue(Commands.runOnce(() -> setManualFlywheelRPM(_manualFlywheelRPM + MANUAL_FLYWHEEL_STEP_RPM))); + _operator.a().onTrue(Commands.runOnce(_shooter::stopManualFlywheel)); _operator.rightBumper().whileTrue(_shooter.runManualFeeder()); _operator.povDown().onTrue(_intake.getRetractCmd()); _operator.povUp().onTrue(_intake.getExtendCmd()); - _operator.povLeft().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(-MANUAL_TURRET_STEP_DEG)).onlyIf(_shooter::inManualMode)); - _operator.povRight().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(MANUAL_TURRET_STEP_DEG)).onlyIf(_shooter::inManualMode)); + _operator.povLeft().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(-MANUAL_TURRET_STEP_DEG))); + _operator.povRight().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(MANUAL_TURRET_STEP_DEG))); } public Command getAutonomousCommand() diff --git a/src/main/java/frc/robot/subsystems/shooter/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/Rotor.java index 01b7f1f..e7a25bb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Rotor.java +++ b/src/main/java/frc/robot/subsystems/shooter/Rotor.java @@ -3,12 +3,12 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Volts; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.units.measure.Voltage; @@ -20,25 +20,29 @@ @Logged public class Rotor { - private final SparkFlex _rotorMotor; + private final TalonFX _rotorMotor; @Logged - private Voltage _rotorMotorVoltage = Volts.zero(); + private Voltage _rotorMotorVoltage = Volts.zero(); @Logged - private boolean _enabled = false; + private boolean _enabled = false; public Rotor() { - _rotorMotor = new SparkFlex(CANConstants.ROTOR_MOTOR, MotorType.kBrushless); + _rotorMotor = new TalonFX(CANConstants.ROTOR_MOTOR); - var config = new SparkFlexConfig(); - config.inverted(false).idleMode(IdleMode.kBrake).smartCurrentLimit((int)ShooterConstants.ROTOR_CURRENT_LIMIT.in(Amps)).voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts)); + var currentConfig = new CurrentLimitsConfigs(); + currentConfig.StatorCurrentLimit = ShooterConstants.ROTOR_CURRENT_LIMIT.in(Amps); + currentConfig.StatorCurrentLimitEnable = true; - _rotorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + var outputConfig = new MotorOutputConfigs(); + outputConfig.NeutralMode = NeutralModeValue.Brake; + outputConfig.Inverted = InvertedValue.Clockwise_Positive; + _rotorMotor.getConfigurator().apply(new TalonFXConfiguration().withCurrentLimits(currentConfig).withMotorOutput(outputConfig)); } public void periodic() { - _rotorMotorVoltage = Volts.of(_rotorMotor.getAppliedOutput() * _rotorMotor.getBusVoltage()); + _rotorMotorVoltage = _rotorMotor.getMotorVoltage().getValue(); } public void set(boolean on) @@ -47,4 +51,9 @@ public void set(boolean on) var targetVoltage = on ? ShooterConstants.ROTOR_VOLTAGE : Volts.zero(); _rotorMotor.setVoltage(targetVoltage.in(Volts)); } + + public void stop() + { + set(false); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ca0be8c..e782ebb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -93,7 +93,7 @@ public Shooter(Supplier swerveStateSupplier) _feedReady = false; _feeder.set(false); - _rotor.set(false); + // _rotor.set(false); _flywheel.stop(); _turret.clearTargetAngle(); _turret.setDisabled(false); @@ -124,44 +124,19 @@ public Command pass() return startEnd(() -> startPass(true), this::stopShooter); } - public Command setManualMode(boolean manual) - { - return runOnce(() -> - { - _state = manual ? ShooterState.Manual : ShooterState.Idle; - _autoShootEnabled = false; - _requestedShotMode = ShotMode.Idle; - _currentShotSolution = createIdleSolution(); - _feeder.set(false); - _rotor.set(false); - _turret.clearTargetAngle(); - _turret.setDisabled(manual); - - if (manual) - { - _flywheel.stop(); - } - }); - } - - public boolean inManualMode() - { - return _state == ShooterState.Manual; - } - public void setManualFlywheel(double rpm) { - if (inManualMode()) - { - _flywheel.setVelocity(RPM.of(Math.max(0.0, rpm))); - } + beginManualControl(false); + _flywheel.setVelocity(RPM.of(Math.max(0.0, rpm))); } public void stopManualFlywheel() { - if (inManualMode()) + _flywheel.stop(); + + if (_state == ShooterState.Manual) { - _flywheel.stop(); + _state = ShooterState.Idle; } } @@ -202,7 +177,7 @@ public Command setFlywheelVelocity(AngularVelocity velocity) { return runOnce(() -> { - _state = ShooterState.Manual; + beginManualControl(false); _flywheel.setVelocity(velocity); }); } @@ -211,13 +186,14 @@ public Command runManualFeeder() { return startEnd(() -> { + beginManualControl(false); _feeder.set(true); - _rotor.set(true); + // _rotor.set(true); }, () -> { _feeder.set(false); - _rotor.set(false); - }).onlyIf(this::inManualMode); + // _rotor.set(false); + }); } public Command runFeeder() @@ -226,11 +202,11 @@ public Command runFeeder() { _state = ShooterState.Manual; _feeder.set(true); - _rotor.set(true); + // _rotor.set(true); }, () -> { _feeder.set(false); - _rotor.set(false); + // _rotor.set(false); _flywheel.stop(); _state = ShooterState.Idle; }); @@ -253,11 +229,9 @@ public Command trackOnly() public void bumpManualTurretAngle(double deltaDeg) { - if (inManualMode()) - { - _turret.setDisabled(false); - _turret.bumpManualAngle(Degrees.of(deltaDeg)); - } + beginManualControl(true); + _turret.setDisabled(false); + _turret.bumpManualAngle(Degrees.of(deltaDeg)); } public double getManualTurretAngleDeg() @@ -273,7 +247,7 @@ public void periodic() case Preparing: case Ready: _feeder.set(false); - _rotor.set(false); + // _rotor.set(false); runRequestedShot(true); if (isReadyToFeed()) @@ -298,12 +272,12 @@ public void periodic() if (isReadyToFeed()) { _feeder.set(true); - _rotor.set(true); + // _rotor.set(true); } else { _feeder.set(false); - _rotor.set(false); + // _rotor.set(false); _state = ShooterState.Preparing; } break; @@ -317,7 +291,7 @@ public void periodic() case TrackingOnly: _feeder.set(false); - _rotor.set(false); + // _rotor.set(false); _flywheel.stop(); _requestedShotMode = ShotMode.Track; runRequestedShot(false); @@ -327,7 +301,7 @@ public void periodic() default: _flywheel.stop(); _feeder.set(false); - _rotor.set(false); + // _rotor.set(false); clearShotRequest(); break; } @@ -363,6 +337,23 @@ private void beginShootingSequence(ShotMode shotMode, boolean autoShoot) _state = ShooterState.Preparing; } + private void beginManualControl(boolean clearTurretTarget) + { + _state = ShooterState.Manual; + _autoShootEnabled = false; + _requestedShotMode = ShotMode.Idle; + _currentShotSolution = createIdleSolution(); + clearReadinessGate(); + _feeder.set(false); + // _rotor.set(false); + _turret.setDisabled(false); + + if (clearTurretTarget) + { + _turret.clearTargetAngle(); + } + } + public void stopShooter() { _state = ShooterState.Idle; @@ -406,7 +397,7 @@ private void clearShotRequest() _currentShotSolution = createIdleSolution(); clearReadinessGate(); _turret.clearTargetAngle(); - _rotor.set(false); + // _rotor.set(false); } private ShotSolution createIdleSolution() @@ -461,7 +452,7 @@ private boolean shouldUseMovingShotMath() return false; } - if (_state == ShooterState.TrackingOnly) + if (_state == ShooterState.TrackingOnly || _autoShootEnabled) { return true; } diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 77382f8..93abc54 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -36,11 +36,15 @@ enum ControlMode private final AnalogPotentiometer _turretSensor; private final PIDController _pidController; private ControlMode _controlMode; + @Logged private Angle _manualAngleSetpoint; + @Logged private Angle _targetAngleSetpoint; @Logged private Angle _turretAngle; @Logged + private Angle _rawTurretAngle; + @Logged private Angle _turretSetpoint; @Logged private boolean _hasSetpoint; @@ -48,6 +52,8 @@ enum ControlMode private Voltage _motorVoltage; @Logged private boolean _disabled; + @Logged + private boolean _linedUp; public Turret() { @@ -71,6 +77,7 @@ public Turret() _hasSetpoint = false; _motorVoltage = Volts.zero(); _disabled = false; + _linedUp = false; var currentConfig = new CurrentLimitsConfigs(); currentConfig.StatorCurrentLimit = ShooterConstants.TURRET_CURRENT_LIMIT.in(Amps); @@ -88,8 +95,9 @@ public Turret() public void periodic() { - _turretAngle = Degrees.of(_turretSensor.get()); - _motorVoltage = _turretMotor.getMotorVoltage().getValue(); + _rawTurretAngle = Degrees.of(_turretSensor.get()); + _turretAngle = _rawTurretAngle.plus(ShooterConstants.TURRET_SENSOR_ZERO_OFFSET); + _motorVoltage = _turretMotor.getMotorVoltage().getValue(); var motorOutput = Volts.zero(); @@ -112,9 +120,16 @@ public void periodic() break; } + updateLinedUpState(); + if (_hasSetpoint && !_disabled) { motorOutput = Volts.of(_pidController.calculate(_turretAngle.in(Degrees), _turretSetpoint.in(Degrees))); + + if (_pidController.atSetpoint()) + { + motorOutput = Volts.zero(); + } } motorOutput = applySoftLimit(motorOutput); @@ -127,8 +142,14 @@ public void simulationPeriodic() public void setTargetAngle(Angle angle) { - _targetAngleSetpoint = angle; - _controlMode = ControlMode.TargetAngle; + var targetDeltaDegrees = Math.abs(MathUtil.inputModulus(angle.minus(_targetAngleSetpoint).in(Degrees), -180.0, 180.0)); + + if (_controlMode != ControlMode.TargetAngle || targetDeltaDegrees >= ShooterConstants.TURRET_TARGET_SETPOINT_DEADBAND.in(Degrees)) + { + _targetAngleSetpoint = angle; + } + + _controlMode = ControlMode.TargetAngle; } public void clearTargetAngle() @@ -155,7 +176,7 @@ public Angle getManualAngle() public boolean isLinedUp() { - return _hasSetpoint && _pidController.atSetpoint(); + return _linedUp; } public void setDisabled(boolean disabled) @@ -221,4 +242,20 @@ private Voltage applySoftLimit(Voltage requestedVoltage) return requestedVoltage; } + + private void updateLinedUpState() + { + if (!_hasSetpoint || _disabled) + { + _linedUp = false; + return; + } + + var angleErrorDegrees = Math.abs(_turretSetpoint.minus(_turretAngle).in(Degrees)); + var acquireTolerance = ShooterConstants.TURRET_TOLERANCE.in(Degrees); + var holdTolerance = ShooterConstants.TURRET_LINED_UP_HOLD_TOLERANCE.in(Degrees); + var allowedErrorDegrees = _linedUp ? holdTolerance : acquireTolerance; + + _linedUp = angleErrorDegrees <= allowedErrorDegrees; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index 6aa09e8..2c40968 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -58,6 +58,8 @@ private record MovingTrackState(Pose2d releasePose, Translation2d lookaheadTurre @Logged private boolean _targetValid; @Logged + private Angle _desiredTurretAngle; + @Logged private Pose2d _predictedReleasePose; @Logged private Pose2d _lookaheadTurretOriginPose; @@ -75,6 +77,7 @@ public TurretDirector(Supplier swerveStateSupplier) _limelightHasTarget = false; _hubActive = false; _targetValid = false; + _desiredTurretAngle = ShooterConstants.TURRET_HOME_ANGLE; _predictedReleasePose = new Pose2d(); _lookaheadTurretOriginPose = new Pose2d(); _shotTimeOfFlight = Seconds.zero(); @@ -112,6 +115,7 @@ public ShotSolution calculate(ShotMode shotMode, boolean useMovingShotMath) _targetPose = solution.targetPose(); _limelightHasTarget = solution.hasVisionTarget(); _targetValid = solution.valid(); + _desiredTurretAngle = solution.turretAngle(); return solution; } @@ -227,9 +231,13 @@ private Angle getTurretSetpointForFieldTarget(Pose2d robotPose, Angle fieldTarge private Angle getLegacyReleaseLateralCorrection(Angle rawSetpoint, Distance distance) { - var safeDistanceM = Math.max(distance.in(Meters), 0.5); - var releaseLateralOffset = ShooterConstants.TURRET_PIVOT_TO_RELEASE.getY(); - var lateralErrorM = releaseLateralOffset * Math.sin(Math.toRadians(rawSetpoint.in(Degrees))); + var safeDistanceM = Math.max(distance.in(Meters), 0.5); + var shotDirectionDeg = rawSetpoint.plus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD).in(Degrees); + var shotDirectionRad = Math.toRadians(shotDirectionDeg); + var releaseOffset = ShooterConstants.TURRET_PIVOT_TO_RELEASE; + var perpendicularX = -Math.sin(shotDirectionRad); + var perpendicularY = Math.cos(shotDirectionRad); + var lateralErrorM = releaseOffset.getX() * perpendicularX + releaseOffset.getY() * perpendicularY; return Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM))); } From f79c17a524aa120cce1802d903b43388156019ba Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Wed, 18 Mar 2026 21:23:59 -0500 Subject: [PATCH 14/32] Refactor RobotContainer and Shooter subsystems --- src/main/java/frc/robot/Constants.java | 122 ++++----- src/main/java/frc/robot/RobotContainer.java | 35 +-- .../frc/robot/subsystems/shooter/Shooter.java | 75 +++--- .../frc/robot/subsystems/shooter/Turret.java | 121 ++++++--- .../subsystems/shooter/TurretDirector.java | 234 +++++++++++++----- 5 files changed, 374 insertions(+), 213 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index af767c0..d1c91b9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -121,8 +121,8 @@ public static class IntakeConstants public static final Per EXTENSION_CONVERSION_FACTOR = Inches.of(12).div(Rotations.of(6)); public static final Distance EXTENSION_MAX_POSITION = Inches.of(12.0); public static final Distance EXTENSION_MIN_POSITION = Inches.of(0); - public static final Voltage EXTEND_VOLTS = Volts.of(2.04); - public static final Voltage RETRACT_VOLTS = Volts.of(-2.04); + public static final Voltage EXTEND_VOLTS = Volts.of(1.5); + public static final Voltage RETRACT_VOLTS = Volts.of(-2.5); } public static class GeneralConstants @@ -148,60 +148,70 @@ public static class ShooterConstants public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(60); public static final AngularVelocity MANUAL_SHOOT_RPM = RPM.of(3500.0); public static final AngularVelocity PASS_FLYWHEEL_VELOCITY = RPM.of(3000.0); // TODO: Tune - private static final AngularVelocity SMART_SHOOT_RPM_BIAS = RPM.of(-150.0); + private static final AngularVelocity SMART_SHOOT_RPM_BIAS = RPM.of(0.0); // Turret - public static final Current TURRET_CURRENT_LIMIT = Amps.of(40.0); - public static final double TURRET_KP = 0.12; // TODO: Tune (onboard TalonFX PID) - public static final double TURRET_KI = 0.0; - public static final double TURRET_KD = 0.0115; - public static final Dimensionless TURRET_GEAR_RATIO = Value.of(13.0 / 1.0); // 13 : 1 - public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); // degrees (full 360 rotation) - public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); // degrees - public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); // degrees (full 360 rotation) - public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); // degrees - public static final boolean TURRET_SENSOR_INVERTED = true; - public static final Angle TURRET_SENSOR_ZERO_OFFSET = Degrees.of(-4.67); - public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); // Forward-facing when no target - public static final Angle TURRET_TOLERANCE = Degrees.of(3.5); // degrees - public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(5.0); // degrees - public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(1.0); // degrees - public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(3.5); - public static final String LIMELIGHT_NAME = "limelight-shooter"; - public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice - public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); - public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); - public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); - public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); - public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); - public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees - // left of robot forward - public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); - public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); - public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not - // lateral - public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); - public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); - @Deprecated - public static final Distance SHOOTER_LATERAL_OFFSET = Inches.of(1.0); - @Deprecated - public static final Translation2d TURRET_POSITION = ROBOT_TO_TURRET_PIVOT; - @Deprecated - public static final Translation2d TURRET_CAMERA_POSITION = ROBOT_TO_TURRET_CAMERA; - public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" - public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); - public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); - public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); - public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); - public static final int SWM_LOOKAHEAD_ITERATIONS = 8; - public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); - public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); - public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.0); - public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(20.0); - public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(125.0); - private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; - private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; + public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0); + public static final double TURRET_KP = 0.19; // TODO: Tune (onboard TalonFX PID) + public static final double TURRET_KI = 0.0; + public static final double TURRET_KD = 0.0115; + public static final Voltage TURRET_STATIC_FF = Volts.of(0.22); + public static final Angle TURRET_STATIC_FF_ERROR_DEADBAND = Degrees.of(0.75); + public static final Voltage TURRET_MAX_OUTPUT_STEP_PER_LOOP = Volts.of(3.0); + public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); // degrees (full 360 rotation) + public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); // degrees + public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); // degrees (full 360 rotation) + public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); // degrees + public static final boolean TURRET_SENSOR_INVERTED = true; + public static final Angle TURRET_SENSOR_ZERO_OFFSET = Degrees.of(-5); + public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); + public static final Angle TURRET_TOLERANCE = Degrees.of(1.25); // degrees + public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(2.0); // degrees + public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(0.1); // degrees + public static final Angle TURRET_MAX_SETPOINT_STEP_PER_LOOP = Degrees.of(4.0); + public static final Angle TURRET_TRACK_COMMAND_DEADBAND = Degrees.of(0.5); + public static final Angle TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP = Degrees.of(4.0); + public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(1.0); + public static final boolean TURRET_USE_VISION_TX_TRIM = false; + public static final Time TURRET_VISION_TX_FILTER_WINDOW = Milliseconds.of(100.0); + public static final Angle TURRET_VISION_TX_MAX_STEP_PER_LOOP = Degrees.of(0.4); + public static final Angle TURRET_TRACK_BIAS = Degrees.zero(); // TODO: Tune static shot bias from log data + public static final boolean TURRET_USE_LINEAR_DRIFT_COMPENSATION = true; + public static final Distance TURRET_DRIFT_LATERAL_BIAS = Inches.of(11.060660171779821); + public static final Distance TURRET_DRIFT_LATERAL_SINE_AMPLITUDE = Inches.of(7.1890180494515015); + public static final Angle TURRET_DRIFT_LATERAL_PHASE_OFFSET = Degrees.of(-20.866427056070258); + public static final Angle TURRET_LINEAR_DRIFT_MAX_CORRECTION = Degrees.of(15.0); + public static final double TURRET_LINEAR_DRIFT_CORRECTION_SIGN = -1.0; + public static final String LIMELIGHT_NAME = "limelight-shooter"; + public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice + public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); + public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); + public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); + public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); + public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); + public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees + // left of robot forward + public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); + public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); + public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not + // lateral + public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); + public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); + public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" + public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); + public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); + public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); + public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); + public static final int SWM_LOOKAHEAD_ITERATIONS = 8; + public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); + public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); + public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.5); + public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(45.0); + public static final Angle SWM_FEED_TURRET_TOLERANCE = Degrees.of(5.0); + public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(60.0); + private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; + private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; // @formatter:off private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries @@ -210,7 +220,7 @@ public static class ShooterConstants Map.entry(75.5, 2950.0), Map.entry(101.0, 3400.0), Map.entry(113.0, 3850.0), - Map.entry(130.0, 4050.0), + Map.entry(130.0, 3900.0), Map.entry(151.0, 4600.0), Map.entry(183.0, 5100.0), Map.entry(204.0, 5850.0) @@ -232,7 +242,7 @@ public static class ShooterConstants public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60); public static final Voltage FEEDER_VOLTAGE = Volts.of(6); public static final Current ROTOR_CURRENT_LIMIT = Amps.of(60); - public static final Voltage ROTOR_VOLTAGE = Volts.of(6); + public static final Voltage ROTOR_VOLTAGE = Volts.of(3); // TODO: Tune these values with testing! public static AngularVelocity getFlywheelSpeedForDistance(Distance distance) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9247937..982e063 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,22 +29,19 @@ @Logged public class RobotContainer { - private static final double MANUAL_FLYWHEEL_START_RPM = 3500.0; - private static final double MANUAL_FLYWHEEL_STEP_RPM = 50.0; - private static final double MANUAL_TURRET_STEP_DEG = 2.0; - - /* Setting up bindings for necessary control of the swerve drive platform */ - private final SwerveRequest.FieldCentric _fieldCentric = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final SwerveRequest.RobotCentric _robotCentric = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final CommandJoystick _driver = new CommandJoystick(0); - private final CommandXboxController _operator = new CommandXboxController(1); - private final Drive _drive = TunerConstants.createDrivetrain(); - private final Intake _intake = new Intake(); - private final Shooter _shooter = new Shooter(_drive::getState); + private static final double MANUAL_FLYWHEEL_START_RPM = 3500.0; + private static final double MANUAL_FLYWHEEL_STEP_RPM = 50.0; + private final SwerveRequest.FieldCentric _fieldCentric = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.RobotCentric _robotCentric = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final CommandJoystick _driver = new CommandJoystick(0); + private final CommandXboxController _operator = new CommandXboxController(1); + private final Drive _drive = TunerConstants.createDrivetrain(); + private final Intake _intake = new Intake(); + private final Shooter _shooter = new Shooter(_drive::getState); @NotLogged - private final Autos _autos = new Autos(_drive, _shooter); - private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; - private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; + private final Autos _autos = new Autos(_drive, _shooter); + private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; + private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; public RobotContainer() { @@ -77,11 +74,6 @@ private void setManualFlywheelRPM(double rpm) _shooter.setManualFlywheel(_manualFlywheelRPM); } - private void bumpManualTurretAngle(double deltaDeg) - { - _shooter.bumpManualTurretAngle(deltaDeg); - } - private void configureBindings() { _drive.setDefaultCommand(_drive.applyRequest(this::getFieldCentricRequest)); @@ -95,6 +87,7 @@ private void configureBindings() _driver.button(5).whileTrue(_shooter.pass()); _driver.button(6).whileTrue(_shooter.trackOnly()); _driver.button(7).onTrue(_drive.runOnce(_drive::seedFieldCentric)); + _driver.button(8).onTrue(_shooter.homeTurret()); _operator.leftTrigger().whileTrue(_intake.runRollersForward()); _operator.leftBumper().whileTrue(_intake.runRollersReverse()); @@ -106,8 +99,6 @@ private void configureBindings() _operator.rightBumper().whileTrue(_shooter.runManualFeeder()); _operator.povDown().onTrue(_intake.getRetractCmd()); _operator.povUp().onTrue(_intake.getExtendCmd()); - _operator.povLeft().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(-MANUAL_TURRET_STEP_DEG))); - _operator.povRight().onTrue(Commands.runOnce(() -> bumpManualTurretAngle(MANUAL_TURRET_STEP_DEG))); } public Command getAutonomousCommand() diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index e782ebb..814bf75 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -14,6 +14,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -93,7 +94,7 @@ public Shooter(Supplier swerveStateSupplier) _feedReady = false; _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); _flywheel.stop(); _turret.clearTargetAngle(); _turret.setDisabled(false); @@ -188,11 +189,11 @@ public Command runManualFeeder() { beginManualControl(false); _feeder.set(true); - // _rotor.set(true); + _rotor.set(true); }, () -> { _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); }); } @@ -202,11 +203,11 @@ public Command runFeeder() { _state = ShooterState.Manual; _feeder.set(true); - // _rotor.set(true); + _rotor.set(true); }, () -> { _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); _flywheel.stop(); _state = ShooterState.Idle; }); @@ -217,6 +218,11 @@ public Command stop() return runOnce(this::stopShooter); } + public Command homeTurret() + { + return runOnce(() -> setManualTurretAngle(ShooterConstants.TURRET_HOME_ANGLE)); + } + public Command trackOnly() { return startEnd(() -> @@ -229,14 +235,7 @@ public Command trackOnly() public void bumpManualTurretAngle(double deltaDeg) { - beginManualControl(true); - _turret.setDisabled(false); - _turret.bumpManualAngle(Degrees.of(deltaDeg)); - } - - public double getManualTurretAngleDeg() - { - return _turret.getManualAngle().in(Degrees); + setManualTurretAngle(_turret.getManualAngle().plus(Degrees.of(deltaDeg))); } @Override @@ -247,7 +246,7 @@ public void periodic() case Preparing: case Ready: _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); runRequestedShot(true); if (isReadyToFeed()) @@ -272,12 +271,12 @@ public void periodic() if (isReadyToFeed()) { _feeder.set(true); - // _rotor.set(true); + _rotor.set(true); } else { _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); _state = ShooterState.Preparing; } break; @@ -291,7 +290,7 @@ public void periodic() case TrackingOnly: _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); _flywheel.stop(); _requestedShotMode = ShotMode.Track; runRequestedShot(false); @@ -301,7 +300,7 @@ public void periodic() default: _flywheel.stop(); _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); clearShotRequest(); break; } @@ -345,7 +344,7 @@ private void beginManualControl(boolean clearTurretTarget) _currentShotSolution = createIdleSolution(); clearReadinessGate(); _feeder.set(false); - // _rotor.set(false); + _rotor.set(false); _turret.setDisabled(false); if (clearTurretTarget) @@ -354,6 +353,13 @@ private void beginManualControl(boolean clearTurretTarget) } } + private void setManualTurretAngle(Angle angle) + { + beginManualControl(true); + _turret.setDisabled(false); + _turret.setManualAngle(angle); + } + public void stopShooter() { _state = ShooterState.Idle; @@ -396,8 +402,7 @@ private void clearShotRequest() _requestedShotMode = ShotMode.Idle; _currentShotSolution = createIdleSolution(); clearReadinessGate(); - _turret.clearTargetAngle(); - // _rotor.set(false); + _rotor.set(false); } private ShotSolution createIdleSolution() @@ -408,7 +413,7 @@ private ShotSolution createIdleSolution() private boolean isReadyToFeed() { _solutionReady = _currentShotSolution.valid(); - _turretReady = _turret.isLinedUp(); + _turretReady = isTurretReadyForFeed(); _flywheelReady = _flywheel.atSpeed(); _hubReady = hasActiveHubForCurrentShot(); _inRangeReady = hasValidDistanceForCurrentShot(); @@ -425,6 +430,18 @@ private boolean isReadyToFeed() return _feedReady; } + private boolean isTurretReadyForFeed() + { + if (!_usingMovingShotMath || _currentShotSolution.mode() != ShotMode.Track) + { + return _turret.isLinedUp(); + } + + var movingToleranceDeg = ShooterConstants.SWM_FEED_TURRET_TOLERANCE.in(Degrees); + var turretErrorDeg = Math.abs(_turret.getTargetAngleError().in(Degrees)); + return turretErrorDeg <= movingToleranceDeg; + } + private boolean hasActiveHubForCurrentShot() { return switch (_currentShotSolution.mode()) @@ -447,17 +464,9 @@ private boolean hasValidDistanceForCurrentShot() private boolean shouldUseMovingShotMath() { - if (_requestedShotMode != ShotMode.Track) - { - return false; - } - - if (_state == ShooterState.TrackingOnly || _autoShootEnabled) - { - return true; - } - - return _robotTranslationSpeedMetersPerSecond >= ShooterConstants.SWM_ENABLE_TRANSLATIONAL_SPEED.in(MetersPerSecond) || Math.abs(_robotAngularSpeedRadiansPerSecond) >= ShooterConstants.SWM_ENABLE_ANGULAR_SPEED.in(RadiansPerSecond); + // Keep track-mode behavior consistent between `trackOnly` and `shoot`: + // both should run the moving-shot calculator path. + return _requestedShotMode == ShotMode.Track; } private boolean isWithinMovingFeedRolloutEnvelope() diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 93abc54..2a4bcb0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -47,9 +47,14 @@ enum ControlMode @Logged private Angle _turretSetpoint; @Logged + private Angle _commandedTargetAngle; + @Logged + private Angle _targetAngleError; + @Logged private boolean _hasSetpoint; @Logged private Voltage _motorVoltage; + private Voltage _lastCommandedMotorVoltage; @Logged private boolean _disabled; @Logged @@ -66,18 +71,21 @@ public Turret() sensorOffset = sensorOffset.unaryMinus(); } - _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); - _turretSensor = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); - _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); - _controlMode = ControlMode.Idle; - _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _turretAngle = Degrees.zero(); - _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _hasSetpoint = false; - _motorVoltage = Volts.zero(); - _disabled = false; - _linedUp = false; + _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); + _turretSensor = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); + _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); + _controlMode = ControlMode.Idle; + _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _turretAngle = Degrees.zero(); + _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _commandedTargetAngle = ShooterConstants.TURRET_HOME_ANGLE; + _targetAngleError = Degrees.zero(); + _hasSetpoint = false; + _motorVoltage = Volts.zero(); + _lastCommandedMotorVoltage = Volts.zero(); + _disabled = false; + _linedUp = false; var currentConfig = new CurrentLimitsConfigs(); currentConfig.StatorCurrentLimit = ShooterConstants.TURRET_CURRENT_LIMIT.in(Amps); @@ -90,7 +98,6 @@ public Turret() _turretMotor.getConfigurator().apply(new TalonFXConfiguration().withCurrentLimits(currentConfig).withMotorOutput(outputConfig)); _pidController.setTolerance(ShooterConstants.TURRET_TOLERANCE.in(Degrees)); - _pidController.setSetpoint(ShooterConstants.TURRET_HOME_ANGLE.in(Degrees)); } public void periodic() @@ -120,20 +127,34 @@ public void periodic() break; } + if (_hasSetpoint) + { + _turretSetpoint = limitSetpointStep(_commandedTargetAngle, _turretSetpoint); + } + updateLinedUpState(); + _commandedTargetAngle = _turretSetpoint; + _targetAngleError = _commandedTargetAngle.minus(_turretAngle); + if (_hasSetpoint && !_disabled) { - motorOutput = Volts.of(_pidController.calculate(_turretAngle.in(Degrees), _turretSetpoint.in(Degrees))); + var errorDegrees = _turretSetpoint.minus(_turretAngle).in(Degrees); + var outputVolts = _pidController.calculate(_turretAngle.in(Degrees), _turretSetpoint.in(Degrees)); + var staticFFVolts = ShooterConstants.TURRET_STATIC_FF.in(Volts); - if (_pidController.atSetpoint()) + if (!MathUtil.isNear(0.0, errorDegrees, ShooterConstants.TURRET_STATIC_FF_ERROR_DEADBAND.in(Degrees))) { - motorOutput = Volts.zero(); + outputVolts += Math.copySign(staticFFVolts, errorDegrees); } + + motorOutput = Volts.of(outputVolts); } + motorOutput = Volts.of(limitOutputStep(motorOutput.in(Volts))); motorOutput = applySoftLimit(motorOutput); _turretMotor.setVoltage(motorOutput.in(Volts)); + _lastCommandedMotorVoltage = motorOutput; } public void simulationPeriodic() @@ -179,6 +200,11 @@ public boolean isLinedUp() return _linedUp; } + public Angle getTargetAngleError() + { + return _targetAngleError; + } + public void setDisabled(boolean disabled) { _disabled = disabled; @@ -191,41 +217,62 @@ private Angle selectLegalSetpoint(Angle requestedAngle) static Angle chooseNearestLegalAngle(Angle currentAngle, Angle requestedAngle) { - var currentDegrees = currentAngle.in(Degrees); - var requestedDegrees = requestedAngle.in(Degrees); - var minDegrees = ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Degrees); - var maxDegrees = ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Degrees); - var wrapCenter = (int)Math.round((currentDegrees - requestedDegrees) / 360.0); - var bestAngle = MathUtil.clamp(requestedDegrees, minDegrees, maxDegrees); - var bestTravel = Double.POSITIVE_INFINITY; - var bestClampAmount = Double.POSITIVE_INFINITY; - var bestWrapDistance = Integer.MAX_VALUE; + var currentDegrees = currentAngle.in(Degrees); + var requestedDegrees = requestedAngle.in(Degrees); + var minDegrees = ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Degrees); + var maxDegrees = ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Degrees); + var wrapCenter = (int)Math.round((currentDegrees - requestedDegrees) / 360.0); + var bestTravel = Double.POSITIVE_INFINITY; + var bestWrapDistance = Integer.MAX_VALUE; + Double bestLegalAngle = null; for (int wrapOffset = -kCandidateWrapCount; wrapOffset <= kCandidateWrapCount; wrapOffset++) { var wrapIndex = wrapCenter + wrapOffset; var wrappedDegrees = requestedDegrees + 360.0 * wrapIndex; - var legalDegrees = MathUtil.clamp(wrappedDegrees, minDegrees, maxDegrees); - var travelDegrees = Math.abs(legalDegrees - currentDegrees); - var clampAmount = Math.abs(legalDegrees - wrappedDegrees); - var wrapDistance = Math.abs(wrapIndex); + if (wrappedDegrees < minDegrees || wrappedDegrees > maxDegrees) + { + continue; + } + + var travelDegrees = Math.abs(wrappedDegrees - currentDegrees); + var wrapDistance = Math.abs(wrapIndex); - // Inspired by Team 5000's TurretCalculator and 6328's unwrap-style helpers: - // choose a legal branch relative to the current azimuth instead of moduloing - // first, so requests near +/- soft limits stay on the local side. - var isBetterCandidate = travelDegrees < bestTravel || MathUtil.isNear(travelDegrees, bestTravel, kComparisonToleranceDeg) - && (clampAmount < bestClampAmount || MathUtil.isNear(clampAmount, bestClampAmount, kComparisonToleranceDeg) && (wrapDistance < bestWrapDistance || wrapDistance == bestWrapDistance && legalDegrees > bestAngle)); + var isBetterCandidate = travelDegrees < bestTravel + || MathUtil.isNear(travelDegrees, bestTravel, kComparisonToleranceDeg) && (wrapDistance < bestWrapDistance || wrapDistance == bestWrapDistance && (bestLegalAngle == null || wrappedDegrees > bestLegalAngle)); if (isBetterCandidate) { - bestAngle = legalDegrees; + bestLegalAngle = wrappedDegrees; bestTravel = travelDegrees; - bestClampAmount = clampAmount; bestWrapDistance = wrapDistance; } } - return Degrees.of(bestAngle); + if (bestLegalAngle != null) + { + return Degrees.of(bestLegalAngle); + } + + return Degrees.of(MathUtil.clamp(requestedDegrees, minDegrees, maxDegrees)); + } + + private Angle limitSetpointStep(Angle previousSetpoint, Angle requestedSetpoint) + { + var maxStepDeg = ShooterConstants.TURRET_MAX_SETPOINT_STEP_PER_LOOP.in(Degrees); + var deltaDeg = requestedSetpoint.minus(previousSetpoint).in(Degrees); + var limitedDeltaDeg = MathUtil.clamp(deltaDeg, -maxStepDeg, maxStepDeg); + var limitedSetpoint = previousSetpoint.plus(Degrees.of(limitedDeltaDeg)); + return selectLegalSetpoint(limitedSetpoint); + } + + private double limitOutputStep(double requestedVolts) + { + var previousVolts = _lastCommandedMotorVoltage.in(Volts); + var maxStepVolts = ShooterConstants.TURRET_MAX_OUTPUT_STEP_PER_LOOP.in(Volts); + var deltaVolts = requestedVolts - previousVolts; + var limitedDeltaVolts = MathUtil.clamp(deltaVolts, -maxStepVolts, maxStepVolts); + return previousVolts + limitedDeltaVolts; } private Voltage applySoftLimit(Voltage requestedVoltage) diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java index 2c40968..ca90a66 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretDirector.java @@ -11,12 +11,14 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; @@ -45,6 +47,7 @@ private record MovingTrackState(Pose2d releasePose, Translation2d lookaheadTurre private final Supplier _swerveStateSupplier; private final Limelight _limelight; + private final LinearFilter _visionTxFilter; private List _targetTagFilter; private ShotSolution _currentSolution; @Logged @@ -65,22 +68,54 @@ private record MovingTrackState(Pose2d releasePose, Translation2d lookaheadTurre private Pose2d _lookaheadTurretOriginPose; @Logged private Time _shotTimeOfFlight; + @Logged + private Angle _visionTxRaw; + @Logged + private Angle _visionTxFiltered; + @Logged + private Angle _visionTxApplied; + @Logged + private int _selectedVisionTagId; + @Logged + private Angle _poseOnlyTrackAngle; + @Logged + private Angle _poseVisionTrackDelta; + @Logged + private Angle _empiricalDriftCorrection; + @Logged + private double _empiricalDriftLateralErrorInches; + @Logged + private double _empiricalDriftSign; + @Logged + private Angle _trackCommandDelta; public TurretDirector(Supplier swerveStateSupplier) { - _swerveStateSupplier = swerveStateSupplier; - _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); - _targetTagFilter = List.of(); - _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false, false, false); - _targetDistance = Meters.zero(); - _targetPose = new Pose2d(); - _limelightHasTarget = false; - _hubActive = false; - _targetValid = false; - _desiredTurretAngle = ShooterConstants.TURRET_HOME_ANGLE; - _predictedReleasePose = new Pose2d(); - _lookaheadTurretOriginPose = new Pose2d(); - _shotTimeOfFlight = Seconds.zero(); + var txFilterWindowSamples = Math.max(1, (int)Math.round(ShooterConstants.TURRET_VISION_TX_FILTER_WINDOW.in(Seconds) / GeneralConstants.LOOP_PERIOD.in(Seconds))); + _swerveStateSupplier = swerveStateSupplier; + _limelight = new Limelight(ShooterConstants.LIMELIGHT_NAME); + _visionTxFilter = LinearFilter.movingAverage(txFilterWindowSamples); + _targetTagFilter = List.of(); + _currentSolution = new ShotSolution(false, ShooterConstants.TURRET_HOME_ANGLE, Meters.zero(), ShotMode.Idle, new Pose2d(), false, false, false); + _targetDistance = Meters.zero(); + _targetPose = new Pose2d(); + _limelightHasTarget = false; + _hubActive = false; + _targetValid = false; + _desiredTurretAngle = ShooterConstants.TURRET_HOME_ANGLE; + _predictedReleasePose = new Pose2d(); + _lookaheadTurretOriginPose = new Pose2d(); + _shotTimeOfFlight = Seconds.zero(); + _visionTxRaw = Degrees.zero(); + _visionTxFiltered = Degrees.zero(); + _visionTxApplied = Degrees.zero(); + _selectedVisionTagId = -1; + _poseOnlyTrackAngle = ShooterConstants.TURRET_HOME_ANGLE; + _poseVisionTrackDelta = Degrees.zero(); + _empiricalDriftCorrection = Degrees.zero(); + _empiricalDriftLateralErrorInches = 0.0; + _empiricalDriftSign = ShooterConstants.TURRET_LINEAR_DRIFT_CORRECTION_SIGN; + _trackCommandDelta = Degrees.zero(); _limelight.getSettings().withAprilTagOffset(ShooterConstants.CENTER_TAG_TO_HUB_CENTER_OFFSET).save(); updateFilter(Utilities.getOurHubTagIds(), true); @@ -110,6 +145,7 @@ public ShotSolution calculate(ShotMode shotMode, boolean useMovingShotMath) } var solution = calculate(shotMode, useMovingShotMath, swerveState, Utilities.getHubCoordinates(), targetTagIds, isBlueAlliance, _hubActive, fiducials); + solution = stabilizeTrackCommand(solution); _currentSolution = solution; _targetDistance = solution.distance(); _targetPose = solution.targetPose(); @@ -134,34 +170,113 @@ ShotSolution calculate(ShotMode shotMode, boolean useMovingShotMath, SwerveDrive }; } - private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) + private ShotSolution stabilizeTrackCommand(ShotSolution solution) { - // Keep the named geometry points here, but preserve the old static shot - // behavior for now: distance is still pivot-to-hub, and the release point - // only contributes its lateral component as the legacy angle trim. - var hubTranslationInRobotFrame = getHubTranslationInRobotFrame(swerveState, hubCoordinates); - var pivotToHubTranslation = hubTranslationInRobotFrame.minus(ShooterConstants.ROBOT_TO_TURRET_PIVOT); - var distance = Meters.of(pivotToHubTranslation.getNorm()); - var rawSetpoint = getTurretSetpointForRobotFrameTarget(pivotToHubTranslation); - var bestTag = selectBestTag(targetTagIds, fiducials); - - if (bestTag != null) + if (solution.mode() != ShotMode.Track || _currentSolution.mode() != ShotMode.Track || !_currentSolution.valid()) { - var txCorrection = Degrees.of(-bestTag.tx); + _trackCommandDelta = Degrees.zero(); + return solution; + } - if (!MathUtil.isNear(0.0, txCorrection.in(Degrees), ShooterConstants.TURRET_TRACK_TX_DEADBAND.in(Degrees))) - { - rawSetpoint = rawSetpoint.minus(txCorrection); - } + var requestedDeg = solution.turretAngle().in(Degrees); + var previousDeg = _currentSolution.turretAngle().in(Degrees); + var deltaDeg = MathUtil.inputModulus(requestedDeg - previousDeg, -180.0, 180.0); + _trackCommandDelta = Degrees.of(deltaDeg); + + var deadbandDeg = ShooterConstants.TURRET_TRACK_COMMAND_DEADBAND.in(Degrees); + + if (Math.abs(deltaDeg) <= deadbandDeg) + { + return new ShotSolution(solution.valid(), _currentSolution.turretAngle(), solution.distance(), solution.mode(), solution.targetPose(), solution.hasVisionTarget(), solution.inRange(), solution.hubActive()); } - rawSetpoint = rawSetpoint.plus(getLegacyReleaseLateralCorrection(rawSetpoint, distance)); + var maxStepDeg = ShooterConstants.TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP.in(Degrees); + var stepDeg = MathUtil.clamp(deltaDeg, -maxStepDeg, maxStepDeg); + var stableDeg = previousDeg + stepDeg; - logShotKinematics(swerveState.Pose, getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_PIVOT), distance, ShooterConstants.getShotTimeOfFlight(distance)); + return new ShotSolution(solution.valid(), Degrees.of(stableDeg), solution.distance(), solution.mode(), solution.targetPose(), solution.hasVisionTarget(), solution.inRange(), solution.hubActive()); + } + + private ShotSolution calculateTrack(SwerveDriveState swerveState, Translation2d hubCoordinates, List targetTagIds, boolean hubActive, AprilTagFiducial... fiducials) + { + var turretReleaseField = getPointInField(swerveState.Pose, ShooterConstants.ROBOT_TO_TURRET_RELEASE); + var releaseToHubField = hubCoordinates.minus(turretReleaseField); + var distance = Meters.of(releaseToHubField.getNorm()); + var rawSetpoint = getTurretSetpointForFieldTarget(swerveState.Pose, releaseToHubField.getAngle().getMeasure()); + _poseOnlyTrackAngle = rawSetpoint; + var bestTag = selectBestTag(targetTagIds, fiducials); + rawSetpoint = applyVisionTrim(rawSetpoint, bestTag); + rawSetpoint = rawSetpoint.plus(ShooterConstants.TURRET_TRACK_BIAS); + rawSetpoint = rawSetpoint.plus(getEmpiricalLinearDriftCorrection(rawSetpoint, distance)); + _poseVisionTrackDelta = rawSetpoint.minus(_poseOnlyTrackAngle); + + logShotKinematics(swerveState.Pose, turretReleaseField, distance, ShooterConstants.getShotTimeOfFlight(distance)); return new ShotSolution(true, rawSetpoint, distance, ShotMode.Track, buildTargetPose(swerveState.Pose, distance, rawSetpoint), bestTag != null, true, hubActive); } + private Angle applyVisionTrim(Angle rawSetpoint, AprilTagFiducial bestTag) + { + if (bestTag == null) + { + _visionTxRaw = Degrees.zero(); + _visionTxFiltered = Degrees.zero(); + _visionTxApplied = Degrees.zero(); + _visionTxFilter.reset(); + return rawSetpoint; + } + + var rawTxDegrees = bestTag.tx; + _visionTxRaw = Degrees.of(rawTxDegrees); + _visionTxFiltered = Degrees.of(_visionTxFilter.calculate(rawTxDegrees)); + + if (!ShooterConstants.TURRET_USE_VISION_TX_TRIM) + { + _visionTxApplied = Degrees.zero(); + return rawSetpoint; + } + + var desiredTxDegrees = _visionTxFiltered.in(Degrees); + + if (MathUtil.isNear(0.0, desiredTxDegrees, ShooterConstants.TURRET_TRACK_TX_DEADBAND.in(Degrees))) + { + desiredTxDegrees = 0.0; + } + + var previousTxDegrees = _visionTxApplied.in(Degrees); + var deltaTxDegrees = desiredTxDegrees - previousTxDegrees; + var maxStepDegrees = ShooterConstants.TURRET_VISION_TX_MAX_STEP_PER_LOOP.in(Degrees); + var stepTxDegrees = MathUtil.clamp(deltaTxDegrees, -maxStepDegrees, maxStepDegrees); + _visionTxApplied = Degrees.of(previousTxDegrees + stepTxDegrees); + + return rawSetpoint.plus(_visionTxApplied); + } + + private Angle getEmpiricalLinearDriftCorrection(Angle commandedAngle, Distance distance) + { + if (!ShooterConstants.TURRET_USE_LINEAR_DRIFT_COMPENSATION) + { + _empiricalDriftCorrection = Degrees.zero(); + _empiricalDriftLateralErrorInches = 0.0; + return Degrees.zero(); + } + + _empiricalDriftSign = ShooterConstants.TURRET_LINEAR_DRIFT_CORRECTION_SIGN; + + var commandedDegrees = commandedAngle.in(Degrees); + var rawLateralOffsetInches = ShooterConstants.TURRET_DRIFT_LATERAL_BIAS.in(edu.wpi.first.units.Units.Inches) + + ShooterConstants.TURRET_DRIFT_LATERAL_SINE_AMPLITUDE.in(edu.wpi.first.units.Units.Inches) * Math.sin(Math.toRadians(commandedDegrees + ShooterConstants.TURRET_DRIFT_LATERAL_PHASE_OFFSET.in(Degrees))); + var signedLateralOffsetInches = _empiricalDriftSign * rawLateralOffsetInches; + var distanceMeters = Math.max(distance.in(Meters), 0.25); + var signedCorrectionDegrees = Math.toDegrees(Math.atan2(Units.inchesToMeters(signedLateralOffsetInches), distanceMeters)); + var maxCorrectionDeg = ShooterConstants.TURRET_LINEAR_DRIFT_MAX_CORRECTION.in(Degrees); + signedCorrectionDegrees = MathUtil.clamp(signedCorrectionDegrees, -maxCorrectionDeg, maxCorrectionDeg); + + _empiricalDriftLateralErrorInches = signedLateralOffsetInches; + _empiricalDriftCorrection = Degrees.of(signedCorrectionDegrees); + return _empiricalDriftCorrection; + } + private ShotSolution calculatePass(SwerveDriveState swerveState, boolean isBlueAlliance) { Angle fieldTargetAngle; @@ -191,8 +306,10 @@ private ShotSolution calculateMovingTrack(SwerveDriveState swerveState, Translat var lookaheadToHub = hubCoordinates.minus(movingTrackState.lookaheadTurretOrigin()); var rawSetpoint = getTurretSetpointForFieldTarget(movingTrackState.releasePose(), lookaheadToHub.getAngle().getMeasure()); var bestTag = selectBestTag(targetTagIds, fiducials); - - rawSetpoint = rawSetpoint.plus(getLegacyReleaseLateralCorrection(rawSetpoint, movingTrackState.lookaheadDistance())); + rawSetpoint = rawSetpoint.plus(ShooterConstants.TURRET_TRACK_BIAS); + _poseVisionTrackDelta = Degrees.zero(); + _empiricalDriftCorrection = Degrees.zero(); + _empiricalDriftLateralErrorInches = 0.0; logShotKinematics(movingTrackState.releasePose(), movingTrackState.lookaheadTurretOrigin(), movingTrackState.lookaheadDistance(), movingTrackState.timeOfFlight()); @@ -212,44 +329,16 @@ private Pose2d buildTargetPose(Pose2d robotPose, Distance distance, Angle rawSet return robotPose.plus(new Transform2d(rotatedBySetpoint, new Rotation2d())); } - private Translation2d getHubTranslationInRobotFrame(SwerveDriveState swerveState, Translation2d hubCoordinates) - { - var robotToHubField = hubCoordinates.minus(swerveState.Pose.getTranslation()); - - return robotToHubField.rotateBy(swerveState.Pose.getRotation().unaryMinus()); - } - - private Angle getTurretSetpointForRobotFrameTarget(Translation2d targetTranslation) - { - return targetTranslation.getAngle().getMeasure().minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); - } - private Angle getTurretSetpointForFieldTarget(Pose2d robotPose, Angle fieldTargetAngle) { return fieldTargetAngle.minus(robotPose.getRotation().getMeasure()).minus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD); } - private Angle getLegacyReleaseLateralCorrection(Angle rawSetpoint, Distance distance) - { - var safeDistanceM = Math.max(distance.in(Meters), 0.5); - var shotDirectionDeg = rawSetpoint.plus(ShooterConstants.TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD).in(Degrees); - var shotDirectionRad = Math.toRadians(shotDirectionDeg); - var releaseOffset = ShooterConstants.TURRET_PIVOT_TO_RELEASE; - var perpendicularX = -Math.sin(shotDirectionRad); - var perpendicularY = Math.cos(shotDirectionRad); - var lateralErrorM = releaseOffset.getX() * perpendicularX + releaseOffset.getY() * perpendicularY; - - return Degrees.of(Math.toDegrees(Math.atan2(lateralErrorM, safeDistanceM))); - } - private MovingTrackState calculateMovingTrackState(SwerveDriveState swerveState, Translation2d hubCoordinates) { - // This follows the turret-origin solve pattern described publicly by 5000 - // and 6328: predict the release pose, compute turret-point velocity as - // v + omega x r, then iterate TOF lookahead from the turret pivot. var releasePose = predictReleasePose(swerveState.Pose, swerveState.Speeds); - var turretOriginField = getPointInField(releasePose, ShooterConstants.ROBOT_TO_TURRET_PIVOT); - var turretPointVelocity = getTurretPointVelocityField(releasePose, swerveState.Speeds, ShooterConstants.ROBOT_TO_TURRET_PIVOT); + var turretOriginField = getPointInField(releasePose, ShooterConstants.ROBOT_TO_TURRET_RELEASE); + var turretPointVelocity = getTurretPointVelocityField(releasePose, swerveState.Speeds, ShooterConstants.ROBOT_TO_TURRET_RELEASE); var lookaheadOrigin = turretOriginField; var lookaheadDistance = Meters.of(hubCoordinates.getDistance(lookaheadOrigin)); var timeOfFlight = ShooterConstants.getShotTimeOfFlight(lookaheadDistance); @@ -301,7 +390,8 @@ private void logShotKinematics(Pose2d releasePose, Translation2d lookaheadTurret private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiducial... fiducials) { - AprilTagFiducial bestTag = null; + AprilTagFiducial bestTag = null; + AprilTagFiducial lockedTag = null; for (AprilTagFiducial tag : fiducials) { @@ -312,13 +402,27 @@ private AprilTagFiducial selectBestTag(List targetTagIds, AprilTagFiduc continue; } + if (tagId == _selectedVisionTagId) + { + lockedTag = tag; + } + if (bestTag == null || tag.ta > bestTag.ta || tag.ta == bestTag.ta && Math.abs(tag.tx) < Math.abs(bestTag.tx)) { bestTag = tag; } } - return bestTag; + var selectedTag = lockedTag != null ? lockedTag : bestTag; + + if (selectedTag == null) + { + _selectedVisionTagId = -1; + return null; + } + + _selectedVisionTagId = (int)Math.round(selectedTag.fiducialID); + return selectedTag; } private void updateFilter(List filters, boolean force) From 89f3d12f38752f5d7ab97afaa6385cad3416d6d7 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Wed, 18 Mar 2026 21:32:08 -0500 Subject: [PATCH 15/32] Tune rotor behavior and clean shooter controls --- src/main/java/frc/robot/Constants.java | 6 +-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/shooter/Rotor.java | 13 +++--- .../frc/robot/subsystems/shooter/Shooter.java | 41 +++++++++++++------ 4 files changed, 39 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1c91b9..5652134 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -148,7 +148,6 @@ public static class ShooterConstants public static final Current FLYWHEEL_CURRENT_LIMIT = Amps.of(60); public static final AngularVelocity MANUAL_SHOOT_RPM = RPM.of(3500.0); public static final AngularVelocity PASS_FLYWHEEL_VELOCITY = RPM.of(3000.0); // TODO: Tune - private static final AngularVelocity SMART_SHOOT_RPM_BIAS = RPM.of(0.0); // Turret public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0); @@ -242,12 +241,13 @@ public static class ShooterConstants public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60); public static final Voltage FEEDER_VOLTAGE = Volts.of(6); public static final Current ROTOR_CURRENT_LIMIT = Amps.of(60); - public static final Voltage ROTOR_VOLTAGE = Volts.of(3); + public static final Voltage ROTOR_FAST_VOLTAGE = Volts.of(3.0); + public static final Voltage ROTOR_SLOW_VOLTAGE = Volts.of(1.5); // TODO: Tune these values with testing! public static AngularVelocity getFlywheelSpeedForDistance(Distance distance) { - return RPM.of(FLYWHEEL_SPEED_TABLE.get(distance.in(Inches))).plus(SMART_SHOOT_RPM_BIAS); + return RPM.of(FLYWHEEL_SPEED_TABLE.get(distance.in(Inches))); } public static Time getShotTimeOfFlight(Distance distance) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 982e063..cb4b9e1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,7 +37,7 @@ public class RobotContainer private final CommandXboxController _operator = new CommandXboxController(1); private final Drive _drive = TunerConstants.createDrivetrain(); private final Intake _intake = new Intake(); - private final Shooter _shooter = new Shooter(_drive::getState); + private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended); @NotLogged private final Autos _autos = new Autos(_drive, _shooter); private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; diff --git a/src/main/java/frc/robot/subsystems/shooter/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/Rotor.java index e7a25bb..c2e62fb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Rotor.java +++ b/src/main/java/frc/robot/subsystems/shooter/Rotor.java @@ -23,8 +23,6 @@ public class Rotor private final TalonFX _rotorMotor; @Logged private Voltage _rotorMotorVoltage = Volts.zero(); - @Logged - private boolean _enabled = false; public Rotor() { @@ -47,13 +45,16 @@ public void periodic() public void set(boolean on) { - _enabled = on; - var targetVoltage = on ? ShooterConstants.ROTOR_VOLTAGE : Volts.zero(); - _rotorMotor.setVoltage(targetVoltage.in(Volts)); + setVoltage(on ? ShooterConstants.ROTOR_FAST_VOLTAGE : Volts.zero()); + } + + public void setVoltage(Voltage voltage) + { + _rotorMotor.setVoltage(voltage.in(Volts)); } public void stop() { - set(false); + setVoltage(Volts.zero()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 814bf75..504ebc4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import java.util.function.Supplier; @@ -37,6 +38,7 @@ public enum ShooterState public final Turret _turret; public final TurretDirector _turretDirector; private final Supplier _swerveStateSupplier; + private final Supplier _intakeExtendedSupplier; private final Debouncer _movingFeedDebouncer; @Logged private ShooterState _state; @@ -68,9 +70,10 @@ public enum ShooterState @Logged private boolean _feedReady; - public Shooter(Supplier swerveStateSupplier) + public Shooter(Supplier swerveStateSupplier, Supplier intakeExtendedSupplier) { _swerveStateSupplier = swerveStateSupplier; + _intakeExtendedSupplier = intakeExtendedSupplier; _flywheel = new Flywheel(); _feeder = new Feeder(); _rotor = new Rotor(); @@ -94,7 +97,7 @@ public Shooter(Supplier swerveStateSupplier) _feedReady = false; _feeder.set(false); - _rotor.set(false); + _rotor.stop(); _flywheel.stop(); _turret.clearTargetAngle(); _turret.setDisabled(false); @@ -189,11 +192,11 @@ public Command runManualFeeder() { beginManualControl(false); _feeder.set(true); - _rotor.set(true); + setRotorEnabled(true); }, () -> { _feeder.set(false); - _rotor.set(false); + setRotorEnabled(false); }); } @@ -203,11 +206,11 @@ public Command runFeeder() { _state = ShooterState.Manual; _feeder.set(true); - _rotor.set(true); + setRotorEnabled(true); }, () -> { _feeder.set(false); - _rotor.set(false); + setRotorEnabled(false); _flywheel.stop(); _state = ShooterState.Idle; }); @@ -246,7 +249,7 @@ public void periodic() case Preparing: case Ready: _feeder.set(false); - _rotor.set(false); + setRotorEnabled(false); runRequestedShot(true); if (isReadyToFeed()) @@ -271,12 +274,12 @@ public void periodic() if (isReadyToFeed()) { _feeder.set(true); - _rotor.set(true); + setRotorEnabled(true); } else { _feeder.set(false); - _rotor.set(false); + setRotorEnabled(false); _state = ShooterState.Preparing; } break; @@ -290,7 +293,7 @@ public void periodic() case TrackingOnly: _feeder.set(false); - _rotor.set(false); + setRotorEnabled(false); _flywheel.stop(); _requestedShotMode = ShotMode.Track; runRequestedShot(false); @@ -300,7 +303,7 @@ public void periodic() default: _flywheel.stop(); _feeder.set(false); - _rotor.set(false); + setRotorEnabled(false); clearShotRequest(); break; } @@ -344,7 +347,7 @@ private void beginManualControl(boolean clearTurretTarget) _currentShotSolution = createIdleSolution(); clearReadinessGate(); _feeder.set(false); - _rotor.set(false); + setRotorEnabled(false); _turret.setDisabled(false); if (clearTurretTarget) @@ -402,7 +405,7 @@ private void clearShotRequest() _requestedShotMode = ShotMode.Idle; _currentShotSolution = createIdleSolution(); clearReadinessGate(); - _rotor.set(false); + setRotorEnabled(false); } private ShotSolution createIdleSolution() @@ -503,4 +506,16 @@ private void clearReadinessGate() _movingFeedDebouncer.calculate(false); _feedReady = false; } + + private void setRotorEnabled(boolean enabled) + { + if (!enabled) + { + _rotor.stop(); + return; + } + + var rotorVoltage = _intakeExtendedSupplier.get() ? ShooterConstants.ROTOR_FAST_VOLTAGE : ShooterConstants.ROTOR_SLOW_VOLTAGE; + _rotor.setVoltage(rotorVoltage); + } } From 47b9af4dc753c9cbc92a6482dba3e8227f5c5af8 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Wed, 18 Mar 2026 22:21:29 -0500 Subject: [PATCH 16/32] integration of Motion Magic --- src/main/java/frc/robot/Constants.java | 121 ++++++------ .../frc/robot/subsystems/shooter/Turret.java | 181 +++++++++--------- 2 files changed, 149 insertions(+), 153 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5652134..4f1b6c7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -150,67 +150,66 @@ public static class ShooterConstants public static final AngularVelocity PASS_FLYWHEEL_VELOCITY = RPM.of(3000.0); // TODO: Tune // Turret - public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0); - public static final double TURRET_KP = 0.19; // TODO: Tune (onboard TalonFX PID) - public static final double TURRET_KI = 0.0; - public static final double TURRET_KD = 0.0115; - public static final Voltage TURRET_STATIC_FF = Volts.of(0.22); - public static final Angle TURRET_STATIC_FF_ERROR_DEADBAND = Degrees.of(0.75); - public static final Voltage TURRET_MAX_OUTPUT_STEP_PER_LOOP = Volts.of(3.0); - public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); // degrees (full 360 rotation) - public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); // degrees - public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); // degrees (full 360 rotation) - public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); // degrees - public static final boolean TURRET_SENSOR_INVERTED = true; - public static final Angle TURRET_SENSOR_ZERO_OFFSET = Degrees.of(-5); - public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); - public static final Angle TURRET_TOLERANCE = Degrees.of(1.25); // degrees - public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(2.0); // degrees - public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(0.1); // degrees - public static final Angle TURRET_MAX_SETPOINT_STEP_PER_LOOP = Degrees.of(4.0); - public static final Angle TURRET_TRACK_COMMAND_DEADBAND = Degrees.of(0.5); - public static final Angle TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP = Degrees.of(4.0); - public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(1.0); - public static final boolean TURRET_USE_VISION_TX_TRIM = false; - public static final Time TURRET_VISION_TX_FILTER_WINDOW = Milliseconds.of(100.0); - public static final Angle TURRET_VISION_TX_MAX_STEP_PER_LOOP = Degrees.of(0.4); - public static final Angle TURRET_TRACK_BIAS = Degrees.zero(); // TODO: Tune static shot bias from log data - public static final boolean TURRET_USE_LINEAR_DRIFT_COMPENSATION = true; - public static final Distance TURRET_DRIFT_LATERAL_BIAS = Inches.of(11.060660171779821); - public static final Distance TURRET_DRIFT_LATERAL_SINE_AMPLITUDE = Inches.of(7.1890180494515015); - public static final Angle TURRET_DRIFT_LATERAL_PHASE_OFFSET = Degrees.of(-20.866427056070258); - public static final Angle TURRET_LINEAR_DRIFT_MAX_CORRECTION = Degrees.of(15.0); - public static final double TURRET_LINEAR_DRIFT_CORRECTION_SIGN = -1.0; - public static final String LIMELIGHT_NAME = "limelight-shooter"; - public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice - public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); - public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); - public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); - public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); - public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); - public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees - // left of robot forward - public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); - public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); - public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not - // lateral - public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); - public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); - public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" - public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); - public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); - public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); - public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); - public static final int SWM_LOOKAHEAD_ITERATIONS = 8; - public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); - public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); - public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.5); - public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(45.0); - public static final Angle SWM_FEED_TURRET_TOLERANCE = Degrees.of(5.0); - public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(60.0); - private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; - private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; + public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0); + public static final Dimensionless TURRET_GEAR_RATIO = Value.of(13.0); // 13:1 motor rotations per turret rotation + public static final double TURRET_KP = 2.0; + public static final double TURRET_KS = 0.498; + public static final AngularVelocity TURRET_MM_CRUISE_VELOCITY = RotationsPerSecond.of(54.0); + public static final AngularAcceleration TURRET_MM_ACCELERATION = RotationsPerSecondPerSecond.of(108.0); + public static final double TURRET_MM_EXPO_KV = 0.12; // V/rps + public static final double TURRET_MM_EXPO_KA = 0.1; // V/(rps/s) + public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); + public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); + public static final boolean TURRET_SENSOR_INVERTED = true; + public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); + public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); + public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); + public static final Angle TURRET_TOLERANCE = Degrees.of(1.25); + public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(2.0); + public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(0.1); + public static final Angle TURRET_TRACK_COMMAND_DEADBAND = Degrees.of(0.5); + public static final Angle TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP = Degrees.of(4.0); + public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(1.0); + public static final boolean TURRET_USE_VISION_TX_TRIM = false; + public static final Time TURRET_VISION_TX_FILTER_WINDOW = Milliseconds.of(100.0); + public static final Angle TURRET_VISION_TX_MAX_STEP_PER_LOOP = Degrees.of(0.4); + public static final Angle TURRET_TRACK_BIAS = Degrees.zero(); // TODO: Tune static shot bias from log data + public static final boolean TURRET_USE_LINEAR_DRIFT_COMPENSATION = true; + public static final Distance TURRET_DRIFT_LATERAL_BIAS = Inches.of(11.060660171779821); + public static final Distance TURRET_DRIFT_LATERAL_SINE_AMPLITUDE = Inches.of(7.1890180494515015); + public static final Angle TURRET_DRIFT_LATERAL_PHASE_OFFSET = Degrees.of(-20.866427056070258); + public static final Angle TURRET_LINEAR_DRIFT_MAX_CORRECTION = Degrees.of(15.0); + public static final double TURRET_LINEAR_DRIFT_CORRECTION_SIGN = -1.0; + public static final String LIMELIGHT_NAME = "limelight-shooter"; + public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice + public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); + public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); + public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); + public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); + public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); + public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees + // left of robot forward + public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); + public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); + public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not + // lateral + public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); + public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); + public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" + public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); + public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); + public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); + public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); + public static final int SWM_LOOKAHEAD_ITERATIONS = 8; + public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); + public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); + public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.5); + public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(45.0); + public static final Angle SWM_FEED_TURRET_TOLERANCE = Degrees.of(5.0); + public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(60.0); + private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; + private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; // @formatter:off private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 2a4bcb0..4153f76 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -2,18 +2,26 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.AnalogPotentiometer; @@ -32,33 +40,34 @@ enum ControlMode Idle, TargetAngle, ManualAngle } - private final TalonFX _turretMotor; - private final AnalogPotentiometer _turretSensor; - private final PIDController _pidController; - private ControlMode _controlMode; + private final TalonFX _turretMotor; + private final AnalogPotentiometer _turretPotentiometer; + private final MotionMagicExpoVoltage _motionMagicRequest; + private final NeutralOut _neutralRequest; + private ControlMode _controlMode; + private boolean _encoderSeeded; @Logged - private Angle _manualAngleSetpoint; + private Angle _manualAngleSetpoint; @Logged - private Angle _targetAngleSetpoint; + private Angle _targetAngleSetpoint; @Logged - private Angle _turretAngle; + private Angle _turretAngle; @Logged - private Angle _rawTurretAngle; + private Angle _turretSetpoint; @Logged - private Angle _turretSetpoint; + private Angle _commandedTargetAngle; @Logged - private Angle _commandedTargetAngle; + private Angle _targetAngleError; @Logged - private Angle _targetAngleError; + private boolean _hasSetpoint; @Logged - private boolean _hasSetpoint; + private Voltage _motorVoltage; @Logged - private Voltage _motorVoltage; - private Voltage _lastCommandedMotorVoltage; + private boolean _disabled; @Logged - private boolean _disabled; + private boolean _linedUp; @Logged - private boolean _linedUp; + private double _motorPositionRotations; public Turret() { @@ -71,42 +80,77 @@ public Turret() sensorOffset = sensorOffset.unaryMinus(); } - _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); - _turretSensor = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); - _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); - _controlMode = ControlMode.Idle; - _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _turretAngle = Degrees.zero(); - _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _commandedTargetAngle = ShooterConstants.TURRET_HOME_ANGLE; - _targetAngleError = Degrees.zero(); - _hasSetpoint = false; - _motorVoltage = Volts.zero(); - _lastCommandedMotorVoltage = Volts.zero(); - _disabled = false; - _linedUp = false; - + _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); + _turretPotentiometer = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); + _motionMagicRequest = new MotionMagicExpoVoltage(0); + _neutralRequest = new NeutralOut(); + _controlMode = ControlMode.Idle; + _encoderSeeded = false; + _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _turretAngle = Degrees.zero(); + _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _commandedTargetAngle = ShooterConstants.TURRET_HOME_ANGLE; + _targetAngleError = Degrees.zero(); + _hasSetpoint = false; + _motorVoltage = Volts.zero(); + _disabled = false; + _linedUp = false; + _motorPositionRotations = 0.0; + + var talonConfig = new TalonFXConfiguration(); + + // Current limits var currentConfig = new CurrentLimitsConfigs(); currentConfig.StatorCurrentLimit = ShooterConstants.TURRET_CURRENT_LIMIT.in(Amps); currentConfig.StatorCurrentLimitEnable = true; + // Motor output var outputConfig = new MotorOutputConfigs(); outputConfig.NeutralMode = NeutralModeValue.Brake; outputConfig.Inverted = InvertedValue.Clockwise_Positive; - _turretMotor.getConfigurator().apply(new TalonFXConfiguration().withCurrentLimits(currentConfig).withMotorOutput(outputConfig)); + // Feedback (gear ratio: motor rotations to mechanism rotations) + var feedbackConfig = new FeedbackConfigs(); + feedbackConfig.SensorToMechanismRatio = ShooterConstants.TURRET_GEAR_RATIO.magnitude(); + + // PID Slot 0 gains + var slot0Config = new Slot0Configs(); + slot0Config.kP = ShooterConstants.TURRET_KP; + slot0Config.kS = ShooterConstants.TURRET_KS; + + // Motion Magic Expo settings + var motionMagicConfig = new MotionMagicConfigs(); + motionMagicConfig.MotionMagicCruiseVelocity = ShooterConstants.TURRET_MM_CRUISE_VELOCITY.in(RotationsPerSecond); + motionMagicConfig.MotionMagicAcceleration = ShooterConstants.TURRET_MM_ACCELERATION.in(RotationsPerSecondPerSecond); + motionMagicConfig.MotionMagicExpo_kV = ShooterConstants.TURRET_MM_EXPO_KV; + motionMagicConfig.MotionMagicExpo_kA = ShooterConstants.TURRET_MM_EXPO_KA; + + // Software limits (in mechanism rotations, i.e. turret rotations) + var softLimitConfig = new SoftwareLimitSwitchConfigs(); + softLimitConfig.ForwardSoftLimitEnable = true; + softLimitConfig.ForwardSoftLimitThreshold = ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Rotations); + softLimitConfig.ReverseSoftLimitEnable = true; + softLimitConfig.ReverseSoftLimitThreshold = ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Rotations); - _pidController.setTolerance(ShooterConstants.TURRET_TOLERANCE.in(Degrees)); + talonConfig.withCurrentLimits(currentConfig).withMotorOutput(outputConfig).withFeedback(feedbackConfig).withSlot0(slot0Config).withMotionMagic(motionMagicConfig).withSoftwareLimitSwitch(softLimitConfig); + + _turretMotor.getConfigurator().apply(talonConfig); } public void periodic() { - _rawTurretAngle = Degrees.of(_turretSensor.get()); - _turretAngle = _rawTurretAngle.plus(ShooterConstants.TURRET_SENSOR_ZERO_OFFSET); - _motorVoltage = _turretMotor.getMotorVoltage().getValue(); + // Seed motor encoder from potentiometer on first run + if (!_encoderSeeded) + { + var potAngle = Degrees.of(_turretPotentiometer.get()); + _turretMotor.setPosition(potAngle.in(Rotations)); + _encoderSeeded = true; + } - var motorOutput = Volts.zero(); + _motorPositionRotations = _turretMotor.getPosition().getValue().in(Rotations); + _turretAngle = Degrees.of(_motorPositionRotations * 360.0); + _motorVoltage = _turretMotor.getMotorVoltage().getValue(); switch (_controlMode) { @@ -127,11 +171,6 @@ public void periodic() break; } - if (_hasSetpoint) - { - _turretSetpoint = limitSetpointStep(_commandedTargetAngle, _turretSetpoint); - } - updateLinedUpState(); _commandedTargetAngle = _turretSetpoint; @@ -139,22 +178,13 @@ public void periodic() if (_hasSetpoint && !_disabled) { - var errorDegrees = _turretSetpoint.minus(_turretAngle).in(Degrees); - var outputVolts = _pidController.calculate(_turretAngle.in(Degrees), _turretSetpoint.in(Degrees)); - var staticFFVolts = ShooterConstants.TURRET_STATIC_FF.in(Volts); - - if (!MathUtil.isNear(0.0, errorDegrees, ShooterConstants.TURRET_STATIC_FF_ERROR_DEADBAND.in(Degrees))) - { - outputVolts += Math.copySign(staticFFVolts, errorDegrees); - } - - motorOutput = Volts.of(outputVolts); + var setpointRotations = _turretSetpoint.in(Degrees) / 360.0; + _turretMotor.setControl(_motionMagicRequest.withPosition(setpointRotations)); + } + else + { + _turretMotor.setControl(_neutralRequest); } - - motorOutput = Volts.of(limitOutputStep(motorOutput.in(Volts))); - motorOutput = applySoftLimit(motorOutput); - _turretMotor.setVoltage(motorOutput.in(Volts)); - _lastCommandedMotorVoltage = motorOutput; } public void simulationPeriodic() @@ -257,39 +287,6 @@ static Angle chooseNearestLegalAngle(Angle currentAngle, Angle requestedAngle) return Degrees.of(MathUtil.clamp(requestedDegrees, minDegrees, maxDegrees)); } - private Angle limitSetpointStep(Angle previousSetpoint, Angle requestedSetpoint) - { - var maxStepDeg = ShooterConstants.TURRET_MAX_SETPOINT_STEP_PER_LOOP.in(Degrees); - var deltaDeg = requestedSetpoint.minus(previousSetpoint).in(Degrees); - var limitedDeltaDeg = MathUtil.clamp(deltaDeg, -maxStepDeg, maxStepDeg); - var limitedSetpoint = previousSetpoint.plus(Degrees.of(limitedDeltaDeg)); - return selectLegalSetpoint(limitedSetpoint); - } - - private double limitOutputStep(double requestedVolts) - { - var previousVolts = _lastCommandedMotorVoltage.in(Volts); - var maxStepVolts = ShooterConstants.TURRET_MAX_OUTPUT_STEP_PER_LOOP.in(Volts); - var deltaVolts = requestedVolts - previousVolts; - var limitedDeltaVolts = MathUtil.clamp(deltaVolts, -maxStepVolts, maxStepVolts); - return previousVolts + limitedDeltaVolts; - } - - private Voltage applySoftLimit(Voltage requestedVoltage) - { - if (_turretAngle.lte(ShooterConstants.TURRET_SOFT_MIN_ANGLE) && requestedVoltage.lt(Volts.zero())) - { - return Volts.zero(); - } - - if (_turretAngle.gte(ShooterConstants.TURRET_SOFT_MAX_ANGLE) && requestedVoltage.gt(Volts.zero())) - { - return Volts.zero(); - } - - return requestedVoltage; - } - private void updateLinedUpState() { if (!_hasSetpoint || _disabled) From f5f7db872856bc71874c00de0c914eee28c52d9b Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Thu, 19 Mar 2026 13:54:13 -0500 Subject: [PATCH 17/32] Add chooser-driven auto dashboard --- src/main/deploy/elastic-layout.json | 163 ++++++++++++++++++ src/main/java/frc/robot/Autos.java | 256 ++++++++++++++++++++++------ 2 files changed, 368 insertions(+), 51 deletions(-) create mode 100644 src/main/deploy/elastic-layout.json diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 0000000..ec6c1f8 --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,163 @@ +{ + "version": 1.0, + "grid_size": 32, + "tabs": [ + { + "name": "Autonomous", + "grid_layout": { + "layouts": [ + { + "title": "Auto Setup", + "x": 1536.0, + "y": 0.0, + "width": 384.0, + "height": 544.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Auto Mode", + "x": 0.0, + "y": 0.0, + "width": 320.0, + "height": 112.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Mode", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Start Position", + "x": 0.0, + "y": 0.0, + "width": 320.0, + "height": 112.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Start Position", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Drive Path", + "x": 0.0, + "y": 0.0, + "width": 320.0, + "height": 112.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Drive Path", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Auto Delay", + "x": 0.0, + "y": 0.0, + "width": 320.0, + "height": 112.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Delay", + "period": 0.06, + "sort_options": false + } + } + ] + } + ], + "containers": [ + { + "title": "Autonomous Preview", + "x": 0.0, + "y": 0.0, + "width": 1536.0, + "height": 768.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Autonomous Preview", + "period": 0.06, + "field_game": "Rebuilt", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Auto Summary", + "x": 1536.0, + "y": 544.0, + "width": 384.0, + "height": 96.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Auto Summary", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Selected Trajectory", + "x": 1536.0, + "y": 640.0, + "width": 384.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Auto Selected Trajectory", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Uses Trajectory", + "x": 1536.0, + "y": 704.0, + "width": 192.0, + "height": 64.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Auto Uses Trajectory", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Selection Valid", + "x": 1728.0, + "y": 704.0, + "width": 192.0, + "height": 64.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Auto Selection Valid", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + } + ] +} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 72e70b4..afe899a 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -1,7 +1,10 @@ package frc.robot; +import java.util.stream.IntStream; + import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; + import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -9,45 +12,88 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.GeneralConstants; +import frc.robot.generated.ChoreoTraj; import frc.robot.generated.ChoreoVars; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Utilities; -public class Autos +public class Autos extends SubsystemBase { private enum AutoMode { - DoNothing, ShootOnly, ShootWithDelay, ShootThenDrive, ShootWithDelayThenDrive + DoNothing("Do Nothing"), DriveOnly("Drive Only"), ShootOnly("Shoot Only"), ShootWithDelay("Shoot + Delay"), ShootThenDrive("Shoot Then Drive"), ShootWithDelayThenDrive("Shoot + Delay + Drive"); + + public final String displayName; + + private AutoMode(String displayName) + { + this.displayName = displayName; + } + } + + private record AutoDriveOption(String displayName, String trajectoryName, Pose2d endPoseBlue) + { + boolean staysPut() + { + return trajectoryName == null; + } } private enum StartPosition { - LeftTrench(ChoreoVars.Poses.LeftTrench), LeftBump(ChoreoVars.Poses.LeftBump), HubStart(ChoreoVars.Poses.HubStart), RightBump(ChoreoVars.Poses.RightBump), RightTrench(ChoreoVars.Poses.RightTrench); + LeftTrench("Left Trench", ChoreoVars.Poses.LeftTrench, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.LeftTrench), + new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftTrenchToDepot.name(), ChoreoTraj.LeftTrenchToDepot.endPoseBlue())), + LeftBump("Left Bump", ChoreoVars.Poses.LeftBump, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.LeftBump), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftBumpToDepot.name(), ChoreoTraj.LeftBumpToDepot.endPoseBlue()), + new AutoDriveOption("Drive To Tower", ChoreoTraj.LeftBumpToTower.name(), ChoreoTraj.LeftBumpToTower.endPoseBlue()), + new AutoDriveOption("Pickup From Mid", ChoreoTraj.LeftBumpPickupFromMidScore.name(), ChoreoTraj.LeftBumpPickupFromMidScore.endPoseBlue())), + HubStart("Hub", ChoreoVars.Poses.HubStart, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.HubStart), new AutoDriveOption("Drive To Depot", ChoreoTraj.HubToDepot.name(), ChoreoTraj.HubToDepot.endPoseBlue()), + new AutoDriveOption("Drive To Outpost", ChoreoTraj.HubToOutpost.name(), ChoreoTraj.HubToOutpost.endPoseBlue()), new AutoDriveOption("Drive To Tower", ChoreoTraj.HubToTower.name(), ChoreoTraj.HubToTower.endPoseBlue())), + RightBump("Right Bump", ChoreoVars.Poses.RightBump, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.RightBump), + new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightBumpToOutpost.name(), ChoreoTraj.RightBumpToOutpost.endPoseBlue()), + new AutoDriveOption("Drive To Tower", ChoreoTraj.RightBumpToTower.name(), ChoreoTraj.RightBumpToTower.endPoseBlue()), + new AutoDriveOption("Pickup From Mid", ChoreoTraj.RightBumpPickupFromMidScore.name(), ChoreoTraj.RightBumpPickupFromMidScore.endPoseBlue())), + RightTrench("Right Trench", ChoreoVars.Poses.RightTrench, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.RightTrench), + new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightTrenchToOutpost.name(), ChoreoTraj.RightTrenchToOutpost.endPoseBlue())); - public final Pose2d pose; + public final String displayName; + public final Pose2d pose; + public final AutoDriveOption[] driveOptions; - private StartPosition(Pose2d bluePose) + private StartPosition(String displayName, Pose2d pose, AutoDriveOption... driveOptions) { - pose = bluePose; + this.displayName = displayName; + this.pose = pose; + this.driveOptions = driveOptions; } } - private static final String NO_TRAJECTORY = "None (Stay)"; - private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; - private static final int DEFAULT_AUTO_DELAY_SECS = 0; - private static final StartPosition DEFAULT_START_POSITION = StartPosition.LeftTrench; - private static final String DEFAULT_AUTO_TRAJECTORY = NO_TRAJECTORY; - private final AutoFactory _autoFactory; - private final Drive _driveSubsystem; - private final Shooter _shooterSubsystem; - private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); - private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); - private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); + private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; + private static final int DEFAULT_AUTO_DELAY_SECS = 0; + private static final StartPosition DEFAULT_START_POSITION = StartPosition.HubStart; + private final AutoFactory _autoFactory; + private final Drive _driveSubsystem; + private final Shooter _shooterSubsystem; + private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); + private final SendableChooser _modeChooser = new SendableChooser<>(); + private final SendableChooser _delayChooser = new SendableChooser<>(); + private final SendableChooser _startChooser = new SendableChooser<>(); + private SendableChooser _drivePathChooser = new SendableChooser<>(); + private final Field2d _previewField = new Field2d(); + private final Alert _invalidAutoAlert = new Alert("Selected auto configuration is invalid.", AlertType.kWarning); + private StartPosition _lastStartPosition; public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { @@ -56,61 +102,169 @@ public Autos(Drive driveSubsystem, Shooter shooterSubsystem) _headingController.enableContinuousInput(-Math.PI, Math.PI); - // @formatter:off - _autoFactory = new AutoFactory( - () -> driveSubsystem.getState().Pose, - driveSubsystem::resetPose, - this::followTrajectory, - true, - driveSubsystem - ); - // @formatter:on + _autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem); + + configureChoosers(); + rebuildDrivePathChooser(DEFAULT_START_POSITION); + updateDashboardPreview(); + } + + @Override + public void periodic() + { + var selectedStart = getSelectedStartPosition(); + + if (_lastStartPosition != selectedStart) + { + rebuildDrivePathChooser(selectedStart); + } + + updateDashboardPreview(); } public Command buildAuto() { - var start = DEFAULT_START_POSITION; - var mode = DEFAULT_AUTO_MODE; - var trajName = DEFAULT_AUTO_TRAJECTORY; - var reset = Commands.runOnce(() -> _driveSubsystem.resetPose(flip(start.pose))); - var shoot = _shooterSubsystem.shoot().withTimeout(4.0); - var delay = Commands.waitSeconds(DEFAULT_AUTO_DELAY_SECS); - var drive = NO_TRAJECTORY.equals(trajName) ? Commands.none() : _autoFactory.trajectoryCmd(trajName); + var startPosition = getSelectedStartPosition(); + var autoMode = getSelectedMode(); + var driveOption = getSelectedDriveOption(startPosition); + var autoDelaySecs = getSelectedDelaySeconds(); + var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(flip(startPosition.pose))); + var shoot = _shooterSubsystem.shoot().withTimeout(4.0); + var delay = Commands.waitSeconds(autoDelaySecs); + var drive = driveOption.staysPut() ? Commands.none() : _autoFactory.trajectoryCmd(driveOption.trajectoryName()); + + return switch (autoMode) + { + case DoNothing -> resetPose; + case DriveOnly -> Commands.sequence(resetPose, drive); + case ShootOnly -> Commands.sequence(resetPose, shoot); + case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot); + case ShootThenDrive -> Commands.sequence(resetPose, shoot, drive); + case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive); + }; + } + + private void configureChoosers() + { + _modeChooser.setDefaultOption(DEFAULT_AUTO_MODE.displayName, DEFAULT_AUTO_MODE); + _modeChooser.addOption(AutoMode.DriveOnly.displayName, AutoMode.DriveOnly); + _modeChooser.addOption(AutoMode.ShootWithDelay.displayName, AutoMode.ShootWithDelay); + _modeChooser.addOption(AutoMode.ShootThenDrive.displayName, AutoMode.ShootThenDrive); + _modeChooser.addOption(AutoMode.ShootWithDelayThenDrive.displayName, AutoMode.ShootWithDelayThenDrive); + _modeChooser.addOption(AutoMode.DoNothing.displayName, AutoMode.DoNothing); + + _delayChooser.setDefaultOption(String.valueOf(DEFAULT_AUTO_DELAY_SECS), DEFAULT_AUTO_DELAY_SECS); + IntStream.rangeClosed(1, 5).forEach(seconds -> _delayChooser.addOption(String.valueOf(seconds), seconds)); + + _startChooser.setDefaultOption(DEFAULT_START_POSITION.displayName, DEFAULT_START_POSITION); + _startChooser.addOption(StartPosition.LeftTrench.displayName, StartPosition.LeftTrench); + _startChooser.addOption(StartPosition.LeftBump.displayName, StartPosition.LeftBump); + _startChooser.addOption(StartPosition.HubStart.displayName, StartPosition.HubStart); + _startChooser.addOption(StartPosition.RightBump.displayName, StartPosition.RightBump); + _startChooser.addOption(StartPosition.RightTrench.displayName, StartPosition.RightTrench); + + SmartDashboard.putData("Auto Mode", _modeChooser); + SmartDashboard.putData("Auto Start Position", _startChooser); + SmartDashboard.putData("Auto Delay", _delayChooser); + SmartDashboard.putData("Auto Drive Path", _drivePathChooser); + SmartDashboard.putData("Autonomous Preview", _previewField); + } + + private void rebuildDrivePathChooser(StartPosition startPosition) + { + var newChooser = new SendableChooser(); + var options = startPosition.driveOptions; + + newChooser.setDefaultOption(options[0].displayName(), options[0]); + + for (int index = 1; index < options.length; index++) + { + var option = options[index]; + newChooser.addOption(option.displayName(), option); + } + + _drivePathChooser = newChooser; + _lastStartPosition = startPosition; + SmartDashboard.putData("Auto Drive Path", _drivePathChooser); + } + + private void updateDashboardPreview() + { + var startPosition = getSelectedStartPosition(); + var autoMode = getSelectedMode(); + var driveOption = getSelectedDriveOption(startPosition); + var startPose = flip(startPosition.pose); + var endPose = flip(driveOption.endPoseBlue()); + + _previewField.setRobotPose(startPose); + _previewField.getObject("Auto End Pose").setPose(endPose); + + SmartDashboard.putString("Auto Summary", buildSummary(autoMode, startPosition, driveOption, getSelectedDelaySeconds())); + SmartDashboard.putString("Auto Selected Trajectory", driveOption.staysPut() ? "None" : driveOption.trajectoryName()); + SmartDashboard.putBoolean("Auto Uses Trajectory", !driveOption.staysPut()); + SmartDashboard.putBoolean("Auto Selection Valid", true); + _invalidAutoAlert.set(false); + } + + private AutoMode getSelectedMode() + { + var selected = _modeChooser.getSelected(); + return selected != null ? selected : DEFAULT_AUTO_MODE; + } + + private int getSelectedDelaySeconds() + { + var selected = _delayChooser.getSelected(); + return selected != null ? selected : DEFAULT_AUTO_DELAY_SECS; + } - // @formatter:off + private StartPosition getSelectedStartPosition() + { + var selected = _startChooser.getSelected(); + return selected != null ? selected : DEFAULT_START_POSITION; + } + + private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) + { + var selected = _drivePathChooser.getSelected(); + if (selected != null) + { + return selected; + } + + return startPosition.driveOptions[0]; + } + + private String buildSummary(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption, int delaySeconds) + { return switch (mode) { - case DoNothing -> reset; - case ShootOnly -> Commands.sequence(reset, shoot); - case ShootWithDelay -> Commands.sequence(reset, delay, shoot); - case ShootThenDrive -> Commands.sequence(reset, shoot, drive); - case ShootWithDelayThenDrive -> Commands.sequence(reset, delay, shoot, drive); + case DoNothing -> "Do Nothing | " + startPosition.displayName; + case DriveOnly -> "Drive Only | " + startPosition.displayName + " | " + driveOption.displayName; + case ShootOnly -> "Shoot Only | " + startPosition.displayName; + case ShootWithDelay -> "Shoot Only | " + startPosition.displayName + " | Delay " + delaySeconds + "s"; + case ShootThenDrive -> "Shoot + Drive | " + startPosition.displayName + " | " + driveOption.displayName; + case ShootWithDelayThenDrive -> "Shoot + Drive | " + startPosition.displayName + " | " + driveOption.displayName + " | Delay " + delaySeconds + "s"; }; - // @formatter:on } private void followTrajectory(SwerveSample sample) { var pose = _driveSubsystem.getState().Pose; - // @formatter:off - _driveSubsystem.setControl(_autoRequest - .withVelocityX(sample.vx + _xController.calculate(pose.getX(), sample.x)) - .withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) - .withRotationalRate(sample.omega + _headingController.calculate(pose.getRotation().getRadians(), sample.heading)) + _driveSubsystem.setControl( + _autoRequest.withVelocityX(sample.vx + _xController.calculate(pose.getX(), sample.x)).withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) + .withRotationalRate(sample.omega + _headingController.calculate(pose.getRotation().getRadians(), sample.heading)) ); - // @formatter:on } private Pose2d flip(Pose2d bluePose) { - var pose = bluePose; - if (Utilities.isRedAlliance()) { - pose = bluePose.rotateAround(new Translation2d(GeneralConstants.FIELD_SIZE_X.div(2), GeneralConstants.FIELD_SIZE_Y.div(2)), Rotation2d.k180deg); + return bluePose.rotateAround(new Translation2d(GeneralConstants.FIELD_SIZE_X.div(2), GeneralConstants.FIELD_SIZE_Y.div(2)), Rotation2d.k180deg); } - return pose; + return bluePose; } } From 993922b2d3d3cb1f28b0759e1ca8c50d086eda25 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Thu, 19 Mar 2026 16:26:51 -0500 Subject: [PATCH 18/32] clean up warnings --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/subsystems/shooter/Rotor.java | 1 - .../frc/robot/subsystems/shooter/Shooter.java | 35 ------------------- 3 files changed, 37 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cb4b9e1..026b48c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Value; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; diff --git a/src/main/java/frc/robot/subsystems/shooter/Rotor.java b/src/main/java/frc/robot/subsystems/shooter/Rotor.java index c2e62fb..585012c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Rotor.java +++ b/src/main/java/frc/robot/subsystems/shooter/Rotor.java @@ -14,7 +14,6 @@ import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.CANConstants; -import frc.robot.Constants.GeneralConstants; import frc.robot.Constants.ShooterConstants; @Logged diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 504ebc4..d5bedb9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -6,7 +6,6 @@ import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; import java.util.function.Supplier; @@ -18,7 +17,6 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.shooter.TurretDirector.ShotMode; @@ -144,39 +142,6 @@ public void stopManualFlywheel() } } - public Command runRotor() - { - return startEnd(() -> _feeder.set(true), () -> _feeder.set(false)).onlyIf(this::inManualMode); - } - - public Command manualShoot() - { - // @formatter:off - return Commands.sequence - ( - runOnce(() -> _flywheel.setVelocity(ShooterConstants.MANUAL_SHOOT_RPM)), - Commands.waitUntil(_flywheel::atSpeed), - startEnd( - () -> - { - _feeder.set(true); - _rotor.set(true); - }, - () -> - { - _feeder.set(false); - _rotor.set(false); - }) - ) - .finallyDo(() -> { - _flywheel.stop(); - _feeder.set(false); - _rotor.set(false); - }) - .onlyIf(this::inManualMode); - // @formatter:on - } - public Command setFlywheelVelocity(AngularVelocity velocity) { return runOnce(() -> From f7da2ea6e7fc20a86459f6d11f31d91e2d2c9014 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Thu, 19 Mar 2026 18:36:25 -0500 Subject: [PATCH 19/32] Refactor Intake and Shooter Subsystems --- src/main/java/frc/robot/Autos.java | 229 +++--------------- src/main/java/frc/robot/Constants.java | 168 +++++++------ src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/subsystems/intake/Intake.java | 155 +++++++----- .../frc/robot/subsystems/shooter/Shooter.java | 28 ++- .../frc/robot/subsystems/shooter/Turret.java | 190 ++++++++------- 6 files changed, 342 insertions(+), 435 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index afe899a..b6790e7 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -1,10 +1,7 @@ package frc.robot; -import java.util.stream.IntStream; - import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; - import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -12,88 +9,45 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.GeneralConstants; -import frc.robot.generated.ChoreoTraj; import frc.robot.generated.ChoreoVars; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Utilities; -public class Autos extends SubsystemBase +public class Autos { private enum AutoMode { - DoNothing("Do Nothing"), DriveOnly("Drive Only"), ShootOnly("Shoot Only"), ShootWithDelay("Shoot + Delay"), ShootThenDrive("Shoot Then Drive"), ShootWithDelayThenDrive("Shoot + Delay + Drive"); - - public final String displayName; - - private AutoMode(String displayName) - { - this.displayName = displayName; - } - } - - private record AutoDriveOption(String displayName, String trajectoryName, Pose2d endPoseBlue) - { - boolean staysPut() - { - return trajectoryName == null; - } + DoNothing, ShootOnly, ShootWithDelay, ShootThenDrive, ShootWithDelayThenDrive } private enum StartPosition { - LeftTrench("Left Trench", ChoreoVars.Poses.LeftTrench, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.LeftTrench), - new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftTrenchToDepot.name(), ChoreoTraj.LeftTrenchToDepot.endPoseBlue())), - LeftBump("Left Bump", ChoreoVars.Poses.LeftBump, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.LeftBump), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftBumpToDepot.name(), ChoreoTraj.LeftBumpToDepot.endPoseBlue()), - new AutoDriveOption("Drive To Tower", ChoreoTraj.LeftBumpToTower.name(), ChoreoTraj.LeftBumpToTower.endPoseBlue()), - new AutoDriveOption("Pickup From Mid", ChoreoTraj.LeftBumpPickupFromMidScore.name(), ChoreoTraj.LeftBumpPickupFromMidScore.endPoseBlue())), - HubStart("Hub", ChoreoVars.Poses.HubStart, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.HubStart), new AutoDriveOption("Drive To Depot", ChoreoTraj.HubToDepot.name(), ChoreoTraj.HubToDepot.endPoseBlue()), - new AutoDriveOption("Drive To Outpost", ChoreoTraj.HubToOutpost.name(), ChoreoTraj.HubToOutpost.endPoseBlue()), new AutoDriveOption("Drive To Tower", ChoreoTraj.HubToTower.name(), ChoreoTraj.HubToTower.endPoseBlue())), - RightBump("Right Bump", ChoreoVars.Poses.RightBump, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.RightBump), - new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightBumpToOutpost.name(), ChoreoTraj.RightBumpToOutpost.endPoseBlue()), - new AutoDriveOption("Drive To Tower", ChoreoTraj.RightBumpToTower.name(), ChoreoTraj.RightBumpToTower.endPoseBlue()), - new AutoDriveOption("Pickup From Mid", ChoreoTraj.RightBumpPickupFromMidScore.name(), ChoreoTraj.RightBumpPickupFromMidScore.endPoseBlue())), - RightTrench("Right Trench", ChoreoVars.Poses.RightTrench, new AutoDriveOption("Stay Put", null, ChoreoVars.Poses.RightTrench), - new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightTrenchToOutpost.name(), ChoreoTraj.RightTrenchToOutpost.endPoseBlue())); + LeftTrench(ChoreoVars.Poses.LeftTrench), LeftBump(ChoreoVars.Poses.LeftBump), HubStart(ChoreoVars.Poses.HubStart), RightBump(ChoreoVars.Poses.RightBump), RightTrench(ChoreoVars.Poses.RightTrench); - public final String displayName; - public final Pose2d pose; - public final AutoDriveOption[] driveOptions; + public final Pose2d pose; - private StartPosition(String displayName, Pose2d pose, AutoDriveOption... driveOptions) + private StartPosition(Pose2d bluePose) { - this.displayName = displayName; - this.pose = pose; - this.driveOptions = driveOptions; + pose = bluePose; } } - private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; - private static final int DEFAULT_AUTO_DELAY_SECS = 0; - private static final StartPosition DEFAULT_START_POSITION = StartPosition.HubStart; - private final AutoFactory _autoFactory; - private final Drive _driveSubsystem; - private final Shooter _shooterSubsystem; - private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); - private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); - private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); - private final SendableChooser _modeChooser = new SendableChooser<>(); - private final SendableChooser _delayChooser = new SendableChooser<>(); - private final SendableChooser _startChooser = new SendableChooser<>(); - private SendableChooser _drivePathChooser = new SendableChooser<>(); - private final Field2d _previewField = new Field2d(); - private final Alert _invalidAutoAlert = new Alert("Selected auto configuration is invalid.", AlertType.kWarning); - private StartPosition _lastStartPosition; + private static final String NO_TRAJECTORY = "None (Stay)"; + private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; + private static final int DEFAULT_AUTO_DELAY_SECS = 0; + private static final StartPosition DEFAULT_START_POSITION = StartPosition.LeftTrench; + private static final String DEFAULT_AUTO_TRAJECTORY = NO_TRAJECTORY; + private final AutoFactory _autoFactory; + private final Drive _driveSubsystem; + private final Shooter _shooterSubsystem; + private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { @@ -103,148 +57,25 @@ public Autos(Drive driveSubsystem, Shooter shooterSubsystem) _headingController.enableContinuousInput(-Math.PI, Math.PI); _autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem); - - configureChoosers(); - rebuildDrivePathChooser(DEFAULT_START_POSITION); - updateDashboardPreview(); - } - - @Override - public void periodic() - { - var selectedStart = getSelectedStartPosition(); - - if (_lastStartPosition != selectedStart) - { - rebuildDrivePathChooser(selectedStart); - } - - updateDashboardPreview(); } public Command buildAuto() { - var startPosition = getSelectedStartPosition(); - var autoMode = getSelectedMode(); - var driveOption = getSelectedDriveOption(startPosition); - var autoDelaySecs = getSelectedDelaySeconds(); - var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(flip(startPosition.pose))); - var shoot = _shooterSubsystem.shoot().withTimeout(4.0); - var delay = Commands.waitSeconds(autoDelaySecs); - var drive = driveOption.staysPut() ? Commands.none() : _autoFactory.trajectoryCmd(driveOption.trajectoryName()); - - return switch (autoMode) - { - case DoNothing -> resetPose; - case DriveOnly -> Commands.sequence(resetPose, drive); - case ShootOnly -> Commands.sequence(resetPose, shoot); - case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot); - case ShootThenDrive -> Commands.sequence(resetPose, shoot, drive); - case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive); - }; - } - - private void configureChoosers() - { - _modeChooser.setDefaultOption(DEFAULT_AUTO_MODE.displayName, DEFAULT_AUTO_MODE); - _modeChooser.addOption(AutoMode.DriveOnly.displayName, AutoMode.DriveOnly); - _modeChooser.addOption(AutoMode.ShootWithDelay.displayName, AutoMode.ShootWithDelay); - _modeChooser.addOption(AutoMode.ShootThenDrive.displayName, AutoMode.ShootThenDrive); - _modeChooser.addOption(AutoMode.ShootWithDelayThenDrive.displayName, AutoMode.ShootWithDelayThenDrive); - _modeChooser.addOption(AutoMode.DoNothing.displayName, AutoMode.DoNothing); - - _delayChooser.setDefaultOption(String.valueOf(DEFAULT_AUTO_DELAY_SECS), DEFAULT_AUTO_DELAY_SECS); - IntStream.rangeClosed(1, 5).forEach(seconds -> _delayChooser.addOption(String.valueOf(seconds), seconds)); - - _startChooser.setDefaultOption(DEFAULT_START_POSITION.displayName, DEFAULT_START_POSITION); - _startChooser.addOption(StartPosition.LeftTrench.displayName, StartPosition.LeftTrench); - _startChooser.addOption(StartPosition.LeftBump.displayName, StartPosition.LeftBump); - _startChooser.addOption(StartPosition.HubStart.displayName, StartPosition.HubStart); - _startChooser.addOption(StartPosition.RightBump.displayName, StartPosition.RightBump); - _startChooser.addOption(StartPosition.RightTrench.displayName, StartPosition.RightTrench); - - SmartDashboard.putData("Auto Mode", _modeChooser); - SmartDashboard.putData("Auto Start Position", _startChooser); - SmartDashboard.putData("Auto Delay", _delayChooser); - SmartDashboard.putData("Auto Drive Path", _drivePathChooser); - SmartDashboard.putData("Autonomous Preview", _previewField); - } - - private void rebuildDrivePathChooser(StartPosition startPosition) - { - var newChooser = new SendableChooser(); - var options = startPosition.driveOptions; - - newChooser.setDefaultOption(options[0].displayName(), options[0]); - - for (int index = 1; index < options.length; index++) - { - var option = options[index]; - newChooser.addOption(option.displayName(), option); - } - - _drivePathChooser = newChooser; - _lastStartPosition = startPosition; - SmartDashboard.putData("Auto Drive Path", _drivePathChooser); - } + var start = DEFAULT_START_POSITION; + var mode = DEFAULT_AUTO_MODE; + var trajName = DEFAULT_AUTO_TRAJECTORY; + var reset = Commands.runOnce(() -> _driveSubsystem.resetPose(flip(start.pose))); + var shoot = _shooterSubsystem.shoot().withTimeout(4.0); + var delay = Commands.waitSeconds(DEFAULT_AUTO_DELAY_SECS); + var drive = NO_TRAJECTORY.equals(trajName) ? Commands.none() : _autoFactory.trajectoryCmd(trajName); - private void updateDashboardPreview() - { - var startPosition = getSelectedStartPosition(); - var autoMode = getSelectedMode(); - var driveOption = getSelectedDriveOption(startPosition); - var startPose = flip(startPosition.pose); - var endPose = flip(driveOption.endPoseBlue()); - - _previewField.setRobotPose(startPose); - _previewField.getObject("Auto End Pose").setPose(endPose); - - SmartDashboard.putString("Auto Summary", buildSummary(autoMode, startPosition, driveOption, getSelectedDelaySeconds())); - SmartDashboard.putString("Auto Selected Trajectory", driveOption.staysPut() ? "None" : driveOption.trajectoryName()); - SmartDashboard.putBoolean("Auto Uses Trajectory", !driveOption.staysPut()); - SmartDashboard.putBoolean("Auto Selection Valid", true); - _invalidAutoAlert.set(false); - } - - private AutoMode getSelectedMode() - { - var selected = _modeChooser.getSelected(); - return selected != null ? selected : DEFAULT_AUTO_MODE; - } - - private int getSelectedDelaySeconds() - { - var selected = _delayChooser.getSelected(); - return selected != null ? selected : DEFAULT_AUTO_DELAY_SECS; - } - - private StartPosition getSelectedStartPosition() - { - var selected = _startChooser.getSelected(); - return selected != null ? selected : DEFAULT_START_POSITION; - } - - private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) - { - var selected = _drivePathChooser.getSelected(); - if (selected != null) - { - return selected; - } - - return startPosition.driveOptions[0]; - } - - private String buildSummary(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption, int delaySeconds) - { return switch (mode) { - case DoNothing -> "Do Nothing | " + startPosition.displayName; - case DriveOnly -> "Drive Only | " + startPosition.displayName + " | " + driveOption.displayName; - case ShootOnly -> "Shoot Only | " + startPosition.displayName; - case ShootWithDelay -> "Shoot Only | " + startPosition.displayName + " | Delay " + delaySeconds + "s"; - case ShootThenDrive -> "Shoot + Drive | " + startPosition.displayName + " | " + driveOption.displayName; - case ShootWithDelayThenDrive -> "Shoot + Drive | " + startPosition.displayName + " | " + driveOption.displayName + " | Delay " + delaySeconds + "s"; + case DoNothing -> reset; + case ShootOnly -> Commands.sequence(reset, shoot); + case ShootWithDelay -> Commands.sequence(reset, delay, shoot); + case ShootThenDrive -> Commands.sequence(reset, shoot, drive); + case ShootWithDelayThenDrive -> Commands.sequence(reset, delay, shoot, drive); }; } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4f1b6c7..fe012fc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -109,20 +109,28 @@ public static class DriveConstants public static class IntakeConstants { - public static final Voltage INTAKE_VOLTS = Volts.of(9.0); - public static final Voltage REVERSE_VOLTS = Volts.of(-9.0); - public static final Current ROLLER_CURRENT_LIMIT = Amps.of(80); - public static final Current EXTENSION_CURRENT_LIMIT = Amps.of(60); - public static final int CAMERA_DEVICE_INDEX = 0; - public static final String CAMERA_NAME = "IntakeCam"; - public static final int CAMERA_WIDTH = 320; - public static final int CAMERA_HEIGHT = 240; - public static final int CAMERA_FPS = 15; - public static final Per EXTENSION_CONVERSION_FACTOR = Inches.of(12).div(Rotations.of(6)); - public static final Distance EXTENSION_MAX_POSITION = Inches.of(12.0); - public static final Distance EXTENSION_MIN_POSITION = Inches.of(0); - public static final Voltage EXTEND_VOLTS = Volts.of(1.5); - public static final Voltage RETRACT_VOLTS = Volts.of(-2.5); + public static final Voltage INTAKE_VOLTS = Volts.of(9.0); + public static final Voltage REVERSE_VOLTS = Volts.of(-9.0); + public static final Current ROLLER_CURRENT_LIMIT_EXTENDED = Amps.of(80); + public static final Current ROLLER_CURRENT_LIMIT_ACTIVE = Amps.of(40); + public static final Current EXTENSION_CURRENT_LIMIT = Amps.of(60); + public static final int CAMERA_DEVICE_INDEX = 0; + public static final String CAMERA_NAME = "IntakeCam"; + public static final int CAMERA_WIDTH = 320; + public static final int CAMERA_HEIGHT = 240; + public static final int CAMERA_FPS = 15; + public static final Per EXTENSION_CONVERSION_FACTOR = Inches.of(12).div(Rotations.of(6)); + public static final Distance EXTENSION_MAX_POSITION = Inches.of(12.0); + public static final Distance EXTENSION_MIN_POSITION = Inches.of(0); + public static final Voltage EXTEND_VOLTS = Volts.of(1.5); + public static final Voltage RETRACT_VOLTS = Volts.of(-2.5); + public static final Voltage JIGGLE_RETRACT_VOLTS = Volts.of(-6.0); + public static final Voltage JIGGLE_EXTEND_VOLTS = Volts.of(2.5); + public static final double JIGGLE_RETRACT_FRACTION = 0.2; + public static final double JIGGLE_RETRACT_STEP_FRACTION = 0.3; + public static final Distance JIGGLE_LIMIT_MARGIN = Inches.of(0.0); + public static final Time JIGGLE_MOVE_TIMEOUT = Seconds.of(0.60); + public static final Time JIGGLE_PAUSE_TIME = Seconds.of(0.02); } public static class GeneralConstants @@ -150,66 +158,67 @@ public static class ShooterConstants public static final AngularVelocity PASS_FLYWHEEL_VELOCITY = RPM.of(3000.0); // TODO: Tune // Turret - public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0); - public static final Dimensionless TURRET_GEAR_RATIO = Value.of(13.0); // 13:1 motor rotations per turret rotation - public static final double TURRET_KP = 2.0; - public static final double TURRET_KS = 0.498; - public static final AngularVelocity TURRET_MM_CRUISE_VELOCITY = RotationsPerSecond.of(54.0); - public static final AngularAcceleration TURRET_MM_ACCELERATION = RotationsPerSecondPerSecond.of(108.0); - public static final double TURRET_MM_EXPO_KV = 0.12; // V/rps - public static final double TURRET_MM_EXPO_KA = 0.1; // V/(rps/s) - public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); - public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); - public static final boolean TURRET_SENSOR_INVERTED = true; - public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); - public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); - public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); - public static final Angle TURRET_TOLERANCE = Degrees.of(1.25); - public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(2.0); - public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(0.1); - public static final Angle TURRET_TRACK_COMMAND_DEADBAND = Degrees.of(0.5); - public static final Angle TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP = Degrees.of(4.0); - public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(1.0); - public static final boolean TURRET_USE_VISION_TX_TRIM = false; - public static final Time TURRET_VISION_TX_FILTER_WINDOW = Milliseconds.of(100.0); - public static final Angle TURRET_VISION_TX_MAX_STEP_PER_LOOP = Degrees.of(0.4); - public static final Angle TURRET_TRACK_BIAS = Degrees.zero(); // TODO: Tune static shot bias from log data - public static final boolean TURRET_USE_LINEAR_DRIFT_COMPENSATION = true; - public static final Distance TURRET_DRIFT_LATERAL_BIAS = Inches.of(11.060660171779821); - public static final Distance TURRET_DRIFT_LATERAL_SINE_AMPLITUDE = Inches.of(7.1890180494515015); - public static final Angle TURRET_DRIFT_LATERAL_PHASE_OFFSET = Degrees.of(-20.866427056070258); - public static final Angle TURRET_LINEAR_DRIFT_MAX_CORRECTION = Degrees.of(15.0); - public static final double TURRET_LINEAR_DRIFT_CORRECTION_SIGN = -1.0; - public static final String LIMELIGHT_NAME = "limelight-shooter"; - public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice - public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); - public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); - public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); - public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); - public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); - public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees - // left of robot forward - public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); - public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); - public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); - public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not - // lateral - public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); - public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); - public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" - public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); - public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); - public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); - public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); - public static final int SWM_LOOKAHEAD_ITERATIONS = 8; - public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); - public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); - public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.5); - public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(45.0); - public static final Angle SWM_FEED_TURRET_TOLERANCE = Degrees.of(5.0); - public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(60.0); - private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; - private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; + public static final Current TURRET_CURRENT_LIMIT = Amps.of(60.0); + public static final double TURRET_KP = 0.19; + public static final double TURRET_KI = 0.01; + public static final double TURRET_KD = 0.0115; + public static final Voltage TURRET_STATIC_FF = Volts.of(0.22); + public static final Angle TURRET_STATIC_FF_ERROR_DEADBAND = Degrees.of(0.75); + public static final Voltage TURRET_MAX_OUTPUT_STEP_PER_LOOP = Volts.of(3.0); + public static final Angle TURRET_HARD_MIN_ANGLE = Degrees.of(-180.0); + public static final Angle TURRET_HARD_MAX_ANGLE = Degrees.of(180.0); + public static final boolean TURRET_SENSOR_INVERTED = true; + public static final Angle TURRET_POT_OFFSET = Degrees.of(-10.0); + public static final Angle TURRET_SOFT_MIN_ANGLE = Degrees.of(-160.0); + public static final Angle TURRET_SOFT_MAX_ANGLE = Degrees.of(160.0); + public static final Angle TURRET_HOME_ANGLE = Degrees.of(0.0); + public static final Angle TURRET_TOLERANCE = Degrees.of(1.25); + public static final Angle TURRET_LINED_UP_HOLD_TOLERANCE = Degrees.of(2.0); + public static final Angle TURRET_TARGET_SETPOINT_DEADBAND = Degrees.of(0.1); + public static final Angle TURRET_MAX_SETPOINT_STEP_PER_LOOP = Degrees.of(4.0); + public static final Angle TURRET_TRACK_COMMAND_DEADBAND = Degrees.of(0.5); + public static final Angle TURRET_TRACK_COMMAND_MAX_STEP_PER_LOOP = Degrees.of(4.0); + public static final Angle TURRET_TRACK_TX_DEADBAND = Degrees.of(1.0); + public static final boolean TURRET_USE_VISION_TX_TRIM = false; + public static final Time TURRET_VISION_TX_FILTER_WINDOW = Milliseconds.of(100.0); + public static final Angle TURRET_VISION_TX_MAX_STEP_PER_LOOP = Degrees.of(0.4); + public static final Angle TURRET_TRACK_BIAS = Degrees.zero(); // TODO: Tune static shot bias from log data + public static final boolean TURRET_USE_LINEAR_DRIFT_COMPENSATION = true; + public static final Distance TURRET_DRIFT_LATERAL_BIAS = Inches.of(11.060660171779821); + public static final Distance TURRET_DRIFT_LATERAL_SINE_AMPLITUDE = Inches.of(7.1890180494515015); + public static final Angle TURRET_DRIFT_LATERAL_PHASE_OFFSET = Degrees.of(-20.866427056070258); + public static final Angle TURRET_LINEAR_DRIFT_MAX_CORRECTION = Degrees.of(15.0); + public static final double TURRET_LINEAR_DRIFT_CORRECTION_SIGN = -1.0; + public static final String LIMELIGHT_NAME = "limelight-shooter"; + public static final Angle TURRET_PASS_TARGET = Degrees.of(180.0); // TODO: Validate in driver practice + public static final Distance TURRET_CENTER_TAG_TO_HUB_CENTER = Inches.of(23.5); + public static final List BLUE_HUB_TAG_IDS = List.of(18, 20, 21, 26); + public static final List RED_HUB_TAG_IDS = List.of(2, 4, 5, 10); + public static final List ALL_HUB_TAG_IDS = Stream.concat(BLUE_HUB_TAG_IDS.stream(), RED_HUB_TAG_IDS.stream()).toList(); + public static final Translation3d CENTER_TAG_TO_HUB_CENTER_OFFSET = new Translation3d(Inches.of(-23.5), Inches.zero(), Inches.zero()); + public static final Angle TURRET_ZERO_OFFSET_FROM_ROBOT_FORWARD = Degrees.of(90); // Hub "zero" is 90 degrees + // left of robot forward + public static final Translation2d BLUE_HUB = new Translation2d(Inches.of(182.1), Inches.of(158.85)); + public static final Translation2d RED_HUB = new Translation2d(Inches.of(469.1), Inches.of(158.85)); + public static final Translation2d ROBOT_TO_TURRET_PIVOT = new Translation2d(Inches.of(-0.5), Inches.of(5.75)); + public static final Translation2d TURRET_PIVOT_TO_RELEASE = new Translation2d(Inches.of(1.0), Inches.zero()); // Measured release point is ~1 in forward of the pivot (turret -90), not + // lateral + public static final Translation2d ROBOT_TO_TURRET_RELEASE = ROBOT_TO_TURRET_PIVOT.plus(TURRET_PIVOT_TO_RELEASE); + public static final Translation2d ROBOT_TO_TURRET_CAMERA = new Translation2d(Inches.zero(), Inches.of(8.0)); + public static final Distance TURRET_LIMELIGHT_HEIGHT = Inches.of(24.625); // 24 5/8" + public static final Distance HUB_TAG_HEIGHT = Inches.of(44.25); + public static final Distance TURRET_TO_HUB_HEIGHT_DELTA = HUB_TAG_HEIGHT.minus(TURRET_LIMELIGHT_HEIGHT); + public static final Angle TURRET_LIMELIGHT_PITCH = Degrees.zero(); + public static final Time SWM_RELEASE_PHASE_DELAY = Seconds.of(0.03); + public static final int SWM_LOOKAHEAD_ITERATIONS = 8; + public static final LinearVelocity SWM_ENABLE_TRANSLATIONAL_SPEED = MetersPerSecond.of(0.15); + public static final AngularVelocity SWM_ENABLE_ANGULAR_SPEED = DegreesPerSecond.of(10.0); + public static final LinearVelocity SWM_FEED_MAX_TRANSLATIONAL_SPEED = MetersPerSecond.of(1.5); + public static final AngularVelocity SWM_FEED_MAX_ANGULAR_SPEED = DegreesPerSecond.of(45.0); + public static final Angle SWM_FEED_TURRET_TOLERANCE = Degrees.of(5.0); + public static final Time SWM_FEED_STABILITY_WINDOW = Milliseconds.of(60.0); + private static final double SWM_MIN_TOF_DISTANCE_IN = 64.0; + private static final double SWM_MAX_TOF_DISTANCE_IN = 159.0; // @formatter:off private static final InterpolatingDoubleTreeMap FLYWHEEL_SPEED_TABLE = InterpolatingDoubleTreeMap.ofEntries @@ -237,11 +246,12 @@ public static class ShooterConstants // @formatter:on // Feeder - public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60); - public static final Voltage FEEDER_VOLTAGE = Volts.of(6); - public static final Current ROTOR_CURRENT_LIMIT = Amps.of(60); - public static final Voltage ROTOR_FAST_VOLTAGE = Volts.of(3.0); - public static final Voltage ROTOR_SLOW_VOLTAGE = Volts.of(1.5); + public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60); + public static final Voltage FEEDER_VOLTAGE = Volts.of(6); + public static final Current ROTOR_CURRENT_LIMIT = Amps.of(60); + public static final Voltage ROTOR_FAST_VOLTAGE = Volts.of(3.0); + public static final Voltage ROTOR_MID_VOLTAGE = Volts.of(1.5); + public static final Voltage ROTOR_RETRACTED_VOLTAGE = Volts.of(1.0); // TODO: Tune these values with testing! public static AngularVelocity getFlywheelSpeedForDistance(Distance distance) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 026b48c..e6076a7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Value; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -36,7 +37,7 @@ public class RobotContainer private final CommandXboxController _operator = new CommandXboxController(1); private final Drive _drive = TunerConstants.createDrivetrain(); private final Intake _intake = new Intake(); - private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended); + private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended, _intake::isRetracted); @NotLogged private final Autos _autos = new Autos(_drive, _shooter); private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; @@ -87,6 +88,10 @@ private void configureBindings() _driver.button(6).whileTrue(_shooter.trackOnly()); _driver.button(7).onTrue(_drive.runOnce(_drive::seedFieldCentric)); _driver.button(8).onTrue(_shooter.homeTurret()); + _driver.button(9).onTrue(_shooter.setManualTurretAngle(Degrees.of(45.0))); + _driver.button(10).onTrue(_shooter.setManualTurretAngle(Degrees.of(-45.0))); + _driver.button(11).onTrue(_shooter.setManualTurretAngle(Degrees.of(90.0))); + _driver.button(12).onTrue(_shooter.setManualTurretAngle(Degrees.of(-90.0))); _operator.leftTrigger().whileTrue(_intake.runRollersForward()); _operator.leftBumper().whileTrue(_intake.runRollersReverse()); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 77f6d05..90a8ae5 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -44,15 +44,16 @@ @Logged public class Intake extends SubsystemBase { + private enum RollerCurrentLimitMode + { + AutoByExtension, ForceActive + } + public enum IntakeState { Off, Forward, Reverse } - /************ - * COMMANDS * - ************/ - public Command runRollersForward() { return startEnd(() -> setIntakeState(IntakeState.Forward), () -> setIntakeState(IntakeState.Off)); @@ -68,50 +69,46 @@ public Command runRollersReverse() return startEnd(() -> setIntakeState(IntakeState.Reverse), () -> setIntakeState(IntakeState.Off)); } - public Command startRollersReverse() - { - return runOnce(() -> setIntakeState(IntakeState.Reverse)); - } - - public Command stopRollers() - { - return runOnce(() -> setIntakeState(IntakeState.Off)); - } - public Command jiggle() { - // @formatter:off - return - runOnce(() -> setIntakeState(IntakeState.Forward)) - .andThen - ( - Commands.repeatingSequence - ( - getExtendCmd(), - Commands.waitSeconds(0.4), - setExtensionCmd(false), - Commands.waitUntil(this::isRetracted) + Distance extensionRange = IntakeConstants.EXTENSION_MAX_POSITION.minus(IntakeConstants.EXTENSION_MIN_POSITION); + double extensionRangeInches = extensionRange.in(Inches); + + return runOnce(() -> + { + setIntakeVoltage(GeneralConstants.MOTOR_VOLTAGE); + setRollerCurrentLimitMode(RollerCurrentLimitMode.ForceActive); + _jiggleRetractFraction = IntakeConstants.JIGGLE_RETRACT_FRACTION; + _jiggleRetractTravelInches = extensionRangeInches * _jiggleRetractFraction; + }).andThen( + Commands.sequence( + runOnce(() -> setExtensionVoltage(IntakeConstants.JIGGLE_EXTEND_VOLTS)), Commands.waitUntil(this::isExtended).withTimeout(IntakeConstants.JIGGLE_MOVE_TIMEOUT.in(Seconds)), + runOnce(() -> setExtensionVoltage(Volts.zero())), Commands.waitSeconds(IntakeConstants.JIGGLE_PAUSE_TIME.in(Seconds)), Commands.repeatingSequence(runOnce(() -> + { + _jiggleLegStartInches = getMotorPosition().in(Inches); + _jiggleRetractTravelInches = extensionRangeInches * _jiggleRetractFraction; + }), runOnce(() -> setExtensionVoltage(IntakeConstants.JIGGLE_RETRACT_VOLTS)), + Commands.waitUntil(() -> isRetracted() || Math.abs(getMotorPosition().in(Inches) - _jiggleLegStartInches) >= _jiggleRetractTravelInches).withTimeout(IntakeConstants.JIGGLE_MOVE_TIMEOUT.in(Seconds)), + runOnce(() -> setExtensionVoltage(Volts.zero())), Commands.waitSeconds(IntakeConstants.JIGGLE_PAUSE_TIME.in(Seconds)), runOnce(() -> setExtensionVoltage(IntakeConstants.JIGGLE_EXTEND_VOLTS)), + Commands.waitUntil(this::isExtended).withTimeout(IntakeConstants.JIGGLE_MOVE_TIMEOUT.in(Seconds)), runOnce(() -> setExtensionVoltage(Volts.zero())), + Commands.waitSeconds(IntakeConstants.JIGGLE_PAUSE_TIME.in(Seconds)), runOnce(() -> _jiggleRetractFraction = Math.min(1.0, _jiggleRetractFraction + IntakeConstants.JIGGLE_RETRACT_STEP_FRACTION)) + ) ) - ) - .finallyDo(() -> { - setIntakeState(IntakeState.Off); - extend(false); - }) - .onlyIf(this::isRetracted); - // @formatter:on + ).finallyDo(() -> + { + setIntakeState(IntakeState.Off); + setRollerCurrentLimitMode(RollerCurrentLimitMode.AutoByExtension); + setExtensionVoltage(Volts.zero()); + }); } public Command getRetractCmd() { - // @formatter:off - return - Commands.sequence - ( - startRollersForward(), - setExtensionCmd(false), - Commands.waitUntil(this::isRetracted) - ).finallyDo(() -> setIntakeState(IntakeState.Off)); - // @formatter:on + return Commands.sequence(runOnce(() -> setRollerCurrentLimitMode(RollerCurrentLimitMode.ForceActive)), startRollersForward(), setExtensionCmd(false), Commands.waitUntil(this::isRetracted)).finallyDo(() -> + { + setIntakeState(IntakeState.Off); + setRollerCurrentLimitMode(RollerCurrentLimitMode.AutoByExtension); + }); } @NotLogged @@ -125,9 +122,6 @@ private Command setExtensionCmd(boolean finalState) return runOnce(() -> extend(finalState)); } - /************* - * SUBSYSTEM * - *************/ private final SparkFlex _intakeMotor; private final SparkFlex _extendMotor; private final SparkLimitSwitch _outLimitSwitch; @@ -137,18 +131,30 @@ private Command setExtensionCmd(boolean finalState) private final DCMotor _neoVortex; private final DCMotor _extensionMotorModel; private final Alert _limitSwitchAlert; + @NotLogged + private final SparkFlexConfig _rollerBaseConfig; + @NotLogged + private double _jiggleLegStartInches = 0.0; + @NotLogged + private double _jiggleRetractTravelInches = 0.0; + @NotLogged + private double _jiggleRetractFraction = IntakeConstants.JIGGLE_RETRACT_FRACTION; + @NotLogged + private RollerCurrentLimitMode _rollerCurrentLimitMode = RollerCurrentLimitMode.AutoByExtension; + @NotLogged + private int _lastAppliedCurrentLimitAmps = Integer.MIN_VALUE; @Logged - private IntakeState _intakeState = IntakeState.Off; + private IntakeState _intakeState = IntakeState.Off; @Logged - private Voltage _intakeMotorVoltage = Volts.of(0.0); + private Voltage _intakeMotorVoltage = Volts.of(0.0); @Logged - private Distance _currentExtension = Inches.zero(); + private Distance _currentExtension = Inches.zero(); @Logged - private Voltage _motorVoltage = Volts.zero(); + private Voltage _motorVoltage = Volts.zero(); @Logged - private boolean _outSwitchTriggered = false; + private boolean _outSwitchTriggered = false; @Logged - private boolean _inSwitchTriggered = false; + private boolean _inSwitchTriggered = false; public Intake() { @@ -173,10 +179,9 @@ public Intake() _intakeMotor = new SparkFlex(CANConstants.INTAKE, MotorType.kBrushless); - var intakeConfig = new SparkFlexConfig(); - intakeConfig.inverted(false).idleMode(IdleMode.kBrake).smartCurrentLimit((int)IntakeConstants.ROLLER_CURRENT_LIMIT.in(Amps)).voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts)); - - _intakeMotor.configure(intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + _rollerBaseConfig = new SparkFlexConfig(); + _rollerBaseConfig.inverted(false).idleMode(IdleMode.kBrake).voltageCompensation(GeneralConstants.MOTOR_VOLTAGE.in(Volts)); + applyRollerCurrentLimit((int)IntakeConstants.ROLLER_CURRENT_LIMIT_ACTIVE.in(Amps)); if (RobotBase.isReal()) { @@ -204,6 +209,8 @@ public void periodic() _currentExtension = Inches.of(_extendMotor.getEncoder().getPosition()); _motorVoltage = Volts.of(_extendMotor.getAppliedOutput() * _extendMotor.getBusVoltage()); _intakeMotorVoltage = Volts.of(_intakeMotor.getAppliedOutput() * _intakeMotor.getBusVoltage()); + + applyDesiredRollerCurrentLimit(); } @Override @@ -231,7 +238,7 @@ public void simulationPeriodic() public void extend(boolean finalState) { - _extendMotor.setVoltage(finalState ? IntakeConstants.EXTEND_VOLTS : IntakeConstants.RETRACT_VOLTS); + setExtensionVoltage(finalState ? IntakeConstants.EXTEND_VOLTS : IntakeConstants.RETRACT_VOLTS); } public Voltage getMotorVoltage() @@ -278,6 +285,44 @@ private void setIntakeVoltage(Voltage volts) } } + private void setExtensionVoltage(Voltage volts) + { + _extendMotor.setVoltage(volts); + + if (RobotBase.isSimulation()) + { + _extensionMotorSim.setAppliedOutput(volts.div(GeneralConstants.MOTOR_VOLTAGE).in(Value)); + } + } + + private void setRollerCurrentLimitMode(RollerCurrentLimitMode mode) + { + _rollerCurrentLimitMode = mode; + applyDesiredRollerCurrentLimit(); + } + + private void applyDesiredRollerCurrentLimit() + { + int desiredAmps = switch (_rollerCurrentLimitMode) + { + case ForceActive -> (int)IntakeConstants.ROLLER_CURRENT_LIMIT_ACTIVE.in(Amps); + case AutoByExtension -> isExtended() ? (int)IntakeConstants.ROLLER_CURRENT_LIMIT_EXTENDED.in(Amps) : (int)IntakeConstants.ROLLER_CURRENT_LIMIT_ACTIVE.in(Amps); + }; + + applyRollerCurrentLimit(desiredAmps); + } + + private void applyRollerCurrentLimit(int amps) + { + if (amps == _lastAppliedCurrentLimitAmps) + { + return; + } + + _intakeMotor.configure(_rollerBaseConfig.smartCurrentLimit(amps), ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + _lastAppliedCurrentLimitAmps = amps; + } + public IntakeState getIntakeState() { return _intakeState; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d5bedb9..c3ec92a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -37,6 +37,7 @@ public enum ShooterState public final TurretDirector _turretDirector; private final Supplier _swerveStateSupplier; private final Supplier _intakeExtendedSupplier; + private final Supplier _intakeRetractedSupplier; private final Debouncer _movingFeedDebouncer; @Logged private ShooterState _state; @@ -68,10 +69,11 @@ public enum ShooterState @Logged private boolean _feedReady; - public Shooter(Supplier swerveStateSupplier, Supplier intakeExtendedSupplier) + public Shooter(Supplier swerveStateSupplier, Supplier intakeExtendedSupplier, Supplier intakeRetractedSupplier) { _swerveStateSupplier = swerveStateSupplier; _intakeExtendedSupplier = intakeExtendedSupplier; + _intakeRetractedSupplier = intakeRetractedSupplier; _flywheel = new Flywheel(); _feeder = new Feeder(); _rotor = new Rotor(); @@ -188,7 +190,12 @@ public Command stop() public Command homeTurret() { - return runOnce(() -> setManualTurretAngle(ShooterConstants.TURRET_HOME_ANGLE)); + return setManualTurretAngle(ShooterConstants.TURRET_HOME_ANGLE); + } + + public Command setManualTurretAngle(Angle angle) + { + return runOnce(() -> setManualTurretAngleCommand(angle)); } public Command trackOnly() @@ -321,10 +328,9 @@ private void beginManualControl(boolean clearTurretTarget) } } - private void setManualTurretAngle(Angle angle) + private void setManualTurretAngleCommand(Angle angle) { beginManualControl(true); - _turret.setDisabled(false); _turret.setManualAngle(angle); } @@ -387,12 +393,8 @@ private boolean isReadyToFeed() _inRangeReady = hasValidDistanceForCurrentShot(); _rolloutReady = isWithinMovingFeedRolloutEnvelope(); - // Temporary rollout override: keep logging hub-active state, but do not block - // feed on it while we validate the rest of the moving-shot gate. var rawFeedReady = _solutionReady && _turretReady && _flywheelReady && _inRangeReady && _rolloutReady; - // Keep moving feed behind an explicit gate and short stability window so we - // can roll it out conservatively and see which bit is blocking launch. _stabilityReady = !_usingMovingShotMath || _movingFeedDebouncer.calculate(rawFeedReady); _feedReady = rawFeedReady && _stabilityReady; return _feedReady; @@ -432,8 +434,6 @@ private boolean hasValidDistanceForCurrentShot() private boolean shouldUseMovingShotMath() { - // Keep track-mode behavior consistent between `trackOnly` and `shoot`: - // both should run the moving-shot calculator path. return _requestedShotMode == ShotMode.Track; } @@ -480,7 +480,13 @@ private void setRotorEnabled(boolean enabled) return; } - var rotorVoltage = _intakeExtendedSupplier.get() ? ShooterConstants.ROTOR_FAST_VOLTAGE : ShooterConstants.ROTOR_SLOW_VOLTAGE; + if (_intakeExtendedSupplier.get()) + { + _rotor.setVoltage(ShooterConstants.ROTOR_FAST_VOLTAGE); + return; + } + + var rotorVoltage = _intakeRetractedSupplier.get() ? ShooterConstants.ROTOR_RETRACTED_VOLTAGE : ShooterConstants.ROTOR_MID_VOLTAGE; _rotor.setVoltage(rotorVoltage); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 4153f76..e49274e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -2,26 +2,18 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.FeedbackConfigs; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; -import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.AnalogPotentiometer; @@ -40,34 +32,35 @@ enum ControlMode Idle, TargetAngle, ManualAngle } - private final TalonFX _turretMotor; - private final AnalogPotentiometer _turretPotentiometer; - private final MotionMagicExpoVoltage _motionMagicRequest; - private final NeutralOut _neutralRequest; - private ControlMode _controlMode; - private boolean _encoderSeeded; + private final TalonFX _turretMotor; + private final AnalogPotentiometer _turretPotentiometer; + private final PIDController _pidController; + private ControlMode _controlMode; @Logged - private Angle _manualAngleSetpoint; + private Angle _manualAngleSetpoint; @Logged - private Angle _targetAngleSetpoint; + private Angle _targetAngleSetpoint; @Logged - private Angle _turretAngle; + private Angle _turretAngle; @Logged - private Angle _turretSetpoint; + private Angle _rawTurretAngle; @Logged - private Angle _commandedTargetAngle; + private Angle _turretSetpoint; @Logged - private Angle _targetAngleError; + private Angle _commandedTargetAngle; @Logged - private boolean _hasSetpoint; + private Angle _targetAngleError; @Logged - private Voltage _motorVoltage; + private boolean _hasSetpoint; @Logged - private boolean _disabled; + private Voltage _motorVoltage; + private Voltage _lastCommandedMotorVoltage; @Logged - private boolean _linedUp; + private boolean _disabled; @Logged - private double _motorPositionRotations; + private boolean _linedUp; + @Logged + private double _motorPositionRotations; public Turret() { @@ -80,78 +73,45 @@ public Turret() sensorOffset = sensorOffset.unaryMinus(); } - _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); - _turretPotentiometer = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); - _motionMagicRequest = new MotionMagicExpoVoltage(0); - _neutralRequest = new NeutralOut(); - _controlMode = ControlMode.Idle; - _encoderSeeded = false; - _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _turretAngle = Degrees.zero(); - _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; - _commandedTargetAngle = ShooterConstants.TURRET_HOME_ANGLE; - _targetAngleError = Degrees.zero(); - _hasSetpoint = false; - _motorVoltage = Volts.zero(); - _disabled = false; - _linedUp = false; - _motorPositionRotations = 0.0; - - var talonConfig = new TalonFXConfiguration(); - - // Current limits + _turretMotor = new TalonFX(CANConstants.TURRET_MOTOR); + _turretPotentiometer = new AnalogPotentiometer(AIOConstants.TURRET_POTENTIOMETER, sensorRange.in(Degrees), sensorOffset.in(Degrees)); + _pidController = new PIDController(ShooterConstants.TURRET_KP, ShooterConstants.TURRET_KI, ShooterConstants.TURRET_KD); + _controlMode = ControlMode.Idle; + _manualAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _targetAngleSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _turretAngle = Degrees.zero(); + _rawTurretAngle = Degrees.zero(); + _turretSetpoint = ShooterConstants.TURRET_HOME_ANGLE; + _commandedTargetAngle = ShooterConstants.TURRET_HOME_ANGLE; + _targetAngleError = Degrees.zero(); + _hasSetpoint = false; + _motorVoltage = Volts.zero(); + _lastCommandedMotorVoltage = Volts.zero(); + _disabled = false; + _linedUp = false; + _motorPositionRotations = 0.0; + var currentConfig = new CurrentLimitsConfigs(); currentConfig.StatorCurrentLimit = ShooterConstants.TURRET_CURRENT_LIMIT.in(Amps); currentConfig.StatorCurrentLimitEnable = true; - // Motor output var outputConfig = new MotorOutputConfigs(); outputConfig.NeutralMode = NeutralModeValue.Brake; outputConfig.Inverted = InvertedValue.Clockwise_Positive; - // Feedback (gear ratio: motor rotations to mechanism rotations) - var feedbackConfig = new FeedbackConfigs(); - feedbackConfig.SensorToMechanismRatio = ShooterConstants.TURRET_GEAR_RATIO.magnitude(); - - // PID Slot 0 gains - var slot0Config = new Slot0Configs(); - slot0Config.kP = ShooterConstants.TURRET_KP; - slot0Config.kS = ShooterConstants.TURRET_KS; - - // Motion Magic Expo settings - var motionMagicConfig = new MotionMagicConfigs(); - motionMagicConfig.MotionMagicCruiseVelocity = ShooterConstants.TURRET_MM_CRUISE_VELOCITY.in(RotationsPerSecond); - motionMagicConfig.MotionMagicAcceleration = ShooterConstants.TURRET_MM_ACCELERATION.in(RotationsPerSecondPerSecond); - motionMagicConfig.MotionMagicExpo_kV = ShooterConstants.TURRET_MM_EXPO_KV; - motionMagicConfig.MotionMagicExpo_kA = ShooterConstants.TURRET_MM_EXPO_KA; - - // Software limits (in mechanism rotations, i.e. turret rotations) - var softLimitConfig = new SoftwareLimitSwitchConfigs(); - softLimitConfig.ForwardSoftLimitEnable = true; - softLimitConfig.ForwardSoftLimitThreshold = ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Rotations); - softLimitConfig.ReverseSoftLimitEnable = true; - softLimitConfig.ReverseSoftLimitThreshold = ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Rotations); - - talonConfig.withCurrentLimits(currentConfig).withMotorOutput(outputConfig).withFeedback(feedbackConfig).withSlot0(slot0Config).withMotionMagic(motionMagicConfig).withSoftwareLimitSwitch(softLimitConfig); - - _turretMotor.getConfigurator().apply(talonConfig); + _turretMotor.getConfigurator().apply(new TalonFXConfiguration().withCurrentLimits(currentConfig).withMotorOutput(outputConfig)); + _pidController.setTolerance(ShooterConstants.TURRET_TOLERANCE.in(Degrees)); } public void periodic() { - // Seed motor encoder from potentiometer on first run - if (!_encoderSeeded) - { - var potAngle = Degrees.of(_turretPotentiometer.get()); - _turretMotor.setPosition(potAngle.in(Rotations)); - _encoderSeeded = true; - } - - _motorPositionRotations = _turretMotor.getPosition().getValue().in(Rotations); - _turretAngle = Degrees.of(_motorPositionRotations * 360.0); + _rawTurretAngle = Degrees.of(_turretPotentiometer.get()); + _turretAngle = _rawTurretAngle.plus(ShooterConstants.TURRET_POT_OFFSET); + _motorPositionRotations = _turretMotor.getPosition().getValue().baseUnitMagnitude(); _motorVoltage = _turretMotor.getMotorVoltage().getValue(); + var motorOutput = Volts.zero(); + switch (_controlMode) { case TargetAngle: @@ -171,6 +131,11 @@ public void periodic() break; } + if (_hasSetpoint) + { + _turretSetpoint = limitSetpointStep(_commandedTargetAngle, _turretSetpoint); + } + updateLinedUpState(); _commandedTargetAngle = _turretSetpoint; @@ -178,13 +143,22 @@ public void periodic() if (_hasSetpoint && !_disabled) { - var setpointRotations = _turretSetpoint.in(Degrees) / 360.0; - _turretMotor.setControl(_motionMagicRequest.withPosition(setpointRotations)); - } - else - { - _turretMotor.setControl(_neutralRequest); + var errorDegrees = _turretSetpoint.minus(_turretAngle).in(Degrees); + var outputVolts = _pidController.calculate(_turretAngle.in(Degrees), _turretSetpoint.in(Degrees)); + var staticFFVolts = ShooterConstants.TURRET_STATIC_FF.in(Volts); + + if (!MathUtil.isNear(0.0, errorDegrees, ShooterConstants.TURRET_STATIC_FF_ERROR_DEADBAND.in(Degrees))) + { + outputVolts += Math.copySign(staticFFVolts, errorDegrees); + } + + motorOutput = Volts.of(outputVolts); } + + motorOutput = Volts.of(limitOutputStep(motorOutput.in(Volts))); + motorOutput = applySoftLimit(motorOutput); + _turretMotor.setVoltage(motorOutput.in(Volts)); + _lastCommandedMotorVoltage = motorOutput; } public void simulationPeriodic() @@ -287,6 +261,42 @@ static Angle chooseNearestLegalAngle(Angle currentAngle, Angle requestedAngle) return Degrees.of(MathUtil.clamp(requestedDegrees, minDegrees, maxDegrees)); } + private Angle limitSetpointStep(Angle previousSetpoint, Angle requestedSetpoint) + { + var maxStepDeg = ShooterConstants.TURRET_MAX_SETPOINT_STEP_PER_LOOP.in(Degrees); + var deltaDeg = requestedSetpoint.minus(previousSetpoint).in(Degrees); + var limitedDeltaDeg = MathUtil.clamp(deltaDeg, -maxStepDeg, maxStepDeg); + var limitedSetpoint = previousSetpoint.plus(Degrees.of(limitedDeltaDeg)); + return selectLegalSetpoint(limitedSetpoint); + } + + private double limitOutputStep(double requestedVolts) + { + var previousVolts = _lastCommandedMotorVoltage.in(Volts); + var maxStepVolts = ShooterConstants.TURRET_MAX_OUTPUT_STEP_PER_LOOP.in(Volts); + var deltaVolts = requestedVolts - previousVolts; + var limitedDeltaVolts = MathUtil.clamp(deltaVolts, -maxStepVolts, maxStepVolts); + return previousVolts + limitedDeltaVolts; + } + + private Voltage applySoftLimit(Voltage requestedVoltage) + { + var requestedVolts = requestedVoltage.in(Volts); + var turretDegrees = _turretAngle.in(Degrees); + + if (turretDegrees <= ShooterConstants.TURRET_SOFT_MIN_ANGLE.in(Degrees) && requestedVolts < 0.0) + { + return Volts.zero(); + } + + if (turretDegrees >= ShooterConstants.TURRET_SOFT_MAX_ANGLE.in(Degrees) && requestedVolts > 0.0) + { + return Volts.zero(); + } + + return requestedVoltage; + } + private void updateLinedUpState() { if (!_hasSetpoint || _disabled) From 325999d540c3a368233ddbcdf7f9f87beda2ddd5 Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Fri, 20 Mar 2026 14:34:50 -0500 Subject: [PATCH 20/32] Auto maybe? --- src/main/deploy/elastic-layout.json | 48 ---- src/main/java/frc/robot/Autos.java | 304 +++++++++++++++++++++++-- src/main/java/frc/robot/Constants.java | 2 +- 3 files changed, 281 insertions(+), 73 deletions(-) diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index ec6c1f8..6b7f361 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -107,54 +107,6 @@ "data_type": "string", "show_submit_button": false } - }, - { - "title": "Selected Trajectory", - "x": 1536.0, - "y": 640.0, - "width": 384.0, - "height": 64.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Auto Selected Trajectory", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Uses Trajectory", - "x": 1536.0, - "y": 704.0, - "width": 192.0, - "height": 64.0, - "type": "Boolean Box", - "properties": { - "topic": "/SmartDashboard/Auto Uses Trajectory", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Selection Valid", - "x": 1728.0, - "y": 704.0, - "width": 192.0, - "height": 64.0, - "type": "Boolean Box", - "properties": { - "topic": "/SmartDashboard/Auto Selection Valid", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } } ] } diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index b6790e7..b39409d 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -1,5 +1,7 @@ package frc.robot; +import java.util.stream.IntStream; + import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -9,45 +11,106 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.GeneralConstants; +import frc.robot.generated.ChoreoTraj; import frc.robot.generated.ChoreoVars; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Utilities; -public class Autos +public class Autos extends SubsystemBase { private enum AutoMode { - DoNothing, ShootOnly, ShootWithDelay, ShootThenDrive, ShootWithDelayThenDrive + DoNothing("Do Nothing"), DriveOnly("Drive Only"), ShootOnly("Shoot Only"), ShootWithDelay("Shoot + Delay"), ShootThenDrive("Shoot Then Drive"), ShootWithDelayThenDrive("Shoot + Delay + Drive"); + + public final String displayName; + + private AutoMode(String displayName) + { + this.displayName = displayName; + } + + public boolean usesDrivePath() + { + return this == DriveOnly || this == ShootThenDrive || this == ShootWithDelayThenDrive; + } + + public boolean usesDelay() + { + return this == ShootWithDelay || this == ShootWithDelayThenDrive; + } + } + + private record AutoDriveOption(String displayName, ChoreoTraj trajectory) + { + boolean staysPut() + { + return trajectory == null; + } + + String key() + { + return staysPut() ? STAY_PUT_KEY : trajectory.name(); + } + + Pose2d endPoseBlue() + { + return staysPut() ? null : trajectory.endPoseBlue(); + } } private enum StartPosition { - LeftTrench(ChoreoVars.Poses.LeftTrench), LeftBump(ChoreoVars.Poses.LeftBump), HubStart(ChoreoVars.Poses.HubStart), RightBump(ChoreoVars.Poses.RightBump), RightTrench(ChoreoVars.Poses.RightTrench); + LeftTrench("Left Trench", ChoreoVars.Poses.LeftTrench, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftTrenchToDepot)), + LeftBump("Left Bump", ChoreoVars.Poses.LeftBump, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftBumpToDepot), new AutoDriveOption("Drive To Tower", ChoreoTraj.LeftBumpToTower), + new AutoDriveOption("Pickup From Mid", ChoreoTraj.LeftBumpPickupFromMidScore)), + Hub("Hub", ChoreoVars.Poses.HubStart, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.HubToDepot), new AutoDriveOption("Drive To Outpost", ChoreoTraj.HubToOutpost), + new AutoDriveOption("Drive To Tower", ChoreoTraj.HubToTower)), + RightBump("Right Bump", ChoreoVars.Poses.RightBump, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightBumpToOutpost), new AutoDriveOption("Drive To Tower", ChoreoTraj.RightBumpToTower), + new AutoDriveOption("Pickup From Mid", ChoreoTraj.RightBumpPickupFromMidScore)), + RightTrench("Right Trench", ChoreoVars.Poses.RightTrench, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightTrenchToOutpost)); - public final Pose2d pose; + public final String displayName; + public final Pose2d blueStartPose; + public final AutoDriveOption[] driveOptions; - private StartPosition(Pose2d bluePose) + private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption... driveOptions) { - pose = bluePose; + this.displayName = displayName; + this.blueStartPose = blueStartPose; + this.driveOptions = driveOptions; } } - private static final String NO_TRAJECTORY = "None (Stay)"; + private static final String STAY_PUT_KEY = "__STAY_PUT__"; + private static final String AUTO_MODE_CHOOSER_NAME = "Auto Mode"; + private static final String AUTO_START_CHOOSER_NAME = "Auto Start Position"; + private static final String AUTO_DRIVE_CHOOSER_NAME = "Auto Drive Path"; + private static final String AUTO_DELAY_CHOOSER_NAME = "Auto Delay"; private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; - private static final int DEFAULT_AUTO_DELAY_SECS = 0; - private static final StartPosition DEFAULT_START_POSITION = StartPosition.LeftTrench; - private static final String DEFAULT_AUTO_TRAJECTORY = NO_TRAJECTORY; - private final AutoFactory _autoFactory; + private static final StartPosition DEFAULT_START_POSITION = StartPosition.Hub; + private static final int DEFAULT_DELAY_SECONDS = 0; private final Drive _driveSubsystem; private final Shooter _shooterSubsystem; + private final AutoFactory _autoFactory; private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); + private final SendableChooser _modeChooser = new SendableChooser<>(); + private final SendableChooser _startChooser = new SendableChooser<>(); + private final SendableChooser _delayChooser = new SendableChooser<>(); + private SendableChooser _driveChooser = new SendableChooser<>(); + private final Field2d _previewField = new Field2d(); + private StartPosition _lastStartPosition = null; public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { @@ -55,30 +118,223 @@ public Autos(Drive driveSubsystem, Shooter shooterSubsystem) _shooterSubsystem = shooterSubsystem; _headingController.enableContinuousInput(-Math.PI, Math.PI); - _autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem); + + configureChoosers(); + rebuildDriveChooser(DEFAULT_START_POSITION); + updateDashboard(); + } + + @Override + public void periodic() + { + var selectedStart = getSelectedStartPosition(); + if (_lastStartPosition != selectedStart) + { + rebuildDriveChooser(selectedStart); + } + + updateDashboard(); } public Command buildAuto() { - var start = DEFAULT_START_POSITION; - var mode = DEFAULT_AUTO_MODE; - var trajName = DEFAULT_AUTO_TRAJECTORY; - var reset = Commands.runOnce(() -> _driveSubsystem.resetPose(flip(start.pose))); - var shoot = _shooterSubsystem.shoot().withTimeout(4.0); - var delay = Commands.waitSeconds(DEFAULT_AUTO_DELAY_SECS); - var drive = NO_TRAJECTORY.equals(trajName) ? Commands.none() : _autoFactory.trajectoryCmd(trajName); + var mode = getSelectedMode(); + var startPosition = getSelectedStartPosition(); + var delaySeconds = getSelectedDelaySeconds(); + var driveOption = getSelectedDriveOption(startPosition); + var startPose = flip(startPosition.blueStartPose); + var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); + var shoot = _shooterSubsystem.shoot().withTimeout(4.0); + var delay = Commands.waitSeconds(delaySeconds); + var drive = driveOption.staysPut() ? Commands.none() : _autoFactory.trajectoryCmd(driveOption.trajectory().name()); return switch (mode) { - case DoNothing -> reset; - case ShootOnly -> Commands.sequence(reset, shoot); - case ShootWithDelay -> Commands.sequence(reset, delay, shoot); - case ShootThenDrive -> Commands.sequence(reset, shoot, drive); - case ShootWithDelayThenDrive -> Commands.sequence(reset, delay, shoot, drive); + case DoNothing -> resetPose; + case DriveOnly -> Commands.sequence(resetPose, drive); + case ShootOnly -> Commands.sequence(resetPose, shoot); + case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot); + case ShootThenDrive -> Commands.sequence(resetPose, shoot, drive); + case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive); }; } + private void configureChoosers() + { + _modeChooser.setDefaultOption(DEFAULT_AUTO_MODE.displayName, DEFAULT_AUTO_MODE.name()); + for (var mode : AutoMode.values()) + { + if (mode != DEFAULT_AUTO_MODE) + { + _modeChooser.addOption(mode.displayName, mode.name()); + } + } + + _startChooser.setDefaultOption(DEFAULT_START_POSITION.displayName, DEFAULT_START_POSITION.name()); + for (var startPosition : StartPosition.values()) + { + if (startPosition != DEFAULT_START_POSITION) + { + _startChooser.addOption(startPosition.displayName, startPosition.name()); + } + } + + _delayChooser.setDefaultOption(String.valueOf(DEFAULT_DELAY_SECONDS), String.valueOf(DEFAULT_DELAY_SECONDS)); + IntStream.rangeClosed(1, 5).forEach(seconds -> _delayChooser.addOption(String.valueOf(seconds), String.valueOf(seconds))); + + SmartDashboard.putData(AUTO_MODE_CHOOSER_NAME, _modeChooser); + SmartDashboard.putData(AUTO_START_CHOOSER_NAME, _startChooser); + SmartDashboard.putData(AUTO_DELAY_CHOOSER_NAME, _delayChooser); + SmartDashboard.putData(AUTO_DRIVE_CHOOSER_NAME, _driveChooser); + SmartDashboard.putData("Autonomous Preview", _previewField); + } + + private void rebuildDriveChooser(StartPosition startPosition) + { + var chooser = new SendableChooser(); + var options = startPosition.driveOptions; + var defaultKey = firstDrivePathKey(startPosition); + + for (var option : options) + { + if (option.key().equals(defaultKey)) + { + chooser.setDefaultOption(option.displayName(), option.key()); + } + else + { + chooser.addOption(option.displayName(), option.key()); + } + } + + _driveChooser = chooser; + _lastStartPosition = startPosition; + SmartDashboard.putData(AUTO_DRIVE_CHOOSER_NAME, _driveChooser); + } + + private void updateDashboard() + { + var mode = getSelectedMode(); + var startPosition = getSelectedStartPosition(); + var driveOption = getSelectedDriveOption(startPosition); + var startPose = flip(startPosition.blueStartPose); + var endPose = driveOption.staysPut() ? startPose : flip(driveOption.endPoseBlue()); + + _previewField.setRobotPose(startPose); + _previewField.getObject("Auto End Pose").setPose(endPose); + + SmartDashboard.putString("Auto Summary", buildSummary(mode, startPosition, driveOption, getSelectedDelaySeconds())); + } + + private AutoMode getSelectedMode() + { + var selected = getSelectedChooserValue(AUTO_MODE_CHOOSER_NAME, DEFAULT_AUTO_MODE.name()); + for (var mode : AutoMode.values()) + { + if (mode.name().equals(selected)) + { + return mode; + } + } + + return DEFAULT_AUTO_MODE; + } + + private StartPosition getSelectedStartPosition() + { + var selected = getSelectedChooserValue(AUTO_START_CHOOSER_NAME, DEFAULT_START_POSITION.name()); + for (var startPosition : StartPosition.values()) + { + if (startPosition.name().equals(selected)) + { + return startPosition; + } + } + + return DEFAULT_START_POSITION; + } + + private int getSelectedDelaySeconds() + { + var selected = getSelectedChooserValue(AUTO_DELAY_CHOOSER_NAME, String.valueOf(DEFAULT_DELAY_SECONDS)); + + try + { + return Integer.parseInt(selected); + } + catch (NumberFormatException exception) + { + return DEFAULT_DELAY_SECONDS; + } + } + + private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) + { + var fallback = firstDrivePathKey(startPosition); + var selected = getSelectedChooserValue(AUTO_DRIVE_CHOOSER_NAME, fallback); + + for (var option : startPosition.driveOptions) + { + if (option.key().equals(selected)) + { + return option; + } + } + + for (var option : startPosition.driveOptions) + { + if (option.key().equals(fallback)) + { + return option; + } + } + + return startPosition.driveOptions[0]; + } + + private String getSelectedChooserValue(String chooserName, String fallback) + { + var selected = NetworkTableInstance.getDefault().getTable("SmartDashboard").getSubTable(chooserName).getEntry("selected").getString(""); + if (!selected.isEmpty()) + { + return selected; + } + + return fallback; + } + + private String buildSummary(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption, int delaySeconds) + { + var summary = new StringBuilder(); + summary.append(mode.displayName).append(" | ").append(startPosition.displayName); + + if (mode.usesDelay()) + { + summary.append(" | ").append(delaySeconds).append("s Delay"); + } + + if (mode.usesDrivePath()) + { + summary.append(" | ").append(driveOption.displayName()); + } + + return summary.toString(); + } + + private String firstDrivePathKey(StartPosition startPosition) + { + for (var option : startPosition.driveOptions) + { + if (!option.staysPut()) + { + return option.key(); + } + } + + return startPosition.driveOptions[0].key(); + } + private void followTrajectory(SwerveSample sample) { var pose = _driveSubsystem.getState().Pose; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fe012fc..1db688e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -277,7 +277,7 @@ public static class VisionConstants public static final String RIGHT_CAMERA_NAME = "limelight-right"; public static final Distance MAX_DETECTION_RANGE = Meters.of(6.0); public static final Distance XY_STD_DEV = Meters.of(0.7); - public static final Angle THETA_STD_DEV = Degrees.of(0.7); // Trust gyro for heading, not vision + public static final Angle THETA_STD_DEV = Degrees.of(999999); // Trust gyro for heading, not vision public static final Matrix STD_DEVS = VecBuilder.fill(XY_STD_DEV.in(Meters), XY_STD_DEV.in(Meters), THETA_STD_DEV.in(Degrees)); // Camera translations From b084bfd9bf3bf1579ec03c2f01d881e4479301aa Mon Sep 17 00:00:00 2001 From: SAKETH11111 Date: Fri, 20 Mar 2026 15:42:02 -0500 Subject: [PATCH 21/32] changes to make the auto work --- documentation/shooter-turret-cheat-sheet.md | 53 +++++++++++++++++++++ src/main/java/frc/robot/Autos.java | 41 +++++++++------- 2 files changed, 77 insertions(+), 17 deletions(-) create mode 100644 documentation/shooter-turret-cheat-sheet.md diff --git a/documentation/shooter-turret-cheat-sheet.md b/documentation/shooter-turret-cheat-sheet.md new file mode 100644 index 0000000..5f71a45 --- /dev/null +++ b/documentation/shooter-turret-cheat-sheet.md @@ -0,0 +1,53 @@ +Right now, the most important thing to understand is that the shooter code is not one single blob. It is split into three layers on purpose. `Shooter` is the coordinator that owns the top-level state machine and decides when the robot is allowed to feed a note. `TurretDirector` is the math layer that figures out what shot we want right now. `Turret` is the actuator/sensor layer that reads the live turret angle and physically drives the turret to the requested angle. If those three responsibilities stay mentally separate, the code becomes a lot easier to reason about. + +```mermaid +flowchart LR + A["Driver / Operator command"] --> B["Shooter"] + B --> C["TurretDirector"] + C --> D["ShotSolution"] + D --> E["Turret target angle"] + D --> F["Flywheel speed target"] + B --> G["Feed gating"] + E --> H["Turret"] + F --> I["Flywheel"] + G --> J["Feeder + Rotor"] +``` + +## How the pieces fit together + +`Shooter` exists because shooting is not just “point and spin.” It has to coordinate the turret, the flywheel, the feeder, the rotor, and all of the readiness conditions that decide whether we are actually allowed to fire. That is why `Shooter` owns the high-level states like `Idle`, `Preparing`, `Ready`, `Firing`, `Manual`, and `TrackingOnly`. It is the part of the system that answers questions like “are we trying to shoot right now,” “should the feeder be running,” and “what should happen if readiness drops out in the middle of a shot.” + +`TurretDirector` exists because shot calculation is a different problem from actuation. Its job is to look at the current robot state, the selected shot mode, the target geometry, and any active adjustments, and then condense that down into one `ShotSolution`. That solution includes the turret angle we want, the effective shot distance, and enough context for the rest of the shooter stack to act on it. Static aiming, moving-shot aiming, pass aiming, vision trim, and the empirical drift correction all live there because those are all part of “what shot are we trying to take,” not “how do we move the turret motor.” + +`Turret` exists because it should be the only place that really cares about the live potentiometer angle, wrap handling, PID output, and lined-up reporting. In other words, `TurretDirector` should not need to know how the turret motor is controlled, and `Shooter` should not need to know how the potentiometer reading becomes a motor command. That separation is what lets us debug the system in chunks instead of trying to hold the entire subsystem in our heads at once. + +## A good debugging mental model + +The fastest way to debug this subsystem is to ask where the mistake first appears. If the **desired angle itself is wrong**, then the problem is usually in `TurretDirector`, because that means the shot math produced the wrong answer. If the **desired angle looks correct, but the turret is physically somewhere else**, then the problem is usually in `Turret`, because the actuation layer is not getting to the requested setpoint. If the **turret is aimed correctly and the flywheel is up to speed, but the robot still will not fire**, then the problem is usually in `Shooter`, because the feed gating or top-level state machine is what decides whether the feeder and rotor are allowed to run. + +That one mental split is probably the most useful thing to keep in mind while reading the code or looking at logs. + +## What happens during a normal shot + +When the driver presses the normal shoot button, `Shooter.shoot()` requests `ShotMode.Track` and moves the subsystem into `Preparing`. From there, `Shooter.periodic()` keeps asking `TurretDirector` for an updated `ShotSolution`. That `ShotSolution` is used to command a turret angle and a flywheel speed. While this is happening, `Shooter` is also checking whether the various readiness gates are true. Those gates answer questions like whether the shot solution is valid, whether the turret is lined up closely enough, whether the flywheel is at speed, and whether any moving-shot constraints are satisfied. Once those conditions are satisfied, the subsystem becomes effectively ready to fire, and if auto-shoot is enabled, it transitions into the firing path. The feeder and rotor only stay on while those readiness checks continue to pass. If readiness drops, the subsystem falls back out of firing and returns to preparing. + +That is why `Shooter` exists as a coordinator. The robot needs one place that can say, “yes, the math looks good, the turret is there, the flywheel is ready, and we are now allowed to feed.” + +## Why `TrackingOnly` exists + +`TrackingOnly` is there because aiming and firing are not the same problem. Sometimes we want to verify the turret math, the alignment behavior, or the shoot-while-moving solve without feeding a note. `TrackingOnly` keeps the targeting stack alive, keeps producing fresh shot solutions, and keeps commanding the turret, but it intentionally does not run the feeder or rotor. That makes it the safest state to use when we want to validate whether the robot is leading the target correctly or whether the turret is hunting around a setpoint. + +## Where shoot-while-moving fits + +Shoot-while-moving does not replace the shooter subsystem. It only changes the shot calculation that `TurretDirector` performs. `Shooter` still owns whether moving-shot math should be active, and it still owns whether we are actually allowed to feed once that moving-shot solution exists. In practice, that means SWM lives in two places conceptually. The math lives in `TurretDirector`, because that is where the lead angle and effective shot distance are calculated. The readiness logic lives in `Shooter`, because that is where we decide whether the moving-shot solution is trustworthy enough to actually fire from. + +This is also why SWM can feel confusing if you only look at one class. The aiming math and the feed decision are intentionally split between two layers. + +```mermaid +flowchart TD + A["ShotMode.Track requested"] --> B["Shooter decides whether SWM is active"] + B --> C["TurretDirector.calculate(..., useMovingShotMath)"] + C --> D["Static track solution or moving-shot solution"] + D --> E["Shooter checks feed readiness"] + E --> F["If ready, feeder + rotor run"] +``` diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index b39409d..9803056 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -5,13 +5,13 @@ import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -101,7 +101,7 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. private final Drive _driveSubsystem; private final Shooter _shooterSubsystem; private final AutoFactory _autoFactory; - private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage).withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); @@ -229,7 +229,12 @@ private void updateDashboard() private AutoMode getSelectedMode() { - var selected = getSelectedChooserValue(AUTO_MODE_CHOOSER_NAME, DEFAULT_AUTO_MODE.name()); + var selected = _modeChooser.getSelected(); + if (selected == null || selected.isEmpty()) + { + return DEFAULT_AUTO_MODE; + } + for (var mode : AutoMode.values()) { if (mode.name().equals(selected)) @@ -243,7 +248,12 @@ private AutoMode getSelectedMode() private StartPosition getSelectedStartPosition() { - var selected = getSelectedChooserValue(AUTO_START_CHOOSER_NAME, DEFAULT_START_POSITION.name()); + var selected = _startChooser.getSelected(); + if (selected == null || selected.isEmpty()) + { + return DEFAULT_START_POSITION; + } + for (var startPosition : StartPosition.values()) { if (startPosition.name().equals(selected)) @@ -257,7 +267,11 @@ private StartPosition getSelectedStartPosition() private int getSelectedDelaySeconds() { - var selected = getSelectedChooserValue(AUTO_DELAY_CHOOSER_NAME, String.valueOf(DEFAULT_DELAY_SECONDS)); + var selected = _delayChooser.getSelected(); + if (selected == null || selected.isEmpty()) + { + return DEFAULT_DELAY_SECONDS; + } try { @@ -272,7 +286,11 @@ private int getSelectedDelaySeconds() private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) { var fallback = firstDrivePathKey(startPosition); - var selected = getSelectedChooserValue(AUTO_DRIVE_CHOOSER_NAME, fallback); + var selected = _driveChooser.getSelected(); + if (selected == null || selected.isEmpty()) + { + selected = fallback; + } for (var option : startPosition.driveOptions) { @@ -293,17 +311,6 @@ private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) return startPosition.driveOptions[0]; } - private String getSelectedChooserValue(String chooserName, String fallback) - { - var selected = NetworkTableInstance.getDefault().getTable("SmartDashboard").getSubTable(chooserName).getEntry("selected").getString(""); - if (!selected.isEmpty()) - { - return selected; - } - - return fallback; - } - private String buildSummary(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption, int delaySeconds) { var summary = new StringBuilder(); From a419ffa59ea2524a015418b49b43a0aa044f25a2 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Fri, 20 Mar 2026 17:16:42 -0500 Subject: [PATCH 22/32] left bump path update --- .../choreo/LeftBumpPickupFromMidScore.traj | 441 +++++++++--------- src/main/java/frc/robot/Autos.java | 4 +- 2 files changed, 211 insertions(+), 234 deletions(-) diff --git a/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj b/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj index a5f3673..f326ecc 100644 --- a/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj +++ b/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj @@ -3,16 +3,16 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.61569, "y":5.558789999999999, "heading":3.141592653589793, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.61569, "y":5.558789999999999, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":6.61945915222168, "y":5.5876383781433105, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":7.301177978515625, "y":5.6295905113220215, "heading":-0.5585991876326754, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":7.584353923797607, "y":4.738111972808838, "heading":-1.2360600261652088, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.594841480255127, "y":3.8151695728302, "heading":-1.8233502920797129, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.1019062995910645, "y":2.9971067905426025, "heading":-2.561194419201345, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.591637134552002, "y":2.6614913940429688, "heading":-2.9881687107743993, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.399648904800415, "y":2.5356357097625732, "heading":2.103952218077775, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.6445140838623047, "y":3.7732174396514897, "heading":0.31204229296728064, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.61569, "y":4.03479, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":7.594841480255127, "y":3.8151695728302, "heading":-1.8233502920797129, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1019062995910645, "y":2.9971067905426025, "heading":2.3880715859232047, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.496963977813721, "y":3.878139019012451, "heading":1.774814111378504, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.184691429138184, "y":4.932059288024902, "heading":2.194818993599192, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.68187952041626, "y":5.595638275146484, "heading":-3.1138221488567326, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.218101739883423, "y":5.673706531524658, "heading":3.14, "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}, @@ -21,16 +21,16 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"LeftBump.x", "val":3.61569}, "y":{"exp":"LeftBump.y", "val":5.558789999999999}, "heading":{"exp":"LeftBump.heading", "val":3.141592653589793}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"LeftBump.x", "val":3.61569}, "y":{"exp":"LeftBump.y", "val":5.558789999999999}, "heading":{"exp":"-0 mrad", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"LeftMiddle.x", "val":6.61945915222168}, "y":{"exp":"LeftMiddle.y", "val":5.5876383781433105}, "heading":{"exp":"LeftMiddle.heading", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.301177978515625 m", "val":7.301177978515625}, "y":{"exp":"5.6295905113220215 m", "val":5.6295905113220215}, "heading":{"exp":"-0.5585991876326754 rad", "val":-0.5585991876326754}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"7.584353923797607 m", "val":7.584353923797607}, "y":{"exp":"4.738111972808838 m", "val":4.738111972808838}, "heading":{"exp":"-1.2360600261652088 rad", "val":-1.2360600261652088}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.594841480255127 m", "val":7.594841480255127}, "y":{"exp":"3.8151695728302 m", "val":3.8151695728302}, "heading":{"exp":"-1.8233502920797129 rad", "val":-1.8233502920797129}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1019062995910645 m", "val":7.1019062995910645}, "y":{"exp":"2.9971067905426025 m", "val":2.9971067905426025}, "heading":{"exp":"-2.561194419201345 rad", "val":-2.561194419201345}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.591637134552002 m", "val":5.591637134552002}, "y":{"exp":"2.6614913940429688 m", "val":2.6614913940429688}, "heading":{"exp":"-2.9881687107743993 rad", "val":-2.9881687107743993}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.399648904800415 m", "val":3.399648904800415}, "y":{"exp":"2.5356357097625732 m", "val":2.5356357097625732}, "heading":{"exp":"2.103952218077775 rad", "val":2.103952218077775}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.6445140838623047 m", "val":2.6445140838623047}, "y":{"exp":"3.7732174396514893 m", "val":3.7732174396514897}, "heading":{"exp":"0.31204229296728064 rad", "val":0.31204229296728064}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"HubStart.x", "val":3.61569}, "y":{"exp":"HubStart.y", "val":4.03479}, "heading":{"exp":"HubStart.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"7.594841480255127 m", "val":7.594841480255127}, "y":{"exp":"3.8151695728302 m", "val":3.8151695728302}, "heading":{"exp":"-1.8233502920797129 rad", "val":-1.8233502920797129}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1019062995910645 m", "val":7.1019062995910645}, "y":{"exp":"2.9971067905426025 m", "val":2.9971067905426025}, "heading":{"exp":"2.3880715859232047 rad", "val":2.3880715859232047}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.496963977813721 m", "val":6.496963977813721}, "y":{"exp":"3.878139019012451 m", "val":3.878139019012451}, "heading":{"exp":"1.774814111378504 rad", "val":1.774814111378504}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.184691429138184 m", "val":6.184691429138184}, "y":{"exp":"4.932059288024902 m", "val":4.932059288024902}, "heading":{"exp":"2.194818993599192 rad", "val":2.194818993599192}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.68187952041626 m", "val":4.68187952041626}, "y":{"exp":"5.595638275146484 m", "val":5.595638275146484}, "heading":{"exp":"-3.1138221488567326 rad", "val":-3.1138221488567326}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"3.218101739883423 m", "val":3.218101739883423}, "y":{"exp":"5.673706531524658 m", "val":5.673706531524658}, "heading":{"exp":"3.14 rad", "val":3.14}, "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}, @@ -65,225 +65,202 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.03231,1.32513,1.70421,1.93364,2.21247,2.60385,3.15052,3.70848,4.29782], + "waypoints":[0.0,0.9112,1.19379,1.58112,1.80941,2.26968,2.70584,3.00289,3.44004,4.01201], "samples":[ - {"t":0.0, "x":3.61569, "y":5.55879, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.5206, "ay":-0.15251, "alpha":27.95492, "fx":[139.06905,142.56798,50.36489,43.61307], "fy":[-64.69006,56.55179,144.59063,-146.82892]}, - {"t":0.02717, "x":3.61773, "y":5.55873, "heading":3.14159, "vx":0.14997, "vy":-0.00414, "omega":0.75942, "ax":5.56197, "ay":-0.15412, "alpha":27.77442, "fx":[139.16314,142.64046,51.76497,44.86136], "fy":[-64.45599,56.33195,144.05843,-146.42058]}, - {"t":0.05433, "x":3.62385, "y":5.55856, "heading":-3.12096, "vx":0.30107, "vy":-0.00833, "omega":1.51394, "ax":5.61451, "ay":-0.09487, "alpha":27.54094, "fx":[139.95527,142.12992,51.75575,48.16384], "fy":[-62.68111,57.56301,144.00327,-145.34026]}, - {"t":0.0815, "x":3.6341, "y":5.5583, "heading":-3.07983, "vx":0.45359, "vy":-0.01091, "omega":2.26212, "ax":5.68123, "ay":0.02389, "alpha":27.2232, "fx":[141.3793,141.02746,50.70817,53.42922], "fy":[-59.35532,60.15918,144.28625,-143.46462]}, - {"t":0.10866, "x":3.64852, "y":5.55802, "heading":-3.01838, "vx":0.60793, "vy":-0.01026, "omega":3.00166, "ax":5.76778, "ay":0.19932, "alpha":26.75996, "fx":[143.31486,139.3073,49.28426,60.5267], "fy":[-54.45903,63.97579,144.64061,-140.59604]}, - {"t":0.13583, "x":3.66717, "y":5.55781, "heading":-2.93684, "vx":0.76461, "vy":-0.00484, "omega":3.72862, "ax":5.88965, "ay":0.42457, "alpha":26.0254, "fx":[145.58554,136.95805,48.88353,69.29767], "fy":[-47.98008,68.78017,144.551,-136.46369]}, - {"t":0.163, "x":3.69011, "y":5.55784, "heading":-2.83555, "vx":0.92461, "vy":0.00669, "omega":4.43562, "ax":6.09118, "ay":0.68004, "alpha":24.73306, "fx":[147.95889,134.04203,52.88014,79.55558], "fy":[-39.94299,74.19948,142.73316,-130.72018]}, - {"t":0.19016, "x":3.71748, "y":5.55827, "heading":-2.71505, "vx":1.09009, "vy":0.02516, "omega":5.10752, "ax":6.50032, "ay":0.88808, "alpha":22.14427, "fx":[150.15283,130.80676,70.25092,91.06367], "fy":[-30.46036,79.62648,134.19442,-122.93658]}, - {"t":0.21733, "x":3.74949, "y":5.55928, "heading":-2.5763, "vx":1.26667, "vy":0.04929, "omega":5.70909, "ax":7.35611, "ay":0.66579, "alpha":16.84441, "fx":[151.85836,127.86701,117.32973,103.44635], "fy":[-19.82886,84.06924,93.70097,-112.64143]}, - {"t":0.24449, "x":3.78661, "y":5.56086, "heading":-2.42121, "vx":1.46651, "vy":0.06738, "omega":6.16669, "ax":8.01606, "ay":-0.33476, "alpha":12.43362, "fx":[152.80263,126.51795,149.91256,116.17039], "fy":[-8.68023,85.77927,-0.54633,-99.32949]}, - {"t":0.27166, "x":3.82941, "y":5.56257, "heading":-2.25368, "vx":1.68427, "vy":0.05828, "omega":6.50446, "ax":8.16786, "ay":-0.63023, "alpha":11.49361, "fx":[152.87112,129.5311,144.40642,128.92297], "fy":[1.68156,80.60701,-43.36223,-81.80663]}, - {"t":0.29883, "x":3.87818, "y":5.56392, "heading":-2.07698, "vx":1.90616, "vy":0.04116, "omega":6.81669, "ax":8.53783, "ay":-0.42318, "alpha":8.45778, "fx":[152.31876,140.69011,146.32601,141.56948], "fy":[8.14538,57.70721,-38.31321,-56.3322]}, - {"t":0.32599, "x":3.93311, "y":5.56488, "heading":-1.8918, "vx":2.1381, "vy":0.02967, "omega":7.04646, "ax":8.90235, "ay":-0.24373, "alpha":0.48736, "fx":[151.52775,151.48003,151.34655,151.35147], "fy":[-2.45956,-0.89208,-5.9048,-7.32657]}, - {"t":0.35316, "x":3.99448, "y":5.5656, "heading":-1.70038, "vx":2.37994, "vy":0.02304, "omega":7.0597, "ax":8.09134, "ay":0.418, "alpha":-12.06415, "fx":[136.24049,146.21387,146.08375,121.98761], "fy":[-59.71273,-38.88855,39.96814,87.07366]}, - {"t":0.38032, "x":4.06212, "y":5.56638, "heading":-1.50859, "vx":2.59975, "vy":0.0344, "omega":6.73196, "ax":5.25963, "ay":0.08423, "alpha":-28.04957, "fx":[44.73638,139.61402,137.53553,35.97296], "fy":[-141.84587,-58.38753,62.91261,143.05186]}, - {"t":0.40749, "x":4.13468, "y":5.56735, "heading":-1.32571, "vx":2.74263, "vy":0.03669, "omega":5.96997, "ax":5.90408, "ay":-0.06373, "alpha":-24.33356, "fx":[62.38429,140.96369,138.48586,59.87323], "fy":[-135.90383,-54.63139,59.41099,126.78821]}, - {"t":0.43466, "x":4.21137, "y":5.56832, "heading":-1.16353, "vx":2.90302, "vy":0.03496, "omega":5.30892, "ax":6.14696, "ay":-0.68695, "alpha":-21.04826, "fx":[67.0671,142.39512,136.47755,72.29211], "fy":[-133.98075,-50.26631,62.47863,75.02928]}, - {"t":0.46182, "x":4.2925, "y":5.56901, "heading":-1.01931, "vx":3.07001, "vy":0.0163, "omega":4.73713, "ax":6.02744, "ay":-3.15506, "alpha":-18.05215, "fx":[77.3536,144.42998,135.00351,53.31301], "fy":[-128.27229,-43.14238,63.92488,-107.17679]}, - {"t":0.48899, "x":4.37812, "y":5.56829, "heading":-0.89062, "vx":3.23375, "vy":-0.06941, "omega":4.24672, "ax":6.43947, "ay":-3.1002, "alpha":-16.76676, "fx":[90.11257,146.12459,135.37268,66.52422], "fy":[-119.31137,-35.33445,60.97768,-117.26603]}, - {"t":0.51615, "x":4.46835, "y":5.56526, "heading":-0.77526, "vx":3.40868, "vy":-0.15363, "omega":3.79124, "ax":6.70484, "ay":-2.85056, "alpha":-16.07046, "fx":[99.90646,147.3013,134.63916,74.34257], "fy":[-110.62077,-27.27506,59.91297,-115.9661]}, - {"t":0.54332, "x":4.56342, "y":5.56004, "heading":-0.67226, "vx":3.59083, "vy":-0.23107, "omega":3.35467, "ax":6.84281, "ay":-2.52742, "alpha":-15.84903, "fx":[107.28128,147.92515,132.01645,78.35375], "fy":[-102.4817,-18.60591,62.23324,-113.10855]}, - {"t":0.57048, "x":4.6635, "y":5.55283, "heading":-0.58113, "vx":3.77672, "vy":-0.29973, "omega":2.92412, "ax":6.87661, "ay":-2.12883, "alpha":-16.00251, "fx":[112.98639,147.81869,126.97203,80.09985], "fy":[-94.54216,-9.03954,68.02412,-109.2859]}, - {"t":0.59765, "x":4.76863, "y":5.5439, "heading":-0.5017, "vx":3.96353, "vy":-0.35756, "omega":2.48939, "ax":6.8111, "ay":-1.64237, "alpha":-16.3861, "fx":[117.4645,146.63281,118.91848,80.40361], "fy":[-86.23177,1.54873,76.49753,-103.55927]}, - {"t":0.62482, "x":4.87882, "y":5.53358, "heading":-0.43407, "vx":4.14856, "vy":-0.40218, "omega":2.04425, "ax":6.63281, "ay":-1.04924, "alpha":-16.77775, "fx":[120.74302,143.78784,107.3249,79.43308], "fy":[-76.80344,13.11407,86.08364,-93.78362]}, - {"t":0.65198, "x":4.99396, "y":5.52227, "heading":-0.37853, "vx":4.32874, "vy":-0.43068, "omega":1.58847, "ax":6.29787, "ay":-0.30257, "alpha":-16.81502, "fx":[122.20229,138.2391,91.96873,76.08975], "fy":[-65.04128,25.46317,94.36142,-75.36999]}, - {"t":0.67915, "x":5.11388, "y":5.51046, "heading":-0.33538, "vx":4.49983, "vy":-0.4389, "omega":1.13167, "ax":5.67482, "ay":0.73633, "alpha":-15.81415, "fx":[119.64341,127.49033,72.96133,66.01351], "fy":[-47.75119,38.44278,97.79338,-38.38597]}, - {"t":0.70631, "x":5.23822, "y":5.4988, "heading":-0.30464, "vx":4.65399, "vy":-0.4189, "omega":0.70206, "ax":4.34189, "ay":2.27393, "alpha":-12.23623, "fx":[103.10437,102.31056,49.11549,40.8867], "fy":[-11.84732,53.48691,91.49038,21.58535]}, - {"t":0.73348, "x":5.36625, "y":5.48826, "heading":-0.28557, "vx":4.77195, "vy":-0.35713, "omega":0.36965, "ax":0.98117, "ay":4.22189, "alpha":-2.23782, "fx":[22.63178,25.00782,11.27874,7.83908], "fy":[65.4146,74.35425,78.06724,69.41672]}, - {"t":0.76065, "x":5.49625, "y":5.48012, "heading":-0.27553, "vx":4.7986, "vy":-0.24244, "omega":0.30886, "ax":-2.8106, "ay":4.99866, "alpha":8.60898, "fx":[-57.43273,-87.42775,-37.9945,-8.3752], "fy":[102.32836,63.0867,67.26613,107.4222]}, - {"t":0.78781, "x":5.62557, "y":5.47538, "heading":-0.26713, "vx":4.72225, "vy":-0.10664, "omega":0.54273, "ax":-4.31841, "ay":5.11587, "alpha":12.37915, "fx":[-79.83068,-124.35668,-80.22179,-9.41061], "fy":[112.18257,48.58279,57.07716,130.23568]}, - {"t":0.81498, "x":5.75226, "y":5.47437, "heading":-0.25239, "vx":4.60493, "vy":0.03233, "omega":0.87902, "ax":-4.76526, "ay":5.39691, "alpha":13.03008, "fx":[-85.00162,-134.37783,-98.00389,-6.83919], "fy":[117.22917,46.94057,62.55321,140.47649]}, - {"t":0.84214, "x":5.8756, "y":5.47724, "heading":-0.22851, "vx":4.47548, "vy":0.17895, "omega":1.233, "ax":-4.84134, "ay":5.79307, "alpha":12.56766, "fx":[-86.37885,-137.85064,-99.86144,-5.30798], "fy":[120.1934,48.94731,79.59908,145.41409]}, - {"t":0.86931, "x":5.99539, "y":5.48424, "heading":-0.19502, "vx":4.34396, "vy":0.33632, "omega":1.57441, "ax":-4.87907, "ay":6.1782, "alpha":11.4716, "fx":[-88.16196,-138.56209,-95.21132,-10.03097], "fy":[120.97517,53.12645,98.50363,147.75258]}, - {"t":0.89648, "x":6.1116, "y":5.49565, "heading":-0.15225, "vx":4.21142, "vy":0.50416, "omega":1.88605, "ax":-7.46974, "ay":4.43281, "alpha":-5.11103, "fx":[-136.83885,-105.85699,-123.09698,-142.43974], "fy":[59.53482,106.19205,87.13934,48.73708]}, - {"t":0.92364, "x":6.22325, "y":5.51099, "heading":-0.10101, "vx":4.00849, "vy":0.62458, "omega":1.7472, "ax":-7.20839, "ay":2.16897, "alpha":-16.74986, "fx":[-138.94691,-70.72876,-128.96505,-151.81], "fy":[-56.29611,133.70551,81.05384,-10.88905]}, - {"t":0.95081, "x":6.32949, "y":5.52875, "heading":-0.05354, "vx":3.81267, "vy":0.6835, "omega":1.29218, "ax":-6.49416, "ay":1.17966, "alpha":-22.45532, "fx":[-105.65504,-54.48612,-132.08525,-149.62888], "fy":[-107.91691,141.88119,76.74714,-30.44896]}, - {"t":0.97797, "x":6.43066, "y":5.54776, "heading":-0.01844, "vx":3.63625, "vy":0.71555, "omega":0.68216, "ax":-6.01978, "ay":0.73607, "alpha":-25.23258, "fx":[-81.33641,-46.69593,-134.09557,-147.45082], "fy":[-128.03567,145.05413,73.61457,-40.55198]}, - {"t":1.00514, "x":6.52723, "y":5.56747, "heading":0.00009, "vx":3.47272, "vy":0.73554, "omega":-0.00331, "ax":-5.7085, "ay":0.51423, "alpha":-26.821, "fx":[-65.4709,-41.83856,-135.20623,-145.88378], "fy":[-137.29008,146.77772,71.81318,-46.313]}, - {"t":1.03231, "x":6.61946, "y":5.58764, "heading":0.0, "vx":3.31764, "vy":0.74951, "omega":-0.73193, "ax":-6.68595, "ay":0.32471, "alpha":-22.1368, "fx":[-91.79796,-77.20509,-140.73476,-145.16679], "fy":[-120.60767,130.7984,59.48802,-47.58617]}, - {"t":1.04857, "x":6.67255, "y":5.59987, "heading":-0.01191, "vx":3.20888, "vy":0.7548, "omega":-1.09205, "ax":-7.29633, "ay":-0.5711, "alpha":-18.54258, "fx":[-92.57359,-115.11299,-147.23062,-141.51674], "fy":[-120.5483,98.59018,40.70028,-57.5992]}, - {"t":1.06484, "x":6.72378, "y":5.61208, "heading":-0.02967, "vx":3.09018, "vy":0.74551, "omega":-1.3937, "ax":-7.75208, "ay":-1.82976, "alpha":-13.91929, "fx":[-93.17632,-145.26349,-151.45492,-137.54788], "fy":[-120.43326,42.86009,19.65672,-66.57851]}, - {"t":1.08111, "x":6.77303, "y":5.62396, "heading":-0.05235, "vx":2.96407, "vy":0.71574, "omega":-1.62014, "ax":-7.79689, "ay":-3.14773, "alpha":-10.06729, "fx":[-93.65243,-150.80234,-152.68684,-133.35007], "fy":[-120.30838,-16.32715,-2.84867,-74.68357]}, - {"t":1.09738, "x":6.82022, "y":5.63519, "heading":-0.0787, "vx":2.83723, "vy":0.66453, "omega":-1.78391, "ax":-7.55785, "ay":-4.19569, "alpha":-7.61642, "fx":[-94.02083,-140.6742,-150.56796,-128.96472], "fy":[-120.20023,-57.64832,-25.54957,-82.07199]}, - {"t":1.11365, "x":6.86537, "y":5.64545, "heading":-0.10772, "vx":2.71428, "vy":0.59628, "omega":-1.90782, "ax":-7.23575, "ay":-4.96978, "alpha":-5.93784, "fx":[-94.29566,-128.26762,-145.33296,-124.41583], "fy":[-120.12107,-82.15125,-47.00523,-88.86056]}, - {"t":1.12991, "x":6.90857, "y":5.65449, "heading":-0.13876, "vx":2.59657, "vy":0.51543, "omega":-2.00441, "ax":-6.90742, "ay":-5.55459, "alpha":-4.5866, "fx":[-94.48724,-117.99129,-137.76834,-119.72584], "fy":[-120.07696,-96.67164,-66.05383,-95.12567]}, - {"t":1.14618, "x":6.9499, "y":5.66214, "heading":-0.17137, "vx":2.4842, "vy":0.42507, "omega":-2.07903, "ax":-6.59377, "ay":-6.00809, "alpha":-3.41788, "fx":[-94.60117,-110.2076,-128.90624,-114.91774], "fy":[-120.07245,-105.68767,-82.10466,-100.91886]}, - {"t":1.16245, "x":6.98944, "y":5.66826, "heading":-0.20519, "vx":2.37693, "vy":0.32733, "omega":-2.13463, "ax":-6.30186, "ay":-6.36511, "alpha":-2.39772, "fx":[-94.63933,-104.42587,-119.69008,-110.01605], "fy":[-120.11187,-111.56524,-95.12173,-106.2758]}, - {"t":1.17872, "x":7.02727, "y":5.67274, "heading":-0.23991, "vx":2.27441, "vy":0.22378, "omega":-2.17364, "ax":-6.03451, "ay":-6.64905, "alpha":-1.51388, "fx":[-94.60146,-100.13544,-110.79581,-105.04833], "fy":[-120.19929,-115.55068,-105.42251,-111.22112]}, - {"t":1.19499, "x":7.06347, "y":5.6755, "heading":-0.27527, "vx":2.17624, "vy":0.11561, "omega":-2.19827, "ax":-5.79215, "ay":-6.87708, "alpha":-0.75227, "fx":[-94.48629,-96.9441,-102.61535,-100.04571], "fy":[-120.33821,-118.33197,-113.46639,-115.77216]}, - {"t":1.21125, "x":7.09811, "y":5.67647, "heading":-0.31104, "vx":2.08201, "vy":0.00374, "omega":-2.2105, "ax":-5.57366, "ay":-7.06214, "alpha":-0.09539, "fx":[-94.29225,-94.56905,-95.32191,-95.04245], "fy":[-120.53142,-120.31028,-119.71621,-119.94225]}, - {"t":1.22752, "x":7.13124, "y":5.6756, "heading":-0.347, "vx":1.99134, "vy":-0.11115, "omega":-2.21206, "ax":-5.37707, "ay":-7.21397, "alpha":0.47461, "fx":[-94.01788,-92.80779,-88.9491,-90.07466], "fy":[-120.78086,-121.73194,-124.57396,-123.74353]}, - {"t":1.24379, "x":7.16293, "y":5.67284, "heading":-0.38298, "vx":1.90387, "vy":-0.22851, "omega":-2.20433, "ax":-5.20009, "ay":-7.33986, "alpha":0.9737, "fx":[-93.66203,-91.51377,-83.45346,-85.17865], "fy":[-121.08762,-122.7553,-128.36355,-127.18887]}, - {"t":1.26006, "x":7.19321, "y":5.66815, "heading":-0.41884, "vx":1.81927, "vy":-0.34791, "omega":-2.18849, "ax":-5.04047, "ay":-7.44526, "alpha":1.41498, "fx":[-93.22403,-90.57918,-78.75559,-80.38921], "fy":[-121.45191,-123.48632,-131.33579,-130.29307]}, - {"t":1.27633, "x":7.22214, "y":5.6615, "heading":-0.45444, "vx":1.73727, "vy":-0.46903, "omega":-2.16548, "ax":-4.89616, "ay":-7.53431, "alpha":1.8086, "fx":[-92.70379,-89.92338,-74.76398,-75.73815], "fy":[-121.87303,-123.99844,-133.68109,-133.07337]}, - {"t":1.29259, "x":7.24975, "y":5.65288, "heading":-0.48967, "vx":1.65762, "vy":-0.5916, "omega":-2.13605, "ax":-4.76533, "ay":-7.61015, "alpha":2.16218, "fx":[-92.10185,-89.48501,-71.38776,-71.25309], "fy":[-122.34949,-124.34414,-135.54283,-135.54941]}, - {"t":1.30886, "x":7.27609, "y":5.64224, "heading":-0.52442, "vx":1.5801, "vy":-0.7154, "omega":-2.10088, "ax":-4.6464, "ay":-7.67521, "alpha":2.48135, "fx":[-91.41943,-89.21672,-68.54265,-66.95668], "fy":[-122.87891,-124.56191,-137.02885,-137.74273]}, - {"t":1.32513, "x":7.30118, "y":5.62959, "heading":-0.5586, "vx":1.50451, "vy":-0.84026, "omega":-2.06051, "ax":-4.57934, "ay":-7.72421, "alpha":2.30582, "fx":[-89.00321,-87.89956,-68.47718,-66.1928], "fy":[-124.70851,-125.54854,-137.11281,-138.17649]}, - {"t":1.34408, "x":7.32887, "y":5.61228, "heading":-0.59765, "vx":1.41772, "vy":-0.98667, "omega":-2.01681, "ax":-4.54211, "ay":-7.74676, "alpha":2.24681, "fx":[-87.65167,-87.47146,-68.51095,-65.40573], "fy":[-125.64006,-125.82722,-137.07959,-138.53387]}, - {"t":1.36304, "x":7.35493, "y":5.59218, "heading":-0.63588, "vx":1.33162, "vy":-1.1335, "omega":-1.97422, "ax":-4.50057, "ay":-7.77159, "alpha":2.18097, "fx":[-86.19505,-86.90726,-68.48016,-64.63087], "fy":[-126.61979,-126.19539,-137.07666,-138.87832]}, - {"t":1.38199, "x":7.37936, "y":5.5693, "heading":-0.6733, "vx":1.24632, "vy":-1.28081, "omega":-1.93288, "ax":-4.45392, "ay":-7.79905, "alpha":2.10733, "fx":[-84.6211,-86.18634,-68.3714,-63.86092], "fy":[-127.65077,-126.66409,-137.11027,-139.21317]}, - {"t":1.40095, "x":7.40218, "y":5.54362, "heading":-0.70994, "vx":1.1619, "vy":-1.42863, "omega":-1.89294, "ax":-4.40119, "ay":-7.82955, "alpha":2.02462, "fx":[-82.91415,-85.28246,-68.16815,-63.08721], "fy":[-128.73718,-127.24663,-137.18797,-139.54209]}, - {"t":1.4199, "x":7.42341, "y":5.51514, "heading":-0.74582, "vx":1.07848, "vy":-1.57704, "omega":-1.85456, "ax":-4.34112, "ay":-7.86362, "alpha":1.93123, "fx":[-81.05401,-84.16179,-67.84957,-62.29921], "fy":[-129.88446,-127.95923,-137.31898,-139.86923]}, - {"t":1.43886, "x":7.44308, "y":5.48384, "heading":-0.78097, "vx":0.99619, "vy":-1.72608, "omega":-1.81796, "ax":-4.27208, "ay":-7.90189, "alpha":1.82499, "fx":[-79.01466,-82.78003,-67.38873,-61.48399], "fy":[-131.09957,-128.82175,-137.5148,-140.19939]}, - {"t":1.45781, "x":7.46119, "y":5.4497, "heading":-0.81543, "vx":0.91522, "vy":-1.87586, "omega":-1.78337, "ax":-4.19195, "ay":-7.94514, "alpha":1.70302, "fx":[-76.76213,-81.07827,-66.75004,-60.62528], "fy":[-132.39129,-129.85869,-137.78995,-140.53823]}, - {"t":1.47676, "x":7.47779, "y":5.41272, "heading":-0.84923, "vx":0.83577, "vy":-2.02645, "omega":-1.75109, "ax":-4.09789, "ay":-7.99435, "alpha":1.56143, "fx":[-74.25173,-78.97663,-65.88514,-59.70216], "fy":[-133.77046,-131.10026,-138.16322,-140.89267]}, - {"t":1.49572, "x":7.49289, "y":5.37287, "heading":-0.88242, "vx":0.75809, "vy":-2.17798, "omega":-1.72149, "ax":-3.98599, "ay":-8.05076, "alpha":1.39483, "fx":[-71.42386,-76.36462,-64.72654,-58.68691], "fy":[-135.25029,-132.58365,-138.65924,-141.27133]}, - {"t":1.51467, "x":7.50654, "y":5.33014, "heading":-0.91505, "vx":0.68254, "vy":-2.33058, "omega":-1.69505, "ax":-3.85077, "ay":-8.1159, "alpha":1.19567, "fx":[-68.19771,-73.08578,-63.17689,-57.54175], "fy":[-136.84646,-134.354,-139.31092,-141.68531]}, - {"t":1.53363, "x":7.51879, "y":5.28451, "heading":-0.94718, "vx":0.60955, "vy":-2.48441, "omega":-1.67239, "ax":-3.68434, "ay":-8.19169, "alpha":0.95302, "fx":[-64.46177,-68.91261,-61.09049,-56.21327], "fy":[-138.57688,-136.46396,-140.16283,-142.14934]}, - {"t":1.55258, "x":7.52968, "y":5.23595, "heading":-0.97888, "vx":0.53972, "vy":-2.63967, "omega":-1.65433, "ax":-3.47487, "ay":-8.2804, "alpha":0.65071, "fx":[-60.05866,-63.50479,-58.23951,-54.62289], "fy":[-140.46034,-138.96906,-141.27586,-142.68355]}, - {"t":1.57153, "x":7.53929, "y":5.18443, "heading":-1.01023, "vx":0.47386, "vy":-2.79662, "omega":-1.64199, "ax":-3.20399, "ay":-8.38449, "alpha":0.26398, "fx":[-54.76074,-56.33748,-54.24832,-52.64942], "fy":[-142.51253,-141.91045,-142.73211,-143.31634]}, - {"t":1.59049, "x":7.54769, "y":5.12992, "heading":-1.04136, "vx":0.41313, "vy":-2.95554, "omega":-1.63699, "ax":-2.84187, "ay":-8.50574, "alpha":-0.2463, "fx":[-48.22928,-46.57771,-48.45586,-50.09481], "fy":[-144.73557,-145.2622,-144.63443,-144.08883]}, - {"t":1.60944, "x":7.55501, "y":5.07237, "heading":-1.07238, "vx":0.35926, "vy":-3.11676, "omega":-1.64166, "ax":-2.33745, "ay":-8.64215, "alpha":-0.94431, "fx":[-39.94491,-32.87977,-39.60158,-46.61113], "fy":[-147.09124,-148.7789,-147.07031,-145.06165]}, - {"t":1.6284, "x":7.5614, "y":5.01174, "heading":-1.1035, "vx":0.31496, "vy":-3.28057, "omega":-1.65956, "ax":-1.59859, "ay":-8.77749, "alpha":-1.94047, "fx":[-29.08776,-13.11277,-25.03905,-41.52667], "fy":[-149.43195,-151.57117,-149.88555,-146.32149]}, - {"t":1.64735, "x":7.56709, "y":4.94798, "heading":-1.13496, "vx":0.28466, "vy":-3.44694, "omega":-1.69634, "ax":-0.45202, "ay":-8.84375, "alpha":-3.44966, "fx":[-14.33966,15.63188,1.31794,-33.36535], "fy":[-151.31955,-151.0437,-151.39358,-147.96195]}, - {"t":1.66631, "x":7.5724, "y":4.88106, "heading":-1.16711, "vx":0.27609, "vy":-3.61457, "omega":-1.76172, "ax":1.40423, "ay":-8.57858, "alpha":-6.0316, "fx":[6.3742,55.06763,52.31889,-18.21873], "fy":[-151.54673,-141.19524,-141.12582,-149.80912]}, - {"t":1.68526, "x":7.57788, "y":4.81101, "heading":-1.2005, "vx":0.3027, "vy":-3.77717, "omega":-1.87605, "ax":4.07269, "ay":-7.24277, "alpha":-10.22791, "fx":[35.8297,99.14905,124.67445,17.4479], "fy":[-146.97142,-114.40344,-83.20647,-148.20833]}, - {"t":1.70421, "x":7.58435, "y":4.73811, "heading":-1.23606, "vx":0.3799, "vy":-3.91445, "omega":-2.06991, "ax":5.76406, "ay":-6.01551, "alpha":-7.85677, "fx":[65.9523,113.66134,135.20524,77.36085], "fy":[-132.97294,-96.4863,-57.96943,-121.85941]}, - {"t":1.71569, "x":7.58909, "y":4.69281, "heading":-1.2598, "vx":0.44602, "vy":-3.98345, "omega":-2.16004, "ax":5.89291, "ay":-5.87005, "alpha":-6.10926, "fx":[73.8301,111.36193,128.71744,87.03747], "fy":[-125.89206,-95.77485,-65.84974,-111.8749]}, - {"t":1.72716, "x":7.5946, "y":4.64673, "heading":-1.28458, "vx":0.51362, "vy":-4.05079, "omega":-2.23012, "ax":5.98599, "ay":-5.53002, "alpha":-2.51745, "fx":[90.8155,105.91706,112.95683,97.59068], "fy":[-106.16401,-92.61624,-80.8203,-96.65552]}, - {"t":1.73863, "x":7.60088, "y":4.5999, "heading":-1.31016, "vx":0.58228, "vy":-4.11422, "omega":-2.25899, "ax":4.79656, "ay":-4.54806, "alpha":7.04051, "fx":[107.68072,64.48968,57.83725,96.34454], "fy":[-44.17951,-78.80177,-106.0922,-80.37151]}, - {"t":1.7501, "x":7.60788, "y":4.55241, "heading":-1.33608, "vx":0.63731, "vy":-4.1664, "omega":-2.17823, "ax":-5.526, "ay":-1.64186, "alpha":1.07066, "fx":[-93.39442,-96.7222,-94.6516,-91.21474], "fy":[-23.66158,-24.39526,-32.0092,-31.64402]}, - {"t":1.76157, "x":7.61482, "y":4.50451, "heading":-1.36106, "vx":0.57392, "vy":-4.18523, "omega":-2.16595, "ax":-7.58747, "ay":0.76062, "alpha":-5.79163, "fx":[-134.46774,-126.52826,-121.29968,-133.94709], "fy":[-16.05056,-8.9594,48.98706,27.77436]}, - {"t":1.77304, "x":7.62091, "y":4.45655, "heading":-1.38591, "vx":0.48688, "vy":-4.1765, "omega":-2.23239, "ax":-7.90942, "ay":1.33608, "alpha":-7.13091, "fx":[-142.3374,-136.07455,-121.28788,-138.44777], "fy":[-13.89459,-4.92944,69.91213,39.81746]}, - {"t":1.78451, "x":7.62597, "y":4.40873, "heading":-1.41152, "vx":0.39615, "vy":-4.16118, "omega":-2.31419, "ax":-8.03847, "ay":1.61807, "alpha":-7.5742, "fx":[-145.72928,-140.86364,-120.62669,-139.70846], "fy":[-11.53045,-2.55733,78.58709,45.59207]}, - {"t":1.79598, "x":7.62999, "y":4.3611, "heading":-1.43806, "vx":0.30394, "vy":-4.14262, "omega":-2.40107, "ax":-8.10755, "ay":1.80769, "alpha":-7.72622, "fx":[-147.64966,-143.73235,-120.20766,-140.03894], "fy":[-8.94093,-0.65889,83.14694,49.44615]}, - {"t":1.80745, "x":7.63294, "y":4.3137, "heading":-1.46561, "vx":0.21094, "vy":-4.12188, "omega":-2.4897, "ax":-8.1496, "ay":1.95904, "alpha":-7.74191, "fx":[-148.88506,-145.62758,-120.01586,-139.96086], "fy":[-6.15588,1.13586,85.83086,52.48032]}, - {"t":1.81893, "x":7.63482, "y":4.26654, "heading":-1.49417, "vx":0.11745, "vy":-4.09941, "omega":-2.57851, "ax":-8.17714, "ay":2.09892, "alpha":-7.65247, "fx":[-149.72822,-146.9542,-120.03952,-139.64152], "fy":[-3.01798,3.26217,87.43428,55.12966]}, - {"t":1.8304, "x":7.63563, "y":4.21966, "heading":-1.52375, "vx":0.02365, "vy":-4.07533, "omega":-2.66629, "ax":-8.19625, "ay":2.26173, "alpha":-7.38547, "fx":[-150.28605,-147.86935,-120.40513,-139.10296], "fy":[1.16007,6.89396,88.13671,57.69479]}, - {"t":1.84187, "x":7.63537, "y":4.17306, "heading":-1.55433, "vx":-0.07037, "vy":-4.04939, "omega":-2.75101, "ax":-8.20623, "ay":2.50557, "alpha":-6.74913, "fx":[-150.45938,-148.20177,-121.42307,-138.25795], "fy":[7.9417,14.32077,87.70038,60.51319]}, - {"t":1.85334, "x":7.63402, "y":4.12677, "heading":-1.58589, "vx":-0.1645, "vy":-4.02064, "omega":-2.82843, "ax":-8.19259, "ay":2.85201, "alpha":-5.68201, "fx":[-149.83377,-147.35744,-123.15487,-137.06834], "fy":[18.22067,26.03282,86.08963,63.70451]}, - {"t":1.86481, "x":7.63159, "y":4.08084, "heading":-1.61833, "vx":-0.25848, "vy":-3.98793, "omega":-2.89361, "ax":-8.14352, "ay":3.23282, "alpha":-4.44642, "fx":[-148.06445,-145.18501,-125.11299,-135.71342], "fy":[30.3618,38.72749,83.94506,66.92269]}, - {"t":1.87628, "x":7.62809, "y":4.0353, "heading":-1.65153, "vx":-0.3519, "vy":-3.95085, "omega":-2.94461, "ax":-8.06497, "ay":3.58975, "alpha":-3.25277, "fx":[-145.21859,-142.2078,-126.91916,-134.38551], "fy":[42.54163,50.04717,81.81093,69.84309]}, - {"t":1.88775, "x":7.62352, "y":3.99022, "heading":-1.6853, "vx":-0.44441, "vy":-3.90967, "omega":-2.98193, "ax":-7.9683, "ay":3.90416, "alpha":-2.16937, "fx":[-141.57768,-138.93329,-128.45243,-133.19046], "fy":[53.86276,59.52287,79.91341,72.33562]}, - {"t":1.89922, "x":7.6179, "y":3.94563, "heading":-1.71951, "vx":-0.53582, "vy":-3.86488, "omega":-3.00681, "ax":-7.86299, "ay":4.17457, "alpha":-1.20794, "fx":[-137.45654,-135.65218,-129.70017,-132.18011], "fy":[63.97998,67.35594,78.32649,74.37087]}, - {"t":1.91069, "x":7.61124, "y":3.90157, "heading":-1.754, "vx":-0.62601, "vy":-3.81699, "omega":-3.02067, "ax":-7.75562, "ay":4.40513, "alpha":-0.36186, "fx":[-133.12385,-132.49692,-130.68667,-131.37605], "fy":[72.82651,73.87133,77.05906,75.96295]}, - {"t":1.92217, "x":7.60355, "y":3.85807, "heading":-1.78865, "vx":-0.71498, "vy":-3.76646, "omega":-3.02482, "ax":-7.65031, "ay":4.60139, "alpha":0.38163, "fx":[-128.77724,-129.51338,-131.44641,-130.78127], "fy":[80.4736,79.36312,76.09155,77.14516]}, - {"t":1.93364, "x":7.59484, "y":3.81517, "heading":-1.82335, "vx":-0.80274, "vy":-3.71368, "omega":-3.02044, "ax":-7.54043, "ay":4.80919, "alpha":0.89511, "fx":[-124.90759,-126.76453,-131.41325,-129.95658], "fy":[86.92214,84.33033,76.83873,79.12039]}, - {"t":1.94691, "x":7.58352, "y":3.76628, "heading":-1.86345, "vx":-0.90286, "vy":-3.64982, "omega":-3.00856, "ax":-7.40779, "ay":5.01107, "alpha":1.34932, "fx":[-120.6975,-123.65438,-130.84796,-128.81797], "fy":[92.81798,89.02378,78.0076,81.09796]}, - {"t":1.96019, "x":7.57088, "y":3.71826, "heading":-1.9034, "vx":-1.00122, "vy":-3.58329, "omega":-2.99064, "ax":-7.28557, "ay":5.18377, "alpha":1.7526, "fx":[-116.76692,-120.73508,-130.29069,-127.90923], "fy":[97.83307,93.10261,79.11498,82.64726]}, - {"t":1.97347, "x":7.55694, "y":3.67114, "heading":-1.94311, "vx":-1.09795, "vy":-3.51446, "omega":-2.96737, "ax":-7.1733, "ay":5.33252, "alpha":2.1123, "fx":[-113.13498,-117.98874,-129.73182,-127.20778], "fy":[102.11017,96.68927,80.1837,83.83524]}, - {"t":1.98675, "x":7.54173, "y":3.62495, "heading":-1.98251, "vx":-1.1932, "vy":-3.44366, "omega":-2.93932, "ax":-7.07029, "ay":5.46151, "alpha":2.43492, "fx":[-109.80282,-115.39677,-129.16538,-126.68928], "fy":[105.77008,99.87671,81.22829,84.71977]}, - {"t":2.00003, "x":7.52526, "y":3.57971, "heading":-2.02154, "vx":-1.28707, "vy":-3.37114, "omega":-2.90699, "ax":-6.97573, "ay":5.57413, "alpha":2.72598, "fx":[-106.76154,-112.942,-128.58782,-126.32926], "fy":[108.91267,102.73577,82.25791,85.3508]}, - {"t":2.0133, "x":7.50756, "y":3.53544, "heading":-2.06014, "vx":-1.3797, "vy":-3.29713, "omega":-2.8708, "ax":-6.88883, "ay":5.67308, "alpha":2.99003, "fx":[-103.99705,-110.60945,-127.99719,-126.10411], "fy":[111.61973,105.32092,83.27808,85.77139]}, - {"t":2.02658, "x":7.48863, "y":3.49216, "heading":-2.09825, "vx":-1.47116, "vy":-3.2218, "omega":-2.8311, "ax":-6.80881, "ay":5.76058, "alpha":3.23078, "fx":[-101.49298,-108.3864,-127.39269,-125.99173], "fy":[113.95802,107.67446,84.29195,86.0187]}, - {"t":2.03986, "x":7.4685, "y":3.44989, "heading":-2.13584, "vx":-1.56157, "vy":-3.14532, "omega":-2.7882, "ax":-6.73499, "ay":5.83839, "alpha":3.45119, "fx":[-99.23232,-106.26222,-126.77427,-125.97188], "fy":[115.98212,109.82965,85.30097,86.12483]}, - {"t":2.05314, "x":7.44717, "y":3.40864, "heading":-2.17287, "vx":-1.65099, "vy":-3.0678, "omega":-2.74238, "ax":-6.66671, "ay":5.90799, "alpha":3.65366, "fx":[-97.1983,-104.22809,-126.14246,-126.02633], "fy":[117.7368,111.81297,86.30544,86.11748]}, - {"t":2.06641, "x":7.42466, "y":3.36843, "heading":-2.20928, "vx":-1.73951, "vy":-2.98935, "omega":-2.69386, "ax":-6.60341, "ay":5.97056, "alpha":3.84008, "fx":[-95.37485,-102.27671,-125.4982,-126.13895], "fy":[119.259,113.64571,87.30491,86.02055]}, - {"t":2.07969, "x":7.40098, "y":3.32926, "heading":-2.24505, "vx":-1.82719, "vy":-2.91008, "omega":-2.64288, "ax":-6.5446, "ay":6.02711, "alpha":4.01198, "fx":[-93.74677,-100.40204,-124.8427,-126.29565], "fy":[120.57939,115.34525,88.29838,85.85454]}, - {"t":2.09297, "x":7.37615, "y":3.29116, "heading":-2.28014, "vx":-1.91409, "vy":-2.83005, "omega":-2.58961, "ax":-6.48983, "ay":6.07845, "alpha":4.1706, "fx":[-92.2998,-98.59908,-124.17738,-126.48431], "fy":[121.72361,116.9259,89.28453,85.63704]}, - {"t":2.10625, "x":7.35016, "y":3.25411, "heading":-2.31452, "vx":-2.00026, "vy":-2.74934, "omega":-2.53423, "ax":-6.43871, "ay":6.12529, "alpha":4.31695, "fx":[-91.02057,-96.86364,-123.50382,-126.69464], "fy":[122.7133,118.39959,90.26181,85.38296]}, - {"t":2.11953, "x":7.32303, "y":3.21815, "heading":-2.34817, "vx":-2.08575, "vy":-2.66801, "omega":-2.47691, "ax":-6.39091, "ay":6.16819, "alpha":4.4519, "fx":[-89.89656,-95.19224,-122.82368,-126.91802], "fy":[123.56679,119.77639,91.22858,85.10489]}, - {"t":2.1328, "x":7.29478, "y":3.18327, "heading":-2.38106, "vx":-2.17061, "vy":-2.58611, "omega":-2.4178, "ax":-6.34614, "ay":6.20765, "alpha":4.5762, "fx":[-88.91596,-93.58193,-122.13869,-127.14736], "fy":[124.29981,121.06489,92.18318,84.81334]}, - {"t":2.14608, "x":7.2654, "y":3.14948, "heading":-2.41316, "vx":-2.25487, "vy":-2.50369, "omega":-2.35704, "ax":-6.30412, "ay":6.24407, "alpha":4.69053, "fx":[-88.06763,-92.03019,-121.45062,-127.37692], "fy":[124.92595,122.27246,93.12392,84.51695]}, - {"t":2.15936, "x":7.2349, "y":3.11678, "heading":-2.44446, "vx":-2.33857, "vy":-2.42078, "omega":-2.29476, "ax":-6.26464, "ay":6.2778, "alpha":4.79552, "fx":[-87.34103,-90.53483,-120.76122,-127.60214], "fy":[125.45702,123.40553,94.04921,84.22277]}, - {"t":2.17264, "x":7.2033, "y":3.0852, "heading":-2.47493, "vx":-2.42175, "vy":-2.33743, "omega":-2.23109, "ax":-6.2275, "ay":6.30915, "alpha":4.89179, "fx":[-86.72612,-89.09395,-120.07228,-127.8195], "fy":[125.90343,124.46974,94.9575,83.93636]}, - {"t":2.18591, "x":7.17059, "y":3.05472, "heading":-2.50455, "vx":-2.50444, "vy":-2.25366, "omega":-2.16613, "ax":-6.19251, "ay":6.33835, "alpha":4.9799, "fx":[-86.21333,-87.70585,-119.38551,-128.02636], "fy":[126.27438,125.47008,95.84737,83.66207]}, - {"t":2.19919, "x":7.13679, "y":3.02535, "heading":-2.53331, "vx":-2.58666, "vy":-2.1695, "omega":-2.10001, "ax":-6.15951, "ay":6.36562, "alpha":5.06045, "fx":[-85.79354,-86.36901,-118.70264,-128.22083], "fy":[126.5781,126.41099,96.71746,83.40314]}, - {"t":2.21247, "x":7.10191, "y":2.99711, "heading":-2.56119, "vx":-2.66845, "vy":-2.08498, "omega":-2.03282, "ax":-6.15665, "ay":6.39202, "alpha":4.85824, "fx":[-87.18292,-86.33322,-117.78072,-127.59481], "fy":[125.78975,126.54616,97.97832,84.59118]}, - {"t":2.22949, "x":7.05561, "y":2.96255, "heading":-2.59579, "vx":-2.77321, "vy":-1.97621, "omega":-1.95015, "ax":-6.1541, "ay":6.39255, "alpha":4.85774, "fx":[-87.99066,-85.63931,-117.19395,-127.89394], "fy":[125.19074,126.99306,98.65476,84.10326]}, - {"t":2.2465, "x":7.00752, "y":2.92985, "heading":-2.62897, "vx":-2.87793, "vy":-1.86743, "omega":-1.86749, "ax":-6.15141, "ay":6.39298, "alpha":4.85769, "fx":[-88.78795,-84.97002,-116.61849,-128.15822], "fy":[124.58752,127.41571,99.30709,83.66075]}, - {"t":2.26352, "x":6.95766, "y":2.899, "heading":-2.66075, "vx":-2.98261, "vy":-1.75864, "omega":-1.78483, "ax":-6.14853, "ay":6.39328, "alpha":4.8588, "fx":[-89.56864,-84.32257,-116.05563,-128.39228], "fy":[123.98405,127.81616,99.93405,83.25689]}, - {"t":2.28054, "x":6.90602, "y":2.87, "heading":-2.69112, "vx":-3.08724, "vy":-1.64985, "omega":-1.70215, "ax":-6.14542, "ay":6.39341, "alpha":4.86175, "fx":[-90.32677,-83.69367,-115.5065,-128.60058], "fy":[123.38422,128.19659,100.53448,82.88473]}, - {"t":2.29755, "x":6.85259, "y":2.84285, "heading":-2.72009, "vx":-3.19181, "vy":-1.54105, "omega":-1.61941, "ax":-6.14201, "ay":6.39334, "alpha":4.86726, "fx":[-91.05657,-83.0795,-114.9721,-128.78748], "fy":[122.79181,128.5593,101.10727,82.53699]}, - {"t":2.31457, "x":6.79739, "y":2.81755, "heading":-2.74764, "vx":-3.29633, "vy":-1.43226, "omega":-1.53659, "ax":-6.13823, "ay":6.39303, "alpha":4.87607, "fx":[-91.75231,-82.47553,-114.45329,-128.9573], "fy":[122.21047,128.90675,101.6513,82.20597]}, - {"t":2.33159, "x":6.74041, "y":2.7941, "heading":-2.77379, "vx":-3.40078, "vy":-1.32347, "omega":-1.45362, "ax":-6.13399, "ay":6.39243, "alpha":4.88899, "fx":[-92.40821,-81.87633,-113.9508,-129.11446], "fy":[121.64368,129.24163,102.16538,81.88324]}, - {"t":2.3486, "x":6.68165, "y":2.77251, "heading":-2.79853, "vx":-3.50516, "vy":-1.21469, "omega":-1.37042, "ax":-6.12918, "ay":6.39148, "alpha":4.90697, "fx":[-93.01826,-81.27533,-113.4652,-129.26359], "fy":[121.09474,129.5669,102.6481,81.55929]}, - {"t":2.36562, "x":6.62112, "y":2.75276, "heading":-2.82185, "vx":-3.60946, "vy":-1.10593, "omega":-1.28692, "ax":-6.12366, "ay":6.39007, "alpha":4.93113, "fx":[-93.57595,-80.66435,-112.99699,-129.40975], "fy":[120.56667,129.88591,103.09776,81.22295]}, - {"t":2.38264, "x":6.55881, "y":2.73487, "heading":-2.84375, "vx":-3.71367, "vy":-0.99719, "omega":-1.20301, "ax":-6.11727, "ay":6.38807, "alpha":4.96291, "fx":[-94.07389,-80.03299,-112.5465,-129.55872], "fy":[120.06217,130.20252,103.51208,80.86047]}, - {"t":2.39965, "x":6.49473, "y":2.71883, "heading":-2.86422, "vx":-3.81776, "vy":-0.88849, "omega":-1.11856, "ax":-6.10978, "ay":6.38527, "alpha":5.00423, "fx":[-94.50326,-79.36767,-112.114,-129.71749], "fy":[119.58341,130.52134,103.88786,80.45415]}, - {"t":2.41667, "x":6.42888, "y":2.70463, "heading":-2.88325, "vx":-3.92173, "vy":-0.77983, "omega":-1.0334, "ax":-6.10089, "ay":6.38135, "alpha":5.05774, "fx":[-94.85292,-78.65001,-111.69967,-129.89497], "fy":[119.1317,130.84799,104.2204,79.97984]}, - {"t":2.43369, "x":6.36126, "y":2.69228, "heading":-2.90084, "vx":-4.02555, "vy":-0.67124, "omega":-0.94734, "ax":-6.09019, "ay":6.37579, "alpha":5.12734, "fx":[-95.10803,-77.85421,-111.30381,-130.10333], "fy":[118.70672,131.1895,104.50235,79.40276]}, - {"t":2.4507, "x":6.29188, "y":2.68179, "heading":-2.91696, "vx":-4.12918, "vy":-0.56275, "omega":-0.86009, "ax":-6.07708, "ay":6.3677, "alpha":5.21898, "fx":[-95.24765,-76.94247,-110.92704,-130.36024], "fy":[118.30493,131.55491,104.7216,78.66949]}, - {"t":2.46772, "x":6.22073, "y":2.67313, "heading":-2.93159, "vx":-4.23259, "vy":-0.45439, "omega":-0.77128, "ax":-6.06068, "ay":6.35549, "alpha":5.34234, "fx":[-95.24046,-75.85643,-110.57121,-130.69331], "fy":[117.91551,131.95588,104.85683,77.69205]}, - {"t":2.48474, "x":6.14783, "y":2.66632, "heading":-2.94472, "vx":-4.33573, "vy":-0.34624, "omega":-0.68037, "ax":-6.0396, "ay":6.33603, "alpha":5.51428, "fx":[-95.03677,-74.50022,-110.24155,-131.14916], "fy":[117.50996,132.40698,104.86711,76.31227]}, - {"t":2.50175, "x":6.07318, "y":2.66134, "heading":-2.9563, "vx":-4.4385, "vy":-0.23842, "omega":-0.58653, "ax":-6.01162, "ay":6.30242, "alpha":5.76712, "fx":[-94.55241,-72.70352,-109.95369,-131.81414], "fy":[117.00982,132.92284,104.66453,74.21209]}, - {"t":2.51877, "x":5.99678, "y":2.6582, "heading":-2.96628, "vx":-4.5408, "vy":-0.13118, "omega":-0.4884, "ax":-5.97298, "ay":6.23659, "alpha":6.17188, "fx":[-93.63679,-70.13158,-109.75936,-132.86704], "fy":[116.16547,133.4973,104.02604,70.64155]}, - {"t":2.53579, "x":5.91864, "y":2.65687, "heading":-2.97459, "vx":-4.64244, "vy":-0.02505, "omega":-0.38337, "ax":-5.91829, "ay":6.07596, "alpha":6.92246, "fx":[-92.04146,-66.03831,-109.87101,-134.72319], "fy":[113.90842,133.95685,102.2109,63.32516]}, - {"t":2.5528, "x":5.83879, "y":2.65732, "heading":-2.98111, "vx":-4.74315, "vy":0.07834, "omega":-0.26557, "ax":-5.86039, "ay":5.43011, "alpha":8.83226, "fx":[-90.35648,-58.70948,-111.66726,-138.00103], "fy":[101.08755,132.3329,94.97516,41.06293]}, - {"t":2.56982, "x":5.75723, "y":2.65944, "heading":-2.98563, "vx":-4.84287, "vy":0.17074, "omega":-0.11528, "ax":-2.48319, "ay":-6.05862, "alpha":4.7801, "fx":[-19.1798,-31.8018,-67.13137,-50.84071], "fy":[-114.51291,-98.60257,-90.2192,-108.88694]}, - {"t":2.58684, "x":5.67446, "y":2.66147, "heading":-2.98759, "vx":-4.88513, "vy":0.06765, "omega":-0.03394, "ax":2.13147, "ay":-7.81511, "alpha":-8.73335, "fx":[-10.78979,3.49636,64.58404,87.73213], "fy":[-141.00448,-146.48247,-131.80536,-112.43878]}, - {"t":2.60385, "x":5.59164, "y":2.66149, "heading":-2.98817, "vx":-4.84886, "vy":-0.06534, "omega":-0.18255, "ax":1.58812, "ay":-8.37429, "alpha":-4.63096, "fx":[1.23125,7.01728,46.13677,53.6686], "fy":[-145.8327,-147.69669,-140.52702,-135.72039]}, - {"t":2.6227, "x":5.50052, "y":2.65877, "heading":-2.99161, "vx":-4.81892, "vy":-0.2232, "omega":-0.26984, "ax":1.76033, "ay":-8.23967, "alpha":-4.56128, "fx":[5.50932,9.97287,48.12992,56.15892], "fy":[-143.6923,-145.99424,-138.29538,-132.63569]}, - {"t":2.64155, "x":5.40999, "y":2.6531, "heading":-2.9967, "vx":-4.78574, "vy":-0.37852, "omega":-0.35583, "ax":1.92457, "ay":-8.04944, "alpha":-4.518, "fx":[9.64695,12.80457,50.0018,58.49238], "fy":[-140.40783,-143.47601,-135.2879,-128.50293]}, - {"t":2.6604, "x":5.32012, "y":2.64453, "heading":-3.0034, "vx":-4.74946, "vy":-0.53026, "omega":-0.44099, "ax":2.09342, "ay":-7.78466, "alpha":-3.98216, "fx":[16.70619,17.71365,50.28117,57.73286], "fy":[-135.17458,-139.09426,-131.24275,-124.14758]}, - {"t":2.67925, "x":5.23096, "y":2.63316, "heading":-3.01172, "vx":-4.71, "vy":-0.677, "omega":-0.51606, "ax":2.23605, "ay":-7.41257, "alpha":-2.67554, "fx":[26.67161,25.70055,47.66442,52.10143], "fy":[-127.34004,-131.36954,-125.57619,-120.05684]}, - {"t":2.69811, "x":5.14257, "y":2.61908, "heading":-3.02144, "vx":-4.66784, "vy":-0.81673, "omega":-0.56649, "ax":2.3491, "ay":-6.77457, "alpha":-2.44834, "fx":[30.63616,29.15694,48.0817,51.95494], "fy":[-115.49197,-120.88684,-115.547,-109.00786]}, - {"t":2.71696, "x":5.055, "y":2.60248, "heading":-3.03212, "vx":-4.62356, "vy":-0.94444, "omega":-0.61265, "ax":2.81529, "ay":-5.47311, "alpha":-4.1247, "fx":[34.91127,31.95508,58.82229,65.86046], "fy":[-91.23691,-105.11454,-96.15683,-79.87588]}, - {"t":2.73581, "x":4.96834, "y":2.5837, "heading":-3.04367, "vx":-4.57049, "vy":-1.04761, "omega":-0.6904, "ax":3.81321, "ay":-2.71013, "alpha":-8.73245, "fx":[45.78916,40.20304,82.61407,90.8402], "fy":[-27.87775,-80.0521,-61.76105,-14.70316]}, - {"t":2.75466, "x":4.88286, "y":2.56347, "heading":-3.05669, "vx":-4.49861, "vy":-1.0987, "omega":-0.85501, "ax":4.46237, "ay":0.52422, "alpha":-12.82642, "fx":[45.30989,55.42417,104.79768,98.08304], "fy":[59.05765,-44.88659,-23.39755,44.89367]}, - {"t":2.77351, "x":4.79885, "y":2.54285, "heading":-3.0728, "vx":-4.41449, "vy":-1.08881, "omega":-1.0968, "ax":4.60552, "ay":2.289, "alpha":-14.22105, "fx":[34.51859,63.93838,117.03073,97.86634], "fy":[98.63545,-14.25022,-1.4919,72.84781]}, - {"t":2.79236, "x":4.71646, "y":2.52274, "heading":-3.09348, "vx":-4.32768, "vy":-1.04567, "omega":-1.36487, "ax":4.65522, "ay":3.2169, "alpha":-14.57573, "fx":[27.62947,66.81614,124.18129,98.109], "fy":[115.82802,9.04661,8.45552,85.54422]}, - {"t":2.81121, "x":4.63571, "y":2.5036, "heading":-3.11921, "vx":-4.23992, "vy":-0.98503, "omega":-1.63963, "ax":4.67029, "ay":3.83033, "alpha":-14.50468, "fx":[24.06374,66.03757,128.78374,98.87608], "fy":[124.99199,30.0502,13.50569,92.06368]}, - {"t":2.83006, "x":4.55661, "y":2.48571, "heading":3.13307, "vx":-4.15189, "vy":-0.91282, "omega":-1.91305, "ax":4.62948, "ay":4.3606, "alpha":-14.16772, "fx":[22.48038,61.16235,131.7428,99.59911], "fy":[130.6431,52.7133,17.34935,95.98418]}, - {"t":2.84891, "x":4.47917, "y":2.46928, "heading":3.09701, "vx":-4.06462, "vy":-0.83062, "omega":-2.18012, "ax":4.52747, "ay":4.89372, "alpha":-13.63958, "fx":[22.47237,52.36737,133.32903,99.87503], "fy":[134.40329,77.25617,22.41214,98.89144]}, - {"t":2.86776, "x":4.40335, "y":2.45449, "heading":3.05591, "vx":-3.97927, "vy":-0.73837, "omega":-2.43723, "ax":4.38972, "ay":5.4186, "alpha":-12.98465, "fx":[23.7645,42.19948,133.36271,99.34515], "fy":[136.98877,99.43079,30.56639,101.68956]}, - {"t":2.88661, "x":4.32912, "y":2.44153, "heading":3.00997, "vx":-3.89653, "vy":-0.63623, "omega":-2.682, "ax":4.22174, "ay":5.91858, "alpha":-12.19986, "fx":[25.46699,32.95333,131.21632,97.60566], "fy":[138.88567,115.95449,42.81809,105.0349]}, - {"t":2.90546, "x":4.25642, "y":2.43059, "heading":2.95941, "vx":-3.81694, "vy":-0.52466, "omega":-2.91197, "ax":3.99119, "ay":6.41293, "alpha":-11.20147, "fx":[26.5742,24.93633,125.78859,94.25685], "fy":[140.47485,127.20898,59.29457,109.35032]}, - {"t":2.92431, "x":4.18518, "y":2.42184, "heading":2.90452, "vx":-3.74171, "vy":-0.40377, "omega":-3.12313, "ax":3.66364, "ay":6.91742, "alpha":-9.88782, "fx":[26.58579,18.04556,115.67614,88.96213], "fy":[141.97618,134.73277,79.17696,114.76738]}, - {"t":2.94316, "x":4.11529, "y":2.41546, "heading":2.84565, "vx":-3.67265, "vy":-0.27338, "omega":-3.30952, "ax":3.23454, "ay":7.41602, "alpha":-8.21164, "fx":[25.72839,12.91415,99.9396,81.49243], "fy":[143.40491,139.7598,100.30093,121.11175]}, - {"t":2.96201, "x":4.04664, "y":2.41162, "heading":2.78326, "vx":-3.61167, "vy":-0.13358, "omega":-3.46431, "ax":2.74962, "ay":7.86202, "alpha":-6.25528, "fx":[24.84252,10.83664,79.64898,71.75296], "fy":[144.6256,143.1099,119.22952,127.95813]}, - {"t":2.98086, "x":3.97904, "y":2.4105, "heading":2.71796, "vx":-3.55984, "vy":0.01462, "omega":-3.58223, "ax":2.29143, "ay":8.20815, "alpha":-4.23008, "fx":[24.72641,12.59452,58.74876,59.83637], "fy":[145.51927,145.26887,132.96544,134.71976]}, - {"t":2.99971, "x":3.91235, "y":2.41224, "heading":2.65043, "vx":-3.51665, "vy":0.16935, "omega":-3.66196, "ax":1.92031, "ay":8.4429, "alpha":-2.32895, "fx":[25.38731,17.41583,41.59885,46.25384], "fy":[146.11648,146.44821,141.1742,140.70604]}, - {"t":3.01856, "x":3.8464, "y":2.41693, "heading":2.5814, "vx":-3.48045, "vy":0.3285, "omega":-3.70587, "ax":1.64618, "ay":8.58656, "alpha":-0.62542, "fx":[26.25835,23.80007,29.85161,32.09388], "fy":[146.5478,146.77687,145.5856,145.30932]}, - {"t":3.03741, "x":3.78108, "y":2.42465, "heading":2.51154, "vx":-3.44942, "vy":0.49036, "omega":-3.71766, "ax":1.45746, "ay":8.66503, "alpha":0.87129, "fx":[26.86674,30.73727,22.8923,18.6678], "fy":[146.92695,146.39581,147.92639,148.30984]}, - {"t":3.05626, "x":3.71632, "y":2.43543, "heading":2.44146, "vx":-3.42194, "vy":0.6537, "omega":-3.70123, "ax":1.34241, "ay":8.69888, "alpha":2.16523, "fx":[27.01493,37.82441,19.52642,6.97006], "fy":[147.31294,145.41826,149.20248,149.92795]}, - {"t":3.07512, "x":3.65205, "y":2.4493, "heading":2.37169, "vx":-3.39664, "vy":0.81768, "omega":-3.66042, "ax":1.29006, "ay":8.70291, "alpha":3.26988, "fx":[26.65268,44.93569,18.72741,-2.54126], "fy":[147.72937,143.91476,149.89171,150.60025]}, - {"t":3.09397, "x":3.58825, "y":2.46626, "heading":2.30269, "vx":-3.37232, "vy":0.98174, "omega":-3.59878, "ax":1.28859, "ay":8.68726, "alpha":4.20994, "fx":[25.76457,52.0105,19.74074,-9.84192], "fy":[148.18551,141.93297,150.20238,150.75013]}, - {"t":3.11282, "x":3.52491, "y":2.48631, "heading":2.23485, "vx":-3.34803, "vy":1.1455, "omega":-3.51942, "ax":1.32728, "ay":8.65778, "alpha":5.02337, "fx":[24.37294,59.05345,22.04683,-15.16639], "fy":[148.67583,139.4886,150.21871,150.68225]}, - {"t":3.13167, "x":3.46203, "y":2.50944, "heading":2.16851, "vx":-3.32301, "vy":1.3087, "omega":-3.42472, "ax":1.4363, "ay":8.6002, "alpha":5.90543, "fx":[23.44959,67.77855,25.85775,-19.36141], "fy":[149.00702,135.74832,149.88201,150.51026]}, - {"t":3.15052, "x":3.39965, "y":2.53564, "heading":2.10395, "vx":-3.29593, "vy":1.47082, "omega":-3.3134, "ax":2.26231, "ay":8.52014, "alpha":4.4167, "fx":[39.13188,69.99594,38.83959,5.95777], "fy":[146.15401,134.74911,147.07516,151.72234]}, - {"t":3.17044, "x":3.33442, "y":2.56664, "heading":2.03793, "vx":-3.25085, "vy":1.6406, "omega":-3.22539, "ax":3.22195, "ay":8.26375, "alpha":2.9607, "fx":[56.29029,75.02384,54.21035,33.69307], "fy":[140.52458,131.88483,141.93302,147.91363]}, - {"t":3.19037, "x":3.27028, "y":2.60097, "heading":1.97365, "vx":-3.18665, "vy":1.80527, "omega":-3.16639, "ax":4.23127, "ay":7.83609, "alpha":1.2238, "fx":[72.94505,79.88599,71.23207,63.8277], "fy":[132.78099,128.88764,133.96935,137.52081]}, - {"t":3.2103, "x":3.20762, "y":2.6385, "heading":1.91056, "vx":-3.10233, "vy":1.96142, "omega":-3.14201, "ax":5.21919, "ay":7.2202, "alpha":-0.67198, "fx":[88.12711,84.7082,89.51807,92.75423], "fy":[123.40857,125.69266,122.2453,119.90749]}, - {"t":3.23023, "x":3.14683, "y":2.67902, "heading":1.84794, "vx":-2.99833, "vy":2.1053, "omega":-3.1554, "ax":6.10382, "ay":6.4434, "alpha":-2.5685, "fx":[101.25792,89.46133,107.93824,116.6397], "fy":[113.09816,122.31836,106.0491,96.9361]}, - {"t":3.25015, "x":3.0883, "y":2.72225, "heading":1.78507, "vx":-2.8767, "vy":2.2337, "omega":-3.20658, "ax":6.83307, "ay":5.56424, "alpha":-4.31498, "fx":[112.30819,94.3929,124.68874,133.52442], "fy":[102.36854,118.56581,85.46251,72.18793]}, - {"t":3.27008, "x":3.03233, "y":2.76787, "heading":1.72117, "vx":-2.74053, "vy":2.34458, "omega":-3.29257, "ax":7.41489, "ay":4.63159, "alpha":-5.6815, "fx":[121.89027,101.00797,137.81017,143.79226], "fy":[90.98624,113.04921,62.00987,49.08263]}, - {"t":3.29001, "x":2.97919, "y":2.81551, "heading":1.65556, "vx":-2.59278, "vy":2.43687, "omega":-3.40578, "ax":7.98062, "ay":3.58447, "alpha":-5.72373, "fx":[132.12396,115.91967,146.02654,148.92198], "fy":[75.5488,97.85499,39.68958,30.7902]}, - {"t":3.30993, "x":2.92911, "y":2.86478, "heading":1.58769, "vx":-2.43374, "vy":2.5083, "omega":-3.51984, "ax":8.51403, "ay":2.39833, "alpha":-4.10112, "fx":[142.47001,135.80979,149.91661,151.08856], "fy":[53.68585,68.00179,23.3265,18.1657]}, - {"t":3.32986, "x":2.8823, "y":2.91524, "heading":1.51755, "vx":-2.26408, "vy":2.55609, "omega":-3.60156, "ax":8.82163, "ay":1.30432, "alpha":-2.32265, "fx":[149.14072,147.45469,151.59702,152.02129], "fy":[30.90089,37.32727,11.74419,8.7721]}, - {"t":3.34979, "x":2.83894, "y":2.96643, "heading":1.44578, "vx":-2.08829, "vy":2.58209, "omega":-3.64785, "ax":8.94379, "ay":0.38999, "alpha":-0.82346, "fx":[152.043,151.85352,152.27046,152.35812], "fy":[10.10253,11.79947,2.93065,1.70158]}, - {"t":3.36972, "x":2.7991, "y":3.01796, "heading":1.37309, "vx":-1.91007, "vy":2.58986, "omega":-3.66426, "ax":8.95599, "ay":-0.34866, "alpha":0.39095, "fx":[152.25781,152.25518,152.42639,152.4158], "fy":[-7.68398,-8.28932,-4.23128,-3.5179]}, - {"t":3.38964, "x":2.76281, "y":3.0695, "heading":1.30007, "vx":-1.7316, "vy":2.58291, "omega":-3.65647, "ax":8.90995, "ay":-0.94153, "alpha":1.37843, "fx":[150.87597,150.7038,152.27734,152.36594], "fy":[-22.34863,-24.08054,-10.35153,-7.28002]}, - {"t":3.40957, "x":2.73008, "y":3.12079, "heading":1.22721, "vx":-1.55405, "vy":2.56415, "omega":-3.629, "ax":8.83569, "ay":-1.41994, "alpha":2.19026, "fx":[148.70716,148.24445,151.9248,152.29387], "fy":[-34.19018,-36.71164,-15.77815,-9.93127]}, - {"t":3.4295, "x":2.70086, "y":3.1716, "heading":1.15489, "vx":-1.37798, "vy":2.53585, "omega":-3.58535, "ax":8.74974, "ay":-1.81001, "alpha":2.8639, "fx":[146.27679,145.38888,151.42203,152.23492], "fy":[-43.62989,-47.03558,-20.71765,-11.76768]}, - {"t":3.44942, "x":2.67514, "y":3.22177, "heading":1.08345, "vx":-1.20363, "vy":2.49978, "omega":-3.52828, "ax":8.66103, "ay":-2.13195, "alpha":3.42598, "fx":[143.89952,142.3893,150.79994,152.19799], "fy":[-51.07849,-55.64791,-25.29614,-13.03308]}, - {"t":3.46935, "x":2.65288, "y":3.27116, "heading":1.01314, "vx":-1.03104, "vy":2.4573, "omega":-3.46001, "ax":8.57426, "ay":-2.40102, "alpha":3.89572, "fx":[141.75139,139.37287,150.07861,152.17992], "fy":[-56.88918,-62.9597,-29.59224,-13.92135]}, - {"t":3.48928, "x":2.63403, "y":3.31965, "heading":0.94419, "vx":-0.86018, "vy":2.40945, "omega":-3.38238, "ax":8.49182, "ay":-2.62857, "alpha":4.28767, "fx":[139.92184,136.40556,149.27266,152.17337], "fy":[-61.35022,-69.25763,-33.65562,-14.58132]}, - {"t":3.50921, "x":2.61858, "y":3.36714, "heading":0.87679, "vx":-0.69096, "vy":2.35707, "omega":-3.29694, "ax":8.41482, "ay":-2.82308, "alpha":4.61362, "fx":[138.4472,133.52257,148.39394,152.17073], "fy":[-64.69251,-74.74551,-37.51805,-15.12305]}, - {"t":3.52913, "x":2.60648, "y":3.41355, "heading":0.81109, "vx":-0.52327, "vy":2.30082, "omega":-3.205, "ax":8.34366, "ay":-2.99089, "alpha":4.88377, "fx":[137.33127,130.74318,147.45282,152.16562], "fy":[-67.10077,-79.57155,-41.20001,-15.62456]}, - {"t":3.54906, "x":2.59771, "y":3.45881, "heading":0.74722, "vx":-0.35701, "vy":2.24122, "omega":-3.10769, "ax":8.27833, "ay":-3.13679, "alpha":5.10738, "fx":[136.55792,128.07818,146.45885,152.15323], "fy":[-68.72444,-83.8461,-44.71489,-16.13829]}, - {"t":3.56899, "x":2.59224, "y":3.50285, "heading":0.6853, "vx":-0.19205, "vy":2.17871, "omega":-3.00591, "ax":8.21859, "ay":-3.26444, "alpha":5.29309, "fx":[136.09874,125.5336,145.42109,152.13022], "fy":[-69.68658,-87.65326,-48.07165,-16.69697]}, - {"t":3.58891, "x":2.59004, "y":3.54561, "heading":0.6254, "vx":-0.02827, "vy":2.11366, "omega":-2.90043, "ax":8.16406, "ay":-3.37667, "alpha":5.44888, "fx":[135.91807,123.11269,144.34818,152.09438], "fy":[-70.09085,-91.05856,-51.27644,-17.31864]}, - {"t":3.60884, "x":2.5911, "y":3.58706, "heading":0.5676, "vx":0.13441, "vy":2.04637, "omega":-2.79185, "ax":8.11428, "ay":-3.47574, "alpha":5.58205, "fx":[135.97639,120.81693,143.24846,152.04441], "fy":[-70.02672,-94.11419,-54.33372,-18.01074]}, - {"t":3.62877, "x":2.59539, "y":3.62715, "heading":0.51197, "vx":0.29611, "vy":1.97711, "omega":-2.68062, "ax":8.06876, "ay":-3.56349, "alpha":5.69904, "fx":[136.23275,118.64661,142.12995,151.97964], "fy":[-69.57319,-96.86248,-57.24688,-18.77328]}, - {"t":3.6487, "x":2.60289, "y":3.66584, "heading":0.45855, "vx":0.4569, "vy":1.9061, "omega":-2.56705, "ax":8.027, "ay":-3.64145, "alpha":5.80529, "fx":[136.64664,116.60111,141.00037,151.89995], "fy":[-68.80138,-99.33844,-60.01868,-19.60132]}, - {"t":3.66862, "x":2.61359, "y":3.7031, "heading":0.40739, "vx":0.61685, "vy":1.83354, "omega":-2.45137, "ax":7.98854, "ay":-3.71091, "alpha":5.90519, "fx":[137.17943,114.67907,139.86714,151.8056], "fy":[-67.77613,-101.57152,-62.65146,-20.4867]}, - {"t":3.68855, "x":2.62747, "y":3.7389, "heading":0.35855, "vx":0.77604, "vy":1.75959, "omega":-2.3337, "ax":7.95293, "ay":-3.773, "alpha":6.00203, "fx":[137.79548,112.87847,138.73737,151.69723], "fy":[-66.55697,-103.58683,-65.1473,-21.41939]}, - {"t":3.70848, "x":2.64451, "y":3.77322, "heading":0.31204, "vx":0.93452, "vy":1.6844, "omega":-2.21409, "ax":7.92804, "ay":-3.88002, "alpha":5.75067, "fx":[138.10724,112.70625,137.45756,151.14363], "fy":[-66.32301,-103.93671,-68.00663,-25.72596]}, - {"t":3.7395, "x":2.67731, "y":3.8236, "heading":0.24337, "vx":1.18043, "vy":1.56405, "omega":-2.03572, "ax":7.83609, "ay":-4.0301, "alpha":5.94719, "fx":[138.14558,109.11353,135.20108,150.69863], "fy":[-66.12912,-107.6556,-72.34149,-28.07722]}, - {"t":3.77051, "x":2.7177, "y":3.87017, "heading":0.18022, "vx":1.42349, "vy":1.43905, "omega":-1.85125, "ax":7.71121, "ay":-4.22292, "alpha":6.20241, "fx":[137.57448,104.49467,132.48892,150.10379], "fy":[-67.16007,-112.08756,-77.1399,-30.93503]}, - {"t":3.80153, "x":2.76556, "y":3.91278, "heading":0.1228, "vx":1.66267, "vy":1.30806, "omega":-1.65887, "ax":7.53409, "ay":-4.4788, "alpha":6.53082, "fx":[136.03584,98.2255,129.07076,149.27875], "fy":[-70.02413,-117.54972,-82.66278,-34.49574]}, - {"t":3.83255, "x":2.82076, "y":3.9512, "heading":0.07135, "vx":1.89636, "vy":1.16914, "omega":-1.45629, "ax":7.26708, "ay":-4.83255, "alpha":6.95576, "fx":[132.70802,89.19461,124.48214,148.05914], "fy":[-75.86544,-124.45197,-89.33615,-39.14738]}, - {"t":3.86357, "x":2.88308, "y":3.98514, "heading":0.02618, "vx":2.12177, "vy":1.01925, "omega":-1.24054, "ax":6.82694, "ay":-5.34819, "alpha":7.51528, "fx":[125.34908,75.30247,117.78276,146.06286], "fy":[-87.09286,-133.20236,-97.88564,-45.70384]}, - {"t":3.89458, "x":2.95217, "y":4.01418, "heading":-0.0123, "vx":2.33353, "vy":0.85336, "omega":-1.00743, "ax":5.9923, "ay":-6.14788, "alpha":8.30953, "fx":[106.20304,52.45501,106.83729,142.21389], "fy":[-109.0413,-143.5935,-109.57255,-56.08703]}, - {"t":3.9256, "x":3.02744, "y":4.03769, "heading":-0.04355, "vx":2.5194, "vy":0.66266, "omega":-0.74969, "ax":4.04264, "ay":-7.33938, "alpha":10.16384, "fx":[43.08123,13.51094,86.08888,132.37524], "fy":[-145.19353,-152.09468,-126.3082,-75.76637]}, - {"t":3.95662, "x":3.10753, "y":4.05471, "heading":-0.06681, "vx":2.64479, "vy":0.43501, "omega":-0.43443, "ax":-0.0309, "ay":-7.94073, "alpha":14.22872, "fx":[-85.43275,-45.85425,39.95348,89.23097], "fy":[-124.97597,-145.48074,-147.23873,-122.58241]}, - {"t":3.98764, "x":3.18955, "y":4.06439, "heading":-0.08028, "vx":2.64383, "vy":0.18871, "omega":0.00691, "ax":-5.64383, "ay":-6.47776, "alpha":7.87804, "fx":[-137.71554,-105.49713,-52.72714,-88.06022], "fy":[-64.7518,-110.18924,-142.8923,-122.90572]}, - {"t":4.01866, "x":3.26884, "y":4.06712, "heading":-0.08007, "vx":2.46877, "vy":-0.01222, "omega":0.25127, "ax":-8.22725, "ay":-3.37604, "alpha":3.71757, "fx":[-148.76283,-138.27402,-128.13477,-144.60116], "fy":[-34.14342,-64.81687,-82.73478,-48.00652]}, - {"t":4.04967, "x":3.34146, "y":4.06512, "heading":-0.07227, "vx":2.21358, "vy":-0.11693, "omega":0.36658, "ax":-8.85168, "ay":-1.50447, "alpha":1.15517, "fx":[-151.75151,-149.93837,-149.19238,-151.37567], "fy":[-18.45466,-29.85302,-33.13977,-20.91506]}, - {"t":4.08069, "x":3.40586, "y":4.06077, "heading":-0.0609, "vx":1.93902, "vy":-0.1636, "omega":0.40241, "ax":-8.98429, "ay":-0.44747, "alpha":-0.25546, "fx":[-152.73893,-152.87689,-152.89762,-152.76732], "fy":[-9.13029,-6.39337,-6.11599,-8.80576]}, - {"t":4.11171, "x":3.46168, "y":4.05548, "heading":-0.04842, "vx":1.66035, "vy":-0.17748, "omega":0.39449, "ax":-8.99293, "ay":0.19977, "alpha":-1.11249, "fx":[-153.07662,-152.81391,-152.85582,-153.12205], "fy":[-2.931,9.41576,9.26791,-2.16043]}, - {"t":4.14273, "x":3.50886, "y":4.05007, "heading":-0.03618, "vx":1.38141, "vy":-0.17128, "omega":0.35998, "ax":-8.96848, "ay":0.62951, "alpha":-1.68397, "fx":[-153.16258,-151.79552,-152.04822,-153.19876], "fy":[1.52268,20.48712,18.85291,1.96815]}, - {"t":4.17374, "x":3.54739, "y":4.04506, "heading":-0.02502, "vx":1.10323, "vy":-0.15176, "omega":0.30775, "ax":-8.93653, "ay":0.93328, "alpha":-2.09035, "fx":[-153.14035,-150.5377,-151.16466,-153.18889], "fy":[4.89696,28.5597,25.28893,4.75397]}, - {"t":4.20476, "x":3.57731, "y":4.0408, "heading":-0.01547, "vx":0.82603, "vy":-0.12281, "omega":0.24291, "ax":-8.90491, "ay":1.15851, "alpha":-2.39301, "fx":[-153.07005,-149.29312,-150.36568,-153.15137], "fy":[7.5466,34.65384,29.87104,6.75228]}, - {"t":4.23578, "x":3.59865, "y":4.03755, "heading":-0.00794, "vx":0.54982, "vy":-0.08687, "omega":0.16869, "ax":-8.87594, "ay":1.33178, "alpha":-2.62652, "fx":[-152.97998,-148.14701,-149.67558,-153.10622], "fy":[9.67513,39.38679,33.29167,8.25895]}, - {"t":4.2668, "x":3.61143, "y":4.0355, "heading":-0.00271, "vx":0.27451, "vy":-0.04557, "omega":0.08722, "ax":-8.85009, "ay":1.46902, "alpha":-2.81185, "fx":[-152.88485,-147.12195,-149.0829,-153.06024], "fy":[11.40634,43.14713,35.95074,9.446]}, - {"t":4.29782, "x":3.61569, "y":4.03479, "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":3.61569, "y":5.55879, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.99335, "ay":-0.47437, "alpha":1.42105, "fx":[153.3663,152.48692,152.66074,153.38309], "fy":[-0.51165,-16.41745,-14.8834,-0.46337]}, + {"t":0.03797, "x":3.62217, "y":5.55845, "heading":0.0, "vx":0.34145, "vy":-0.01801, "omega":0.05395, "ax":8.99294, "ay":-0.47426, "alpha":1.39797, "fx":[153.35226,152.48872,152.65854,153.37014], "fy":[-0.63842,-16.27402,-14.77665,-0.5791]}, + {"t":0.07593, "x":3.64162, "y":5.55742, "heading":0.00205, "vx":0.68288, "vy":-0.03602, "omega":0.10703, "ax":8.99247, "ay":-0.47413, "alpha":1.371, "fx":[153.33565,152.48919,152.65737,153.35491], "fy":[-0.80215,-16.12053,-14.63617,-0.70029]}, + {"t":0.1139, "x":3.67402, "y":5.55571, "heading":0.00611, "vx":1.02429, "vy":-0.05402, "omega":0.15908, "ax":8.99189, "ay":-0.47397, "alpha":1.33908, "fx":[153.31569,152.48853,152.65691,153.33675], "fy":[-1.00731,-15.94922,-14.45809,-0.83375]}, + {"t":0.15187, "x":3.71939, "y":5.55332, "heading":0.01215, "vx":1.36568, "vy":-0.07201, "omega":0.20992, "ax":8.99118, "ay":-0.47378, "alpha":1.30069, "fx":[153.2913,152.48692,152.65669,153.31473], "fy":[-1.26027,-15.7492,-14.23686,-0.989]}, + {"t":0.18983, "x":3.77772, "y":5.55025, "heading":0.02012, "vx":1.70704, "vy":-0.09, "omega":0.2593, "ax":8.99029, "ay":-0.47354, "alpha":1.25368, "fx":[153.26091,152.48459,152.65605,153.28741], "fy":[-1.57028,-15.50491,-13.96428,-1.17981]}, + {"t":0.2278, "x":3.84901, "y":5.54649, "heading":0.02997, "vx":2.04837, "vy":-0.10798, "omega":0.3069, "ax":8.98913, "ay":-0.47324, "alpha":1.19475, "fx":[153.22211,152.48177,152.65395,153.25261], "fy":[-1.95117,-15.19328,-13.62783,-1.42677]}, + {"t":0.26577, "x":3.93326, "y":5.54205, "heading":0.04162, "vx":2.38966, "vy":-0.12595, "omega":0.35226, "ax":8.98759, "ay":-0.47286, "alpha":1.11876, "fx":[153.1711,152.47857,152.64873,153.20668], "fy":[-2.42458,-14.77848,-13.20742,-1.76221]}, + {"t":0.30373, "x":4.03047, "y":5.53692, "heading":0.05499, "vx":2.73088, "vy":-0.1439, "omega":0.39474, "ax":8.98541, "ay":-0.47233, "alpha":1.01704, "fx":[153.10144,152.47476,152.63743,153.14319], "fy":[-3.02637,-14.2015,-12.6689,-2.24023]}, + {"t":0.3417, "x":4.14062, "y":5.53112, "heading":0.06998, "vx":3.07203, "vy":-0.16183, "omega":0.43335, "ax":8.98213, "ay":-0.47158, "alpha":0.87385, "fx":[153.00128,152.46889,152.61436,153.04957], "fy":[-3.82052,-13.35722,-11.94901,-2.95914]}, + {"t":0.37966, "x":4.26373, "y":5.52464, "heading":0.08643, "vx":3.41305, "vy":-0.17974, "omega":0.46653, "ax":8.97673, "ay":-0.47042, "alpha":0.65736, "fx":[152.84618,152.4554,152.56688,152.89789], "fy":[-4.93371,-12.03739,-10.91642,-4.11912]}, + {"t":0.41763, "x":4.39978, "y":5.51747, "heading":0.10414, "vx":3.75386, "vy":-0.1976, "omega":0.49148, "ax":8.96636, "ay":-0.46837, "alpha":0.2918, "fx":[152.57654,152.4121,152.46038,152.61207], "fy":[-6.65664,-9.76071,-9.24737,-6.20289]}, + {"t":0.4556, "x":4.54877, "y":5.50963, "heading":0.1228, "vx":4.09428, "vy":-0.21538, "omega":0.50256, "ax":8.94027, "ay":-0.4639, "alpha":-0.45836, "fx":[152.00078,152.22633,152.15891,151.89975], "fy":[-9.8329,-5.10456,-5.87672,-10.74924]}, + {"t":0.49356, "x":4.71066, "y":5.50112, "heading":0.14188, "vx":4.43371, "vy":-0.23299, "omega":0.48516, "ax":8.81235, "ay":-0.44644, "alpha":-2.89605, "fx":[150.04241,150.82739,150.41705,148.29558], "fy":[-18.30739,8.72078,5.83193,-26.62059]}, + {"t":0.53153, "x":4.88534, "y":5.49195, "heading":0.1603, "vx":4.76829, "vy":-0.24994, "omega":0.37521, "ax":3.43612, "ay":-0.0829, "alpha":-9.84386, "fx":[83.52904,77.84673,31.97298,40.44086], "fy":[-23.00935,29.61766,27.48169,-39.73074]}, + {"t":0.5695, "x":5.06885, "y":5.48241, "heading":0.17455, "vx":4.89875, "vy":-0.25309, "omega":0.00147, "ax":0.0263, "ay":0.48143, "alpha":-0.00536, "fx":[0.46405,0.45902,0.4306,0.43562], "fy":[8.17737,8.20564,8.20066,8.1724]}, + {"t":0.60746, "x":5.25486, "y":5.47314, "heading":0.17461, "vx":4.89974, "vy":-0.23481, "omega":0.00127, "ax":0.07624, "ay":1.87869, "alpha":0.00005, "fx":[1.29665,1.2967,1.29697,1.29692], "fy":[31.95601,31.95576,31.9558,31.95605]}, + {"t":0.64543, "x":5.44094, "y":5.46558, "heading":0.17465, "vx":4.90264, "vy":-0.16348, "omega":0.00127, "ax":0.06802, "ay":5.20855, "alpha":0.00017, "fx":[1.15636,1.15658,1.15777,1.15755], "fy":[88.59626,88.59566,88.59576,88.59636]}, + {"t":0.6834, "x":5.62712, "y":5.46313, "heading":0.1747, "vx":4.90522, "vy":0.03427, "omega":0.00128, "ax":-0.30043, "ay":7.57552, "alpha":-0.07825, "fx":[-4.7044,-4.80724,-5.51433,-5.41467], "fy":[128.8205,128.93929,128.89443,128.7758]}, + {"t":0.72136, "x":5.81314, "y":5.46989, "heading":0.17475, "vx":4.89382, "vy":0.32188, "omega":-0.0017, "ax":-4.54495, "ay":5.98188, "alpha":-13.345, "fx":[-32.67137,-28.76331,-106.24979,-141.54856], "fy":[128.33382,143.98997,103.14383,31.53288]}, + {"t":0.75933, "x":5.99567, "y":5.48642, "heading":0.17469, "vx":4.72126, "vy":0.54899, "omega":-0.50836, "ax":-7.47188, "ay":1.84432, "alpha":-14.74701, "fx":[-142.25118,-79.50413,-137.86852,-148.75426], "fy":[-37.31986,128.49562,64.08268,-29.77335]}, + {"t":0.7973, "x":6.16953, "y":5.5086, "heading":0.15539, "vx":4.43758, "vy":0.61902, "omega":-1.06825, "ax":-8.435, "ay":1.46346, "alpha":-8.53874, "fx":[-151.05368,-125.9604,-145.46328,-151.43019], "fy":[-13.71856,85.3847,46.26812,-18.36189]}, + {"t":0.83526, "x":6.33193, "y":5.53315, "heading":0.11483, "vx":4.11733, "vy":0.67458, "omega":-1.39244, "ax":-8.68512, "ay":1.17313, "alpha":-6.31044, "fx":[-152.13595,-138.59493,-147.87598,-152.31872], "fy":[-10.17485,63.93267,38.89092,-12.83001]}, + {"t":0.87323, "x":6.48199, "y":5.55961, "heading":0.06196, "vx":3.78759, "vy":0.71912, "omega":-1.63202, "ax":-8.78756, "ay":1.00924, "alpha":-5.19406, "fx":[-152.51719,-143.70478,-148.93581,-152.73803], "fy":[-9.3123,52.11422,35.2371,-9.37124]}, + {"t":0.9112, "x":6.61946, "y":5.58764, "heading":0.0, "vx":3.45396, "vy":0.75744, "omega":-1.82922, "ax":-8.86785, "ay":0.17183, "alpha":-3.91153, "fx":[-150.80588,-149.8038,-151.07579,-151.673], "fy":[-19.83232,26.40986,20.3655,-15.25178]}, + {"t":0.92689, "x":6.67259, "y":5.59955, "heading":-0.02872, "vx":3.31473, "vy":0.76013, "omega":-1.89063, "ax":-8.81024, "ay":-1.28558, "alpha":-3.12574, "fx":[-146.2854,-152.03952,-152.38758,-148.72615], "fy":[-42.2189,-6.88974,-4.68281,-33.67826]}, + {"t":0.94259, "x":6.72355, "y":5.61133, "heading":-0.0584, "vx":3.17642, "vy":0.73995, "omega":-1.9397, "ax":-8.56299, "ay":-2.53713, "alpha":-2.38585, "fx":[-140.38445,-148.2287,-149.84295,-144.16027], "fy":[-59.30214,-35.05777,-28.36292,-49.90078]}, + {"t":0.95829, "x":6.77236, "y":5.62263, "heading":-0.08885, "vx":3.04198, "vy":0.70012, "omega":-1.97716, "ax":-8.21257, "ay":-3.56018, "alpha":-1.74048, "fx":[-134.31637,-141.4356,-144.40315,-138.61907], "fy":[-72.25509,-56.89309,-49.23606,-63.84628]}, + {"t":0.97399, "x":6.8191, "y":5.63318, "heading":-0.11989, "vx":2.91305, "vy":0.64423, "omega":-2.00448, "ax":-7.82494, "ay":-4.37545, "alpha":-1.18996, "fx":[-128.61274,-133.88264,-137.29311,-132.61172], "fy":[-82.1704,-73.16787,-66.6999,-75.66218]}, + {"t":0.98969, "x":6.86387, "y":5.64276, "heading":-0.15136, "vx":2.7902, "vy":0.57553, "omega":-2.02317, "ax":-7.43991, "ay":-5.01952, "alpha":-0.72396, "fx":[-123.4504,-126.69427,-129.56275,-126.49562], "fy":[-89.88753,-85.20038,-80.83316,-85.60151]}, + {"t":1.00539, "x":6.90676, "y":5.65117, "heading":-0.18312, "vx":2.6734, "vy":0.49673, "omega":-2.03453, "ax":-7.07767, "ay":-5.52914, "alpha":-0.33108, "fx":[-118.84636,-120.29187,-121.91905,-120.499], "fy":[-96.00896,-94.16963,-92.07312,-93.9449]}, + {"t":1.02109, "x":6.94786, "y":5.65829, "heading":-0.21507, "vx":2.56228, "vy":0.40992, "omega":-2.03973, "ax":-6.74642, "ay":-5.93532, "alpha":-0.00003, "fx":[-114.7546,-114.75471,-114.75486,-114.75475], "fy":[-100.95827,-100.95814,-100.95797,-100.9581]}, + {"t":1.03679, "x":6.98725, "y":5.664, "heading":-0.24709, "vx":2.45637, "vy":0.31674, "omega":-2.03973, "ax":-6.44792, "ay":-6.26236, "alpha":0.27985, "fx":[-111.11005,-110.02327,-108.24429,-109.33138], "fy":[-105.03332,-106.18638,-107.99014,-106.87384]}, + {"t":1.05249, "x":7.02502, "y":5.6682, "heading":-0.27911, "vx":2.35514, "vy":0.21843, "omega":-2.03534, "ax":-6.18072, "ay":-6.52867, "alpha":0.51791, "fx":[-107.84708,-105.99429,-102.43153,-104.25656], "fy":[-108.44563,-110.2831,-113.58714,-111.88757]}, + {"t":1.06819, "x":7.06124, "y":5.67082, "heading":-0.31106, "vx":2.25811, "vy":0.11593, "omega":-2.02721, "ax":-5.94208, "ay":-6.74805, "alpha":0.72197, "fx":[-104.90641,-102.56039,-97.29253,-99.53333], "fy":[-111.34746,-113.54547,-118.07704,-116.15968]}, + {"t":1.08389, "x":7.09596, "y":5.67181, "heading":-0.34289, "vx":2.16482, "vy":0.00999, "omega":-2.01587, "ax":-5.72885, "ay":-6.93079, "alpha":0.89836, "fx":[-102.23707,-99.62456,-92.77215,-95.15082], "fy":[-113.85008,-116.18188,-121.71079,-119.82024]}, + {"t":1.09959, "x":7.12924, "y":5.67111, "heading":-0.37454, "vx":2.07488, "vy":-0.09882, "omega":-2.00177, "ax":-5.53795, "ay":-7.08462, "alpha":1.05206, "fx":[-99.79625,-97.10403,-88.80464,-91.09081], "fy":[-116.03602,-118.3408,-124.67833,-122.97428]}, + {"t":1.11529, "x":7.16113, "y":5.66869, "heading":-0.40597, "vx":1.98793, "vy":-0.21005, "omega":-1.98525, "ax":-5.36655, "ay":-7.21539, "alpha":1.18701, "fx":[-97.54837,-94.9299,-85.32406,-87.33185], "fy":[-117.96738,-120.12982,-127.12318,-125.70651]}, + {"t":1.13099, "x":7.19168, "y":5.6645, "heading":-0.43713, "vx":1.90368, "vy":-0.32332, "omega":-1.96661, "ax":-5.21217, "ay":-7.32757, "alpha":1.30629, "fx":[-95.46399,-93.04545,-82.26909,-83.8516], "fy":[-119.69152,-121.6282,-129.15414,-128.08546]}, + {"t":1.14669, "x":7.22092, "y":5.65852, "heading":-0.46801, "vx":1.82185, "vy":-0.43836, "omega":-1.94611, "ax":-5.07263, "ay":-7.4246, "alpha":1.41235, "fx":[-93.51885,-91.40407,-79.58483,-80.62814], "fy":[-121.245,-122.89533,-130.85427,-130.16675]}, + {"t":1.16239, "x":7.2489, "y":5.65073, "heading":-0.49856, "vx":1.74222, "vy":-0.55493, "omega":-1.92393, "ax":-4.94606, "ay":-7.50918, "alpha":1.50714, "fx":[-91.69291,-89.96748,-77.22321,-77.64072], "fy":[-122.65637,-123.97632,-132.28745,-131.99578]}, + {"t":1.17809, "x":7.27564, "y":5.64109, "heading":-0.52877, "vx":1.66457, "vy":-0.67282, "omega":-1.90027, "ax":-4.83086, "ay":-7.58342, "alpha":1.59221, "fx":[-89.9696,-88.70409,-75.14252,-74.87006], "fy":[-123.94812,-124.906,-133.50327,-133.6098]}, + {"t":1.19379, "x":7.30118, "y":5.62959, "heading":-0.5586, "vx":1.58872, "vy":-0.79187, "omega":-1.87528, "ax":-4.77003, "ay":-7.6292, "alpha":1.46229, "fx":[-88.08182,-87.4451,-74.86373,-74.15664], "fy":[-125.38652,-125.86796,-133.7341,-134.09317]}, + {"t":1.21315, "x":7.33105, "y":5.61282, "heading":-0.59492, "vx":1.49634, "vy":-0.93962, "omega":-1.84696, "ax":-4.74746, "ay":-7.64331, "alpha":1.40255, "fx":[-87.1684,-87.06488,-74.97031,-73.80834], "fy":[-126.00376,-126.11231,-133.65768,-134.26849]}, + {"t":1.23252, "x":7.35914, "y":5.59319, "heading":-0.63069, "vx":1.4044, "vy":-1.08765, "omega":-1.81979, "ax":-4.72237, "ay":-7.65885, "alpha":1.33631, "fx":[-86.19294,-86.58429,-75.04343,-73.48385], "fy":[-126.65178,-126.42175,-133.59797,-134.42789]}, + {"t":1.25189, "x":7.38545, "y":5.57069, "heading":-0.66593, "vx":1.31295, "vy":-1.23597, "omega":-1.79391, "ax":-4.6943, "ay":-7.67603, "alpha":1.26249, "fx":[-85.14834,-85.98812,-75.07575,-73.18227], "fy":[-127.33311,-126.80461,-133.5588,-134.57176]}, + {"t":1.27125, "x":7.41, "y":5.54532, "heading":-0.70067, "vx":1.22203, "vy":-1.38463, "omega":-1.76946, "ax":-4.66268, "ay":-7.69512, "alpha":1.17976, "fx":[-84.02562,-85.25723,-75.05826,-72.9026], "fy":[-128.05109,-127.27099,-133.54486,-134.70048]}, + {"t":1.29062, "x":7.43279, "y":5.51706, "heading":-0.73494, "vx":1.13173, "vy":-1.53366, "omega":-1.74662, "ax":-4.62683, "ay":-7.71646, "alpha":1.08644, "fx":[-82.81335,-84.36715,-74.97961,-72.64381], "fy":[-128.80996,-127.83324,-133.56191,-134.81431]}, + {"t":1.30999, "x":7.45384, "y":5.48591, "heading":-0.76877, "vx":1.04213, "vy":-1.6831, "omega":-1.72557, "ax":-4.58581, "ay":-7.74047, "alpha":0.98032, "fx":[-81.49694,-83.28622,-74.82523,-72.40475], "fy":[-129.61512,-128.50664,-133.61727,-134.9134]}, + {"t":1.32935, "x":7.47316, "y":5.45186, "heading":-0.80218, "vx":0.95332, "vy":-1.83301, "omega":-1.70659, "ax":-4.53844, "ay":-7.76765, "alpha":0.85856, "fx":[-80.05756,-81.97272,-74.57594,-72.1842], "fy":[-130.47344,-129.31021,-133.72033,-134.99777]}, + {"t":1.34872, "x":7.49078, "y":5.41491, "heading":-0.83523, "vx":0.86542, "vy":-1.98344, "omega":-1.68996, "ax":-4.48315, "ay":-7.79866, "alpha":0.71736, "fx":[-78.47065,-80.37076,-74.20588,-71.98084], "fy":[-131.39362,-130.26783,-133.88349,-135.0672]}, + {"t":1.36809, "x":7.5067, "y":5.37503, "heading":-0.86796, "vx":0.7786, "vy":-2.13447, "omega":-1.67607, "ax":-4.41777, "ay":-7.83436, "alpha":0.55158, "fx":[-76.70375,-78.40386,-73.67917,-71.79315], "fy":[-132.38678,-131.40969,-134.12343,-135.12122]}, + {"t":1.38745, "x":7.52095, "y":5.33222, "heading":-0.90042, "vx":0.69304, "vy":-2.2862, "omega":-1.66539, "ax":-4.33931, "ay":-7.87584, "alpha":0.3541, "fx":[-74.71318,-75.96487,-72.94444,-71.61942], "fy":[-133.4671,-132.77406,-134.4632,-135.15891]}, + {"t":1.40682, "x":7.53355, "y":5.28647, "heading":-0.93268, "vx":0.609, "vy":-2.43873, "omega":-1.65853, "ax":-4.24348, "ay":-7.92454, "alpha":0.11484, "fx":[-72.43904,-72.89967,-71.92545,-71.45755], "fy":[-134.65282,-134.40929,-134.93558,-135.17874]}, + {"t":1.42619, "x":7.54455, "y":5.23776, "heading":-0.9648, "vx":0.52682, "vy":-2.5922, "omega":-1.65631, "ax":-4.1239, "ay":-7.98234, "alpha":-0.18099, "fx":[-69.79716,-68.97951,-70.50396,-71.30489], "fy":[-135.96748,-136.37528,-135.58859,-135.17819]}, + {"t":1.44555, "x":7.55398, "y":5.18606, "heading":-0.99687, "vx":0.44696, "vy":-2.74679, "omega":-1.65981, "ax":-3.97072, "ay":-8.05175, "alpha":-0.55573, "fx":[-66.6658,-63.85279,-68.48654,-71.15787], "fy":[-137.44144,-138.74196,-136.49521,-135.15312]}, + {"t":1.46492, "x":7.56189, "y":5.13135, "heading":-1.02902, "vx":0.37006, "vy":-2.90272, "omega":-1.67057, "ax":-3.76793, "ay":-8.1359, "alpha":-1.04453, "fx":[-62.86277,-56.95732,-65.53457,-71.01128], "fy":[-139.11348,-141.57695,-137.7704,-135.09656]}, + {"t":1.48428, "x":7.56835, "y":5.07361, "heading":-1.06137, "vx":0.29709, "vy":-3.06029, "omega":-1.6908, "ax":-3.48796, "ay":-8.23832, "alpha":-1.70539, "fx":[-58.10385,-47.3559,-61.00051,-70.85685], "fy":[-141.03123,-144.89698,-139.60126,-134.9961]}, + {"t":1.50365, "x":7.57345, "y":5.0128, "heading":-1.09412, "vx":0.22954, "vy":-3.21984, "omega":-1.72383, "ax":-3.07941, "ay":-8.36088, "alpha":-2.63949, "fx":[-51.92297,-33.42804,-53.48801,-70.68024], "fy":[-143.24617,-148.49997,-142.2905,-134.82785]}, + {"t":1.52302, "x":7.57732, "y":4.94887, "heading":-1.1275, "vx":0.1699, "vy":-3.38176, "omega":-1.77495, "ax":-2.4361, "ay":-8.49428, "alpha":-4.04095, "fx":[-43.50848,-12.35664,-39.43081,-70.45347], "fy":[-145.7869,-151.40735,-146.20684,-134.53977]}, + {"t":1.54238, "x":7.58015, "y":4.88179, "heading":-1.16188, "vx":0.12272, "vy":-3.54626, "omega":-1.85321, "ax":-1.30253, "ay":-8.56531, "alpha":-6.40942, "fx":[-31.34575,20.1851,-7.34581,-70.11603], "fy":[-148.54769,-150.16031,-150.07282,-133.99302]}, + {"t":1.56175, "x":7.58229, "y":4.8115, "heading":-1.19777, "vx":0.09749, "vy":-3.71214, "omega":-1.97734, "ax":0.95463, "ay":-7.98683, "alpha":-12.33553, "fx":[-12.41536,66.85396,80.0284,-69.51485], "fy":[-150.8257,-135.50166,-124.42491,-132.66227]}, + {"t":1.58112, "x":7.58435, "y":4.73811, "heading":-1.23606, "vx":0.11598, "vy":-3.86682, "omega":-2.21623, "ax":2.70334, "ay":-7.55151, "alpha":-10.29534, "fx":[15.13176,81.00572,101.66692,-13.87231], "fy":[-146.93844,-123.54549,-100.87446,-142.43751]}, + {"t":1.59253, "x":7.58585, "y":4.69348, "heading":-1.26136, "vx":0.14684, "vy":-3.95302, "omega":-2.33375, "ax":3.35926, "ay":-7.56341, "alpha":-4.11078, "fx":[39.79837,72.1987,78.91136,37.65206], "fy":[-137.99888,-124.41104,-116.83789,-135.35777]}, + {"t":1.60395, "x":7.58775, "y":4.64787, "heading":-1.288, "vx":0.18518, "vy":-4.03935, "omega":-2.38067, "ax":4.13743, "ay":-6.49304, "alpha":6.92208, "fx":[104.91411,43.89396,43.68257,89.01504], "fy":[-82.15237,-121.3311,-131.07079,-107.22502]}, + {"t":1.61536, "x":7.59013, "y":4.60134, "heading":-1.31517, "vx":0.23241, "vy":-4.11346, "omega":-2.30166, "ax":3.75251, "ay":-5.53136, "alpha":12.32696, "fx":[118.21876,14.47721,27.70284,94.91787], "fy":[-41.66221,-106.7199,-130.62342,-97.34203]}, + {"t":1.62678, "x":7.59303, "y":4.55402, "heading":-1.34144, "vx":0.27524, "vy":-4.1766, "omega":-2.16095, "ax":3.13203, "ay":-5.16191, "alpha":12.22075, "fx":[104.62022,3.74261,19.49412,85.24292], "fy":[-41.54234,-91.26985,-123.15646,-95.24193]}, + {"t":1.63819, "x":7.59638, "y":4.50601, "heading":-1.36611, "vx":0.31099, "vy":-4.23552, "omega":-2.02146, "ax":1.85444, "ay":-4.39469, "alpha":10.56598, "fx":[70.06367,-11.06618,5.10044,62.07599], "fy":[-41.83161,-65.95829,-104.03023,-87.18941]}, + {"t":1.6496, "x":7.60005, "y":4.45738, "heading":-1.38918, "vx":0.33216, "vy":-4.28569, "omega":-1.90085, "ax":-1.727, "ay":-1.31665, "alpha":1.69968, "fx":[-26.05443,-34.8193,-32.67677,-23.95272], "fy":[-17.147,-18.43958,-27.52687,-26.47024]}, + {"t":1.66102, "x":7.60372, "y":4.40838, "heading":-1.41088, "vx":0.31245, "vy":-4.30071, "omega":-1.88145, "ax":-4.14425, "ay":2.49624, "alpha":-9.76172, "fx":[-97.22052,-48.69401,-45.25803,-90.79766], "fy":[5.36165,23.012,82.88878,58.57921]}, + {"t":1.67243, "x":7.60702, "y":4.35945, "heading":-1.43236, "vx":0.26514, "vy":-4.27222, "omega":-1.99288, "ax":-4.66589, "ay":3.52699, "alpha":-12.919, "fx":[-120.74274,-54.57725,-39.08546,-103.0562], "fy":[6.36422,41.30379,114.4266,77.87763]}, + {"t":1.68385, "x":7.60974, "y":4.31091, "heading":-1.4551, "vx":0.21188, "vy":-4.23196, "omega":-2.14034, "ax":-4.9239, "ay":3.90259, "alpha":-14.12473, "fx":[-131.30189,-61.25389,-34.81976,-107.64121], "fy":[4.9043,48.65119,126.70395,85.26862]}, + {"t":1.69526, "x":7.61184, "y":4.26286, "heading":-1.47953, "vx":0.15568, "vy":-4.18742, "omega":-2.30157, "ax":-5.13224, "ay":4.05421, "alpha":-14.65313, "fx":[-137.13072,-70.3992,-31.87272,-109.78937], "fy":[3.8821,49.44459,133.1366,89.38069]}, + {"t":1.70668, "x":7.61328, "y":4.21533, "heading":-1.50581, "vx":0.0971, "vy":-4.14114, "omega":-2.46883, "ax":-5.33101, "ay":4.10838, "alpha":-14.86048, "fx":[-140.74178,-81.6075,-29.67865,-110.68757], "fy":[3.91183,46.18919,137.08025,92.34819]}, + {"t":1.71809, "x":7.61405, "y":4.16833, "heading":-1.53399, "vx":0.03625, "vy":-4.09424, "omega":-2.63845, "ax":-5.51, "ay":4.1497, "alpha":-14.85406, "fx":[-143.10206,-93.16027,-27.97075,-110.6613], "fy":[5.40081,42.1883,139.73176,95.0201]}, + {"t":1.72951, "x":7.6141, "y":4.12186, "heading":-1.5641, "vx":-0.02665, "vy":-4.04688, "omega":-2.808, "ax":-5.62325, "ay":4.41869, "alpha":-14.12861, "fx":[-144.3395,-101.70937,-27.38105,-109.1698], "fy":[11.21533,49.51466,141.47514,98.43744]}, + {"t":1.74092, "x":7.61343, "y":4.07596, "heading":-1.59615, "vx":-0.09083, "vy":-3.99644, "omega":-2.96927, "ax":-5.52288, "ay":5.06339, "alpha":-12.59951, "fx":[-143.62377,-99.85117,-27.22155,-105.07407], "fy":[24.24476,73.78157,142.68873,103.79188]}, + {"t":1.75234, "x":7.61203, "y":4.03067, "heading":-1.63005, "vx":-0.15387, "vy":-3.93864, "omega":-3.11309, "ax":-5.3336, "ay":5.44526, "alpha":-12.13827, "fx":[-142.69885,-96.04766,-23.16476,-100.98051], "fy":[32.55242,85.14403,144.25966,108.53328]}, + {"t":1.76375, "x":7.60993, "y":3.98607, "heading":-1.66558, "vx":-0.21475, "vy":-3.87649, "omega":-3.25164, "ax":-5.16765, "ay":5.69349, "alpha":-11.99593, "fx":[-141.87685,-94.0376,-18.43995,-97.24635], "fy":[38.4154,90.86106,145.59935,112.50306]}, + {"t":1.77516, "x":7.60714, "y":3.94219, "heading":-1.7027, "vx":-0.27374, "vy":-3.8115, "omega":-3.38857, "ax":-5.03857, "ay":5.88717, "alpha":-11.85997, "fx":[-140.88187,-93.77249,-14.4427,-93.72126], "fy":[43.78668,94.23826,146.57793,115.95351]}, + {"t":1.78658, "x":7.60369, "y":3.89907, "heading":-1.74138, "vx":-0.33125, "vy":-3.7443, "omega":-3.52395, "ax":-4.94492, "ay":6.04184, "alpha":-11.70775, "fx":[-139.69692,-95.05083,-11.30087,-90.39835], "fy":[48.90809,95.91485,147.28311,118.97349]}, + {"t":1.79799, "x":7.59959, "y":3.85672, "heading":-1.7816, "vx":-0.3877, "vy":-3.67534, "omega":-3.65759, "ax":-4.88725, "ay":6.14001, "alpha":-11.65641, "fx":[-138.6249,-98.06107,-8.45864,-87.37804], "fy":[53.09592,95.28011,147.82381,121.55969]}, + {"t":1.80941, "x":7.59484, "y":3.81517, "heading":-1.82335, "vx":-0.44348, "vy":-3.60525, "omega":-3.79064, "ax":-4.69043, "ay":6.45717, "alpha":-11.75548, "fx":[-138.79694,-98.10998,0.44287,-82.66765], "fy":[58.35648,103.89078,150.35313,126.73834]}, + {"t":1.82711, "x":7.58626, "y":3.75236, "heading":-1.89046, "vx":-0.52652, "vy":-3.49094, "omega":-3.99874, "ax":-4.24835, "ay":6.77148, "alpha":-11.84044, "fx":[-135.20701,-91.63105,11.73113,-73.94569], "fy":[67.09612,110.98422,150.30076,132.34274]}, + {"t":1.84481, "x":7.57627, "y":3.69162, "heading":-1.96125, "vx":-0.60172, "vy":-3.37107, "omega":-4.20836, "ax":-3.94805, "ay":6.97903, "alpha":-11.85882, "fx":[-131.22146,-90.9112,19.70354,-66.19136], "fy":[75.22866,113.22656,149.75934,136.63096]}, + {"t":1.86252, "x":7.565, "y":3.63303, "heading":-2.03575, "vx":-0.67162, "vy":-3.24752, "omega":-4.41829, "ax":-3.74007, "ay":7.1293, "alpha":-11.8406, "fx":[-126.75447,-93.53197,24.98375,-59.1676], "fy":[83.02114,112.85613,149.18825,140.0041]}, + {"t":1.88022, "x":7.55252, "y":3.57666, "heading":-2.11396, "vx":-0.73783, "vy":-3.12131, "omega":-4.6279, "ax":-3.56329, "ay":7.28883, "alpha":-11.6184, "fx":[-121.23562,-95.59066,27.07435,-52.69015], "fy":[91.26126,112.95835,148.99062,142.71337]}, + {"t":1.89792, "x":7.5389, "y":3.52255, "heading":-2.19589, "vx":-0.80091, "vy":-2.99227, "omega":-4.83358, "ax":-3.25964, "ay":7.71166, "alpha":-9.74979, "fx":[-109.0612,-84.74892,18.37386,-46.34564], "fy":[105.75142,123.47563,150.47523,144.99025]}, + {"t":1.91563, "x":7.52421, "y":3.47078, "heading":-2.28146, "vx":-0.85861, "vy":-2.85575, "omega":-5.00618, "ax":-2.8963, "ay":8.19801, "alpha":-6.47433, "fx":[-88.19746,-66.94047,-0.9214,-41.00177], "fy":[123.85954,135.47961,151.80372,146.64022]}, + {"t":1.93333, "x":7.50856, "y":3.42151, "heading":-2.37008, "vx":-0.90989, "vy":-2.71063, "omega":-5.1208, "ax":-2.60842, "ay":8.47855, "alpha":-3.58272, "fx":[-67.15755,-53.99631,-18.40565,-37.9141], "fy":[136.56676,141.87747,150.91679,147.50967]}, + {"t":1.95103, "x":7.49204, "y":3.37486, "heading":-2.46074, "vx":-0.95606, "vy":-2.56053, "omega":-5.18422, "ax":-2.37089, "ay":8.62347, "alpha":-1.25778, "fx":[-48.61241,-43.86179,-31.60658,-37.2318], "fy":[144.33768,145.73955,148.93018,147.72393]}, + {"t":1.96873, "x":7.47475, "y":3.33088, "heading":-2.55251, "vx":-0.99803, "vy":-2.40787, "omega":-5.20649, "ax":-2.17092, "ay":8.69162, "alpha":0.58234, "fx":[-33.02217,-35.16705,-40.72783,-38.78982], "fy":[148.78143,148.33245,146.88783,147.36602]}, + {"t":1.98644, "x":7.45674, "y":3.28961, "heading":-2.64468, "vx":-1.03647, "vy":-2.254, "omega":-5.19618, "ax":-2.00287, "ay":8.71645, "alpha":2.0701, "fx":[-20.17077,-27.29321,-46.56606,-42.24322], "fy":[151.1407,150.15243,145.30176,146.46246]}, + {"t":2.00414, "x":7.43808, "y":3.25108, "heading":-2.73667, "vx":-1.07192, "vy":-2.0997, "omega":-5.15953, "ax":-1.86182, "ay":8.71566, "alpha":3.32054, "fx":[-9.70187,-19.95235,-49.91345,-47.10874], "fy":[152.23388,151.42527,144.31727,145.02677]}, + {"t":2.02184, "x":7.41881, "y":3.21527, "heading":-2.82801, "vx":-1.10488, "vy":-1.9454, "omega":-5.10075, "ax":-1.742, "ay":8.69893, "alpha":4.40877, "fx":[-1.29292,-13.00499,-51.406,-52.81947], "fy":[152.58535,152.27055,143.89972,143.10933]}, + {"t":2.03955, "x":7.39898, "y":3.1822, "heading":-2.91831, "vx":-1.13572, "vy":-1.79141, "omega":-5.0227, "ax":-1.63735, "ay":8.67254, "alpha":5.36999, "fx":[5.3042,-6.38273,-51.52044,-58.80457], "fy":[152.53536,152.76161,143.94667,140.82584]}, + {"t":2.05725, "x":7.37861, "y":3.15184, "heading":-3.00722, "vx":-1.16471, "vy":-1.63788, "omega":-4.92764, "ax":-1.54277, "ay":8.64141, "alpha":6.21068, "fx":[10.26586,-0.05298,-50.60813,-64.57338], "fy":[152.30941,152.95013,144.3422,138.35009]}, + {"t":2.07495, "x":7.35775, "y":3.1242, "heading":-3.09446, "vx":-1.19202, "vy":-1.4849, "omega":-4.81769, "ax":-1.45495, "ay":8.60968, "alpha":6.92364, "fx":[13.71033,5.9985,-48.92941,-69.77281], "fy":[152.05801,152.87689,144.97961,135.87814]}, + {"t":2.09265, "x":7.33642, "y":3.09926, "heading":3.10344, "vx":-1.21778, "vy":-1.33248, "omega":-4.69512, "ax":-1.37263, "ay":8.58054, "alpha":7.50084, "fx":[15.7122,11.77695,-46.67997,-74.20147], "fy":[151.87808,152.5769,145.76923,133.58614]}, + {"t":2.11036, "x":7.31465, "y":3.07702, "heading":3.02032, "vx":-1.24207, "vy":-1.18058, "omega":-4.56233, "ax":-1.29628, "ay":8.55609, "alpha":7.94132, "fx":[16.31524,17.28382,-44.00912,-77.78747], "fy":[151.82359,152.08164,146.63951,131.60187]}, + {"t":2.12806, "x":7.29246, "y":3.05746, "heading":2.93956, "vx":-1.26502, "vy":-1.02911, "omega":-4.42175, "ax":-1.2275, "ay":8.53721, "alpha":8.25409, "fx":[15.54441,22.51951,-41.03231,-80.54922], "fy":[151.91003,151.42013,147.53555,129.99657]}, + {"t":2.14576, "x":7.26987, "y":3.04058, "heading":2.86128, "vx":-1.28675, "vy":-0.87798, "omega":-4.27563, "ax":-1.16835, "ay":8.52372, "alpha":8.45767, "fx":[13.41828,27.48474,-37.8397,-82.55673], "fy":[152.11587,150.61934,148.41666,128.79203]}, + {"t":2.16347, "x":7.24691, "y":3.02637, "heading":2.78559, "vx":-1.30744, "vy":-0.72708, "omega":-4.1259, "ax":-1.12083, "ay":8.51449, "alpha":8.57816, "fx":[9.96332,32.18095,-34.50216,-83.90233], "fy":[152.38347,149.70441,149.25379,127.97461]}, + {"t":2.18117, "x":7.22359, "y":3.01483, "heading":2.71255, "vx":-1.32728, "vy":-0.57635, "omega":-3.97404, "ax":-1.0864, "ay":8.50774, "alpha":8.64668, "fx":[5.23042,36.61016,-31.07589,-84.68208], "fy":[152.62187,148.69881,150.02708,127.50924]}, + {"t":2.19887, "x":7.19992, "y":3.00596, "heading":2.6422, "vx":-1.34651, "vy":-0.42574, "omega":-3.82097, "ax":-1.06563, "ay":8.50122, "alpha":8.69661, "fx":[-0.68694,40.77455,-27.60583,-84.98604], "fy":[152.71422,147.6246,150.72378,127.35062]}, + {"t":2.21658, "x":7.17592, "y":2.99976, "heading":2.57455, "vx":-1.36538, "vy":-0.27524, "omega":-3.66702, "ax":-1.05797, "ay":8.49253, "alpha":8.76047, "fx":[-7.63658,44.67604,-24.12858,-84.89419], "fy":[152.53197,146.50263,151.33656,127.45064]}, + {"t":2.23428, "x":7.15158, "y":2.99622, "heading":2.50964, "vx":-1.3841, "vy":-0.1249, "omega":-3.51193, "ax":-1.06165, "ay":8.47947, "alpha":8.86633, "fx":[-15.39886,48.316,-20.67462,-84.47582], "fy":[151.95602,145.35281,151.86214,127.7626]}, + {"t":2.25198, "x":7.12691, "y":2.99533, "heading":2.44746, "vx":-1.4029, "vy":0.02521, "omega":-3.35497, "ax":-1.07376, "ay":8.46045, "alpha":9.03424, "fx":[-23.69169,51.69526,-17.27024,-83.79065], "fy":[150.90172,144.19425,152.30032,128.24328]}, + {"t":2.26968, "x":7.10191, "y":2.99711, "heading":2.38807, "vx":-1.42191, "vy":0.17498, "omega":-3.19504, "ax":-1.04227, "ay":8.49759, "alpha":8.78141, "fx":[-29.33829,51.46739,-13.66717,-79.37678], "fy":[150.05635,144.3226,152.70181,131.08559]}, + {"t":2.29045, "x":7.07215, "y":3.00257, "heading":2.32171, "vx":-1.44356, "vy":0.35147, "omega":-3.01265, "ax":-0.96229, "ay":8.49967, "alpha":8.87461, "fx":[-34.45066,54.17917,-9.1421,-76.05964], "fy":[148.9465,143.30347,153.02872,133.02934]}, + {"t":2.31122, "x":7.04196, "y":3.01171, "heading":2.25914, "vx":-1.46354, "vy":0.52801, "omega":-2.82833, "ax":-0.86592, "ay":8.50128, "alpha":8.98846, "fx":[-38.35782,56.73499,-4.70443,-72.58903], "fy":[147.96977,142.2859,153.21759,134.94441]}, + {"t":2.33199, "x":7.01138, "y":3.02451, "heading":2.2004, "vx":-1.48153, "vy":0.70457, "omega":-2.64165, "ax":-0.75235, "ay":8.50384, "alpha":9.09958, "fx":[-41.03981,59.16418,-0.34806,-68.96527], "fy":[147.22929,141.26604,153.27642,136.82001]}, + {"t":2.35276, "x":6.98044, "y":3.04097, "heading":2.14553, "vx":-1.49715, "vy":0.88119, "omega":-2.45266, "ax":-0.62096, "ay":8.5082, "alpha":9.18936, "fx":[-42.50485,61.50065,3.93665,-65.18173], "fy":[146.7924,140.23552,153.21191,138.64867]}, + {"t":2.37353, "x":6.94922, "y":3.06111, "heading":2.09459, "vx":-1.51005, "vy":1.0579, "omega":-2.2618, "ax":-0.47098, "ay":8.51467, "alpha":9.24395, "fx":[-42.76858,63.78264,8.16403,-61.22318], "fy":[146.69277,139.1812,153.02899,140.42572]}, + {"t":2.3943, "x":6.91775, "y":3.08492, "heading":2.04762, "vx":-1.51983, "vy":1.23475, "omega":-2.06981, "ax":-0.30122, "ay":8.52305, "alpha":9.2536, "fx":[-41.8377,66.05204,12.35423,-57.06349], "fy":[146.93478,138.08519,152.73018,142.14865]}, + {"t":2.41507, "x":6.88612, "y":3.1124, "heading":2.00463, "vx":-1.52609, "vy":1.41176, "omega":-1.87762, "ax":-0.10983, "ay":8.53267, "alpha":9.21148, "fx":[-39.69784,68.35338,16.5344,-52.66276], "fy":[147.49723,136.92486,152.31467,143.81621]}, + {"t":2.43584, "x":6.8544, "y":3.14356, "heading":1.96563, "vx":-1.52837, "vy":1.58898, "omega":-1.6863, "ax":0.10588, "ay":8.54234, "alpha":9.11254, "fx":[-36.30512,70.73258,20.74014,-47.96346], "fy":[148.33425,135.67295,151.77717,145.42692]}, + {"t":2.45661, "x":6.82268, "y":3.17841, "heading":1.93061, "vx":-1.52617, "vy":1.7664, "omega":-1.49704, "ax":0.3496, "ay":8.5503, "alpha":8.95261, "fx":[-31.5809,73.23569,25.01725,-42.8854], "fy":[149.37193,134.2976,151.10629,146.97686]}, + {"t":2.47738, "x":6.79106, "y":3.21694, "heading":1.89952, "vx":-1.51891, "vy":1.94399, "omega":-1.3111, "ax":0.62615, "ay":8.55394, "alpha":8.7277, "fx":[-25.41019,75.90766,29.42409,-37.31875], "fy":[150.49986,132.76219,150.28226,148.45599]}, + {"t":2.49815, "x":6.75965, "y":3.25916, "heading":1.87229, "vx":-1.5059, "vy":2.12165, "omega":-1.12983, "ax":0.9416, "ay":8.54948, "alpha":8.43373, "fx":[-17.64597,78.79139,34.03475,-31.11464], "fy":[151.55653,131.02485,149.27359,149.84204]}, + {"t":2.51892, "x":6.72857, "y":3.30507, "heading":1.84882, "vx":-1.48635, "vy":2.29921, "omega":-0.95467, "ax":1.30329, "ay":8.53143, "alpha":8.06671, "fx":[-8.12377,81.92696,38.94328,-24.07239], "fy":[152.30918,129.0376,148.03197,151.09004]}, + {"t":2.53969, "x":6.69798, "y":3.35466, "heading":1.82899, "vx":-1.45928, "vy":2.4764, "omega":-0.78713, "ax":1.71966, "ay":8.49183, "alpha":7.62317, "fx":[3.30602,85.3512,44.26937,-15.92286], "fy":[152.43124,126.74496,146.48443,152.11404]}, + {"t":2.56045, "x":6.66805, "y":3.40793, "heading":1.81264, "vx":-1.42356, "vy":2.65277, "omega":-0.6288, "ax":2.19983, "ay":8.41938, "alpha":7.10052, "fx":[16.71948,89.0973,50.16551,-6.30865], "fy":[151.48712,124.08197,144.52087,152.75502]}, + {"t":2.58122, "x":6.63895, "y":3.46484, "heading":1.79958, "vx":-1.37787, "vy":2.82764, "omega":-0.48133, "ax":2.75259, "ay":8.29841, "alpha":6.49599, "fx":[32.02992,93.19433,56.82563,5.23347], "fy":[148.94426,120.9715,141.97394,152.72459]}, + {"t":2.60199, "x":6.61093, "y":3.52536, "heading":1.78959, "vx":-1.3207, "vy":2.99999, "omega":-0.34641, "ax":3.38499, "ay":8.108, "alpha":5.8022, "fx":[48.88225,97.66618,64.49333,19.2694], "fy":[144.24171,117.32085,138.58626,151.51037]}, + {"t":2.62276, "x":6.58423, "y":3.58941, "heading":1.78239, "vx":-1.2504, "vy":3.16839, "omega":-0.2259, "ax":4.10014, "ay":7.82116, "alpha":4.99805, "fx":[66.58044,102.52953,73.46462,36.39395], "fy":[136.93574,113.0176,133.95696,148.23277]}, + {"t":2.64353, "x":6.55914, "y":3.65691, "heading":1.7777, "vx":-1.16524, "vy":3.33083, "omega":-0.12209, "ax":4.89373, "ay":7.40409, "alpha":4.03929, "fx":[84.12706,107.79007,84.06922,56.97749], "fy":[126.89486,107.9246,127.45698,141.48964]}, + {"t":2.6643, "x":6.536, "y":3.72768, "heading":1.77516, "vx":-1.0636, "vy":3.48461, "omega":-0.0382, "ax":5.74728, "ay":6.81685, "alpha":2.8659, "fx":[100.42407,113.43611,96.59046,80.58777], "fy":[114.44228,101.87461,118.10787,129.38586]}, + {"t":2.68507, "x":6.51515, "y":3.80153, "heading":1.77437, "vx":-0.94424, "vy":3.62619, "omega":0.02132, "ax":6.61704, "ay":6.02112, "alpha":1.44614, "fx":[114.56934,119.42726,111.03546,105.18402], "fy":[100.32471,94.66628,104.4595,110.21963]}, + {"t":2.70584, "x":6.49696, "y":3.87814, "heading":1.77481, "vx":-0.8068, "vy":3.75124, "omega":0.05136, "ax":6.98601, "ay":5.49592, "alpha":2.1922, "fx":[122.51306,127.9345,116.33456,108.53803], "fy":[89.00575,81.55356,97.55202,105.82502]}, + {"t":2.72069, "x":6.48575, "y":3.93446, "heading":1.77558, "vx":-0.70304, "vy":3.83287, "omega":0.08392, "ax":6.92058, "ay":5.43188, "alpha":3.7133, "fx":[124.56245,132.42449,114.20236,99.67884], "fy":[84.29794,72.80107,99.28648,113.19371]}, + {"t":2.73555, "x":6.47607, "y":3.99199, "heading":1.77682, "vx":-0.60026, "vy":3.91355, "omega":0.13907, "ax":6.72159, "ay":5.21602, "alpha":7.06392, "fx":[129.59028,139.55321,110.24048,77.9452], "fy":[70.4135,54.92124,102.36073,127.19665]}, + {"t":2.7504, "x":6.4679, "y":4.05069, "heading":1.77889, "vx":-0.50043, "vy":3.99102, "omega":0.24398, "ax":4.85938, "ay":2.51993, "alpha":24.75897, "fx":[89.77287,147.20494,100.50065,-6.85194], "fy":[-87.79161,5.42677,109.14289,144.67543]}, + {"t":2.76525, "x":6.461, "y":4.11024, "heading":1.78251, "vx":-0.42825, "vy":4.02845, "omega":0.61171, "ax":-1.57071, "ay":-1.08947, "alpha":36.32364, "fx":[-90.89136,77.10617,49.2269,-142.31101], "fy":[-115.85105,-122.0867,130.43414,33.37741]}, + {"t":2.7801, "x":6.45447, "y":4.16995, "heading":1.7916, "vx":-0.45158, "vy":4.01226, "omega":1.1512, "ax":-6.24407, "ay":-4.47429, "alpha":13.87866, "fx":[-104.26681,-33.63514,-140.19478,-146.74255], "fy":[-108.31657,-144.49036,-21.90286,-29.71603]}, + {"t":2.79495, "x":6.44707, "y":4.22905, "heading":1.8087, "vx":-0.54432, "vy":3.94581, "omega":1.35733, "ax":-6.66993, "ay":-5.19, "alpha":8.53599, "fx":[-108.32206,-68.24914,-134.82678,-142.4162], "fy":[-105.78126,-133.9936,-62.94506,-50.40207]}, + {"t":2.80981, "x":6.43825, "y":4.28708, "heading":1.82886, "vx":-0.64339, "vy":3.86873, "omega":1.48411, "ax":-6.81396, "ay":-5.35503, "alpha":6.57372, "fx":[-110.17117,-82.15716,-131.96545,-139.32036], "fy":[-104.62785,-127.04785,-72.57016,-60.10432]}, + {"t":2.82466, "x":6.42795, "y":4.34395, "heading":1.8509, "vx":-0.74459, "vy":3.78919, "omega":1.58175, "ax":-6.88439, "ay":-5.42405, "alpha":5.52545, "fx":[-111.16045,-89.45888,-130.59895,-137.18789], "fy":[-104.04455,-122.65111,-76.62702,-65.72319]}, + {"t":2.83951, "x":6.41613, "y":4.39963, "heading":1.87439, "vx":-0.84684, "vy":3.70863, "omega":1.66382, "ax":-6.92517, "ay":-5.46138, "alpha":4.871, "fx":[-111.72504,-93.94878,-129.86723,-135.63926], "fy":[-103.75167,-119.65727,-78.75859,-69.4183]}, + {"t":2.85436, "x":6.40279, "y":4.45411, "heading":1.8991, "vx":-0.94969, "vy":3.62752, "omega":1.73616, "ax":-6.95138, "ay":-5.4847, "alpha":4.42316, "fx":[-112.04843,-97.00527,-129.45619,-134.4541], "fy":[-103.6268,-117.47993,-80.00224,-72.06344]}, + {"t":2.86922, "x":6.38791, "y":4.50738, "heading":1.92489, "vx":-1.05294, "vy":3.54606, "omega":1.80186, "ax":-6.96949, "ay":-5.50066, "alpha":4.09731, "fx":[-112.22145,-99.24005,-129.22824,-133.50611], "fy":[-103.60783,-115.81006,-80.76417,-74.07645]}, + {"t":2.88407, "x":6.37151, "y":4.55944, "heading":1.95165, "vx":-1.15645, "vy":3.46436, "omega":1.86271, "ax":-6.98265, "ay":-5.5123, "alpha":3.84949, "fx":[-112.29427,-100.96414,-129.11396,-132.71943], "fy":[-103.65984,-114.47283,-81.23602,-75.68167]}, + {"t":2.89892, "x":6.35356, "y":4.61029, "heading":1.97932, "vx":-1.26016, "vy":3.38249, "omega":1.91988, "ax":-6.99262, "ay":-5.52118, "alpha":3.65461, "fx":[-112.29692,-102.35195,-129.07451,-132.04617], "fy":[-103.76157,-113.36267,-81.52025,-77.01012]}, + {"t":2.91377, "x":6.33407, "y":4.65992, "heading":2.00783, "vx":-1.36402, "vy":3.30049, "omega":1.97416, "ax":-7.00039, "ay":-5.52819, "alpha":3.49726, "fx":[-112.24859,-103.5087,-129.08638,-131.45472], "fy":[-103.89928,-112.41226,-81.67678,-78.14346]}, + {"t":2.92863, "x":6.31304, "y":4.70833, "heading":2.03715, "vx":-1.46799, "vy":3.21838, "omega":2.02611, "ax":-7.00661, "ay":-5.53388, "alpha":3.36747, "fx":[-112.16215,-104.50168,-129.13437,-130.92339], "fy":[-104.06361,-111.57663,-81.74323,-79.13521]}, + {"t":2.94348, "x":6.29047, "y":4.75552, "heading":2.06724, "vx":-1.57205, "vy":3.13619, "omega":2.07612, "ax":-7.01169, "ay":-5.53858, "alpha":3.25848, "fx":[-112.04663,-105.37587,-129.20816,-130.43684], "fy":[-104.24791,-110.82459,-81.74451,-80.02192]}, + {"t":2.95833, "x":6.26634, "y":4.80149, "heading":2.09808, "vx":-1.67619, "vy":3.05393, "omega":2.12452, "ax":-7.01592, "ay":-5.54254, "alpha":3.16557, "fx":[-111.90862,-106.16253,-129.30042,-129.98386], "fy":[-104.44725,-110.13381,-81.69796,-80.82939]}, + {"t":2.97318, "x":6.24067, "y":4.84624, "heading":2.12963, "vx":-1.7804, "vy":2.97161, "omega":2.17153, "ax":-7.0195, "ay":-5.54592, "alpha":3.08532, "fx":[-111.7531,-106.88407,-129.40574,-129.55611], "fy":[-104.65785,-109.48782,-81.61608,-81.57633]}, + {"t":2.98804, "x":6.21346, "y":4.88976, "heading":2.16189, "vx":-1.88465, "vy":2.88924, "omega":2.21736, "ax":-7.02258, "ay":-5.54883, "alpha":3.01519, "fx":[-111.58397,-107.55693,-129.51999,-129.14723], "fy":[-104.87671,-108.87418,-81.50827,-82.27667]}, + {"t":3.00289, "x":6.18469, "y":4.93206, "heading":2.19482, "vx":-1.98895, "vy":2.80682, "omega":2.26214, "ax":-7.02406, "ay":-5.56763, "alpha":2.71253, "fx":[-111.94042,-109.13574,-128.75884,-128.0743], "fy":[-104.56525,-107.38496,-82.83663,-84.02812]}, + {"t":3.0197, "x":6.15026, "y":4.97847, "heading":2.23285, "vx":-2.10706, "vy":2.71321, "omega":2.30775, "ax":-7.00879, "ay":-5.59243, "alpha":2.55564, "fx":[-111.70497,-109.79259,-128.23103,-127.14131], "fy":[-104.78925,-106.68672,-83.6238,-85.40303]}, + {"t":3.03651, "x":6.11384, "y":5.02329, "heading":2.27166, "vx":-2.2249, "vy":2.61918, "omega":2.35072, "ax":-6.99183, "ay":-5.61919, "alpha":2.38464, "fx":[-111.52739,-110.44976,-127.59012,-126.14912], "fy":[-104.94804,-105.9778,-84.56875,-86.8289]}, + {"t":3.05333, "x":6.07544, "y":5.06654, "heading":2.31118, "vx":-2.34246, "vy":2.5247, "omega":2.39081, "ax":-6.97291, "ay":-5.64814, "alpha":2.19797, "fx":[-111.41815,-111.09768,-126.82054,-125.09279], "fy":[-105.03071,-105.2679,-85.68642,-88.30774]}, + {"t":3.07014, "x":6.03507, "y":5.10819, "heading":2.35138, "vx":-2.4597, "vy":2.42974, "omega":2.42777, "ax":-6.95168, "ay":-5.67954, "alpha":1.99364, "fx":[-111.38907,-111.72524,-125.9036,-123.96635], "fy":[-105.02467,-104.56889,-86.99334,-89.84223]}, + {"t":3.08696, "x":5.99273, "y":5.14824, "heading":2.3922, "vx":-2.57658, "vy":2.33424, "omega":2.46129, "ax":-6.92769, "ay":-5.71371, "alpha":1.76921, "fx":[-111.45351,-112.31969,-124.81666,-122.76245], "fy":[-104.91524,-103.89494,-88.50797,-91.43603]}, + {"t":3.10377, "x":5.94843, "y":5.18668, "heading":2.43358, "vx":-2.69306, "vy":2.23817, "omega":2.49104, "ax":-6.90042, "ay":-5.75102, "alpha":1.52156, "fx":[-111.62663,-112.86649,-123.53193,-121.47189], "fy":[-104.68507,-103.26276,-90.25116,-93.09411]}, + {"t":3.12058, "x":5.90217, "y":5.2235, "heading":2.47547, "vx":-2.80908, "vy":2.14148, "omega":2.51662, "ax":-6.8692, "ay":-5.79191, "alpha":1.24671, "fx":[-111.92573,-113.34916,-122.01486,-120.08283], "fy":[-104.31347,-102.69176,-92.24668,-94.82329]}, + {"t":3.1374, "x":5.85397, "y":5.25869, "heading":2.51778, "vx":-2.92458, "vy":2.04409, "omega":2.53758, "ax":-6.83318, "ay":-5.83688, "alpha":0.9396, "fx":[-112.3706,-113.74911,-120.22202,-118.57976], "fy":[-103.7754,-102.20423,-94.52178,-96.63291]}, + {"t":3.15421, "x":5.80383, "y":5.29223, "heading":2.56045, "vx":-3.03947, "vy":1.94596, "omega":2.55338, "ax":-6.79126, "ay":-5.88648, "alpha":0.59372, "fx":[-112.98405,-114.04528,-118.09807,-116.94192], "fy":[-103.04012,-101.82545,-97.10782,-98.53589]}, + {"t":3.17102, "x":5.75177, "y":5.32412, "heading":2.60338, "vx":-3.15366, "vy":1.84698, "omega":2.56336, "ax":-6.74201, "ay":-5.94137, "alpha":0.20074, "fx":[-113.79248,-114.21379,-115.57176,-115.14081], "fy":[-102.06934,-101.58387,-100.04075,-100.55024]}, + {"t":3.18784, "x":5.69779, "y":5.35433, "heading":2.64648, "vx":-3.26702, "vy":1.74709, "omega":2.56674, "ax":-6.68354, "ay":-6.00228, "alpha":-0.25001, "fx":[-114.82661,-114.22735,-112.55021,-113.13635], "fy":[-100.81458,-101.5111,-103.36126,-102.70122]}, + {"t":3.20465, "x":5.64192, "y":5.38286, "heading":2.68963, "vx":-3.37939, "vy":1.64616, "omega":2.56253, "ax":-6.61326, "ay":-6.06998, "alpha":-0.77229, "fx":[-116.12232,-114.05457,-108.9112,-110.8706], "fy":[-99.2134,-101.64196,-107.11401,-105.02479]}, + {"t":3.22147, "x":5.58416, "y":5.40968, "heading":2.73272, "vx":-3.49058, "vy":1.54411, "omega":2.54955, "ax":-6.5276, "ay":-6.14525, "alpha":-1.38379, "fx":[-117.72151,-113.65901,-104.49268,-108.25717], "fy":[-97.18402,-102.0142,-111.34487,-107.57282]}, + {"t":3.23828, "x":5.52455, "y":5.43477, "heading":2.77559, "vx":-3.60034, "vy":1.44078, "omega":2.52628, "ax":-6.42153, "ay":-6.2288, "alpha":-2.10739, "fx":[-119.67277,-112.99829,-99.07953,-105.16266], "fy":[-94.61741,-102.66789,-116.09392,-110.42127]}, + {"t":3.25509, "x":5.46311, "y":5.45812, "heading":2.81806, "vx":-3.70831, "vy":1.33605, "omega":2.49085, "ax":-6.28782, "ay":-6.32098, "alpha":-2.97265, "fx":[-122.03088,-112.02348,-92.38897,-101.37279], "fy":[-91.36616,-103.64368,-121.37997,-113.68267]}, + {"t":3.27191, "x":5.39987, "y":5.47969, "heading":2.85994, "vx":-3.81403, "vy":1.22977, "omega":2.44087, "ax":-6.11599, "ay":-6.42134, "alpha":-4.01757, "fx":[-124.85325,-110.68023,-84.06265,-96.5286], "fy":[-87.22955,-104.97869,-127.16865,-117.52362]}, + {"t":3.28872, "x":5.33487, "y":5.49946, "heading":2.90098, "vx":-3.91686, "vy":1.12181, "omega":2.37332, "ax":-5.89079, "ay":-6.52751, "alpha":-5.28972, "fx":[-128.18698,-108.91597,-73.69294,-90.00661], "fy":[-81.93902,-106.69568,-133.31056,-122.17913]}, + {"t":3.30553, "x":5.26818, "y":5.51739, "heading":2.94089, "vx":-4.01591, "vy":1.01206, "omega":2.28438, "ax":-5.59173, "ay":-6.63263, "alpha":-6.84223, "fx":[-132.02835,-106.70924,-60.97987,-80.73717], "fy":[-75.17112,-108.77142,-139.43109,-127.90298]}, + {"t":3.32235, "x":5.19987, "y":5.53347, "heading":2.9793, "vx":-4.10992, "vy":0.90054, "omega":2.16933, "ax":-5.20584, "ay":-6.71733, "alpha":-8.68711, "fx":[-136.19191,-104.19234,-46.38908,-67.42583], "fy":[-66.74328,-111.02383,-144.76142,-134.51114]}, + {"t":3.33916, "x":5.13003, "y":5.54767, "heading":3.01577, "vx":-4.19745, "vy":0.78759, "omega":2.02327, "ax":-4.83046, "ay":-6.7294, "alpha":-10.47271, "fx":[-139.94676,-102.11844,-33.42579,-53.16787], "fy":[-57.629,-112.71224,-148.05701,-139.46247]}, + {"t":3.35598, "x":5.05878, "y":5.55996, "heading":3.04979, "vx":-4.27867, "vy":0.67445, "omega":1.84719, "ax":-4.6857, "ay":-6.62466, "alpha":-11.53366, "fx":[-142.66629,-101.55285,-26.32159,-48.26919], "fy":[-49.33344,-112.88241,-149.1236,-139.39502]}, + {"t":3.37279, "x":4.98617, "y":5.57036, "heading":3.08085, "vx":-4.35746, "vy":0.56306, "omega":1.65326, "ax":-4.6915, "ay":-6.40839, "alpha":-12.33831, "fx":[-145.13539,-102.23816,-21.60372,-50.22698], "fy":[-39.52887,-111.752,-149.32761,-135.41098]}, + {"t":3.3896, "x":4.91224, "y":5.57892, "heading":3.10864, "vx":-4.43634, "vy":0.45531, "omega":1.44581, "ax":-4.80932, "ay":-5.9769, "alpha":-13.38506, "fx":[-147.50282,-104.33179,-16.86178,-58.52439], "fy":[-25.23258,-108.98959,-149.03436,-123.40457]}, + {"t":3.40642, "x":4.83697, "y":5.58573, "heading":3.13295, "vx":-4.5172, "vy":0.35482, "omega":1.22076, "ax":-5.46417, "ay":-4.2716, "alpha":-16.4185, "fx":[-148.41494,-109.58824,-14.28796,-99.48485], "fy":[-3.50937,-102.17891,-147.5134,-37.43294]}, + {"t":3.42323, "x":4.76025, "y":5.59109, "heading":-3.12971, "vx":-4.60907, "vy":0.283, "omega":0.9447, "ax":-6.19378, "ay":-1.51257, "alpha":-20.35475, "fx":[-144.3638,-126.0948,-49.29342,-101.66535], "fy":[25.06522,-76.28086,-133.57922,81.88156]}, + {"t":3.44004, "x":4.68188, "y":5.59564, "heading":-3.11382, "vx":-4.71321, "vy":0.25757, "omega":0.60246, "ax":-4.151, "ay":0.04322, "alpha":-20.85155, "fx":[-114.09573,-113.08668,-25.98141,-29.26547], "fy":[48.61447,-49.67403,-86.73362,90.73369]}, + {"t":3.47182, "x":4.53002, "y":5.60384, "heading":-3.09468, "vx":-4.84511, "vy":0.25894, "omega":-0.06011, "ax":8.76466, "ay":-0.48335, "alpha":-1.74804, "fx":[149.32503,148.20779,149.00452,149.80016], "fy":[0.45642,-18.70489,-15.85245,1.21447]}, + {"t":3.5036, "x":4.38049, "y":5.61183, "heading":-3.09659, "vx":-4.56661, "vy":0.24358, "omega":-0.11565, "ax":8.90506, "ay":-0.48104, "alpha":-0.52757, "fx":[151.57806,151.26896,151.38217,151.66097], "fy":[-5.62632,-11.28646,-10.6393,-5.17767]}, + {"t":3.53537, "x":4.23988, "y":5.61933, "heading":-3.10026, "vx":-4.28365, "vy":0.22829, "omega":-0.13242, "ax":8.94428, "ay":-0.48014, "alpha":-0.15463, "fx":[152.17317,152.08517,152.10753,152.19296], "fy":[-7.42139,-9.06782,-8.90391,-7.27472]}, + {"t":3.56715, "x":4.10828, "y":5.62634, "heading":-3.10447, "vx":-3.99944, "vy":0.21304, "omega":-0.13733, "ax":8.96259, "ay":-0.47967, "alpha":0.02574, "fx":[152.44537,152.45982,152.45697,152.44245], "fy":[-8.28313,-8.01008,-8.03458,-8.30811]}, + {"t":3.59892, "x":3.98572, "y":5.63286, "heading":-3.10883, "vx":-3.71464, "vy":0.1978, "omega":-0.13651, "ax":8.97317, "ay":-0.47938, "alpha":0.13199, "fx":[152.60078,152.67433,152.66237,152.58704], "fy":[-8.79234,-7.39507,-7.50957,-8.91953]}, + {"t":3.6307, "x":3.87221, "y":5.63891, "heading":-3.11317, "vx":-3.42952, "vy":0.18256, "omega":-0.13232, "ax":8.98006, "ay":-0.47919, "alpha":0.20199, "fx":[152.70102,152.81309,152.79756,152.68144], "fy":[-9.13077,-6.9953,-7.15607,-9.32136]}, + {"t":3.66247, "x":3.76777, "y":5.64447, "heading":-3.11738, "vx":-3.14417, "vy":0.16734, "omega":-0.1259, "ax":8.9849, "ay":-0.47905, "alpha":0.25157, "fx":[152.7709,152.91012,152.89331,152.74799], "fy":[-9.37341,-6.71609,-6.90049,-9.6042]}, + {"t":3.69425, "x":3.6724, "y":5.64954, "heading":-3.12138, "vx":-2.85867, "vy":0.15212, "omega":-0.11791, "ax":8.98848, "ay":-0.47895, "alpha":0.28852, "fx":[152.82231,152.98173,152.96469,152.79748], "fy":[-9.55677,-6.51099,-6.70624,-9.81316]}, + {"t":3.72602, "x":3.5861, "y":5.65413, "heading":-3.12512, "vx":-2.57305, "vy":0.1369, "omega":-0.10874, "ax":8.99125, "ay":-0.47887, "alpha":0.31712, "fx":[152.86167,153.03673,153.01996,152.83576], "fy":[-9.70074,-6.35455,-6.55312,-9.9733]}, + {"t":3.7578, "x":3.50888, "y":5.65824, "heading":-3.12858, "vx":-2.28735, "vy":0.12168, "omega":-0.09866, "ax":8.99344, "ay":-0.47881, "alpha":0.3399, "fx":[152.89274,153.08028,153.06404,152.86628], "fy":[-9.81706,-6.23164,-6.42904,-10.09961]}, + {"t":3.78958, "x":3.44074, "y":5.66187, "heading":-3.13171, "vx":-2.00158, "vy":0.10647, "omega":-0.08786, "ax":8.99522, "ay":-0.47875, "alpha":0.35849, "fx":[152.91787,153.11562,153.1,152.89119], "fy":[-9.91309,-6.13268,-6.32637,-10.20163]}, + {"t":3.82135, "x":3.38168, "y":5.66501, "heading":-3.13451, "vx":-1.71575, "vy":0.09125, "omega":-0.07647, "ax":8.9967, "ay":-0.47871, "alpha":0.37393, "fx":[152.93862,153.14486,153.1299,152.9119], "fy":[-9.99371,-6.05132,-6.24004,-10.28572]}, + {"t":3.85313, "x":3.3317, "y":5.66767, "heading":-3.13694, "vx":-1.42987, "vy":0.07604, "omega":-0.06459, "ax":8.99795, "ay":-0.47867, "alpha":0.38696, "fx":[152.95605,153.16946,153.15514,152.9294], "fy":[-10.06223,-5.98319,-6.16654,-10.35631]}, + {"t":3.8849, "x":3.29081, "y":5.66984, "heading":-3.13899, "vx":-1.14396, "vy":0.06083, "omega":-0.05229, "ax":8.99901, "ay":-0.47864, "alpha":0.39811, "fx":[152.9709,153.19045,153.17673,152.94436], "fy":[-10.12102,-5.92517,-6.10338,-10.41653]}, + {"t":3.91668, "x":3.259, "y":5.67153, "heading":-3.14065, "vx":-0.85801, "vy":0.04562, "omega":-0.03964, "ax":8.99993, "ay":-0.47861, "alpha":0.40776, "fx":[152.98373,153.20858,153.19539,152.95729], "fy":[-10.17179,-5.87495,-6.04876,-10.46873]}, + {"t":3.94845, "x":3.23628, "y":5.67274, "heading":3.14128, "vx":-0.57203, "vy":0.03041, "omega":-0.02668, "ax":9.00073, "ay":-0.47859, "alpha":0.41618, "fx":[152.99493,153.2244,153.21168,152.96856], "fy":[-10.21582,-5.83084,-6.0013,-10.51462]}, + {"t":3.98023, "x":3.22265, "y":5.67346, "heading":3.14043, "vx":-0.28603, "vy":0.01521, "omega":-0.01346, "ax":9.00144, "ay":-0.47857, "alpha":0.42361, "fx":[153.00482,153.23834,153.226,152.97845], "fy":[-10.25407,-5.79151,-5.95999,-10.55556]}, + {"t":4.01201, "x":3.2181, "y":5.67371, "heading":3.14, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 9803056..652636f 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -102,8 +102,8 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. private final Shooter _shooterSubsystem; private final AutoFactory _autoFactory; private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage).withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - private final PIDController _xController = new PIDController(0.0, 0.0, 0.0); - private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); + private final PIDController _xController = new PIDController(3.0, 0.0, 0.1); + private final PIDController _yController = new PIDController(3.0, 0.0, 0.1); private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); private final SendableChooser _modeChooser = new SendableChooser<>(); private final SendableChooser _startChooser = new SendableChooser<>(); From 50c28a40f77d59637efe6c896dc1ada6dfe93787 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Fri, 20 Mar 2026 22:37:04 -0500 Subject: [PATCH 23/32] switch to Megatag1 for pose estimation --- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9a23508..02ef7b6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -379,7 +379,7 @@ private VisionState(Limelight limelight, Pose3d cameraOffset) _settings = _limelight.getSettings(); _settings.withCameraOffset(cameraOffset).save(); - _poseEstimator = _limelight.createPoseEstimator(EstimationMode.MEGATAG2); + _poseEstimator = _limelight.createPoseEstimator(EstimationMode.MEGATAG1); } } From 9eebd0cca389b32e275ade14a684019e4935ef77 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Fri, 20 Mar 2026 22:38:08 -0500 Subject: [PATCH 24/32] clean up autos.java formatting, but don't change behavior --- src/main/java/frc/robot/Autos.java | 200 +++++++++++-------------- src/main/java/frc/robot/Constants.java | 16 ++ 2 files changed, 103 insertions(+), 113 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 652636f..f9c6c9a 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.GeneralConstants; import frc.robot.generated.ChoreoTraj; import frc.robot.generated.ChoreoVars; @@ -29,7 +30,14 @@ public class Autos extends SubsystemBase { private enum AutoMode { - DoNothing("Do Nothing"), DriveOnly("Drive Only"), ShootOnly("Shoot Only"), ShootWithDelay("Shoot + Delay"), ShootThenDrive("Shoot Then Drive"), ShootWithDelayThenDrive("Shoot + Delay + Drive"); + // @formatter:off + DoNothing("Do Nothing"), + DriveOnly("Drive Only"), + ShootOnly("Shoot Only"), + ShootWithDelay("Shoot + Delay"), + ShootThenDrive("Shoot Then Drive"), + ShootWithDelayThenDrive("Shoot + Delay + Drive"); + // @formatter:on public final String displayName; @@ -58,7 +66,7 @@ boolean staysPut() String key() { - return staysPut() ? STAY_PUT_KEY : trajectory.name(); + return staysPut() ? AutoConstants.STAY_PUT_KEY : trajectory.name(); } Pose2d endPoseBlue() @@ -69,14 +77,48 @@ Pose2d endPoseBlue() private enum StartPosition { - LeftTrench("Left Trench", ChoreoVars.Poses.LeftTrench, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftTrenchToDepot)), - LeftBump("Left Bump", ChoreoVars.Poses.LeftBump, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftBumpToDepot), new AutoDriveOption("Drive To Tower", ChoreoTraj.LeftBumpToTower), - new AutoDriveOption("Pickup From Mid", ChoreoTraj.LeftBumpPickupFromMidScore)), - Hub("Hub", ChoreoVars.Poses.HubStart, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.HubToDepot), new AutoDriveOption("Drive To Outpost", ChoreoTraj.HubToOutpost), - new AutoDriveOption("Drive To Tower", ChoreoTraj.HubToTower)), - RightBump("Right Bump", ChoreoVars.Poses.RightBump, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightBumpToOutpost), new AutoDriveOption("Drive To Tower", ChoreoTraj.RightBumpToTower), - new AutoDriveOption("Pickup From Mid", ChoreoTraj.RightBumpPickupFromMidScore)), - RightTrench("Right Trench", ChoreoVars.Poses.RightTrench, new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightTrenchToOutpost)); + // @formatter:off + LeftTrench( + "Left Trench", + ChoreoVars.Poses.LeftTrench, + new AutoDriveOption("Stay Put", null), + new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftTrenchToDepot) + ), + + LeftBump( + "Left Bump", + ChoreoVars.Poses.LeftBump, + new AutoDriveOption("Stay Put", null), + new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftBumpToDepot), + new AutoDriveOption("Drive To Tower", ChoreoTraj.LeftBumpToTower), + new AutoDriveOption("Pickup From Mid", ChoreoTraj.LeftBumpPickupFromMidScore) + ), + + Hub( + "Hub", + ChoreoVars.Poses.HubStart, + new AutoDriveOption("Stay Put", null), + new AutoDriveOption("Drive To Depot", ChoreoTraj.HubToDepot), + new AutoDriveOption("Drive To Outpost", ChoreoTraj.HubToOutpost), + new AutoDriveOption("Drive To Tower", ChoreoTraj.HubToTower) + ), + + RightBump( + "Right Bump", + ChoreoVars.Poses.RightBump, + new AutoDriveOption("Stay Put", null), + new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightBumpToOutpost), + new AutoDriveOption("Drive To Tower", ChoreoTraj.RightBumpToTower), + new AutoDriveOption("Pickup From Mid", ChoreoTraj.RightBumpPickupFromMidScore) + ), + + RightTrench( + "Right Trench", + ChoreoVars.Poses.RightTrench, + new AutoDriveOption("Stay Put", null), + new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightTrenchToOutpost) + ); + // @formatter:on public final String displayName; public final Pose2d blueStartPose; @@ -90,27 +132,22 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. } } - private static final String STAY_PUT_KEY = "__STAY_PUT__"; - private static final String AUTO_MODE_CHOOSER_NAME = "Auto Mode"; - private static final String AUTO_START_CHOOSER_NAME = "Auto Start Position"; - private static final String AUTO_DRIVE_CHOOSER_NAME = "Auto Drive Path"; - private static final String AUTO_DELAY_CHOOSER_NAME = "Auto Delay"; - private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; - private static final StartPosition DEFAULT_START_POSITION = StartPosition.Hub; - private static final int DEFAULT_DELAY_SECONDS = 0; - private final Drive _driveSubsystem; - private final Shooter _shooterSubsystem; - private final AutoFactory _autoFactory; - private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage).withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - private final PIDController _xController = new PIDController(3.0, 0.0, 0.1); - private final PIDController _yController = new PIDController(3.0, 0.0, 0.1); - private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); - private final SendableChooser _modeChooser = new SendableChooser<>(); - private final SendableChooser _startChooser = new SendableChooser<>(); - private final SendableChooser _delayChooser = new SendableChooser<>(); - private SendableChooser _driveChooser = new SendableChooser<>(); - private final Field2d _previewField = new Field2d(); - private StartPosition _lastStartPosition = null; + private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; + private static final StartPosition DEFAULT_START_POSITION = StartPosition.Hub; + private static final int DEFAULT_DELAY_SECONDS = 0; + private final Drive _driveSubsystem; + private final Shooter _shooterSubsystem; + private final AutoFactory _autoFactory; + private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage).withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); + private final PIDController _xController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); + private final PIDController _yController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); + private final PIDController _headingController = new PIDController(AutoConstants.ROTATE_KP, 0.0, AutoConstants.ROTATE_KD); + private final SendableChooser _modeChooser = new SendableChooser<>(); + private final SendableChooser _startChooser = new SendableChooser<>(); + private final SendableChooser _delayChooser = new SendableChooser<>(); + private final Field2d _previewField = new Field2d(); + private SendableChooser _driveChooser = new SendableChooser<>(); + private StartPosition _lastStartPosition = null; public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { @@ -128,7 +165,7 @@ public Autos(Drive driveSubsystem, Shooter shooterSubsystem) @Override public void periodic() { - var selectedStart = getSelectedStartPosition(); + var selectedStart = _startChooser.getSelected(); if (_lastStartPosition != selectedStart) { rebuildDriveChooser(selectedStart); @@ -139,9 +176,9 @@ public void periodic() public Command buildAuto() { - var mode = getSelectedMode(); - var startPosition = getSelectedStartPosition(); - var delaySeconds = getSelectedDelaySeconds(); + var mode = _modeChooser.getSelected(); + var startPosition = _startChooser.getSelected(); + var delaySeconds = _delayChooser.getSelected(); var driveOption = getSelectedDriveOption(startPosition); var startPose = flip(startPosition.blueStartPose); var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); @@ -162,31 +199,31 @@ public Command buildAuto() private void configureChoosers() { - _modeChooser.setDefaultOption(DEFAULT_AUTO_MODE.displayName, DEFAULT_AUTO_MODE.name()); + _modeChooser.setDefaultOption(DEFAULT_AUTO_MODE.displayName, DEFAULT_AUTO_MODE); for (var mode : AutoMode.values()) { if (mode != DEFAULT_AUTO_MODE) { - _modeChooser.addOption(mode.displayName, mode.name()); + _modeChooser.addOption(mode.displayName, mode); } } - _startChooser.setDefaultOption(DEFAULT_START_POSITION.displayName, DEFAULT_START_POSITION.name()); + _startChooser.setDefaultOption(DEFAULT_START_POSITION.displayName, DEFAULT_START_POSITION); for (var startPosition : StartPosition.values()) { if (startPosition != DEFAULT_START_POSITION) { - _startChooser.addOption(startPosition.displayName, startPosition.name()); + _startChooser.addOption(startPosition.displayName, startPosition); } } - _delayChooser.setDefaultOption(String.valueOf(DEFAULT_DELAY_SECONDS), String.valueOf(DEFAULT_DELAY_SECONDS)); - IntStream.rangeClosed(1, 5).forEach(seconds -> _delayChooser.addOption(String.valueOf(seconds), String.valueOf(seconds))); + _delayChooser.setDefaultOption(String.valueOf(DEFAULT_DELAY_SECONDS), DEFAULT_DELAY_SECONDS); + IntStream.rangeClosed(1, 5).forEach(seconds -> _delayChooser.addOption(String.valueOf(seconds), seconds)); - SmartDashboard.putData(AUTO_MODE_CHOOSER_NAME, _modeChooser); - SmartDashboard.putData(AUTO_START_CHOOSER_NAME, _startChooser); - SmartDashboard.putData(AUTO_DELAY_CHOOSER_NAME, _delayChooser); - SmartDashboard.putData(AUTO_DRIVE_CHOOSER_NAME, _driveChooser); + SmartDashboard.putData(AutoConstants.AUTO_MODE_CHOOSER_NAME, _modeChooser); + SmartDashboard.putData(AutoConstants.AUTO_START_CHOOSER_NAME, _startChooser); + SmartDashboard.putData(AutoConstants.AUTO_DELAY_CHOOSER_NAME, _delayChooser); + SmartDashboard.putData(AutoConstants.AUTO_DRIVE_CHOOSER_NAME, _driveChooser); SmartDashboard.putData("Autonomous Preview", _previewField); } @@ -210,13 +247,13 @@ private void rebuildDriveChooser(StartPosition startPosition) _driveChooser = chooser; _lastStartPosition = startPosition; - SmartDashboard.putData(AUTO_DRIVE_CHOOSER_NAME, _driveChooser); + SmartDashboard.putData(AutoConstants.AUTO_DRIVE_CHOOSER_NAME, _driveChooser); } private void updateDashboard() { - var mode = getSelectedMode(); - var startPosition = getSelectedStartPosition(); + var mode = _modeChooser.getSelected(); + var startPosition = _startChooser.getSelected(); var driveOption = getSelectedDriveOption(startPosition); var startPose = flip(startPosition.blueStartPose); var endPose = driveOption.staysPut() ? startPose : flip(driveOption.endPoseBlue()); @@ -224,69 +261,14 @@ private void updateDashboard() _previewField.setRobotPose(startPose); _previewField.getObject("Auto End Pose").setPose(endPose); - SmartDashboard.putString("Auto Summary", buildSummary(mode, startPosition, driveOption, getSelectedDelaySeconds())); - } - - private AutoMode getSelectedMode() - { - var selected = _modeChooser.getSelected(); - if (selected == null || selected.isEmpty()) - { - return DEFAULT_AUTO_MODE; - } - - for (var mode : AutoMode.values()) - { - if (mode.name().equals(selected)) - { - return mode; - } - } - - return DEFAULT_AUTO_MODE; - } - - private StartPosition getSelectedStartPosition() - { - var selected = _startChooser.getSelected(); - if (selected == null || selected.isEmpty()) - { - return DEFAULT_START_POSITION; - } - - for (var startPosition : StartPosition.values()) - { - if (startPosition.name().equals(selected)) - { - return startPosition; - } - } - - return DEFAULT_START_POSITION; - } - - private int getSelectedDelaySeconds() - { - var selected = _delayChooser.getSelected(); - if (selected == null || selected.isEmpty()) - { - return DEFAULT_DELAY_SECONDS; - } - - try - { - return Integer.parseInt(selected); - } - catch (NumberFormatException exception) - { - return DEFAULT_DELAY_SECONDS; - } + SmartDashboard.putString("Auto Summary", buildSummary(mode, startPosition, driveOption, _delayChooser.getSelected())); } private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) { var fallback = firstDrivePathKey(startPosition); var selected = _driveChooser.getSelected(); + if (selected == null || selected.isEmpty()) { selected = fallback; @@ -300,14 +282,6 @@ private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) } } - for (var option : startPosition.driveOptions) - { - if (option.key().equals(fallback)) - { - return option; - } - } - return startPosition.driveOptions[0]; } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1db688e..6c82b3a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -94,6 +94,22 @@ public static class AIOConstants public static final int TURRET_POTENTIOMETER = 0; // TODO } + public static class AutoConstants + { + // Dashboard display + public static final String STAY_PUT_KEY = "__STAY_PUT__"; + public static final String AUTO_MODE_CHOOSER_NAME = "Auto Mode"; + public static final String AUTO_START_CHOOSER_NAME = "Auto Start Position"; + public static final String AUTO_DRIVE_CHOOSER_NAME = "Auto Drive Path"; + public static final String AUTO_DELAY_CHOOSER_NAME = "Auto Delay"; + + // Auto driving + public static final double DRIVE_KP = 3.0; + public static final double DRIVE_KD = 0.1; + public static final double ROTATE_KP = 0.0; + public static final double ROTATE_KD = 0.0; + } + public static class DriveConstants { public static final LinearVelocity MAX_SPEED = TunerConstants.kSpeedAt12Volts.times(0.5); // kSpeedAt12Volts From d792e97dc92b14e7d56fdafec81cecf7ad44b373 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Sat, 21 Mar 2026 00:23:47 -0500 Subject: [PATCH 25/32] pull starting pose from path rather than from start if driving --- src/main/java/frc/robot/Autos.java | 95 +++++++++----------------- src/main/java/frc/robot/Constants.java | 3 - 2 files changed, 33 insertions(+), 65 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index f9c6c9a..c11a30f 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -59,17 +59,19 @@ public boolean usesDelay() private record AutoDriveOption(String displayName, ChoreoTraj trajectory) { - boolean staysPut() + public static final AutoDriveOption STAY_PUT = new AutoDriveOption("Stay Put", null); + + public boolean staysPut() { return trajectory == null; } - String key() + public Pose2d startPoseBlue() { - return staysPut() ? AutoConstants.STAY_PUT_KEY : trajectory.name(); + return staysPut() ? null : trajectory.initialPoseBlue(); } - Pose2d endPoseBlue() + public Pose2d endPoseBlue() { return staysPut() ? null : trajectory.endPoseBlue(); } @@ -81,14 +83,12 @@ private enum StartPosition LeftTrench( "Left Trench", ChoreoVars.Poses.LeftTrench, - new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftTrenchToDepot) ), LeftBump( "Left Bump", ChoreoVars.Poses.LeftBump, - new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.LeftBumpToDepot), new AutoDriveOption("Drive To Tower", ChoreoTraj.LeftBumpToTower), new AutoDriveOption("Pickup From Mid", ChoreoTraj.LeftBumpPickupFromMidScore) @@ -97,7 +97,6 @@ private enum StartPosition Hub( "Hub", ChoreoVars.Poses.HubStart, - new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Depot", ChoreoTraj.HubToDepot), new AutoDriveOption("Drive To Outpost", ChoreoTraj.HubToOutpost), new AutoDriveOption("Drive To Tower", ChoreoTraj.HubToTower) @@ -106,7 +105,6 @@ private enum StartPosition RightBump( "Right Bump", ChoreoVars.Poses.RightBump, - new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightBumpToOutpost), new AutoDriveOption("Drive To Tower", ChoreoTraj.RightBumpToTower), new AutoDriveOption("Pickup From Mid", ChoreoTraj.RightBumpPickupFromMidScore) @@ -115,7 +113,6 @@ private enum StartPosition RightTrench( "Right Trench", ChoreoVars.Poses.RightTrench, - new AutoDriveOption("Stay Put", null), new AutoDriveOption("Drive To Outpost", ChoreoTraj.RightTrenchToOutpost) ); // @formatter:on @@ -146,7 +143,7 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. private final SendableChooser _startChooser = new SendableChooser<>(); private final SendableChooser _delayChooser = new SendableChooser<>(); private final Field2d _previewField = new Field2d(); - private SendableChooser _driveChooser = new SendableChooser<>(); + private SendableChooser _driveChooser = new SendableChooser<>(); private StartPosition _lastStartPosition = null; public Autos(Drive driveSubsystem, Shooter shooterSubsystem) @@ -179,8 +176,8 @@ public Command buildAuto() var mode = _modeChooser.getSelected(); var startPosition = _startChooser.getSelected(); var delaySeconds = _delayChooser.getSelected(); - var driveOption = getSelectedDriveOption(startPosition); - var startPose = flip(startPosition.blueStartPose); + var driveOption = _driveChooser.getSelected(); + var startPose = getStartPose(startPosition, driveOption); var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); var shoot = _shooterSubsystem.shoot().withTimeout(4.0); var delay = Commands.waitSeconds(delaySeconds); @@ -229,20 +226,12 @@ private void configureChoosers() private void rebuildDriveChooser(StartPosition startPosition) { - var chooser = new SendableChooser(); - var options = startPosition.driveOptions; - var defaultKey = firstDrivePathKey(startPosition); + var chooser = new SendableChooser(); - for (var option : options) + chooser.setDefaultOption(AutoDriveOption.STAY_PUT.displayName, AutoDriveOption.STAY_PUT); + for (var option : startPosition.driveOptions) { - if (option.key().equals(defaultKey)) - { - chooser.setDefaultOption(option.displayName(), option.key()); - } - else - { - chooser.addOption(option.displayName(), option.key()); - } + chooser.addOption(option.displayName, option); } _driveChooser = chooser; @@ -254,8 +243,8 @@ private void updateDashboard() { var mode = _modeChooser.getSelected(); var startPosition = _startChooser.getSelected(); - var driveOption = getSelectedDriveOption(startPosition); - var startPose = flip(startPosition.blueStartPose); + var driveOption = _driveChooser.getSelected(); + var startPose = getStartPose(startPosition, driveOption); var endPose = driveOption.staysPut() ? startPose : flip(driveOption.endPoseBlue()); _previewField.setRobotPose(startPose); @@ -264,27 +253,6 @@ private void updateDashboard() SmartDashboard.putString("Auto Summary", buildSummary(mode, startPosition, driveOption, _delayChooser.getSelected())); } - private AutoDriveOption getSelectedDriveOption(StartPosition startPosition) - { - var fallback = firstDrivePathKey(startPosition); - var selected = _driveChooser.getSelected(); - - if (selected == null || selected.isEmpty()) - { - selected = fallback; - } - - for (var option : startPosition.driveOptions) - { - if (option.key().equals(selected)) - { - return option; - } - } - - return startPosition.driveOptions[0]; - } - private String buildSummary(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption, int delaySeconds) { var summary = new StringBuilder(); @@ -303,27 +271,30 @@ private String buildSummary(AutoMode mode, StartPosition startPosition, AutoDriv return summary.toString(); } - private String firstDrivePathKey(StartPosition startPosition) - { - for (var option : startPosition.driveOptions) - { - if (!option.staysPut()) - { - return option.key(); - } - } - - return startPosition.driveOptions[0].key(); - } - private void followTrajectory(SwerveSample sample) { var pose = _driveSubsystem.getState().Pose; + // @formatter:off _driveSubsystem.setControl( - _autoRequest.withVelocityX(sample.vx + _xController.calculate(pose.getX(), sample.x)).withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) - .withRotationalRate(sample.omega + _headingController.calculate(pose.getRotation().getRadians(), sample.heading)) + _autoRequest + .withVelocityX(sample.vx + _xController.calculate(pose.getX(), sample.x)) + .withVelocityY(sample.vy + _yController.calculate(pose.getY(), sample.y)) + .withRotationalRate(sample.omega + _headingController.calculate(pose.getRotation().getRadians(), sample.heading)) ); + // @formatter:on + } + + private Pose2d getStartPose(StartPosition startPosition, AutoDriveOption driveOption) + { + var pose = driveOption.startPoseBlue(); + + if (driveOption == AutoDriveOption.STAY_PUT) + { + pose = startPosition.blueStartPose; + } + + return flip(pose); } private Pose2d flip(Pose2d bluePose) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6c82b3a..c2096a8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,6 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Value; import static edu.wpi.first.units.Units.Volts; import java.util.List; @@ -38,7 +37,6 @@ import edu.wpi.first.units.DistanceUnit; import edu.wpi.first.units.VoltageUnit; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Dimensionless; @@ -97,7 +95,6 @@ public static class AIOConstants public static class AutoConstants { // Dashboard display - public static final String STAY_PUT_KEY = "__STAY_PUT__"; public static final String AUTO_MODE_CHOOSER_NAME = "Auto Mode"; public static final String AUTO_START_CHOOSER_NAME = "Auto Start Position"; public static final String AUTO_DRIVE_CHOOSER_NAME = "Auto Drive Path"; From 0d53ad9dcd213387bc179a2720783755308c84d5 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Sat, 21 Mar 2026 01:41:19 -0500 Subject: [PATCH 26/32] draw trajectories --- src/main/java/frc/robot/Autos.java | 51 ++++++++++++++++++++------ src/main/java/frc/robot/Constants.java | 13 ++++--- 2 files changed, 47 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index c11a30f..1b64e7c 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -1,8 +1,12 @@ package frc.robot; +import java.util.Arrays; +import java.util.Collections; +import java.util.List; import java.util.stream.IntStream; import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; @@ -11,7 +15,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -135,6 +139,7 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. private final Drive _driveSubsystem; private final Shooter _shooterSubsystem; private final AutoFactory _autoFactory; + private final AutoRoutine _trajectoryDrawRoutine; private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage).withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); private final PIDController _xController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); private final PIDController _yController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); @@ -148,11 +153,12 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { - _driveSubsystem = driveSubsystem; - _shooterSubsystem = shooterSubsystem; + _driveSubsystem = driveSubsystem; + _shooterSubsystem = shooterSubsystem; + _autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem); + _trajectoryDrawRoutine = _autoFactory.newRoutine("Trajectory Drawing"); _headingController.enableContinuousInput(-Math.PI, Math.PI); - _autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem); configureChoosers(); rebuildDriveChooser(DEFAULT_START_POSITION); @@ -162,6 +168,12 @@ public Autos(Drive driveSubsystem, Shooter shooterSubsystem) @Override public void periodic() { + // Don't update the autonomous selections/display if the robot is enabled + if (DriverStation.isEnabled()) + { + return; + } + var selectedStart = _startChooser.getSelected(); if (_lastStartPosition != selectedStart) { @@ -177,7 +189,7 @@ public Command buildAuto() var startPosition = _startChooser.getSelected(); var delaySeconds = _delayChooser.getSelected(); var driveOption = _driveChooser.getSelected(); - var startPose = getStartPose(startPosition, driveOption); + var startPose = getStartPose(mode, startPosition, driveOption); var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); var shoot = _shooterSubsystem.shoot().withTimeout(4.0); var delay = Commands.waitSeconds(delaySeconds); @@ -244,11 +256,18 @@ private void updateDashboard() var mode = _modeChooser.getSelected(); var startPosition = _startChooser.getSelected(); var driveOption = _driveChooser.getSelected(); - var startPose = getStartPose(startPosition, driveOption); + var startPose = getStartPose(mode, startPosition, driveOption); var endPose = driveOption.staysPut() ? startPose : flip(driveOption.endPoseBlue()); + var trajectory = Collections.emptyList(); + + if (!driveOption.staysPut() && mode.usesDrivePath()) + { + trajectory = flip(Arrays.asList(driveOption.trajectory.asAutoTraj(_trajectoryDrawRoutine).getRawTrajectory().getPoses())); + } _previewField.setRobotPose(startPose); _previewField.getObject("Auto End Pose").setPose(endPose); + _previewField.getObject("Auto Trajectory").setPoses(trajectory); SmartDashboard.putString("Auto Summary", buildSummary(mode, startPosition, driveOption, _delayChooser.getSelected())); } @@ -285,23 +304,33 @@ private void followTrajectory(SwerveSample sample) // @formatter:on } - private Pose2d getStartPose(StartPosition startPosition, AutoDriveOption driveOption) + private Pose2d getStartPose(AutoMode mode, StartPosition startPosition, AutoDriveOption driveOption) { - var pose = driveOption.startPoseBlue(); + var pose = startPosition.blueStartPose; - if (driveOption == AutoDriveOption.STAY_PUT) + if (driveOption != null && (!driveOption.staysPut() || mode.usesDrivePath())) { - pose = startPosition.blueStartPose; + pose = driveOption.startPoseBlue(); } return flip(pose); } + private List flip(List bluePoses) + { + if (Utilities.isRedAlliance()) + { + return bluePoses.stream().map(p -> p.rotateAround(GeneralConstants.FIELD_CENTER, Rotation2d.k180deg)).toList(); + } + + return bluePoses; + } + private Pose2d flip(Pose2d bluePose) { if (Utilities.isRedAlliance()) { - return bluePose.rotateAround(new Translation2d(GeneralConstants.FIELD_SIZE_X.div(2), GeneralConstants.FIELD_SIZE_Y.div(2)), Rotation2d.k180deg); + return bluePose.rotateAround(GeneralConstants.FIELD_CENTER, Rotation2d.k180deg); } return bluePose; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c2096a8..8fd275d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -148,12 +148,13 @@ public static class IntakeConstants public static class GeneralConstants { - public static final Time LOOP_PERIOD = Milliseconds.of(20); - public static final Voltage MOTOR_VOLTAGE = Volts.of(12.0); - public static final Voltage SENSOR_VOLTAGE = Volts.of(5.0); - public static final DCMotor WINDOW_MOTOR = new DCMotor(GeneralConstants.MOTOR_VOLTAGE.in(Volts), 9.2, 16.3, 1.6, RPM.of(90).in(RadiansPerSecond), 1); - public static final Distance FIELD_SIZE_X = Inches.of(651.2); - public static final Distance FIELD_SIZE_Y = Inches.of(317.7); + public static final Time LOOP_PERIOD = Milliseconds.of(20); + public static final Voltage MOTOR_VOLTAGE = Volts.of(12.0); + public static final Voltage SENSOR_VOLTAGE = Volts.of(5.0); + public static final DCMotor WINDOW_MOTOR = new DCMotor(GeneralConstants.MOTOR_VOLTAGE.in(Volts), 9.2, 16.3, 1.6, RPM.of(90).in(RadiansPerSecond), 1); + public static final Distance FIELD_SIZE_X = Inches.of(651.2); + public static final Distance FIELD_SIZE_Y = Inches.of(317.7); + public static final Translation2d FIELD_CENTER = new Translation2d(FIELD_SIZE_X.div(2), FIELD_SIZE_Y.div(2)); } public static class ShooterConstants From 73751018786c5c82aace5891e02674e47c7732cb Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Sat, 21 Mar 2026 01:43:23 -0500 Subject: [PATCH 27/32] add logging of path poses --- src/main/java/frc/robot/Autos.java | 57 +++++++++++++++------ src/main/java/frc/robot/RobotContainer.java | 2 - 2 files changed, 41 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 1b64e7c..3543b13 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -12,6 +12,8 @@ import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -30,6 +32,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Utilities; +@Logged public class Autos extends SubsystemBase { private enum AutoMode @@ -133,23 +136,42 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. } } - private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; - private static final StartPosition DEFAULT_START_POSITION = StartPosition.Hub; - private static final int DEFAULT_DELAY_SECONDS = 0; + @NotLogged + private static final AutoMode DEFAULT_AUTO_MODE = AutoMode.ShootOnly; + @NotLogged + private static final StartPosition DEFAULT_START_POSITION = StartPosition.Hub; + @NotLogged + private static final int DEFAULT_DELAY_SECONDS = 0; + @NotLogged private final Drive _driveSubsystem; + @NotLogged private final Shooter _shooterSubsystem; + @NotLogged private final AutoFactory _autoFactory; + @NotLogged private final AutoRoutine _trajectoryDrawRoutine; - private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage).withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - private final PIDController _xController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); - private final PIDController _yController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); - private final PIDController _headingController = new PIDController(AutoConstants.ROTATE_KP, 0.0, AutoConstants.ROTATE_KD); - private final SendableChooser _modeChooser = new SendableChooser<>(); - private final SendableChooser _startChooser = new SendableChooser<>(); - private final SendableChooser _delayChooser = new SendableChooser<>(); - private final Field2d _previewField = new Field2d(); - private SendableChooser _driveChooser = new SendableChooser<>(); - private StartPosition _lastStartPosition = null; + @NotLogged + private final SwerveRequest.FieldCentric _autoRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage).withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); + @NotLogged + private final PIDController _xController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); + @NotLogged + private final PIDController _yController = new PIDController(AutoConstants.DRIVE_KP, 0.0, AutoConstants.DRIVE_KD); + @NotLogged + private final PIDController _headingController = new PIDController(AutoConstants.ROTATE_KP, 0.0, AutoConstants.ROTATE_KD); + @NotLogged + private final SendableChooser _modeChooser = new SendableChooser<>(); + @NotLogged + private final SendableChooser _startChooser = new SendableChooser<>(); + @NotLogged + private final SendableChooser _delayChooser = new SendableChooser<>(); + @NotLogged + private final Field2d _previewField = new Field2d(); + @NotLogged + private SendableChooser _driveChooser = new SendableChooser<>(); + @NotLogged + private StartPosition _lastStartPosition = null; + @Logged + private Pose2d _currentDesiredPathPose = new Pose2d(); public Autos(Drive driveSubsystem, Shooter shooterSubsystem) { @@ -194,15 +216,16 @@ public Command buildAuto() var shoot = _shooterSubsystem.shoot().withTimeout(4.0); var delay = Commands.waitSeconds(delaySeconds); var drive = driveOption.staysPut() ? Commands.none() : _autoFactory.trajectoryCmd(driveOption.trajectory().name()); + var stop = driveOption.staysPut() ? Commands.none() : _driveSubsystem.runOnce(() -> _driveSubsystem.setControl(_autoRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); return switch (mode) { case DoNothing -> resetPose; - case DriveOnly -> Commands.sequence(resetPose, drive); + case DriveOnly -> Commands.sequence(resetPose, drive, stop); case ShootOnly -> Commands.sequence(resetPose, shoot); case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot); - case ShootThenDrive -> Commands.sequence(resetPose, shoot, drive); - case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive); + case ShootThenDrive -> Commands.sequence(resetPose, shoot, drive, stop); + case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive, stop); }; } @@ -294,6 +317,8 @@ private void followTrajectory(SwerveSample sample) { var pose = _driveSubsystem.getState().Pose; + _currentDesiredPathPose = sample.getPose(); + // @formatter:off _driveSubsystem.setControl( _autoRequest diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e6076a7..1cb3978 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,7 +10,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.units.measure.LinearVelocity; @@ -38,7 +37,6 @@ public class RobotContainer private final Drive _drive = TunerConstants.createDrivetrain(); private final Intake _intake = new Intake(); private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended, _intake::isRetracted); - @NotLogged private final Autos _autos = new Autos(_drive, _shooter); private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; From db0bcf11e35fdb3a69db68b454ea5af584a8fe6e Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Sat, 21 Mar 2026 11:54:27 -0500 Subject: [PATCH 28/32] reject vision estimates if no tag is within 10 feet --- .../choreo/LeftBumpPickupFromMidScore.traj | 451 +++++++++--------- src/main/java/frc/robot/Autos.java | 14 +- src/main/java/frc/robot/Constants.java | 4 + .../java/frc/robot/generated/ChoreoTraj.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 19 + 5 files changed, 273 insertions(+), 217 deletions(-) diff --git a/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj b/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj index f326ecc..5dbc19d 100644 --- a/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj +++ b/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj @@ -3,38 +3,40 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.61569, "y":5.558789999999999, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":3.61569, "y":5.558789999999999, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":6.61945915222168, "y":5.5876383781433105, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.301177978515625, "y":5.6295905113220215, "heading":-0.5585991876326754, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.584353923797607, "y":4.738111972808838, "heading":-1.2360600261652088, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.594841480255127, "y":3.8151695728302, "heading":-1.8233502920797129, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.1019062995910645, "y":2.9971067905426025, "heading":2.3880715859232047, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.496963977813721, "y":3.878139019012451, "heading":1.774814111378504, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.184691429138184, "y":4.932059288024902, "heading":2.194818993599192, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.68187952041626, "y":5.595638275146484, "heading":-3.1138221488567326, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.301177978515625, "y":5.6295905113220215, "heading":-0.5585991876326754, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.584353923797607, "y":4.738111972808838, "heading":-1.2360600261652088, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.594841480255127, "y":3.8151695728302, "heading":-1.8233502920797129, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.1019062995910645, "y":2.9971067905426025, "heading":2.3880715859232047, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.496963977813721, "y":3.878139019012451, "heading":1.774814111378504, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.184691429138184, "y":4.932059288024902, "heading":2.194818993599192, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.68187952041626, "y":5.595638275146484, "heading":-3.1138221488567326, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":3.218101739883423, "y":5.673706531524658, "heading":3.14, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"LeftBump.x", "val":3.61569}, "y":{"exp":"LeftBump.y", "val":5.558789999999999}, "heading":{"exp":"-0 mrad", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"LeftBump.x", "val":3.61569}, "y":{"exp":"LeftBump.y", "val":5.558789999999999}, "heading":{"exp":"-0 mrad", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"LeftMiddle.x", "val":6.61945915222168}, "y":{"exp":"LeftMiddle.y", "val":5.5876383781433105}, "heading":{"exp":"LeftMiddle.heading", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.301177978515625 m", "val":7.301177978515625}, "y":{"exp":"5.6295905113220215 m", "val":5.6295905113220215}, "heading":{"exp":"-0.5585991876326754 rad", "val":-0.5585991876326754}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.584353923797607 m", "val":7.584353923797607}, "y":{"exp":"4.738111972808838 m", "val":4.738111972808838}, "heading":{"exp":"-1.2360600261652088 rad", "val":-1.2360600261652088}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.594841480255127 m", "val":7.594841480255127}, "y":{"exp":"3.8151695728302 m", "val":3.8151695728302}, "heading":{"exp":"-1.8233502920797129 rad", "val":-1.8233502920797129}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1019062995910645 m", "val":7.1019062995910645}, "y":{"exp":"2.9971067905426025 m", "val":2.9971067905426025}, "heading":{"exp":"2.3880715859232047 rad", "val":2.3880715859232047}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.496963977813721 m", "val":6.496963977813721}, "y":{"exp":"3.878139019012451 m", "val":3.878139019012451}, "heading":{"exp":"1.774814111378504 rad", "val":1.774814111378504}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.184691429138184 m", "val":6.184691429138184}, "y":{"exp":"4.932059288024902 m", "val":4.932059288024902}, "heading":{"exp":"2.194818993599192 rad", "val":2.194818993599192}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.68187952041626 m", "val":4.68187952041626}, "y":{"exp":"5.595638275146484 m", "val":5.595638275146484}, "heading":{"exp":"-3.1138221488567326 rad", "val":-3.1138221488567326}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.301177978515625 m", "val":7.301177978515625}, "y":{"exp":"5.6295905113220215 m", "val":5.6295905113220215}, "heading":{"exp":"-0.5585991876326754 rad", "val":-0.5585991876326754}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.584353923797607 m", "val":7.584353923797607}, "y":{"exp":"4.738111972808838 m", "val":4.738111972808838}, "heading":{"exp":"-1.2360600261652088 rad", "val":-1.2360600261652088}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.594841480255127 m", "val":7.594841480255127}, "y":{"exp":"3.8151695728302 m", "val":3.8151695728302}, "heading":{"exp":"-1.8233502920797129 rad", "val":-1.8233502920797129}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.1019062995910645 m", "val":7.1019062995910645}, "y":{"exp":"2.9971067905426025 m", "val":2.9971067905426025}, "heading":{"exp":"2.3880715859232047 rad", "val":2.3880715859232047}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.496963977813721 m", "val":6.496963977813721}, "y":{"exp":"3.878139019012451 m", "val":3.878139019012451}, "heading":{"exp":"1.774814111378504 rad", "val":1.774814111378504}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.184691429138184 m", "val":6.184691429138184}, "y":{"exp":"4.932059288024902 m", "val":4.932059288024902}, "heading":{"exp":"2.194818993599192 rad", "val":2.194818993599192}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.68187952041626 m", "val":4.68187952041626}, "y":{"exp":"5.595638275146484 m", "val":5.595638275146484}, "heading":{"exp":"-3.1138221488567326 rad", "val":-3.1138221488567326}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"3.218101739883423 m", "val":3.218101739883423}, "y":{"exp":"5.673706531524658 m", "val":5.673706531524658}, "heading":{"exp":"3.14 rad", "val":3.14}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -65,202 +67,225 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.9112,1.19379,1.58112,1.80941,2.26968,2.70584,3.00289,3.44004,4.01201], + "waypoints":[0.0,1.61422,1.96942,2.45471,2.91654,3.44568,4.03363,4.58687,5.4126,6.25788], "samples":[ - {"t":0.0, "x":3.61569, "y":5.55879, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.99335, "ay":-0.47437, "alpha":1.42105, "fx":[153.3663,152.48692,152.66074,153.38309], "fy":[-0.51165,-16.41745,-14.8834,-0.46337]}, - {"t":0.03797, "x":3.62217, "y":5.55845, "heading":0.0, "vx":0.34145, "vy":-0.01801, "omega":0.05395, "ax":8.99294, "ay":-0.47426, "alpha":1.39797, "fx":[153.35226,152.48872,152.65854,153.37014], "fy":[-0.63842,-16.27402,-14.77665,-0.5791]}, - {"t":0.07593, "x":3.64162, "y":5.55742, "heading":0.00205, "vx":0.68288, "vy":-0.03602, "omega":0.10703, "ax":8.99247, "ay":-0.47413, "alpha":1.371, "fx":[153.33565,152.48919,152.65737,153.35491], "fy":[-0.80215,-16.12053,-14.63617,-0.70029]}, - {"t":0.1139, "x":3.67402, "y":5.55571, "heading":0.00611, "vx":1.02429, "vy":-0.05402, "omega":0.15908, "ax":8.99189, "ay":-0.47397, "alpha":1.33908, "fx":[153.31569,152.48853,152.65691,153.33675], "fy":[-1.00731,-15.94922,-14.45809,-0.83375]}, - {"t":0.15187, "x":3.71939, "y":5.55332, "heading":0.01215, "vx":1.36568, "vy":-0.07201, "omega":0.20992, "ax":8.99118, "ay":-0.47378, "alpha":1.30069, "fx":[153.2913,152.48692,152.65669,153.31473], "fy":[-1.26027,-15.7492,-14.23686,-0.989]}, - {"t":0.18983, "x":3.77772, "y":5.55025, "heading":0.02012, "vx":1.70704, "vy":-0.09, "omega":0.2593, "ax":8.99029, "ay":-0.47354, "alpha":1.25368, "fx":[153.26091,152.48459,152.65605,153.28741], "fy":[-1.57028,-15.50491,-13.96428,-1.17981]}, - {"t":0.2278, "x":3.84901, "y":5.54649, "heading":0.02997, "vx":2.04837, "vy":-0.10798, "omega":0.3069, "ax":8.98913, "ay":-0.47324, "alpha":1.19475, "fx":[153.22211,152.48177,152.65395,153.25261], "fy":[-1.95117,-15.19328,-13.62783,-1.42677]}, - {"t":0.26577, "x":3.93326, "y":5.54205, "heading":0.04162, "vx":2.38966, "vy":-0.12595, "omega":0.35226, "ax":8.98759, "ay":-0.47286, "alpha":1.11876, "fx":[153.1711,152.47857,152.64873,153.20668], "fy":[-2.42458,-14.77848,-13.20742,-1.76221]}, - {"t":0.30373, "x":4.03047, "y":5.53692, "heading":0.05499, "vx":2.73088, "vy":-0.1439, "omega":0.39474, "ax":8.98541, "ay":-0.47233, "alpha":1.01704, "fx":[153.10144,152.47476,152.63743,153.14319], "fy":[-3.02637,-14.2015,-12.6689,-2.24023]}, - {"t":0.3417, "x":4.14062, "y":5.53112, "heading":0.06998, "vx":3.07203, "vy":-0.16183, "omega":0.43335, "ax":8.98213, "ay":-0.47158, "alpha":0.87385, "fx":[153.00128,152.46889,152.61436,153.04957], "fy":[-3.82052,-13.35722,-11.94901,-2.95914]}, - {"t":0.37966, "x":4.26373, "y":5.52464, "heading":0.08643, "vx":3.41305, "vy":-0.17974, "omega":0.46653, "ax":8.97673, "ay":-0.47042, "alpha":0.65736, "fx":[152.84618,152.4554,152.56688,152.89789], "fy":[-4.93371,-12.03739,-10.91642,-4.11912]}, - {"t":0.41763, "x":4.39978, "y":5.51747, "heading":0.10414, "vx":3.75386, "vy":-0.1976, "omega":0.49148, "ax":8.96636, "ay":-0.46837, "alpha":0.2918, "fx":[152.57654,152.4121,152.46038,152.61207], "fy":[-6.65664,-9.76071,-9.24737,-6.20289]}, - {"t":0.4556, "x":4.54877, "y":5.50963, "heading":0.1228, "vx":4.09428, "vy":-0.21538, "omega":0.50256, "ax":8.94027, "ay":-0.4639, "alpha":-0.45836, "fx":[152.00078,152.22633,152.15891,151.89975], "fy":[-9.8329,-5.10456,-5.87672,-10.74924]}, - {"t":0.49356, "x":4.71066, "y":5.50112, "heading":0.14188, "vx":4.43371, "vy":-0.23299, "omega":0.48516, "ax":8.81235, "ay":-0.44644, "alpha":-2.89605, "fx":[150.04241,150.82739,150.41705,148.29558], "fy":[-18.30739,8.72078,5.83193,-26.62059]}, - {"t":0.53153, "x":4.88534, "y":5.49195, "heading":0.1603, "vx":4.76829, "vy":-0.24994, "omega":0.37521, "ax":3.43612, "ay":-0.0829, "alpha":-9.84386, "fx":[83.52904,77.84673,31.97298,40.44086], "fy":[-23.00935,29.61766,27.48169,-39.73074]}, - {"t":0.5695, "x":5.06885, "y":5.48241, "heading":0.17455, "vx":4.89875, "vy":-0.25309, "omega":0.00147, "ax":0.0263, "ay":0.48143, "alpha":-0.00536, "fx":[0.46405,0.45902,0.4306,0.43562], "fy":[8.17737,8.20564,8.20066,8.1724]}, - {"t":0.60746, "x":5.25486, "y":5.47314, "heading":0.17461, "vx":4.89974, "vy":-0.23481, "omega":0.00127, "ax":0.07624, "ay":1.87869, "alpha":0.00005, "fx":[1.29665,1.2967,1.29697,1.29692], "fy":[31.95601,31.95576,31.9558,31.95605]}, - {"t":0.64543, "x":5.44094, "y":5.46558, "heading":0.17465, "vx":4.90264, "vy":-0.16348, "omega":0.00127, "ax":0.06802, "ay":5.20855, "alpha":0.00017, "fx":[1.15636,1.15658,1.15777,1.15755], "fy":[88.59626,88.59566,88.59576,88.59636]}, - {"t":0.6834, "x":5.62712, "y":5.46313, "heading":0.1747, "vx":4.90522, "vy":0.03427, "omega":0.00128, "ax":-0.30043, "ay":7.57552, "alpha":-0.07825, "fx":[-4.7044,-4.80724,-5.51433,-5.41467], "fy":[128.8205,128.93929,128.89443,128.7758]}, - {"t":0.72136, "x":5.81314, "y":5.46989, "heading":0.17475, "vx":4.89382, "vy":0.32188, "omega":-0.0017, "ax":-4.54495, "ay":5.98188, "alpha":-13.345, "fx":[-32.67137,-28.76331,-106.24979,-141.54856], "fy":[128.33382,143.98997,103.14383,31.53288]}, - {"t":0.75933, "x":5.99567, "y":5.48642, "heading":0.17469, "vx":4.72126, "vy":0.54899, "omega":-0.50836, "ax":-7.47188, "ay":1.84432, "alpha":-14.74701, "fx":[-142.25118,-79.50413,-137.86852,-148.75426], "fy":[-37.31986,128.49562,64.08268,-29.77335]}, - {"t":0.7973, "x":6.16953, "y":5.5086, "heading":0.15539, "vx":4.43758, "vy":0.61902, "omega":-1.06825, "ax":-8.435, "ay":1.46346, "alpha":-8.53874, "fx":[-151.05368,-125.9604,-145.46328,-151.43019], "fy":[-13.71856,85.3847,46.26812,-18.36189]}, - {"t":0.83526, "x":6.33193, "y":5.53315, "heading":0.11483, "vx":4.11733, "vy":0.67458, "omega":-1.39244, "ax":-8.68512, "ay":1.17313, "alpha":-6.31044, "fx":[-152.13595,-138.59493,-147.87598,-152.31872], "fy":[-10.17485,63.93267,38.89092,-12.83001]}, - {"t":0.87323, "x":6.48199, "y":5.55961, "heading":0.06196, "vx":3.78759, "vy":0.71912, "omega":-1.63202, "ax":-8.78756, "ay":1.00924, "alpha":-5.19406, "fx":[-152.51719,-143.70478,-148.93581,-152.73803], "fy":[-9.3123,52.11422,35.2371,-9.37124]}, - {"t":0.9112, "x":6.61946, "y":5.58764, "heading":0.0, "vx":3.45396, "vy":0.75744, "omega":-1.82922, "ax":-8.86785, "ay":0.17183, "alpha":-3.91153, "fx":[-150.80588,-149.8038,-151.07579,-151.673], "fy":[-19.83232,26.40986,20.3655,-15.25178]}, - {"t":0.92689, "x":6.67259, "y":5.59955, "heading":-0.02872, "vx":3.31473, "vy":0.76013, "omega":-1.89063, "ax":-8.81024, "ay":-1.28558, "alpha":-3.12574, "fx":[-146.2854,-152.03952,-152.38758,-148.72615], "fy":[-42.2189,-6.88974,-4.68281,-33.67826]}, - {"t":0.94259, "x":6.72355, "y":5.61133, "heading":-0.0584, "vx":3.17642, "vy":0.73995, "omega":-1.9397, "ax":-8.56299, "ay":-2.53713, "alpha":-2.38585, "fx":[-140.38445,-148.2287,-149.84295,-144.16027], "fy":[-59.30214,-35.05777,-28.36292,-49.90078]}, - {"t":0.95829, "x":6.77236, "y":5.62263, "heading":-0.08885, "vx":3.04198, "vy":0.70012, "omega":-1.97716, "ax":-8.21257, "ay":-3.56018, "alpha":-1.74048, "fx":[-134.31637,-141.4356,-144.40315,-138.61907], "fy":[-72.25509,-56.89309,-49.23606,-63.84628]}, - {"t":0.97399, "x":6.8191, "y":5.63318, "heading":-0.11989, "vx":2.91305, "vy":0.64423, "omega":-2.00448, "ax":-7.82494, "ay":-4.37545, "alpha":-1.18996, "fx":[-128.61274,-133.88264,-137.29311,-132.61172], "fy":[-82.1704,-73.16787,-66.6999,-75.66218]}, - {"t":0.98969, "x":6.86387, "y":5.64276, "heading":-0.15136, "vx":2.7902, "vy":0.57553, "omega":-2.02317, "ax":-7.43991, "ay":-5.01952, "alpha":-0.72396, "fx":[-123.4504,-126.69427,-129.56275,-126.49562], "fy":[-89.88753,-85.20038,-80.83316,-85.60151]}, - {"t":1.00539, "x":6.90676, "y":5.65117, "heading":-0.18312, "vx":2.6734, "vy":0.49673, "omega":-2.03453, "ax":-7.07767, "ay":-5.52914, "alpha":-0.33108, "fx":[-118.84636,-120.29187,-121.91905,-120.499], "fy":[-96.00896,-94.16963,-92.07312,-93.9449]}, - {"t":1.02109, "x":6.94786, "y":5.65829, "heading":-0.21507, "vx":2.56228, "vy":0.40992, "omega":-2.03973, "ax":-6.74642, "ay":-5.93532, "alpha":-0.00003, "fx":[-114.7546,-114.75471,-114.75486,-114.75475], "fy":[-100.95827,-100.95814,-100.95797,-100.9581]}, - {"t":1.03679, "x":6.98725, "y":5.664, "heading":-0.24709, "vx":2.45637, "vy":0.31674, "omega":-2.03973, "ax":-6.44792, "ay":-6.26236, "alpha":0.27985, "fx":[-111.11005,-110.02327,-108.24429,-109.33138], "fy":[-105.03332,-106.18638,-107.99014,-106.87384]}, - {"t":1.05249, "x":7.02502, "y":5.6682, "heading":-0.27911, "vx":2.35514, "vy":0.21843, "omega":-2.03534, "ax":-6.18072, "ay":-6.52867, "alpha":0.51791, "fx":[-107.84708,-105.99429,-102.43153,-104.25656], "fy":[-108.44563,-110.2831,-113.58714,-111.88757]}, - {"t":1.06819, "x":7.06124, "y":5.67082, "heading":-0.31106, "vx":2.25811, "vy":0.11593, "omega":-2.02721, "ax":-5.94208, "ay":-6.74805, "alpha":0.72197, "fx":[-104.90641,-102.56039,-97.29253,-99.53333], "fy":[-111.34746,-113.54547,-118.07704,-116.15968]}, - {"t":1.08389, "x":7.09596, "y":5.67181, "heading":-0.34289, "vx":2.16482, "vy":0.00999, "omega":-2.01587, "ax":-5.72885, "ay":-6.93079, "alpha":0.89836, "fx":[-102.23707,-99.62456,-92.77215,-95.15082], "fy":[-113.85008,-116.18188,-121.71079,-119.82024]}, - {"t":1.09959, "x":7.12924, "y":5.67111, "heading":-0.37454, "vx":2.07488, "vy":-0.09882, "omega":-2.00177, "ax":-5.53795, "ay":-7.08462, "alpha":1.05206, "fx":[-99.79625,-97.10403,-88.80464,-91.09081], "fy":[-116.03602,-118.3408,-124.67833,-122.97428]}, - {"t":1.11529, "x":7.16113, "y":5.66869, "heading":-0.40597, "vx":1.98793, "vy":-0.21005, "omega":-1.98525, "ax":-5.36655, "ay":-7.21539, "alpha":1.18701, "fx":[-97.54837,-94.9299,-85.32406,-87.33185], "fy":[-117.96738,-120.12982,-127.12318,-125.70651]}, - {"t":1.13099, "x":7.19168, "y":5.6645, "heading":-0.43713, "vx":1.90368, "vy":-0.32332, "omega":-1.96661, "ax":-5.21217, "ay":-7.32757, "alpha":1.30629, "fx":[-95.46399,-93.04545,-82.26909,-83.8516], "fy":[-119.69152,-121.6282,-129.15414,-128.08546]}, - {"t":1.14669, "x":7.22092, "y":5.65852, "heading":-0.46801, "vx":1.82185, "vy":-0.43836, "omega":-1.94611, "ax":-5.07263, "ay":-7.4246, "alpha":1.41235, "fx":[-93.51885,-91.40407,-79.58483,-80.62814], "fy":[-121.245,-122.89533,-130.85427,-130.16675]}, - {"t":1.16239, "x":7.2489, "y":5.65073, "heading":-0.49856, "vx":1.74222, "vy":-0.55493, "omega":-1.92393, "ax":-4.94606, "ay":-7.50918, "alpha":1.50714, "fx":[-91.69291,-89.96748,-77.22321,-77.64072], "fy":[-122.65637,-123.97632,-132.28745,-131.99578]}, - {"t":1.17809, "x":7.27564, "y":5.64109, "heading":-0.52877, "vx":1.66457, "vy":-0.67282, "omega":-1.90027, "ax":-4.83086, "ay":-7.58342, "alpha":1.59221, "fx":[-89.9696,-88.70409,-75.14252,-74.87006], "fy":[-123.94812,-124.906,-133.50327,-133.6098]}, - {"t":1.19379, "x":7.30118, "y":5.62959, "heading":-0.5586, "vx":1.58872, "vy":-0.79187, "omega":-1.87528, "ax":-4.77003, "ay":-7.6292, "alpha":1.46229, "fx":[-88.08182,-87.4451,-74.86373,-74.15664], "fy":[-125.38652,-125.86796,-133.7341,-134.09317]}, - {"t":1.21315, "x":7.33105, "y":5.61282, "heading":-0.59492, "vx":1.49634, "vy":-0.93962, "omega":-1.84696, "ax":-4.74746, "ay":-7.64331, "alpha":1.40255, "fx":[-87.1684,-87.06488,-74.97031,-73.80834], "fy":[-126.00376,-126.11231,-133.65768,-134.26849]}, - {"t":1.23252, "x":7.35914, "y":5.59319, "heading":-0.63069, "vx":1.4044, "vy":-1.08765, "omega":-1.81979, "ax":-4.72237, "ay":-7.65885, "alpha":1.33631, "fx":[-86.19294,-86.58429,-75.04343,-73.48385], "fy":[-126.65178,-126.42175,-133.59797,-134.42789]}, - {"t":1.25189, "x":7.38545, "y":5.57069, "heading":-0.66593, "vx":1.31295, "vy":-1.23597, "omega":-1.79391, "ax":-4.6943, "ay":-7.67603, "alpha":1.26249, "fx":[-85.14834,-85.98812,-75.07575,-73.18227], "fy":[-127.33311,-126.80461,-133.5588,-134.57176]}, - {"t":1.27125, "x":7.41, "y":5.54532, "heading":-0.70067, "vx":1.22203, "vy":-1.38463, "omega":-1.76946, "ax":-4.66268, "ay":-7.69512, "alpha":1.17976, "fx":[-84.02562,-85.25723,-75.05826,-72.9026], "fy":[-128.05109,-127.27099,-133.54486,-134.70048]}, - {"t":1.29062, "x":7.43279, "y":5.51706, "heading":-0.73494, "vx":1.13173, "vy":-1.53366, "omega":-1.74662, "ax":-4.62683, "ay":-7.71646, "alpha":1.08644, "fx":[-82.81335,-84.36715,-74.97961,-72.64381], "fy":[-128.80996,-127.83324,-133.56191,-134.81431]}, - {"t":1.30999, "x":7.45384, "y":5.48591, "heading":-0.76877, "vx":1.04213, "vy":-1.6831, "omega":-1.72557, "ax":-4.58581, "ay":-7.74047, "alpha":0.98032, "fx":[-81.49694,-83.28622,-74.82523,-72.40475], "fy":[-129.61512,-128.50664,-133.61727,-134.9134]}, - {"t":1.32935, "x":7.47316, "y":5.45186, "heading":-0.80218, "vx":0.95332, "vy":-1.83301, "omega":-1.70659, "ax":-4.53844, "ay":-7.76765, "alpha":0.85856, "fx":[-80.05756,-81.97272,-74.57594,-72.1842], "fy":[-130.47344,-129.31021,-133.72033,-134.99777]}, - {"t":1.34872, "x":7.49078, "y":5.41491, "heading":-0.83523, "vx":0.86542, "vy":-1.98344, "omega":-1.68996, "ax":-4.48315, "ay":-7.79866, "alpha":0.71736, "fx":[-78.47065,-80.37076,-74.20588,-71.98084], "fy":[-131.39362,-130.26783,-133.88349,-135.0672]}, - {"t":1.36809, "x":7.5067, "y":5.37503, "heading":-0.86796, "vx":0.7786, "vy":-2.13447, "omega":-1.67607, "ax":-4.41777, "ay":-7.83436, "alpha":0.55158, "fx":[-76.70375,-78.40386,-73.67917,-71.79315], "fy":[-132.38678,-131.40969,-134.12343,-135.12122]}, - {"t":1.38745, "x":7.52095, "y":5.33222, "heading":-0.90042, "vx":0.69304, "vy":-2.2862, "omega":-1.66539, "ax":-4.33931, "ay":-7.87584, "alpha":0.3541, "fx":[-74.71318,-75.96487,-72.94444,-71.61942], "fy":[-133.4671,-132.77406,-134.4632,-135.15891]}, - {"t":1.40682, "x":7.53355, "y":5.28647, "heading":-0.93268, "vx":0.609, "vy":-2.43873, "omega":-1.65853, "ax":-4.24348, "ay":-7.92454, "alpha":0.11484, "fx":[-72.43904,-72.89967,-71.92545,-71.45755], "fy":[-134.65282,-134.40929,-134.93558,-135.17874]}, - {"t":1.42619, "x":7.54455, "y":5.23776, "heading":-0.9648, "vx":0.52682, "vy":-2.5922, "omega":-1.65631, "ax":-4.1239, "ay":-7.98234, "alpha":-0.18099, "fx":[-69.79716,-68.97951,-70.50396,-71.30489], "fy":[-135.96748,-136.37528,-135.58859,-135.17819]}, - {"t":1.44555, "x":7.55398, "y":5.18606, "heading":-0.99687, "vx":0.44696, "vy":-2.74679, "omega":-1.65981, "ax":-3.97072, "ay":-8.05175, "alpha":-0.55573, "fx":[-66.6658,-63.85279,-68.48654,-71.15787], "fy":[-137.44144,-138.74196,-136.49521,-135.15312]}, - {"t":1.46492, "x":7.56189, "y":5.13135, "heading":-1.02902, "vx":0.37006, "vy":-2.90272, "omega":-1.67057, "ax":-3.76793, "ay":-8.1359, "alpha":-1.04453, "fx":[-62.86277,-56.95732,-65.53457,-71.01128], "fy":[-139.11348,-141.57695,-137.7704,-135.09656]}, - {"t":1.48428, "x":7.56835, "y":5.07361, "heading":-1.06137, "vx":0.29709, "vy":-3.06029, "omega":-1.6908, "ax":-3.48796, "ay":-8.23832, "alpha":-1.70539, "fx":[-58.10385,-47.3559,-61.00051,-70.85685], "fy":[-141.03123,-144.89698,-139.60126,-134.9961]}, - {"t":1.50365, "x":7.57345, "y":5.0128, "heading":-1.09412, "vx":0.22954, "vy":-3.21984, "omega":-1.72383, "ax":-3.07941, "ay":-8.36088, "alpha":-2.63949, "fx":[-51.92297,-33.42804,-53.48801,-70.68024], "fy":[-143.24617,-148.49997,-142.2905,-134.82785]}, - {"t":1.52302, "x":7.57732, "y":4.94887, "heading":-1.1275, "vx":0.1699, "vy":-3.38176, "omega":-1.77495, "ax":-2.4361, "ay":-8.49428, "alpha":-4.04095, "fx":[-43.50848,-12.35664,-39.43081,-70.45347], "fy":[-145.7869,-151.40735,-146.20684,-134.53977]}, - {"t":1.54238, "x":7.58015, "y":4.88179, "heading":-1.16188, "vx":0.12272, "vy":-3.54626, "omega":-1.85321, "ax":-1.30253, "ay":-8.56531, "alpha":-6.40942, "fx":[-31.34575,20.1851,-7.34581,-70.11603], "fy":[-148.54769,-150.16031,-150.07282,-133.99302]}, - {"t":1.56175, "x":7.58229, "y":4.8115, "heading":-1.19777, "vx":0.09749, "vy":-3.71214, "omega":-1.97734, "ax":0.95463, "ay":-7.98683, "alpha":-12.33553, "fx":[-12.41536,66.85396,80.0284,-69.51485], "fy":[-150.8257,-135.50166,-124.42491,-132.66227]}, - {"t":1.58112, "x":7.58435, "y":4.73811, "heading":-1.23606, "vx":0.11598, "vy":-3.86682, "omega":-2.21623, "ax":2.70334, "ay":-7.55151, "alpha":-10.29534, "fx":[15.13176,81.00572,101.66692,-13.87231], "fy":[-146.93844,-123.54549,-100.87446,-142.43751]}, - {"t":1.59253, "x":7.58585, "y":4.69348, "heading":-1.26136, "vx":0.14684, "vy":-3.95302, "omega":-2.33375, "ax":3.35926, "ay":-7.56341, "alpha":-4.11078, "fx":[39.79837,72.1987,78.91136,37.65206], "fy":[-137.99888,-124.41104,-116.83789,-135.35777]}, - {"t":1.60395, "x":7.58775, "y":4.64787, "heading":-1.288, "vx":0.18518, "vy":-4.03935, "omega":-2.38067, "ax":4.13743, "ay":-6.49304, "alpha":6.92208, "fx":[104.91411,43.89396,43.68257,89.01504], "fy":[-82.15237,-121.3311,-131.07079,-107.22502]}, - {"t":1.61536, "x":7.59013, "y":4.60134, "heading":-1.31517, "vx":0.23241, "vy":-4.11346, "omega":-2.30166, "ax":3.75251, "ay":-5.53136, "alpha":12.32696, "fx":[118.21876,14.47721,27.70284,94.91787], "fy":[-41.66221,-106.7199,-130.62342,-97.34203]}, - {"t":1.62678, "x":7.59303, "y":4.55402, "heading":-1.34144, "vx":0.27524, "vy":-4.1766, "omega":-2.16095, "ax":3.13203, "ay":-5.16191, "alpha":12.22075, "fx":[104.62022,3.74261,19.49412,85.24292], "fy":[-41.54234,-91.26985,-123.15646,-95.24193]}, - {"t":1.63819, "x":7.59638, "y":4.50601, "heading":-1.36611, "vx":0.31099, "vy":-4.23552, "omega":-2.02146, "ax":1.85444, "ay":-4.39469, "alpha":10.56598, "fx":[70.06367,-11.06618,5.10044,62.07599], "fy":[-41.83161,-65.95829,-104.03023,-87.18941]}, - {"t":1.6496, "x":7.60005, "y":4.45738, "heading":-1.38918, "vx":0.33216, "vy":-4.28569, "omega":-1.90085, "ax":-1.727, "ay":-1.31665, "alpha":1.69968, "fx":[-26.05443,-34.8193,-32.67677,-23.95272], "fy":[-17.147,-18.43958,-27.52687,-26.47024]}, - {"t":1.66102, "x":7.60372, "y":4.40838, "heading":-1.41088, "vx":0.31245, "vy":-4.30071, "omega":-1.88145, "ax":-4.14425, "ay":2.49624, "alpha":-9.76172, "fx":[-97.22052,-48.69401,-45.25803,-90.79766], "fy":[5.36165,23.012,82.88878,58.57921]}, - {"t":1.67243, "x":7.60702, "y":4.35945, "heading":-1.43236, "vx":0.26514, "vy":-4.27222, "omega":-1.99288, "ax":-4.66589, "ay":3.52699, "alpha":-12.919, "fx":[-120.74274,-54.57725,-39.08546,-103.0562], "fy":[6.36422,41.30379,114.4266,77.87763]}, - {"t":1.68385, "x":7.60974, "y":4.31091, "heading":-1.4551, "vx":0.21188, "vy":-4.23196, "omega":-2.14034, "ax":-4.9239, "ay":3.90259, "alpha":-14.12473, "fx":[-131.30189,-61.25389,-34.81976,-107.64121], "fy":[4.9043,48.65119,126.70395,85.26862]}, - {"t":1.69526, "x":7.61184, "y":4.26286, "heading":-1.47953, "vx":0.15568, "vy":-4.18742, "omega":-2.30157, "ax":-5.13224, "ay":4.05421, "alpha":-14.65313, "fx":[-137.13072,-70.3992,-31.87272,-109.78937], "fy":[3.8821,49.44459,133.1366,89.38069]}, - {"t":1.70668, "x":7.61328, "y":4.21533, "heading":-1.50581, "vx":0.0971, "vy":-4.14114, "omega":-2.46883, "ax":-5.33101, "ay":4.10838, "alpha":-14.86048, "fx":[-140.74178,-81.6075,-29.67865,-110.68757], "fy":[3.91183,46.18919,137.08025,92.34819]}, - {"t":1.71809, "x":7.61405, "y":4.16833, "heading":-1.53399, "vx":0.03625, "vy":-4.09424, "omega":-2.63845, "ax":-5.51, "ay":4.1497, "alpha":-14.85406, "fx":[-143.10206,-93.16027,-27.97075,-110.6613], "fy":[5.40081,42.1883,139.73176,95.0201]}, - {"t":1.72951, "x":7.6141, "y":4.12186, "heading":-1.5641, "vx":-0.02665, "vy":-4.04688, "omega":-2.808, "ax":-5.62325, "ay":4.41869, "alpha":-14.12861, "fx":[-144.3395,-101.70937,-27.38105,-109.1698], "fy":[11.21533,49.51466,141.47514,98.43744]}, - {"t":1.74092, "x":7.61343, "y":4.07596, "heading":-1.59615, "vx":-0.09083, "vy":-3.99644, "omega":-2.96927, "ax":-5.52288, "ay":5.06339, "alpha":-12.59951, "fx":[-143.62377,-99.85117,-27.22155,-105.07407], "fy":[24.24476,73.78157,142.68873,103.79188]}, - {"t":1.75234, "x":7.61203, "y":4.03067, "heading":-1.63005, "vx":-0.15387, "vy":-3.93864, "omega":-3.11309, "ax":-5.3336, "ay":5.44526, "alpha":-12.13827, "fx":[-142.69885,-96.04766,-23.16476,-100.98051], "fy":[32.55242,85.14403,144.25966,108.53328]}, - {"t":1.76375, "x":7.60993, "y":3.98607, "heading":-1.66558, "vx":-0.21475, "vy":-3.87649, "omega":-3.25164, "ax":-5.16765, "ay":5.69349, "alpha":-11.99593, "fx":[-141.87685,-94.0376,-18.43995,-97.24635], "fy":[38.4154,90.86106,145.59935,112.50306]}, - {"t":1.77516, "x":7.60714, "y":3.94219, "heading":-1.7027, "vx":-0.27374, "vy":-3.8115, "omega":-3.38857, "ax":-5.03857, "ay":5.88717, "alpha":-11.85997, "fx":[-140.88187,-93.77249,-14.4427,-93.72126], "fy":[43.78668,94.23826,146.57793,115.95351]}, - {"t":1.78658, "x":7.60369, "y":3.89907, "heading":-1.74138, "vx":-0.33125, "vy":-3.7443, "omega":-3.52395, "ax":-4.94492, "ay":6.04184, "alpha":-11.70775, "fx":[-139.69692,-95.05083,-11.30087,-90.39835], "fy":[48.90809,95.91485,147.28311,118.97349]}, - {"t":1.79799, "x":7.59959, "y":3.85672, "heading":-1.7816, "vx":-0.3877, "vy":-3.67534, "omega":-3.65759, "ax":-4.88725, "ay":6.14001, "alpha":-11.65641, "fx":[-138.6249,-98.06107,-8.45864,-87.37804], "fy":[53.09592,95.28011,147.82381,121.55969]}, - {"t":1.80941, "x":7.59484, "y":3.81517, "heading":-1.82335, "vx":-0.44348, "vy":-3.60525, "omega":-3.79064, "ax":-4.69043, "ay":6.45717, "alpha":-11.75548, "fx":[-138.79694,-98.10998,0.44287,-82.66765], "fy":[58.35648,103.89078,150.35313,126.73834]}, - {"t":1.82711, "x":7.58626, "y":3.75236, "heading":-1.89046, "vx":-0.52652, "vy":-3.49094, "omega":-3.99874, "ax":-4.24835, "ay":6.77148, "alpha":-11.84044, "fx":[-135.20701,-91.63105,11.73113,-73.94569], "fy":[67.09612,110.98422,150.30076,132.34274]}, - {"t":1.84481, "x":7.57627, "y":3.69162, "heading":-1.96125, "vx":-0.60172, "vy":-3.37107, "omega":-4.20836, "ax":-3.94805, "ay":6.97903, "alpha":-11.85882, "fx":[-131.22146,-90.9112,19.70354,-66.19136], "fy":[75.22866,113.22656,149.75934,136.63096]}, - {"t":1.86252, "x":7.565, "y":3.63303, "heading":-2.03575, "vx":-0.67162, "vy":-3.24752, "omega":-4.41829, "ax":-3.74007, "ay":7.1293, "alpha":-11.8406, "fx":[-126.75447,-93.53197,24.98375,-59.1676], "fy":[83.02114,112.85613,149.18825,140.0041]}, - {"t":1.88022, "x":7.55252, "y":3.57666, "heading":-2.11396, "vx":-0.73783, "vy":-3.12131, "omega":-4.6279, "ax":-3.56329, "ay":7.28883, "alpha":-11.6184, "fx":[-121.23562,-95.59066,27.07435,-52.69015], "fy":[91.26126,112.95835,148.99062,142.71337]}, - {"t":1.89792, "x":7.5389, "y":3.52255, "heading":-2.19589, "vx":-0.80091, "vy":-2.99227, "omega":-4.83358, "ax":-3.25964, "ay":7.71166, "alpha":-9.74979, "fx":[-109.0612,-84.74892,18.37386,-46.34564], "fy":[105.75142,123.47563,150.47523,144.99025]}, - {"t":1.91563, "x":7.52421, "y":3.47078, "heading":-2.28146, "vx":-0.85861, "vy":-2.85575, "omega":-5.00618, "ax":-2.8963, "ay":8.19801, "alpha":-6.47433, "fx":[-88.19746,-66.94047,-0.9214,-41.00177], "fy":[123.85954,135.47961,151.80372,146.64022]}, - {"t":1.93333, "x":7.50856, "y":3.42151, "heading":-2.37008, "vx":-0.90989, "vy":-2.71063, "omega":-5.1208, "ax":-2.60842, "ay":8.47855, "alpha":-3.58272, "fx":[-67.15755,-53.99631,-18.40565,-37.9141], "fy":[136.56676,141.87747,150.91679,147.50967]}, - {"t":1.95103, "x":7.49204, "y":3.37486, "heading":-2.46074, "vx":-0.95606, "vy":-2.56053, "omega":-5.18422, "ax":-2.37089, "ay":8.62347, "alpha":-1.25778, "fx":[-48.61241,-43.86179,-31.60658,-37.2318], "fy":[144.33768,145.73955,148.93018,147.72393]}, - {"t":1.96873, "x":7.47475, "y":3.33088, "heading":-2.55251, "vx":-0.99803, "vy":-2.40787, "omega":-5.20649, "ax":-2.17092, "ay":8.69162, "alpha":0.58234, "fx":[-33.02217,-35.16705,-40.72783,-38.78982], "fy":[148.78143,148.33245,146.88783,147.36602]}, - {"t":1.98644, "x":7.45674, "y":3.28961, "heading":-2.64468, "vx":-1.03647, "vy":-2.254, "omega":-5.19618, "ax":-2.00287, "ay":8.71645, "alpha":2.0701, "fx":[-20.17077,-27.29321,-46.56606,-42.24322], "fy":[151.1407,150.15243,145.30176,146.46246]}, - {"t":2.00414, "x":7.43808, "y":3.25108, "heading":-2.73667, "vx":-1.07192, "vy":-2.0997, "omega":-5.15953, "ax":-1.86182, "ay":8.71566, "alpha":3.32054, "fx":[-9.70187,-19.95235,-49.91345,-47.10874], "fy":[152.23388,151.42527,144.31727,145.02677]}, - {"t":2.02184, "x":7.41881, "y":3.21527, "heading":-2.82801, "vx":-1.10488, "vy":-1.9454, "omega":-5.10075, "ax":-1.742, "ay":8.69893, "alpha":4.40877, "fx":[-1.29292,-13.00499,-51.406,-52.81947], "fy":[152.58535,152.27055,143.89972,143.10933]}, - {"t":2.03955, "x":7.39898, "y":3.1822, "heading":-2.91831, "vx":-1.13572, "vy":-1.79141, "omega":-5.0227, "ax":-1.63735, "ay":8.67254, "alpha":5.36999, "fx":[5.3042,-6.38273,-51.52044,-58.80457], "fy":[152.53536,152.76161,143.94667,140.82584]}, - {"t":2.05725, "x":7.37861, "y":3.15184, "heading":-3.00722, "vx":-1.16471, "vy":-1.63788, "omega":-4.92764, "ax":-1.54277, "ay":8.64141, "alpha":6.21068, "fx":[10.26586,-0.05298,-50.60813,-64.57338], "fy":[152.30941,152.95013,144.3422,138.35009]}, - {"t":2.07495, "x":7.35775, "y":3.1242, "heading":-3.09446, "vx":-1.19202, "vy":-1.4849, "omega":-4.81769, "ax":-1.45495, "ay":8.60968, "alpha":6.92364, "fx":[13.71033,5.9985,-48.92941,-69.77281], "fy":[152.05801,152.87689,144.97961,135.87814]}, - {"t":2.09265, "x":7.33642, "y":3.09926, "heading":3.10344, "vx":-1.21778, "vy":-1.33248, "omega":-4.69512, "ax":-1.37263, "ay":8.58054, "alpha":7.50084, "fx":[15.7122,11.77695,-46.67997,-74.20147], "fy":[151.87808,152.5769,145.76923,133.58614]}, - {"t":2.11036, "x":7.31465, "y":3.07702, "heading":3.02032, "vx":-1.24207, "vy":-1.18058, "omega":-4.56233, "ax":-1.29628, "ay":8.55609, "alpha":7.94132, "fx":[16.31524,17.28382,-44.00912,-77.78747], "fy":[151.82359,152.08164,146.63951,131.60187]}, - {"t":2.12806, "x":7.29246, "y":3.05746, "heading":2.93956, "vx":-1.26502, "vy":-1.02911, "omega":-4.42175, "ax":-1.2275, "ay":8.53721, "alpha":8.25409, "fx":[15.54441,22.51951,-41.03231,-80.54922], "fy":[151.91003,151.42013,147.53555,129.99657]}, - {"t":2.14576, "x":7.26987, "y":3.04058, "heading":2.86128, "vx":-1.28675, "vy":-0.87798, "omega":-4.27563, "ax":-1.16835, "ay":8.52372, "alpha":8.45767, "fx":[13.41828,27.48474,-37.8397,-82.55673], "fy":[152.11587,150.61934,148.41666,128.79203]}, - {"t":2.16347, "x":7.24691, "y":3.02637, "heading":2.78559, "vx":-1.30744, "vy":-0.72708, "omega":-4.1259, "ax":-1.12083, "ay":8.51449, "alpha":8.57816, "fx":[9.96332,32.18095,-34.50216,-83.90233], "fy":[152.38347,149.70441,149.25379,127.97461]}, - {"t":2.18117, "x":7.22359, "y":3.01483, "heading":2.71255, "vx":-1.32728, "vy":-0.57635, "omega":-3.97404, "ax":-1.0864, "ay":8.50774, "alpha":8.64668, "fx":[5.23042,36.61016,-31.07589,-84.68208], "fy":[152.62187,148.69881,150.02708,127.50924]}, - {"t":2.19887, "x":7.19992, "y":3.00596, "heading":2.6422, "vx":-1.34651, "vy":-0.42574, "omega":-3.82097, "ax":-1.06563, "ay":8.50122, "alpha":8.69661, "fx":[-0.68694,40.77455,-27.60583,-84.98604], "fy":[152.71422,147.6246,150.72378,127.35062]}, - {"t":2.21658, "x":7.17592, "y":2.99976, "heading":2.57455, "vx":-1.36538, "vy":-0.27524, "omega":-3.66702, "ax":-1.05797, "ay":8.49253, "alpha":8.76047, "fx":[-7.63658,44.67604,-24.12858,-84.89419], "fy":[152.53197,146.50263,151.33656,127.45064]}, - {"t":2.23428, "x":7.15158, "y":2.99622, "heading":2.50964, "vx":-1.3841, "vy":-0.1249, "omega":-3.51193, "ax":-1.06165, "ay":8.47947, "alpha":8.86633, "fx":[-15.39886,48.316,-20.67462,-84.47582], "fy":[151.95602,145.35281,151.86214,127.7626]}, - {"t":2.25198, "x":7.12691, "y":2.99533, "heading":2.44746, "vx":-1.4029, "vy":0.02521, "omega":-3.35497, "ax":-1.07376, "ay":8.46045, "alpha":9.03424, "fx":[-23.69169,51.69526,-17.27024,-83.79065], "fy":[150.90172,144.19425,152.30032,128.24328]}, - {"t":2.26968, "x":7.10191, "y":2.99711, "heading":2.38807, "vx":-1.42191, "vy":0.17498, "omega":-3.19504, "ax":-1.04227, "ay":8.49759, "alpha":8.78141, "fx":[-29.33829,51.46739,-13.66717,-79.37678], "fy":[150.05635,144.3226,152.70181,131.08559]}, - {"t":2.29045, "x":7.07215, "y":3.00257, "heading":2.32171, "vx":-1.44356, "vy":0.35147, "omega":-3.01265, "ax":-0.96229, "ay":8.49967, "alpha":8.87461, "fx":[-34.45066,54.17917,-9.1421,-76.05964], "fy":[148.9465,143.30347,153.02872,133.02934]}, - {"t":2.31122, "x":7.04196, "y":3.01171, "heading":2.25914, "vx":-1.46354, "vy":0.52801, "omega":-2.82833, "ax":-0.86592, "ay":8.50128, "alpha":8.98846, "fx":[-38.35782,56.73499,-4.70443,-72.58903], "fy":[147.96977,142.2859,153.21759,134.94441]}, - {"t":2.33199, "x":7.01138, "y":3.02451, "heading":2.2004, "vx":-1.48153, "vy":0.70457, "omega":-2.64165, "ax":-0.75235, "ay":8.50384, "alpha":9.09958, "fx":[-41.03981,59.16418,-0.34806,-68.96527], "fy":[147.22929,141.26604,153.27642,136.82001]}, - {"t":2.35276, "x":6.98044, "y":3.04097, "heading":2.14553, "vx":-1.49715, "vy":0.88119, "omega":-2.45266, "ax":-0.62096, "ay":8.5082, "alpha":9.18936, "fx":[-42.50485,61.50065,3.93665,-65.18173], "fy":[146.7924,140.23552,153.21191,138.64867]}, - {"t":2.37353, "x":6.94922, "y":3.06111, "heading":2.09459, "vx":-1.51005, "vy":1.0579, "omega":-2.2618, "ax":-0.47098, "ay":8.51467, "alpha":9.24395, "fx":[-42.76858,63.78264,8.16403,-61.22318], "fy":[146.69277,139.1812,153.02899,140.42572]}, - {"t":2.3943, "x":6.91775, "y":3.08492, "heading":2.04762, "vx":-1.51983, "vy":1.23475, "omega":-2.06981, "ax":-0.30122, "ay":8.52305, "alpha":9.2536, "fx":[-41.8377,66.05204,12.35423,-57.06349], "fy":[146.93478,138.08519,152.73018,142.14865]}, - {"t":2.41507, "x":6.88612, "y":3.1124, "heading":2.00463, "vx":-1.52609, "vy":1.41176, "omega":-1.87762, "ax":-0.10983, "ay":8.53267, "alpha":9.21148, "fx":[-39.69784,68.35338,16.5344,-52.66276], "fy":[147.49723,136.92486,152.31467,143.81621]}, - {"t":2.43584, "x":6.8544, "y":3.14356, "heading":1.96563, "vx":-1.52837, "vy":1.58898, "omega":-1.6863, "ax":0.10588, "ay":8.54234, "alpha":9.11254, "fx":[-36.30512,70.73258,20.74014,-47.96346], "fy":[148.33425,135.67295,151.77717,145.42692]}, - {"t":2.45661, "x":6.82268, "y":3.17841, "heading":1.93061, "vx":-1.52617, "vy":1.7664, "omega":-1.49704, "ax":0.3496, "ay":8.5503, "alpha":8.95261, "fx":[-31.5809,73.23569,25.01725,-42.8854], "fy":[149.37193,134.2976,151.10629,146.97686]}, - {"t":2.47738, "x":6.79106, "y":3.21694, "heading":1.89952, "vx":-1.51891, "vy":1.94399, "omega":-1.3111, "ax":0.62615, "ay":8.55394, "alpha":8.7277, "fx":[-25.41019,75.90766,29.42409,-37.31875], "fy":[150.49986,132.76219,150.28226,148.45599]}, - {"t":2.49815, "x":6.75965, "y":3.25916, "heading":1.87229, "vx":-1.5059, "vy":2.12165, "omega":-1.12983, "ax":0.9416, "ay":8.54948, "alpha":8.43373, "fx":[-17.64597,78.79139,34.03475,-31.11464], "fy":[151.55653,131.02485,149.27359,149.84204]}, - {"t":2.51892, "x":6.72857, "y":3.30507, "heading":1.84882, "vx":-1.48635, "vy":2.29921, "omega":-0.95467, "ax":1.30329, "ay":8.53143, "alpha":8.06671, "fx":[-8.12377,81.92696,38.94328,-24.07239], "fy":[152.30918,129.0376,148.03197,151.09004]}, - {"t":2.53969, "x":6.69798, "y":3.35466, "heading":1.82899, "vx":-1.45928, "vy":2.4764, "omega":-0.78713, "ax":1.71966, "ay":8.49183, "alpha":7.62317, "fx":[3.30602,85.3512,44.26937,-15.92286], "fy":[152.43124,126.74496,146.48443,152.11404]}, - {"t":2.56045, "x":6.66805, "y":3.40793, "heading":1.81264, "vx":-1.42356, "vy":2.65277, "omega":-0.6288, "ax":2.19983, "ay":8.41938, "alpha":7.10052, "fx":[16.71948,89.0973,50.16551,-6.30865], "fy":[151.48712,124.08197,144.52087,152.75502]}, - {"t":2.58122, "x":6.63895, "y":3.46484, "heading":1.79958, "vx":-1.37787, "vy":2.82764, "omega":-0.48133, "ax":2.75259, "ay":8.29841, "alpha":6.49599, "fx":[32.02992,93.19433,56.82563,5.23347], "fy":[148.94426,120.9715,141.97394,152.72459]}, - {"t":2.60199, "x":6.61093, "y":3.52536, "heading":1.78959, "vx":-1.3207, "vy":2.99999, "omega":-0.34641, "ax":3.38499, "ay":8.108, "alpha":5.8022, "fx":[48.88225,97.66618,64.49333,19.2694], "fy":[144.24171,117.32085,138.58626,151.51037]}, - {"t":2.62276, "x":6.58423, "y":3.58941, "heading":1.78239, "vx":-1.2504, "vy":3.16839, "omega":-0.2259, "ax":4.10014, "ay":7.82116, "alpha":4.99805, "fx":[66.58044,102.52953,73.46462,36.39395], "fy":[136.93574,113.0176,133.95696,148.23277]}, - {"t":2.64353, "x":6.55914, "y":3.65691, "heading":1.7777, "vx":-1.16524, "vy":3.33083, "omega":-0.12209, "ax":4.89373, "ay":7.40409, "alpha":4.03929, "fx":[84.12706,107.79007,84.06922,56.97749], "fy":[126.89486,107.9246,127.45698,141.48964]}, - {"t":2.6643, "x":6.536, "y":3.72768, "heading":1.77516, "vx":-1.0636, "vy":3.48461, "omega":-0.0382, "ax":5.74728, "ay":6.81685, "alpha":2.8659, "fx":[100.42407,113.43611,96.59046,80.58777], "fy":[114.44228,101.87461,118.10787,129.38586]}, - {"t":2.68507, "x":6.51515, "y":3.80153, "heading":1.77437, "vx":-0.94424, "vy":3.62619, "omega":0.02132, "ax":6.61704, "ay":6.02112, "alpha":1.44614, "fx":[114.56934,119.42726,111.03546,105.18402], "fy":[100.32471,94.66628,104.4595,110.21963]}, - {"t":2.70584, "x":6.49696, "y":3.87814, "heading":1.77481, "vx":-0.8068, "vy":3.75124, "omega":0.05136, "ax":6.98601, "ay":5.49592, "alpha":2.1922, "fx":[122.51306,127.9345,116.33456,108.53803], "fy":[89.00575,81.55356,97.55202,105.82502]}, - {"t":2.72069, "x":6.48575, "y":3.93446, "heading":1.77558, "vx":-0.70304, "vy":3.83287, "omega":0.08392, "ax":6.92058, "ay":5.43188, "alpha":3.7133, "fx":[124.56245,132.42449,114.20236,99.67884], "fy":[84.29794,72.80107,99.28648,113.19371]}, - {"t":2.73555, "x":6.47607, "y":3.99199, "heading":1.77682, "vx":-0.60026, "vy":3.91355, "omega":0.13907, "ax":6.72159, "ay":5.21602, "alpha":7.06392, "fx":[129.59028,139.55321,110.24048,77.9452], "fy":[70.4135,54.92124,102.36073,127.19665]}, - {"t":2.7504, "x":6.4679, "y":4.05069, "heading":1.77889, "vx":-0.50043, "vy":3.99102, "omega":0.24398, "ax":4.85938, "ay":2.51993, "alpha":24.75897, "fx":[89.77287,147.20494,100.50065,-6.85194], "fy":[-87.79161,5.42677,109.14289,144.67543]}, - {"t":2.76525, "x":6.461, "y":4.11024, "heading":1.78251, "vx":-0.42825, "vy":4.02845, "omega":0.61171, "ax":-1.57071, "ay":-1.08947, "alpha":36.32364, "fx":[-90.89136,77.10617,49.2269,-142.31101], "fy":[-115.85105,-122.0867,130.43414,33.37741]}, - {"t":2.7801, "x":6.45447, "y":4.16995, "heading":1.7916, "vx":-0.45158, "vy":4.01226, "omega":1.1512, "ax":-6.24407, "ay":-4.47429, "alpha":13.87866, "fx":[-104.26681,-33.63514,-140.19478,-146.74255], "fy":[-108.31657,-144.49036,-21.90286,-29.71603]}, - {"t":2.79495, "x":6.44707, "y":4.22905, "heading":1.8087, "vx":-0.54432, "vy":3.94581, "omega":1.35733, "ax":-6.66993, "ay":-5.19, "alpha":8.53599, "fx":[-108.32206,-68.24914,-134.82678,-142.4162], "fy":[-105.78126,-133.9936,-62.94506,-50.40207]}, - {"t":2.80981, "x":6.43825, "y":4.28708, "heading":1.82886, "vx":-0.64339, "vy":3.86873, "omega":1.48411, "ax":-6.81396, "ay":-5.35503, "alpha":6.57372, "fx":[-110.17117,-82.15716,-131.96545,-139.32036], "fy":[-104.62785,-127.04785,-72.57016,-60.10432]}, - {"t":2.82466, "x":6.42795, "y":4.34395, "heading":1.8509, "vx":-0.74459, "vy":3.78919, "omega":1.58175, "ax":-6.88439, "ay":-5.42405, "alpha":5.52545, "fx":[-111.16045,-89.45888,-130.59895,-137.18789], "fy":[-104.04455,-122.65111,-76.62702,-65.72319]}, - {"t":2.83951, "x":6.41613, "y":4.39963, "heading":1.87439, "vx":-0.84684, "vy":3.70863, "omega":1.66382, "ax":-6.92517, "ay":-5.46138, "alpha":4.871, "fx":[-111.72504,-93.94878,-129.86723,-135.63926], "fy":[-103.75167,-119.65727,-78.75859,-69.4183]}, - {"t":2.85436, "x":6.40279, "y":4.45411, "heading":1.8991, "vx":-0.94969, "vy":3.62752, "omega":1.73616, "ax":-6.95138, "ay":-5.4847, "alpha":4.42316, "fx":[-112.04843,-97.00527,-129.45619,-134.4541], "fy":[-103.6268,-117.47993,-80.00224,-72.06344]}, - {"t":2.86922, "x":6.38791, "y":4.50738, "heading":1.92489, "vx":-1.05294, "vy":3.54606, "omega":1.80186, "ax":-6.96949, "ay":-5.50066, "alpha":4.09731, "fx":[-112.22145,-99.24005,-129.22824,-133.50611], "fy":[-103.60783,-115.81006,-80.76417,-74.07645]}, - {"t":2.88407, "x":6.37151, "y":4.55944, "heading":1.95165, "vx":-1.15645, "vy":3.46436, "omega":1.86271, "ax":-6.98265, "ay":-5.5123, "alpha":3.84949, "fx":[-112.29427,-100.96414,-129.11396,-132.71943], "fy":[-103.65984,-114.47283,-81.23602,-75.68167]}, - {"t":2.89892, "x":6.35356, "y":4.61029, "heading":1.97932, "vx":-1.26016, "vy":3.38249, "omega":1.91988, "ax":-6.99262, "ay":-5.52118, "alpha":3.65461, "fx":[-112.29692,-102.35195,-129.07451,-132.04617], "fy":[-103.76157,-113.36267,-81.52025,-77.01012]}, - {"t":2.91377, "x":6.33407, "y":4.65992, "heading":2.00783, "vx":-1.36402, "vy":3.30049, "omega":1.97416, "ax":-7.00039, "ay":-5.52819, "alpha":3.49726, "fx":[-112.24859,-103.5087,-129.08638,-131.45472], "fy":[-103.89928,-112.41226,-81.67678,-78.14346]}, - {"t":2.92863, "x":6.31304, "y":4.70833, "heading":2.03715, "vx":-1.46799, "vy":3.21838, "omega":2.02611, "ax":-7.00661, "ay":-5.53388, "alpha":3.36747, "fx":[-112.16215,-104.50168,-129.13437,-130.92339], "fy":[-104.06361,-111.57663,-81.74323,-79.13521]}, - {"t":2.94348, "x":6.29047, "y":4.75552, "heading":2.06724, "vx":-1.57205, "vy":3.13619, "omega":2.07612, "ax":-7.01169, "ay":-5.53858, "alpha":3.25848, "fx":[-112.04663,-105.37587,-129.20816,-130.43684], "fy":[-104.24791,-110.82459,-81.74451,-80.02192]}, - {"t":2.95833, "x":6.26634, "y":4.80149, "heading":2.09808, "vx":-1.67619, "vy":3.05393, "omega":2.12452, "ax":-7.01592, "ay":-5.54254, "alpha":3.16557, "fx":[-111.90862,-106.16253,-129.30042,-129.98386], "fy":[-104.44725,-110.13381,-81.69796,-80.82939]}, - {"t":2.97318, "x":6.24067, "y":4.84624, "heading":2.12963, "vx":-1.7804, "vy":2.97161, "omega":2.17153, "ax":-7.0195, "ay":-5.54592, "alpha":3.08532, "fx":[-111.7531,-106.88407,-129.40574,-129.55611], "fy":[-104.65785,-109.48782,-81.61608,-81.57633]}, - {"t":2.98804, "x":6.21346, "y":4.88976, "heading":2.16189, "vx":-1.88465, "vy":2.88924, "omega":2.21736, "ax":-7.02258, "ay":-5.54883, "alpha":3.01519, "fx":[-111.58397,-107.55693,-129.51999,-129.14723], "fy":[-104.87671,-108.87418,-81.50827,-82.27667]}, - {"t":3.00289, "x":6.18469, "y":4.93206, "heading":2.19482, "vx":-1.98895, "vy":2.80682, "omega":2.26214, "ax":-7.02406, "ay":-5.56763, "alpha":2.71253, "fx":[-111.94042,-109.13574,-128.75884,-128.0743], "fy":[-104.56525,-107.38496,-82.83663,-84.02812]}, - {"t":3.0197, "x":6.15026, "y":4.97847, "heading":2.23285, "vx":-2.10706, "vy":2.71321, "omega":2.30775, "ax":-7.00879, "ay":-5.59243, "alpha":2.55564, "fx":[-111.70497,-109.79259,-128.23103,-127.14131], "fy":[-104.78925,-106.68672,-83.6238,-85.40303]}, - {"t":3.03651, "x":6.11384, "y":5.02329, "heading":2.27166, "vx":-2.2249, "vy":2.61918, "omega":2.35072, "ax":-6.99183, "ay":-5.61919, "alpha":2.38464, "fx":[-111.52739,-110.44976,-127.59012,-126.14912], "fy":[-104.94804,-105.9778,-84.56875,-86.8289]}, - {"t":3.05333, "x":6.07544, "y":5.06654, "heading":2.31118, "vx":-2.34246, "vy":2.5247, "omega":2.39081, "ax":-6.97291, "ay":-5.64814, "alpha":2.19797, "fx":[-111.41815,-111.09768,-126.82054,-125.09279], "fy":[-105.03071,-105.2679,-85.68642,-88.30774]}, - {"t":3.07014, "x":6.03507, "y":5.10819, "heading":2.35138, "vx":-2.4597, "vy":2.42974, "omega":2.42777, "ax":-6.95168, "ay":-5.67954, "alpha":1.99364, "fx":[-111.38907,-111.72524,-125.9036,-123.96635], "fy":[-105.02467,-104.56889,-86.99334,-89.84223]}, - {"t":3.08696, "x":5.99273, "y":5.14824, "heading":2.3922, "vx":-2.57658, "vy":2.33424, "omega":2.46129, "ax":-6.92769, "ay":-5.71371, "alpha":1.76921, "fx":[-111.45351,-112.31969,-124.81666,-122.76245], "fy":[-104.91524,-103.89494,-88.50797,-91.43603]}, - {"t":3.10377, "x":5.94843, "y":5.18668, "heading":2.43358, "vx":-2.69306, "vy":2.23817, "omega":2.49104, "ax":-6.90042, "ay":-5.75102, "alpha":1.52156, "fx":[-111.62663,-112.86649,-123.53193,-121.47189], "fy":[-104.68507,-103.26276,-90.25116,-93.09411]}, - {"t":3.12058, "x":5.90217, "y":5.2235, "heading":2.47547, "vx":-2.80908, "vy":2.14148, "omega":2.51662, "ax":-6.8692, "ay":-5.79191, "alpha":1.24671, "fx":[-111.92573,-113.34916,-122.01486,-120.08283], "fy":[-104.31347,-102.69176,-92.24668,-94.82329]}, - {"t":3.1374, "x":5.85397, "y":5.25869, "heading":2.51778, "vx":-2.92458, "vy":2.04409, "omega":2.53758, "ax":-6.83318, "ay":-5.83688, "alpha":0.9396, "fx":[-112.3706,-113.74911,-120.22202,-118.57976], "fy":[-103.7754,-102.20423,-94.52178,-96.63291]}, - {"t":3.15421, "x":5.80383, "y":5.29223, "heading":2.56045, "vx":-3.03947, "vy":1.94596, "omega":2.55338, "ax":-6.79126, "ay":-5.88648, "alpha":0.59372, "fx":[-112.98405,-114.04528,-118.09807,-116.94192], "fy":[-103.04012,-101.82545,-97.10782,-98.53589]}, - {"t":3.17102, "x":5.75177, "y":5.32412, "heading":2.60338, "vx":-3.15366, "vy":1.84698, "omega":2.56336, "ax":-6.74201, "ay":-5.94137, "alpha":0.20074, "fx":[-113.79248,-114.21379,-115.57176,-115.14081], "fy":[-102.06934,-101.58387,-100.04075,-100.55024]}, - {"t":3.18784, "x":5.69779, "y":5.35433, "heading":2.64648, "vx":-3.26702, "vy":1.74709, "omega":2.56674, "ax":-6.68354, "ay":-6.00228, "alpha":-0.25001, "fx":[-114.82661,-114.22735,-112.55021,-113.13635], "fy":[-100.81458,-101.5111,-103.36126,-102.70122]}, - {"t":3.20465, "x":5.64192, "y":5.38286, "heading":2.68963, "vx":-3.37939, "vy":1.64616, "omega":2.56253, "ax":-6.61326, "ay":-6.06998, "alpha":-0.77229, "fx":[-116.12232,-114.05457,-108.9112,-110.8706], "fy":[-99.2134,-101.64196,-107.11401,-105.02479]}, - {"t":3.22147, "x":5.58416, "y":5.40968, "heading":2.73272, "vx":-3.49058, "vy":1.54411, "omega":2.54955, "ax":-6.5276, "ay":-6.14525, "alpha":-1.38379, "fx":[-117.72151,-113.65901,-104.49268,-108.25717], "fy":[-97.18402,-102.0142,-111.34487,-107.57282]}, - {"t":3.23828, "x":5.52455, "y":5.43477, "heading":2.77559, "vx":-3.60034, "vy":1.44078, "omega":2.52628, "ax":-6.42153, "ay":-6.2288, "alpha":-2.10739, "fx":[-119.67277,-112.99829,-99.07953,-105.16266], "fy":[-94.61741,-102.66789,-116.09392,-110.42127]}, - {"t":3.25509, "x":5.46311, "y":5.45812, "heading":2.81806, "vx":-3.70831, "vy":1.33605, "omega":2.49085, "ax":-6.28782, "ay":-6.32098, "alpha":-2.97265, "fx":[-122.03088,-112.02348,-92.38897,-101.37279], "fy":[-91.36616,-103.64368,-121.37997,-113.68267]}, - {"t":3.27191, "x":5.39987, "y":5.47969, "heading":2.85994, "vx":-3.81403, "vy":1.22977, "omega":2.44087, "ax":-6.11599, "ay":-6.42134, "alpha":-4.01757, "fx":[-124.85325,-110.68023,-84.06265,-96.5286], "fy":[-87.22955,-104.97869,-127.16865,-117.52362]}, - {"t":3.28872, "x":5.33487, "y":5.49946, "heading":2.90098, "vx":-3.91686, "vy":1.12181, "omega":2.37332, "ax":-5.89079, "ay":-6.52751, "alpha":-5.28972, "fx":[-128.18698,-108.91597,-73.69294,-90.00661], "fy":[-81.93902,-106.69568,-133.31056,-122.17913]}, - {"t":3.30553, "x":5.26818, "y":5.51739, "heading":2.94089, "vx":-4.01591, "vy":1.01206, "omega":2.28438, "ax":-5.59173, "ay":-6.63263, "alpha":-6.84223, "fx":[-132.02835,-106.70924,-60.97987,-80.73717], "fy":[-75.17112,-108.77142,-139.43109,-127.90298]}, - {"t":3.32235, "x":5.19987, "y":5.53347, "heading":2.9793, "vx":-4.10992, "vy":0.90054, "omega":2.16933, "ax":-5.20584, "ay":-6.71733, "alpha":-8.68711, "fx":[-136.19191,-104.19234,-46.38908,-67.42583], "fy":[-66.74328,-111.02383,-144.76142,-134.51114]}, - {"t":3.33916, "x":5.13003, "y":5.54767, "heading":3.01577, "vx":-4.19745, "vy":0.78759, "omega":2.02327, "ax":-4.83046, "ay":-6.7294, "alpha":-10.47271, "fx":[-139.94676,-102.11844,-33.42579,-53.16787], "fy":[-57.629,-112.71224,-148.05701,-139.46247]}, - {"t":3.35598, "x":5.05878, "y":5.55996, "heading":3.04979, "vx":-4.27867, "vy":0.67445, "omega":1.84719, "ax":-4.6857, "ay":-6.62466, "alpha":-11.53366, "fx":[-142.66629,-101.55285,-26.32159,-48.26919], "fy":[-49.33344,-112.88241,-149.1236,-139.39502]}, - {"t":3.37279, "x":4.98617, "y":5.57036, "heading":3.08085, "vx":-4.35746, "vy":0.56306, "omega":1.65326, "ax":-4.6915, "ay":-6.40839, "alpha":-12.33831, "fx":[-145.13539,-102.23816,-21.60372,-50.22698], "fy":[-39.52887,-111.752,-149.32761,-135.41098]}, - {"t":3.3896, "x":4.91224, "y":5.57892, "heading":3.10864, "vx":-4.43634, "vy":0.45531, "omega":1.44581, "ax":-4.80932, "ay":-5.9769, "alpha":-13.38506, "fx":[-147.50282,-104.33179,-16.86178,-58.52439], "fy":[-25.23258,-108.98959,-149.03436,-123.40457]}, - {"t":3.40642, "x":4.83697, "y":5.58573, "heading":3.13295, "vx":-4.5172, "vy":0.35482, "omega":1.22076, "ax":-5.46417, "ay":-4.2716, "alpha":-16.4185, "fx":[-148.41494,-109.58824,-14.28796,-99.48485], "fy":[-3.50937,-102.17891,-147.5134,-37.43294]}, - {"t":3.42323, "x":4.76025, "y":5.59109, "heading":-3.12971, "vx":-4.60907, "vy":0.283, "omega":0.9447, "ax":-6.19378, "ay":-1.51257, "alpha":-20.35475, "fx":[-144.3638,-126.0948,-49.29342,-101.66535], "fy":[25.06522,-76.28086,-133.57922,81.88156]}, - {"t":3.44004, "x":4.68188, "y":5.59564, "heading":-3.11382, "vx":-4.71321, "vy":0.25757, "omega":0.60246, "ax":-4.151, "ay":0.04322, "alpha":-20.85155, "fx":[-114.09573,-113.08668,-25.98141,-29.26547], "fy":[48.61447,-49.67403,-86.73362,90.73369]}, - {"t":3.47182, "x":4.53002, "y":5.60384, "heading":-3.09468, "vx":-4.84511, "vy":0.25894, "omega":-0.06011, "ax":8.76466, "ay":-0.48335, "alpha":-1.74804, "fx":[149.32503,148.20779,149.00452,149.80016], "fy":[0.45642,-18.70489,-15.85245,1.21447]}, - {"t":3.5036, "x":4.38049, "y":5.61183, "heading":-3.09659, "vx":-4.56661, "vy":0.24358, "omega":-0.11565, "ax":8.90506, "ay":-0.48104, "alpha":-0.52757, "fx":[151.57806,151.26896,151.38217,151.66097], "fy":[-5.62632,-11.28646,-10.6393,-5.17767]}, - {"t":3.53537, "x":4.23988, "y":5.61933, "heading":-3.10026, "vx":-4.28365, "vy":0.22829, "omega":-0.13242, "ax":8.94428, "ay":-0.48014, "alpha":-0.15463, "fx":[152.17317,152.08517,152.10753,152.19296], "fy":[-7.42139,-9.06782,-8.90391,-7.27472]}, - {"t":3.56715, "x":4.10828, "y":5.62634, "heading":-3.10447, "vx":-3.99944, "vy":0.21304, "omega":-0.13733, "ax":8.96259, "ay":-0.47967, "alpha":0.02574, "fx":[152.44537,152.45982,152.45697,152.44245], "fy":[-8.28313,-8.01008,-8.03458,-8.30811]}, - {"t":3.59892, "x":3.98572, "y":5.63286, "heading":-3.10883, "vx":-3.71464, "vy":0.1978, "omega":-0.13651, "ax":8.97317, "ay":-0.47938, "alpha":0.13199, "fx":[152.60078,152.67433,152.66237,152.58704], "fy":[-8.79234,-7.39507,-7.50957,-8.91953]}, - {"t":3.6307, "x":3.87221, "y":5.63891, "heading":-3.11317, "vx":-3.42952, "vy":0.18256, "omega":-0.13232, "ax":8.98006, "ay":-0.47919, "alpha":0.20199, "fx":[152.70102,152.81309,152.79756,152.68144], "fy":[-9.13077,-6.9953,-7.15607,-9.32136]}, - {"t":3.66247, "x":3.76777, "y":5.64447, "heading":-3.11738, "vx":-3.14417, "vy":0.16734, "omega":-0.1259, "ax":8.9849, "ay":-0.47905, "alpha":0.25157, "fx":[152.7709,152.91012,152.89331,152.74799], "fy":[-9.37341,-6.71609,-6.90049,-9.6042]}, - {"t":3.69425, "x":3.6724, "y":5.64954, "heading":-3.12138, "vx":-2.85867, "vy":0.15212, "omega":-0.11791, "ax":8.98848, "ay":-0.47895, "alpha":0.28852, "fx":[152.82231,152.98173,152.96469,152.79748], "fy":[-9.55677,-6.51099,-6.70624,-9.81316]}, - {"t":3.72602, "x":3.5861, "y":5.65413, "heading":-3.12512, "vx":-2.57305, "vy":0.1369, "omega":-0.10874, "ax":8.99125, "ay":-0.47887, "alpha":0.31712, "fx":[152.86167,153.03673,153.01996,152.83576], "fy":[-9.70074,-6.35455,-6.55312,-9.9733]}, - {"t":3.7578, "x":3.50888, "y":5.65824, "heading":-3.12858, "vx":-2.28735, "vy":0.12168, "omega":-0.09866, "ax":8.99344, "ay":-0.47881, "alpha":0.3399, "fx":[152.89274,153.08028,153.06404,152.86628], "fy":[-9.81706,-6.23164,-6.42904,-10.09961]}, - {"t":3.78958, "x":3.44074, "y":5.66187, "heading":-3.13171, "vx":-2.00158, "vy":0.10647, "omega":-0.08786, "ax":8.99522, "ay":-0.47875, "alpha":0.35849, "fx":[152.91787,153.11562,153.1,152.89119], "fy":[-9.91309,-6.13268,-6.32637,-10.20163]}, - {"t":3.82135, "x":3.38168, "y":5.66501, "heading":-3.13451, "vx":-1.71575, "vy":0.09125, "omega":-0.07647, "ax":8.9967, "ay":-0.47871, "alpha":0.37393, "fx":[152.93862,153.14486,153.1299,152.9119], "fy":[-9.99371,-6.05132,-6.24004,-10.28572]}, - {"t":3.85313, "x":3.3317, "y":5.66767, "heading":-3.13694, "vx":-1.42987, "vy":0.07604, "omega":-0.06459, "ax":8.99795, "ay":-0.47867, "alpha":0.38696, "fx":[152.95605,153.16946,153.15514,152.9294], "fy":[-10.06223,-5.98319,-6.16654,-10.35631]}, - {"t":3.8849, "x":3.29081, "y":5.66984, "heading":-3.13899, "vx":-1.14396, "vy":0.06083, "omega":-0.05229, "ax":8.99901, "ay":-0.47864, "alpha":0.39811, "fx":[152.9709,153.19045,153.17673,152.94436], "fy":[-10.12102,-5.92517,-6.10338,-10.41653]}, - {"t":3.91668, "x":3.259, "y":5.67153, "heading":-3.14065, "vx":-0.85801, "vy":0.04562, "omega":-0.03964, "ax":8.99993, "ay":-0.47861, "alpha":0.40776, "fx":[152.98373,153.20858,153.19539,152.95729], "fy":[-10.17179,-5.87495,-6.04876,-10.46873]}, - {"t":3.94845, "x":3.23628, "y":5.67274, "heading":3.14128, "vx":-0.57203, "vy":0.03041, "omega":-0.02668, "ax":9.00073, "ay":-0.47859, "alpha":0.41618, "fx":[152.99493,153.2244,153.21168,152.96856], "fy":[-10.21582,-5.83084,-6.0013,-10.51462]}, - {"t":3.98023, "x":3.22265, "y":5.67346, "heading":3.14043, "vx":-0.28603, "vy":0.01521, "omega":-0.01346, "ax":9.00144, "ay":-0.47857, "alpha":0.42361, "fx":[153.00482,153.23834,153.226,152.97845], "fy":[-10.25407,-5.79151,-5.95999,-10.55556]}, - {"t":4.01201, "x":3.2181, "y":5.67371, "heading":3.14, "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":3.61569, "y":5.55879, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.01731, "ay":0.06273, "alpha":0.00283, "fx":[153.3818,153.38201,153.38204,153.38183], "fy":[1.08237,1.05202,1.05181,1.08216]}, + {"t":0.04612, "x":3.62528, "y":5.55886, "heading":0.0, "vx":0.41588, "vy":0.00289, "omega":0.00013, "ax":9.01424, "ay":0.06271, "alpha":0.00337, "fx":[153.32956,153.32981,153.32986,153.32961], "fy":[1.08495,1.04876,1.04851,1.08469]}, + {"t":0.09224, "x":3.65405, "y":5.55906, "heading":0.00001, "vx":0.83163, "vy":0.00579, "omega":0.00029, "ax":9.00814, "ay":0.06267, "alpha":0.00446, "fx":[153.22561,153.22594,153.22604,153.22571], "fy":[1.09007,1.04227,1.04195,1.08973]}, + {"t":0.13836, "x":3.70198, "y":5.55939, "heading":0.00002, "vx":1.24709, "vy":0.00868, "omega":0.00049, "ax":8.99007, "ay":0.06254, "alpha":0.00767, "fx":[152.91802,152.91859,152.91893,152.91836], "fy":[1.10519,1.0231,1.02256,1.1046]}, + {"t":0.18448, "x":3.76906, "y":5.55986, "heading":0.00004, "vx":1.66172, "vy":0.01156, "omega":0.00085, "ax":7.32421, "ay":0.05096, "alpha":0.33089, "fx":[124.27096,124.28756,124.89431,124.8783], "fy":[2.36154,-0.61194,-0.60043,2.31775]}, + {"t":0.2306, "x":3.85349, "y":5.56044, "heading":0.00008, "vx":1.99951, "vy":0.01391, "omega":0.01611, "ax":0.00019, "ay":0.0, "alpha":1.32941, "fx":[-3.56569,-3.56511,3.57203,3.57145], "fy":[3.56831,-3.56884,-3.56825,3.56888]}, + {"t":0.27672, "x":3.94571, "y":5.56109, "heading":0.00082, "vx":1.99952, "vy":0.01391, "omega":0.07742, "ax":0.0, "ay":0.0, "alpha":1.09723, "fx":[-2.94775,-2.9429,2.94775,2.9429], "fy":[2.9429,-2.94775,-2.9429,2.94775]}, + {"t":0.32284, "x":4.03793, "y":5.56173, "heading":0.00439, "vx":1.99952, "vy":0.01391, "omega":0.12803, "ax":0.0, "ay":0.0, "alpha":0.90032, "fx":[-2.42736,-2.40612,2.42736,2.40612], "fy":[2.40612,-2.42736,-2.40612,2.42736]}, + {"t":0.36897, "x":4.13015, "y":5.56237, "heading":0.0103, "vx":1.99952, "vy":0.01391, "omega":0.16955, "ax":0.0, "ay":0.0, "alpha":0.73244, "fx":[-1.98624,-1.94574,1.98624,1.94574], "fy":[1.94574,-1.98624,-1.94574,1.98624]}, + {"t":0.41509, "x":4.22237, "y":5.56301, "heading":0.01812, "vx":1.99952, "vy":0.01391, "omega":0.20333, "ax":0.0, "ay":0.0, "alpha":0.5882, "fx":[-1.60727,-1.55006,1.60727,1.55006], "fy":[1.55006,-1.60727,-1.55006,1.60727]}, + {"t":0.46121, "x":4.31459, "y":5.56365, "heading":0.0275, "vx":1.99952, "vy":0.01391, "omega":0.23046, "ax":0.0, "ay":0.0, "alpha":0.46299, "fx":[-1.27652,-1.20818,1.27652,1.20818], "fy":[1.20818,-1.27652,-1.20818,1.27652]}, + {"t":0.50733, "x":4.40681, "y":5.56429, "heading":0.03813, "vx":1.99952, "vy":0.01391, "omega":0.25181, "ax":0.0, "ay":0.0, "alpha":0.35278, "fx":[-0.98237,-0.91018,0.98237,0.91018], "fy":[0.91018,-0.98237,-0.91018,0.98237]}, + {"t":0.55345, "x":4.49903, "y":5.56494, "heading":0.04974, "vx":1.99952, "vy":0.01391, "omega":0.26808, "ax":0.0, "ay":0.0, "alpha":0.25399, "fx":[-0.71485,-0.64705,0.71485,0.64705], "fy":[0.64705,-0.71485,-0.64705,0.71485]}, + {"t":0.59957, "x":4.59125, "y":5.56558, "heading":0.0621, "vx":1.99952, "vy":0.01391, "omega":0.2798, "ax":0.0, "ay":0.0, "alpha":0.16344, "fx":[-0.46512,-0.41066,0.46512,0.41066], "fy":[0.41066,-0.46512,-0.41066,0.46512]}, + {"t":0.64569, "x":4.68346, "y":5.56622, "heading":0.07501, "vx":1.99952, "vy":0.01391, "omega":0.28733, "ax":0.0, "ay":0.0, "alpha":0.0782, "fx":[-0.22506,-0.1936,0.22506,0.1936], "fy":[0.1936,-0.22506,-0.1936,0.22506]}, + {"t":0.69181, "x":4.77568, "y":5.56686, "heading":0.08826, "vx":1.99952, "vy":0.01391, "omega":0.29094, "ax":0.0, "ay":0.0, "alpha":-0.00451, "fx":[0.01311,0.01098,-0.01311,-0.01098], "fy":[-0.01098,0.01311,0.01098,-0.01311]}, + {"t":0.73793, "x":4.8679, "y":5.5675, "heading":0.10168, "vx":1.99952, "vy":0.01391, "omega":0.29073, "ax":0.0, "ay":0.0, "alpha":-0.08736, "fx":[0.25709,0.20949,-0.25709,-0.20949], "fy":[-0.20949,0.25709,0.20949,-0.25709]}, + {"t":0.78405, "x":4.96012, "y":5.56814, "heading":0.11509, "vx":1.99952, "vy":0.01391, "omega":0.2867, "ax":0.0, "ay":0.0, "alpha":-0.17305, "fx":[0.51478,0.4081,-0.51478,-0.4081], "fy":[-0.4081,0.51478,0.4081,-0.51478]}, + {"t":0.83017, "x":5.05234, "y":5.56878, "heading":0.12831, "vx":1.99952, "vy":0.01391, "omega":0.27872, "ax":0.0, "ay":0.0, "alpha":-0.26435, "fx":[0.79456,0.61296,-0.79456,-0.61296], "fy":[-0.61296,0.79456,0.61296,-0.79456]}, + {"t":0.87629, "x":5.14456, "y":5.56943, "heading":0.14116, "vx":1.99952, "vy":0.01391, "omega":0.26653, "ax":0.0, "ay":0.0, "alpha":-0.36422, "fx":[1.10552,0.8304,-1.10552,-0.8304], "fy":[-0.8304,1.10552,0.8304,-1.10552]}, + {"t":0.92241, "x":5.23678, "y":5.57007, "heading":0.15346, "vx":1.99952, "vy":0.01391, "omega":0.24973, "ax":0.0, "ay":0.0, "alpha":-0.4759, "fx":[1.45772,1.06718,-1.45772,-1.06718], "fy":[-1.06718,1.45772,1.06718,-1.45772]}, + {"t":0.96853, "x":5.329, "y":5.57071, "heading":0.16497, "vx":1.99952, "vy":0.01391, "omega":0.22778, "ax":0.0, "ay":0.0, "alpha":-0.60298, "fx":[1.86244,1.3308,-1.86244,-1.3308], "fy":[-1.3308,1.86244,1.3308,-1.86244]}, + {"t":1.01465, "x":5.42122, "y":5.57135, "heading":0.17548, "vx":1.99952, "vy":0.01391, "omega":0.19997, "ax":0.0, "ay":0.0, "alpha":-0.74957, "fx":[2.33245,1.62991,-2.33245,-1.62991], "fy":[-1.62991,2.33245,1.62991,-2.33245]}, + {"t":1.06077, "x":5.51344, "y":5.57199, "heading":0.1847, "vx":1.99952, "vy":0.01391, "omega":0.1654, "ax":0.0, "ay":0.0, "alpha":-0.92036, "fx":[2.88224,1.97479,-2.88224,-1.97479], "fy":[-1.97479,2.88224,1.97479,-2.88224]}, + {"t":1.1069, "x":5.60566, "y":5.57263, "heading":0.19233, "vx":1.99952, "vy":0.01391, "omega":0.12296, "ax":0.0, "ay":0.0, "alpha":-1.1208, "fx":[3.52821,2.37803,-3.52821,-2.37803], "fy":[-2.37803,3.52821,2.37803,-3.52821]}, + {"t":1.15302, "x":5.69788, "y":5.57328, "heading":0.198, "vx":1.99952, "vy":0.01391, "omega":0.07126, "ax":0.0, "ay":0.0, "alpha":-1.35727, "fx":[4.28884,2.85547,-4.28884,-2.85547], "fy":[-2.85547,4.28884,2.85547,-4.28884]}, + {"t":1.19914, "x":5.7901, "y":5.57392, "heading":0.20129, "vx":1.99952, "vy":0.01391, "omega":0.00867, "ax":0.0, "ay":0.0, "alpha":-1.63719, "fx":[5.18466,3.42736,-5.18466,-3.42736], "fy":[-3.42736,5.18466,3.42736,-5.18466]}, + {"t":1.24526, "x":5.88231, "y":5.57456, "heading":0.20169, "vx":1.99952, "vy":0.01391, "omega":-0.06684, "ax":0.0, "ay":0.0, "alpha":-1.96929, "fx":[6.238,4.1201,-6.238,-4.1201], "fy":[-4.1201,6.238,4.1201,-6.238]}, + {"t":1.29138, "x":5.97453, "y":5.5752, "heading":0.19861, "vx":1.99952, "vy":0.01391, "omega":-0.15767, "ax":0.0, "ay":0.0, "alpha":-2.36375, "fx":[7.47224,4.96844,-7.47224,-4.96844], "fy":[-4.96844,7.47224,4.96844,-7.47224]}, + {"t":1.3375, "x":6.06675, "y":5.57584, "heading":0.19133, "vx":1.99952, "vy":0.01391, "omega":-0.26669, "ax":0.0, "ay":0.0, "alpha":-2.83244, "fx":[8.91031,6.01854,-8.91031,-6.01854], "fy":[-6.01853,8.91033,6.01855,-8.9103]}, + {"t":1.38362, "x":6.15897, "y":5.57648, "heading":0.17903, "vx":1.99952, "vy":0.01391, "omega":-0.39732, "ax":0.0, "ay":0.00002, "alpha":-3.38906, "fx":[10.57197,7.33187,-10.57197,-7.33188], "fy":[-7.33148,10.57236,7.33227,-10.57158]}, + {"t":1.42974, "x":6.25119, "y":5.57713, "heading":0.16071, "vx":1.99952, "vy":0.01391, "omega":-0.55362, "ax":0.0, "ay":0.00061, "alpha":-4.0493, "fx":[12.46895,8.99005,-12.4689,-8.9904], "fy":[-8.97975,12.47933,9.0007,-12.45852]}, + {"t":1.47586, "x":6.34341, "y":5.57777, "heading":0.13518, "vx":1.99952, "vy":0.01394, "omega":-0.74038, "ax":-0.00012, "ay":0.01624, "alpha":-4.83078, "fx":[14.59851,11.09579,-14.59486,-11.10736], "fy":[-10.82424,14.8718,11.37876,-14.32139]}, + {"t":1.52198, "x":6.43563, "y":5.57843, "heading":0.10103, "vx":1.99952, "vy":0.01469, "omega":-0.96318, "ax":-0.0052, "ay":0.42402, "alpha":-5.72816, "fx":[16.93925,13.54853,-16.83128,-14.01008], "fy":[-6.47508,23.99735,20.95436,-9.62662]}, + {"t":1.5681, "x":6.52784, "y":5.57956, "heading":0.05661, "vx":1.99928, "vy":0.03425, "omega":-1.22737, "ax":-0.55675, "ay":6.11403, "alpha":-2.56256, "fx":[1.15465,-0.049,-18.88515,-20.10126], "fy":[101.13029,108.43905,106.98105,99.44135]}, + {"t":1.61422, "x":6.61946, "y":5.58764, "heading":0.0, "vx":1.9736, "vy":0.31623, "omega":-1.34555, "ax":-1.45756, "ay":7.39043, "alpha":-0.51208, "fx":[-22.86479,-22.20022,-26.65875,-27.44664], "fy":[125.5938,126.55163,125.84492,124.84632]}, + {"t":1.63396, "x":6.65812, "y":5.59532, "heading":-0.02655, "vx":1.94484, "vy":0.46206, "omega":-1.35566, "ax":-1.3799, "ay":5.19323, "alpha":-1.29365, "fx":[-19.57232,-18.32416,-27.16223,-28.82801], "fy":[86.48428,91.28038,90.22898,85.3477]}, + {"t":1.65369, "x":6.69623, "y":5.60545, "heading":-0.0533, "vx":1.91761, "vy":0.56454, "omega":-1.38119, "ax":-0.64188, "ay":2.09658, "alpha":-2.28746, "fx":[-4.92935,-4.06471,-16.68513,-17.99376], "fy":[29.67706,41.42657,41.58158,29.96338]}, + {"t":1.67342, "x":6.73394, "y":5.61699, "heading":-0.08056, "vx":1.90494, "vy":0.60591, "omega":-1.42632, "ax":-0.18887, "ay":0.58762, "alpha":-2.43074, "fx":[2.80365,3.82517,-9.16144,-10.31806], "fy":[2.99249,15.98699,16.96445,4.03736]}, + {"t":1.69315, "x":6.7715, "y":5.62907, "heading":-0.1087, "vx":1.90121, "vy":0.61751, "omega":-1.47429, "ax":-0.03323, "ay":0.10214, "alpha":-2.34848, "fx":[5.02304,6.38146,-6.14301,-7.52267], "fy":[-5.21533,7.32335,8.68378,-3.84242]}, + {"t":1.71289, "x":6.80901, "y":5.64127, "heading":-0.1378, "vx":1.90056, "vy":0.61952, "omega":-1.52063, "ax":0.06098, "ay":-0.18771, "alpha":-2.25758, "fx":[6.19895,7.88513,-4.14172,-5.79313], "fy":[-10.01802,1.97148,3.64393,-8.36878]}, + {"t":1.73262, "x":6.84652, "y":5.65346, "heading":-0.1678, "vx":1.90176, "vy":0.61582, "omega":-1.56518, "ax":0.26666, "ay":-0.83586, "alpha":-2.14402, "fx":[9.21588,11.28134,-0.21132,-2.14298], "fy":[-20.75269,-9.52923,-7.63454,-18.95484]}, + {"t":1.75235, "x":6.8841, "y":5.66545, "heading":-0.19869, "vx":1.90702, "vy":0.59933, "omega":-1.60749, "ax":0.83926, "ay":-2.81275, "alpha":-1.71833, "fx":[17.82547,20.46385,10.58873,8.22404], "fy":[-52.55353,-44.27321,-43.0656,-51.48375]}, + {"t":1.77209, "x":6.9219, "y":5.67673, "heading":-0.23041, "vx":1.92358, "vy":0.54382, "omega":-1.6414, "ax":1.48963, "ay":-5.9546, "alpha":-0.76353, "fx":[26.92326,29.16845,23.69326,21.56793], "fy":[-102.42241,-99.83477,-100.17056,-102.71641]}, + {"t":1.79182, "x":6.96014, "y":5.6863, "heading":-0.2628, "vx":1.95298, "vy":0.42632, "omega":-1.65646, "ax":1.37127, "ay":-7.70524, "alpha":-0.28817, "fx":[24.00352,25.09007,22.63243,21.57343], "fy":[-131.20588,-130.65285,-130.93094,-131.46618]}, + {"t":1.81155, "x":6.99895, "y":5.69321, "heading":-0.29548, "vx":1.98004, "vy":0.27427, "omega":-1.66215, "ax":0.80644, "ay":-8.39855, "alpha":-0.13421, "fx":[14.08592,14.59428,13.34463,12.84429], "fy":[-142.88209,-142.7469,-142.83383,-142.96464]}, + {"t":1.83128, "x":7.03818, "y":5.69699, "heading":-0.32828, "vx":1.99595, "vy":0.10855, "omega":-1.6648, "ax":0.09595, "ay":-8.68787, "alpha":-0.07436, "fx":[1.8661,2.12327,1.39646,1.14228], "fy":[-147.79468,-147.76358,-147.76239,-147.79234]}, + {"t":1.85102, "x":7.07758, "y":5.69744, "heading":-0.36113, "vx":1.99785, "vy":-0.06289, "omega":-1.66626, "ax":-0.66481, "ay":-8.78486, "alpha":-0.0463, "fx":[-11.14506,-11.00719,-11.47197,-11.60852], "fy":[-149.44781,-149.44669,-149.40838,-149.40921]}, + {"t":1.87075, "x":7.11688, "y":5.69449, "heading":-0.39401, "vx":1.98473, "vy":-0.23624, "omega":-1.66718, "ax":-1.44158, "ay":-8.75881, "alpha":-0.03124, "fx":[-24.40044,-24.32475,-24.64174,-24.71679], "fy":[-149.00816,-149.01508,-148.9617,-148.95473]}, + {"t":1.89048, "x":7.15576, "y":5.68812, "heading":-0.42691, "vx":1.95628, "vy":-0.40908, "omega":-1.66779, "ax":-2.2182, "ay":-8.63643, "alpha":-0.02233, "fx":[-37.63862,-37.59767,-37.82361,-37.86421], "fy":[-146.92871,-146.93624,-146.87772,-146.87021]}, + {"t":1.91022, "x":7.19393, "y":5.67837, "heading":-0.45982, "vx":1.91251, "vy":-0.5795, "omega":-1.66824, "ax":-2.98581, "ay":-8.4294, "alpha":-0.01667, "fx":[-50.71512,-50.69456,-50.86053,-50.8809], "fy":[-143.40841,-143.41393,-143.35494,-143.34947]}, + {"t":1.92995, "x":7.23109, "y":5.66529, "heading":-0.49274, "vx":1.85359, "vy":-0.74584, "omega":-1.66856, "ax":-3.74461, "ay":-8.14017, "alpha":-0.01287, "fx":[-63.6367,-63.62855,-63.75292,-63.76095], "fy":[-138.48921,-138.49184,-138.43456,-138.43198]}, + {"t":1.94968, "x":7.26694, "y":5.64899, "heading":-0.52567, "vx":1.7797, "vy":-0.90647, "omega":-1.66882, "ax":-4.51324, "ay":-7.75492, "alpha":-0.01019, "fx":[-76.72208,-76.72185,-76.81592,-76.81609], "fy":[-131.93668,-131.93606,-131.88131,-131.88197]}, + {"t":1.96942, "x":7.30118, "y":5.62959, "heading":-0.5586, "vx":1.69064, "vy":-1.0595, "omega":-1.66902, "ax":-5.06559, "ay":-7.41452, "alpha":-0.0083, "fx":[-86.12626,-86.12896,-86.2021,-86.19936], "fy":[-126.14504,-126.14264,-126.09269,-126.09513]}, + {"t":1.99252, "x":7.3389, "y":5.60313, "heading":-0.59717, "vx":1.57358, "vy":-1.23084, "omega":-1.66921, "ax":-5.8615, "ay":-6.78522, "alpha":-0.00798, "fx":[-99.66697,-99.674,-99.73808,-99.73102], "fy":[-115.44567,-115.43886,-115.38359,-115.39044]}, + {"t":2.01563, "x":7.37369, "y":5.57287, "heading":-0.63574, "vx":1.43812, "vy":-1.38764, "omega":-1.6694, "ax":-6.52816, "ay":-6.11704, "alpha":-0.00711, "fx":[-111.01214,-111.02101,-111.07198,-111.06309], "fy":[-104.08151,-104.07106,-104.01684,-104.02732]}, + {"t":2.03874, "x":7.40519, "y":5.53917, "heading":-0.67432, "vx":1.28726, "vy":-1.52901, "omega":-1.66956, "ax":-7.10115, "ay":-5.38751, "alpha":-0.00501, "fx":[-120.76917,-120.77651,-120.80776,-120.80041], "fy":[-91.6658,-91.65505,-91.61414,-91.6249]}, + {"t":2.06185, "x":7.43304, "y":5.5024, "heading":-0.71291, "vx":1.12315, "vy":-1.65351, "omega":-1.66968, "ax":-7.56839, "ay":-4.5984, "alpha":-0.00001, "fx":[-128.73605,-128.73606,-128.73612,-128.7361], "fy":[-78.21755,-78.21752,-78.21744,-78.21747]}, + {"t":2.08496, "x":7.45697, "y":5.46296, "heading":-0.75149, "vx":0.94825, "vy":-1.75978, "omega":-1.66968, "ax":-7.89299, "ay":-3.75483, "alpha":0.01273, "fx":[-134.29332,-134.27605,-134.22169,-134.23896], "fy":[-63.7905,-63.8361,-63.94668,-63.90117]}, + {"t":2.10807, "x":7.47678, "y":5.42129, "heading":-0.79008, "vx":0.76585, "vy":-1.84655, "omega":-1.66938, "ax":-7.96843, "ay":-2.85834, "alpha":0.05072, "fx":[-135.64794,-135.60298,-135.43324,-135.47836], "fy":[-48.29642,-48.51065,-48.94197,-48.72896]}, + {"t":2.13118, "x":7.49235, "y":5.37785, "heading":-0.82865, "vx":0.5817, "vy":-1.9126, "omega":-1.66821, "ax":-7.37955, "ay":-1.89466, "alpha":0.19941, "fx":[-125.77068,-125.82011,-125.27621,-125.22932], "fy":[-31.00711,-31.91808,-33.44152,-32.54358]}, + {"t":2.15429, "x":7.50382, "y":5.33315, "heading":-0.86721, "vx":0.41116, "vy":-1.95639, "omega":-1.6636, "ax":-4.6444, "ay":-0.84474, "alpha":0.90354, "fx":[-79.1351,-81.55631,-78.89415,-76.41406], "fy":[-10.07969,-13.71017,-18.61145,-15.07396]}, + {"t":2.1774, "x":7.51208, "y":5.28771, "heading":-0.90565, "vx":0.30384, "vy":-1.97591, "omega":-1.64272, "ax":-1.29196, "ay":-0.18873, "alpha":1.90528, "fx":[-21.21195,-28.97537,-22.8261,-14.89009], "fy":[4.12779,-2.30128,-10.50932,-4.15795]}, + {"t":2.20051, "x":7.51876, "y":5.242, "heading":-0.94361, "vx":0.27398, "vy":-1.98027, "omega":-1.59869, "ax":-0.26819, "ay":-0.03668, "alpha":2.4016, "fx":[-3.14333,-13.5429,-6.0093,4.44853], "fy":[8.3937,0.80888,-9.62727,-2.07088]}, + {"t":2.22362, "x":7.52502, "y":5.19622, "heading":-0.98056, "vx":0.26778, "vy":-1.98112, "omega":-1.54319, "ax":-0.05461, "ay":-0.00736, "alpha":2.8447, "fx":[1.16116,-11.51852,-3.02686,9.66888], "fy":[10.47149,1.9668,-10.71734,-2.22195]}, + {"t":2.24673, "x":7.53119, "y":5.15044, "heading":-1.01622, "vx":0.26652, "vy":-1.98129, "omega":-1.47745, "ax":-0.01114, "ay":-0.0015, "alpha":3.30664, "fx":[2.6812,-12.4082,-3.06234,12.03129], "fy":[12.19502,2.84559,-12.24455,-2.89799]}, + {"t":2.26984, "x":7.53735, "y":5.10465, "heading":-1.05036, "vx":0.26626, "vy":-1.98132, "omega":-1.40104, "ax":-0.00235, "ay":-0.00032, "alpha":3.79414, "fx":[3.73158,-13.94045,-3.81221,13.86095], "fy":[13.89554,3.7663,-13.90587,-3.7775]}, + {"t":2.29295, "x":7.5435, "y":5.05886, "heading":-1.08274, "vx":0.26621, "vy":-1.98133, "omega":-1.31336, "ax":-0.00086, "ay":-0.00012, "alpha":4.31031, "fx":[4.77919,-15.65938,-4.80881,15.63026], "fy":[15.64295,4.79191,-15.64668,-4.79608]}, + {"t":2.31606, "x":7.54965, "y":5.01308, "heading":-1.11309, "vx":0.26619, "vy":-1.98133, "omega":-1.21375, "ax":-0.00202, "ay":-0.00027, "alpha":4.85793, "fx":[5.90094,-17.49436,-5.97036,17.42635], "fy":[17.45609,5.93068,-17.46462,-5.94062]}, + {"t":2.33917, "x":7.5558, "y":4.96729, "heading":-1.14114, "vx":0.26614, "vy":-1.98134, "omega":-1.10148, "ax":-0.00927, "ay":-0.00124, "alpha":5.43954, "fx":[7.03238,-19.51247,-7.35158,19.20091], "fy":[19.3377,7.16868,-19.37575,-7.21533]}, + {"t":2.36228, "x":7.56195, "y":4.9215, "heading":-1.1666, "vx":0.26593, "vy":-1.98137, "omega":-0.97578, "ax":-0.04421, "ay":-0.00592, "alpha":6.05721, "fx":[7.79165,-22.08486,-9.31699,20.60206], "fy":[21.25687,8.44101,-21.43178,-8.66905]}, + {"t":2.38539, "x":7.56808, "y":4.87571, "heading":-1.18915, "vx":0.2649, "vy":-1.9815, "omega":-0.8358, "ax":-0.20969, "ay":-0.02777, "alpha":6.70659, "fx":[6.3679,-26.91038,-13.61823,19.89342], "fy":[23.03063,9.45913,-23.81563,-10.56369]}, + {"t":2.4085, "x":7.57415, "y":4.82991, "heading":-1.20846, "vx":0.26006, "vy":-1.98215, "omega":-0.68081, "ax":-0.97803, "ay":-0.12263, "alpha":7.24078, "fx":[-5.86446,-41.15645,-27.99934,8.47626], "fy":[23.67537,8.96806,-26.94749,-14.03938]}, + {"t":2.4316, "x":7.5799, "y":4.78407, "heading":-1.22419, "vx":0.23746, "vy":-1.98498, "omega":-0.51348, "ax":-3.8687, "ay":-0.33528, "alpha":5.67378, "fx":[-59.41805,-81.63112,-73.08269,-49.08996], "fy":[18.27299,4.56528,-27.09217,-18.55846]}, + {"t":2.45471, "x":7.58435, "y":4.73811, "heading":-1.23606, "vx":0.14805, "vy":-1.99273, "omega":-0.38236, "ax":-3.97407, "ay":-0.24742, "alpha":4.21944, "fx":[-62.55981,-79.18439,-73.09968,-55.54761], "fy":[13.52665,3.78099,-20.4697,-13.67181]}, + {"t":2.47671, "x":7.58665, "y":4.69423, "heading":-1.24447, "vx":0.06066, "vy":-1.99817, "omega":-0.28957, "ax":-1.11374, "ay":-0.02701, "alpha":4.47354, "fx":[-11.67987,-33.81548,-26.47007,-3.81236], "fy":[15.17768,7.00573,-15.71555,-8.30564]}, + {"t":2.4987, "x":7.58771, "y":4.65028, "heading":-1.25084, "vx":0.03616, "vy":-1.99876, "omega":-0.19119, "ax":-0.2573, "ay":-0.00429, "alpha":2.99376, "fx":[0.70562,-14.51063,-9.48689,5.78521], "fy":[10.1105,5.01274,-10.21755,-5.19775]}, + {"t":2.52069, "x":7.58845, "y":4.60632, "heading":-1.25504, "vx":0.0305, "vy":-1.99886, "omega":-0.12535, "ax":-0.05799, "ay":-0.00087, "alpha":1.37984, "fx":[1.38355,-5.65649,-3.3575,3.68519], "fy":[4.65742,2.35506,-4.68502,-2.38641]}, + {"t":2.54268, "x":7.5891, "y":4.56236, "heading":-1.2578, "vx":0.02923, "vy":-1.99888, "omega":-0.09501, "ax":-0.01296, "ay":-0.00019, "alpha":-0.24187, "fx":[-0.63822,0.59722,0.19738,-1.03805], "fy":[-0.82084,-0.42102,0.81444,0.41459]}, + {"t":2.56467, "x":7.58974, "y":4.5184, "heading":-1.25989, "vx":0.02894, "vy":-1.99888, "omega":-0.10033, "ax":-0.00289, "ay":-0.00004, "alpha":-1.86315, "fx":[-3.28064,6.2425,3.18236,-6.34054], "fy":[-6.29215,-3.23229,6.2909,3.2307]}, + {"t":2.58667, "x":7.59038, "y":4.47444, "heading":-1.26209, "vx":0.02888, "vy":-1.99888, "omega":-0.1413, "ax":-0.00064, "ay":-0.00001, "alpha":-3.4801, "fx":[-6.07289,11.72746,6.05092,-11.74924], "fy":[-11.73844,-6.06213,11.73826,6.06168]}, + {"t":2.60866, "x":7.59101, "y":4.43049, "heading":-1.2652, "vx":0.02887, "vy":-1.99888, "omega":-0.21783, "ax":-0.00014, "ay":0.0, "alpha":-5.0888, "fx":[-8.91984,17.13442,8.91488,-17.13929], "fy":[-17.13686,-8.91743,17.13685,8.91729]}, + {"t":2.63065, "x":7.59165, "y":4.38653, "heading":-1.26999, "vx":0.02886, "vy":-1.99888, "omega":-0.32975, "ax":-0.00003, "ay":0.0, "alpha":-6.68506, "fx":[-11.82285,22.45538,11.82167,-22.45653], "fy":[-22.45595,-11.82228,22.45596,11.82224]}, + {"t":2.65264, "x":7.59228, "y":4.34257, "heading":-1.27724, "vx":0.02886, "vy":-1.99888, "omega":-0.47676, "ax":-0.00001, "ay":0.0, "alpha":-8.26439, "fx":[-14.81644,27.65418,14.81593,-27.65467], "fy":[-27.65442,-14.8162,27.65443,14.81618]}, + {"t":2.67463, "x":7.59292, "y":4.29861, "heading":-1.28773, "vx":0.02886, "vy":-1.99888, "omega":-0.65851, "ax":-0.00003, "ay":0.0, "alpha":-9.82215, "fx":[-17.95309,32.68005,17.95195,-32.68112], "fy":[-32.68056,-17.95255,32.6806,17.95249]}, + {"t":2.69662, "x":7.59355, "y":4.25465, "heading":-1.30221, "vx":0.02886, "vy":-1.99888, "omega":-0.87452, "ax":-0.00012, "ay":0.0, "alpha":-11.35364, "fx":[-21.2988,37.46971,21.2944,-37.47377], "fy":[-37.47162,-21.29677,37.47186,21.29643]}, + {"t":2.71862, "x":7.59419, "y":4.21069, "heading":-1.32144, "vx":0.02886, "vy":-1.99888, "omega":-1.12421, "ax":-0.00048, "ay":-0.00001, "alpha":-12.8544, "fx":[-24.93168,41.94547,24.91443,-41.96117], "fy":[-41.95269,-24.9239,41.95394,24.92221]}, + {"t":2.74061, "x":7.59482, "y":4.16673, "heading":-1.34617, "vx":0.02885, "vy":-1.99888, "omega":-1.4069, "ax":-0.00183, "ay":-0.00003, "alpha":-14.3204, "fx":[-28.94516,46.0078,28.87957,-46.0668], "fy":[-46.0342,-28.91635,46.04041,28.90839]}, + {"t":2.7626, "x":7.59546, "y":4.12277, "heading":-1.37711, "vx":0.02881, "vy":-1.99888, "omega":-1.72183, "ax":-0.00669, "ay":-0.0001, "alpha":-15.74836, "fx":[-33.46646,49.51249,33.22615,-49.72762], "fy":[-49.60565,-33.36406,49.63456,33.32866]}, + {"t":2.78459, "x":7.59609, "y":4.07881, "heading":-1.41497, "vx":0.02866, "vy":-1.99888, "omega":-2.06816, "ax":-0.02362, "ay":-0.00033, "alpha":-17.13577, "fx":[-38.72466,52.19818,37.8787,-52.95957], "fy":[-52.5163,-38.37707,52.64285,38.22776]}, + {"t":2.80658, "x":7.59671, "y":4.03485, "heading":-1.46046, "vx":0.02814, "vy":-1.99889, "omega":-2.44501, "ax":-0.0804, "ay":-0.00109, "alpha":-18.47954, "fx":[-45.26251,53.45234,42.40987,-56.07021], "fy":[-54.50946,-44.14395,55.03108,43.54785]}, + {"t":2.82858, "x":7.59731, "y":3.99089, "heading":-1.51423, "vx":0.02637, "vy":-1.99892, "omega":-2.85141, "ax":-0.26367, "ay":-0.00309, "alpha":-19.75927, "fx":[-54.49234,51.52336,45.3058,-60.27695], "fy":[-54.99572,-51.13195,57.02129,48.89593]}, + {"t":2.85057, "x":7.59783, "y":3.94693, "heading":-1.57693, "vx":0.02057, "vy":-1.99898, "omega":-3.28595, "ax":-0.83399, "ay":-0.00474, "alpha":-20.77077, "fx":[-69.61908,40.5632,41.37887,-69.06706], "fy":[-52.33732,-60.52764,59.68238,52.8598]}, + {"t":2.87256, "x":7.59808, "y":3.90297, "heading":-1.6492, "vx":0.00223, "vy":-1.99909, "omega":-3.74274, "ax":-2.56198, "ay":0.03338, "alpha":-19.71223, "fx":[-94.28403,-1.25604,9.67335,-88.44726], "fy":[-42.66417,-71.66101,64.97689,51.61977]}, + {"t":2.89455, "x":7.59751, "y":3.85902, "heading":-1.73151, "vx":-0.05411, "vy":-1.99835, "omega":-4.17625, "ax":-6.10873, "ay":0.41888, "alpha":-10.86067, "fx":[-123.03656,-93.0778,-83.58639,-115.93018], "fy":[-21.01519,-49.00097,54.77627,43.73989]}, + {"t":2.91654, "x":7.59484, "y":3.81517, "heading":-1.82335, "vx":-0.18845, "vy":-1.98914, "omega":-4.41509, "ax":-6.27439, "ay":0.74411, "alpha":-7.95731, "fx":[-121.61331,-101.90716,-90.77532,-112.60672], "fy":[-7.81165,-29.14369,44.37454,43.20947]}, + {"t":2.93614, "x":7.58994, "y":3.77633, "heading":-1.90988, "vx":-0.31142, "vy":-1.97456, "omega":-4.57104, "ax":-3.23134, "ay":0.56282, "alpha":-13.30622, "fx":[-93.61881,-40.71543,-11.52027,-74.00217], "fy":[-11.90861,-46.22412,42.91869,53.50749]}, + {"t":2.95574, "x":7.58322, "y":3.73774, "heading":-1.99946, "vx":-0.37474, "vy":-1.96353, "omega":-4.83181, "ax":-1.18277, "ay":0.23296, "alpha":-12.73109, "fx":[-63.62778,-5.1411,25.82248,-37.52802], "fy":[-11.97624,-43.51648,23.16957,48.17337]}, + {"t":2.97534, "x":7.57565, "y":3.6993, "heading":-2.09415, "vx":-0.39792, "vy":-1.95896, "omega":-5.08131, "ax":-0.38738, "ay":0.07947, "alpha":-8.9964, "fx":[-39.28098,1.96569,26.59813,-15.63995], "fy":[-7.31829,-31.90294,10.46531,34.16317]}, + {"t":2.99493, "x":7.56778, "y":3.66093, "heading":-2.19373, "vx":-0.40551, "vy":-1.95741, "omega":-5.25762, "ax":-0.11754, "ay":0.02443, "alpha":-4.44839, "fx":[-18.64027,0.7091,14.68415,-4.75045], "fy":[-2.3051,-16.26408,3.16107,17.07009]}, + {"t":3.01453, "x":7.55981, "y":3.62257, "heading":-2.29677, "vx":-0.40782, "vy":-1.95693, "omega":-5.3448, "ax":-0.0361, "ay":0.00753, "alpha":0.36669, "fx":[0.77556,-0.69676,-2.00356,-0.53144], "fy":[0.21079,1.51768,0.04543,-1.26153]}, + {"t":3.03413, "x":7.55181, "y":3.58422, "heading":-2.40152, "vx":-0.40853, "vy":-1.95678, "omega":-5.33761, "ax":-0.02136, "ay":0.00446, "alpha":5.13933, "fx":[19.1325,0.51475,-19.84717,-1.25323], "fy":[-0.80738,19.56519,0.96043,-19.41489]}, + {"t":3.05373, "x":7.5438, "y":3.54587, "heading":-2.50612, "vx":-0.40894, "vy":-1.95669, "omega":-5.23689, "ax":-0.04551, "ay":0.00952, "alpha":9.61451, "fx":[35.35878,4.63329,-36.81812,-6.27059], "fy":[-5.29376,36.25608,5.60918,-35.92394]}, + {"t":3.07332, "x":7.53577, "y":3.50753, "heading":-2.60875, "vx":-0.40984, "vy":-1.95651, "omega":-5.04847, "ax":-0.13394, "ay":0.02814, "alpha":13.61674, "fx":[48.01771,10.3874,-52.07017,-15.44822], "fy":[-12.51481,50.61638,13.31738,-49.50401]}, + {"t":3.09292, "x":7.52772, "y":3.46919, "heading":-2.70769, "vx":-0.41246, "vy":-1.95595, "omega":-4.78161, "ax":-0.37675, "ay":0.08019, "alpha":17.03333, "fx":[55.25057,14.81966,-66.02868,-29.6754], "fy":[-21.46537,62.69099,23.09384,-58.86354]}, + {"t":3.11252, "x":7.51956, "y":3.43088, "heading":-2.8014, "vx":-0.41984, "vy":-1.95438, "omega":-4.4478, "ax":-0.98506, "ay":0.21675, "alpha":19.61158, "fx":[52.99207,11.92967,-80.35645,-51.5877], "fy":[-31.20694,73.89933,33.55855,-61.50367]}, + {"t":3.13212, "x":7.51114, "y":3.39262, "heading":-2.88857, "vx":-0.43915, "vy":-1.95014, "omega":-4.06345, "ax":-2.4087, "ay":0.57337, "alpha":19.92668, "fx":[26.02942,-10.18891,-96.50818,-83.21776], "fy":[-38.81468,85.92592,43.2531,-51.35306]}, + {"t":3.15172, "x":7.50207, "y":3.35451, "heading":-2.9682, "vx":-0.48635, "vy":-1.9389, "omega":-3.67294, "ax":-5.21266, "ay":1.45598, "alpha":13.45895, "fx":[-66.22483,-59.44539,-112.91661,-116.07678], "fy":[-18.57329,90.70324,50.79181,-23.85839]}, + {"t":3.17131, "x":7.49154, "y":3.31679, "heading":-3.04019, "vx":-0.58851, "vy":-1.91036, "omega":-3.40917, "ax":-7.12817, "ay":2.48958, "alpha":6.61621, "fx":[-122.47311,-102.6169,-125.10641,-134.79582], "fy":[22.10962,81.87184,56.36386,9.04307]}, + {"t":3.19091, "x":7.47864, "y":3.27983, "heading":-3.107, "vx":-0.72821, "vy":-1.86157, "omega":-3.27951, "ax":-7.62336, "ay":3.34927, "alpha":3.67809, "fx":[-132.68039,-117.92625,-129.05463,-139.02338], "fy":[47.18455,79.86132,64.83375,36.00059]}, + {"t":3.21051, "x":7.4629, "y":3.24399, "heading":3.11192, "vx":-0.87761, "vy":-1.79594, "omega":-3.20742, "ax":-7.58965, "ay":4.11921, "alpha":2.34975, "fx":[-131.6493,-120.54123,-127.69934,-136.50127], "fy":[64.64547,84.51626,74.82535,56.27897]}, + {"t":3.23011, "x":7.44425, "y":3.20958, "heading":3.04906, "vx":-1.02635, "vy":-1.71521, "omega":-3.16137, "ax":-7.33111, "ay":4.83275, "alpha":1.65844, "fx":[-126.72642,-117.84342,-123.29481,-130.9355], "fy":[78.91079,92.04958,85.2526,72.60168]}, + {"t":3.2497, "x":7.42273, "y":3.1769, "heading":2.9871, "vx":-1.17002, "vy":-1.6205, "omega":-3.12887, "ax":-6.94598, "ay":5.49813, "alpha":1.25487, "fx":[-119.76149,-112.33026,-116.90344,-123.60101], "fy":[91.41033,100.59743,95.54882,86.53018]}, + {"t":3.2693, "x":7.39846, "y":3.14619, "heading":2.92578, "vx":-1.30615, "vy":-1.51275, "omega":-3.10428, "ax":-6.4714, "ay":6.11692, "alpha":0.99814, "fx":[-111.36745,-104.95661,-109.01707,-114.96543], "fy":[102.65636,109.30975,105.42117,98.80105]}, + {"t":3.2889, "x":7.37162, "y":3.11772, "heading":2.86495, "vx":-1.43297, "vy":-1.39287, "omega":-3.08472, "ax":-5.89135, "ay":6.71656, "alpha":0.82376, "fx":[-101.22406,-95.57399,-99.34708,-104.69586], "fy":[113.35125,118.21932,115.15843,110.25774]}, + {"t":3.3085, "x":7.34241, "y":3.09172, "heading":2.80449, "vx":-1.54843, "vy":-1.26124, "omega":-3.06858, "ax":-4.83927, "ay":7.53409, "alpha":0.6959, "fx":[-82.87476,-77.89234,-81.83736,-86.65418], "fy":[127.79553,130.9356,128.55819,125.3213]}, + {"t":3.3281, "x":7.31113, "y":3.06844, "heading":2.74436, "vx":-1.64327, "vy":-1.11359, "omega":-3.05494, "ax":-3.51236, "ay":8.25291, "alpha":0.59441, "fx":[-59.80646,-55.60307,-59.71076,-63.85664], "fy":[140.35814,142.10644,140.45919,138.59478]}, + {"t":3.34769, "x":7.27825, "y":3.04821, "heading":2.68449, "vx":-1.7121, "vy":-0.95185, "omega":-3.04329, "ax":-2.51441, "ay":8.62073, "alpha":0.51753, "fx":[-42.59942,-39.01761,-42.94466,-46.51598], "fy":[146.69068,147.70582,146.63105,145.51671]}, + {"t":3.36729, "x":7.24422, "y":3.03121, "heading":2.62484, "vx":-1.76138, "vy":-0.7829, "omega":-3.03315, "ax":-1.79499, "ay":8.80656, "alpha":0.46052, "fx":[-30.29743,-27.13004,-30.7644,-33.93715], "fy":[149.84971,150.47183,149.78448,149.0821]}, + {"t":3.38689, "x":7.20935, "y":3.01756, "heading":2.5654, "vx":-1.79656, "vy":-0.61031, "omega":-3.02412, "ax":-1.26597, "ay":8.90375, "alpha":0.41765, "fx":[-21.31744,-18.41512,-21.74582,-24.65658], "fy":[151.48569,151.87844,151.4478,150.98926]}, + {"t":3.40649, "x":7.1739, "y":3.0073, "heading":2.50614, "vx":-1.82137, "vy":-0.43582, "omega":-3.01594, "ax":-0.86564, "ay":8.95587, "alpha":0.38466, "fx":[-14.56651,-11.83235,-14.87884,-17.61952], "fy":[152.35661,152.60324,152.34483,152.04278]}, + {"t":3.42608, "x":7.13804, "y":3.00048, "heading":2.44703, "vx":-1.83833, "vy":-0.2603, "omega":-3.0084, "ax":-0.55436, "ay":8.98389, "alpha":0.35865, "fx":[-9.34927,-6.7211,-9.50857,-12.1394], "fy":[152.82267,152.96878,152.828,152.63395]}, + {"t":3.44568, "x":7.10191, "y":2.99711, "heading":2.38807, "vx":-1.8492, "vy":-0.08424, "omega":-3.00137, "ax":-0.24667, "ay":9.00393, "alpha":0.34676, "fx":[-4.18401,-1.56948,-4.20789,-6.82199], "fy":[153.16034,153.21509,153.17066,153.07114]}, + {"t":3.47241, "x":7.0524, "y":2.99807, "heading":2.30786, "vx":-1.85579, "vy":0.15639, "omega":-2.9921, "ax":0.22695, "ay":9.00069, "alpha":0.38942, "fx":[3.79108,6.80896,3.92787,0.9138], "fy":[153.10761,153.01049,153.11884,153.15953]}, + {"t":3.49913, "x":7.00288, "y":3.00546, "heading":2.2279, "vx":-1.84972, "vy":0.39694, "omega":-2.98169, "ax":0.91548, "ay":8.95144, "alpha":0.4498, "fx":[15.48145,18.95787,15.66239,12.18627], "fy":[152.2791,151.89604,152.28153,152.58922]}, + {"t":3.52586, "x":6.95378, "y":3.01927, "heading":2.14821, "vx":-1.82526, "vy":0.63616, "omega":-2.96967, "ax":1.97667, "ay":8.7699, "alpha":0.53928, "fx":[33.68095,37.59349,33.57884,29.63687], "fy":[149.17144,148.25117,149.22725,150.044]}, + {"t":3.55258, "x":6.9057, "y":3.0394, "heading":2.06884, "vx":-1.77243, "vy":0.87054, "omega":-2.95526, "ax":3.67113, "ay":8.19191, "alpha":0.67465, "fx":[63.0929,67.01758,61.87083,57.79843], "fy":[139.06653,137.25333,139.67276,141.37535]}, + {"t":3.57931, "x":6.85964, "y":3.06559, "heading":1.98987, "vx":-1.67432, "vy":1.08947, "omega":-2.93723, "ax":5.287, "ay":7.22984, "alpha":0.8695, "fx":[91.3832,94.95048,88.66171,84.7258], "fy":[121.9301,119.252,124.03729,126.69063]}, + {"t":3.60603, "x":6.81679, "y":3.09729, "heading":1.91137, "vx":-1.53302, "vy":1.28269, "omega":-2.91399, "ax":6.11851, "ay":6.49394, "alpha":1.17771, "fx":[106.16238,110.04657,102.34058,97.74719], "fy":[108.50258,104.74524,112.36111,116.23098]}, + {"t":3.63276, "x":6.778, "y":3.13389, "heading":1.83349, "vx":-1.36951, "vy":1.45624, "omega":-2.88252, "ax":6.79517, "ay":5.67627, "alpha":1.73951, "fx":[118.67943,123.07543,113.25025,107.33023], "fy":[92.80881,87.43652,100.00753,105.95391]}, + {"t":3.65948, "x":6.74383, "y":3.17484, "heading":1.75645, "vx":-1.1879, "vy":1.60794, "omega":-2.83603, "ax":7.2769, "ay":4.75111, "alpha":2.94058, "fx":[128.75061,133.95621,120.79129,111.61397], "fy":[72.78593,65.07243,87.59865,97.8028]}, + {"t":3.68621, "x":6.71468, "y":3.21951, "heading":1.68066, "vx":-0.99343, "vy":1.73491, "omega":-2.75744, "ax":7.28888, "ay":3.66258, "alpha":6.22413, "fx":[132.50066,140.12769,122.16117,101.13725], "fy":[40.38354,32.41972,77.61698,98.77782]}, + {"t":3.71293, "x":6.69073, "y":3.26718, "heading":1.60697, "vx":-0.79863, "vy":1.83279, "omega":-2.5911, "ax":5.19741, "ay":2.03781, "alpha":17.74404, "fx":[76.06791,131.10843,110.68181,35.76775], "fy":[-42.54742,-12.88907,76.95981,117.12727]}, + {"t":3.73966, "x":6.67125, "y":3.31689, "heading":1.53772, "vx":-0.65973, "vy":1.88726, "omega":-2.11689, "ax":1.76011, "ay":0.59101, "alpha":27.85537, "fx":[-49.40082,104.32225,98.44181,-33.60728], "fy":[-79.84632,-56.12845,72.48182,103.7049]}, + {"t":3.76638, "x":6.65424, "y":3.36754, "heading":1.48115, "vx":-0.61269, "vy":1.90305, "omega":-1.37245, "ax":0.57967, "ay":0.18405, "alpha":27.89261, "fx":[-73.42064,79.62179,88.55348,-55.31438], "fy":[-69.48997,-74.08326,67.06451,89.03144]}, + {"t":3.79311, "x":6.63807, "y":3.41846, "heading":1.44447, "vx":-0.5972, "vy":1.90797, "omega":-0.62702, "ax":0.18027, "ay":0.05618, "alpha":26.34164, "fx":[-76.68125,64.98814,81.37951,-57.42099], "fy":[-61.47216,-76.96587,61.0675,81.19275]}, + {"t":3.81983, "x":6.62218, "y":3.46947, "heading":1.42771, "vx":-0.59238, "vy":1.90947, "omega":0.07696, "ax":0.05184, "ay":0.01606, "alpha":24.22926, "fx":[-72.94854,56.1632,74.34545,-54.03277], "fy":[-55.10488,-73.10175,55.09953,74.2]}, + {"t":3.84656, "x":6.60637, "y":3.52051, "heading":1.42977, "vx":-0.591, "vy":1.9099, "omega":0.72449, "ax":0.01365, "ay":0.00422, "alpha":21.52931, "fx":[-65.14674,49.36445,65.53527,-48.82455], "fy":[-49.08042,-65.21198,49.109,65.47057]}, + {"t":3.87328, "x":6.59058, "y":3.57155, "heading":1.44913, "vx":-0.59063, "vy":1.91001, "omega":1.29986, "ax":0.0034, "ay":0.00105, "alpha":18.04581, "fx":[-53.91012,42.26774,54.01342,-42.13951], "fy":[-42.19607,-53.93358,42.21119,53.99]}, + {"t":3.90001, "x":6.57479, "y":3.6226, "heading":1.48387, "vx":-0.59054, "vy":1.91004, "omega":1.78214, "ax":0.00178, "ay":0.00055, "alpha":13.5304, "fx":[-39.30747,33.06144,39.36464,-32.99782], "fy":[-33.02345,-39.32358,33.03581,39.34853]}, + {"t":3.92673, "x":6.55901, "y":3.67364, "heading":1.5315, "vx":-0.59049, "vy":1.91006, "omega":2.14374, "ax":0.00735, "ay":0.00227, "alpha":7.80777, "fx":[-21.64254,20.24582,21.88908,-19.99207], "fy":[-20.08483,-21.72267,20.15307,21.80904]}, + {"t":3.95346, "x":6.54323, "y":3.72469, "heading":1.58879, "vx":-0.5903, "vy":1.91012, "omega":2.35241, "ax":0.04917, "ay":0.01518, "alpha":1.08641, "fx":[-2.02706,3.80474,3.69937,-2.13177], "fy":[-2.71078,-2.60473,3.22584,3.12222]}, + {"t":3.98018, "x":6.52747, "y":3.77574, "heading":1.65166, "vx":-0.58898, "vy":1.91052, "omega":2.38144, "ax":0.31548, "ay":0.09649, "alpha":-5.7843, "fx":[19.54487,-11.34002,-8.84283,22.10303], "fy":[18.26666,16.00531,-15.21165,-12.49512]}, + {"t":4.00691, "x":6.51185, "y":3.82684, "heading":1.7153, "vx":-0.58055, "vy":1.9131, "omega":2.22685, "ax":1.77129, "ay":0.48426, "alpha":-11.01231, "fx":[53.60832,-2.71571,6.90839,62.71566], "fy":[40.18021,37.23357,-28.45368,-16.01164]}, + {"t":4.03363, "x":6.49696, "y":3.87814, "heading":1.77481, "vx":-0.53321, "vy":1.92604, "omega":1.93255, "ax":1.81131, "ay":0.50685, "alpha":-9.73776, "fx":[49.90353,0.38009,12.3096,60.64619], "fy":[38.38402,32.31454,-24.93377,-11.2791]}, + {"t":4.05998, "x":6.48355, "y":3.92906, "heading":1.82573, "vx":-0.4855, "vy":1.9394, "omega":1.67601, "ax":0.32117, "ay":0.07966, "alpha":-9.39958, "fx":[23.55343,-25.36958,-12.46687,36.13539], "fy":[31.8667,19.74498,-29.7438,-16.44769]}, + {"t":4.08632, "x":6.47087, "y":3.98018, "heading":1.86988, "vx":-0.47704, "vy":1.94149, "omega":1.42839, "ax":0.05386, "ay":0.01321, "alpha":-8.33245, "fx":[15.71288,-27.06154,-13.85256,28.8658], "fy":[28.15229,15.04704,-27.77779,-14.52253]}, + {"t":4.11267, "x":6.45832, "y":4.03133, "heading":1.90751, "vx":-0.47562, "vy":1.94184, "omega":1.20888, "ax":0.00886, "ay":0.00217, "alpha":-7.28949, "fx":[12.15654,-24.78479,-11.85097,25.0819], "fy":[24.9657,12.04527,-24.90106,-11.96234]}, + {"t":4.13901, "x":6.44579, "y":4.08248, "heading":1.93936, "vx":-0.47538, "vy":1.9419, "omega":1.01684, "ax":0.00143, "ay":0.00035, "alpha":-6.29066, "fx":[9.69328,-21.81172,-9.6439,21.85994], "fy":[21.84127,9.6751,-21.83039,-9.66208]}, + {"t":4.16536, "x":6.43327, "y":4.13364, "heading":1.96614, "vx":-0.47535, "vy":1.94191, "omega":0.85111, "ax":0.00023, "ay":0.00006, "alpha":-5.33321, "fx":[7.70216,-18.72142,-7.69428,18.72915], "fy":[18.72618,7.69924,-18.72439,-7.6972]}, + {"t":4.1917, "x":6.42075, "y":4.1848, "heading":1.98857, "vx":-0.47534, "vy":1.94191, "omega":0.71061, "ax":0.00004, "ay":0.00001, "alpha":-4.41299, "fx":[6.02157,-15.63265,-6.02032,15.63387], "fy":[15.6334,6.0211,-15.63311,-6.02079]}, + {"t":4.21805, "x":6.40822, "y":4.23596, "heading":2.00729, "vx":-0.47534, "vy":1.94191, "omega":0.59436, "ax":0.00001, "ay":0.0, "alpha":-3.52511, "fx":[4.57503,-12.57563,-4.57484,12.57582], "fy":[12.57575,4.57496,-12.5757,-4.57491]}, + {"t":4.24439, "x":6.3957, "y":4.28712, "heading":2.02295, "vx":-0.47534, "vy":1.94191, "omega":0.50149, "ax":0.0, "ay":0.0, "alpha":-2.66406, "fx":[3.30822,-9.55692,-3.30822,9.55692], "fy":[9.55692,3.30822,-9.55692,-3.30822]}, + {"t":4.27073, "x":6.38318, "y":4.33828, "heading":2.03616, "vx":-0.47534, "vy":1.94191, "omega":0.43131, "ax":-0.00001, "ay":0.0, "alpha":-1.82389, "fx":[2.17818,-6.57239,-2.17836,6.57222], "fy":[6.57229,2.17825,-6.57233,-2.17829]}, + {"t":4.29708, "x":6.37066, "y":4.38943, "heading":2.04752, "vx":-0.47534, "vy":1.94191, "omega":0.38326, "ax":-0.00003, "ay":-0.00001, "alpha":-0.99833, "fx":[1.15077,-3.61131,-1.15192,3.61015], "fy":[3.61059,1.1512,-3.61087,-1.15149]}, + {"t":4.32342, "x":6.35813, "y":4.44059, "heading":2.05762, "vx":-0.47534, "vy":1.94191, "omega":0.35696, "ax":-0.00022, "ay":-0.00005, "alpha":-0.18086, "fx":[0.1982,-0.65995,-0.20572,0.65243], "fy":[0.65527,0.20104,-0.65711,-0.20288]}, + {"t":4.34977, "x":6.34561, "y":4.49175, "heading":2.06702, "vx":-0.47535, "vy":1.94191, "omega":0.35219, "ax":-0.00144, "ay":-0.00035, "alpha":0.63514, "fx":[-0.71201,2.28658,0.66312,-2.33546], "fy":[-2.317,-0.69355,2.30504,0.68157]}, + {"t":4.37611, "x":6.33309, "y":4.54291, "heading":2.0763, "vx":-0.47538, "vy":1.9419, "omega":0.36893, "ax":-0.00932, "ay":-0.00228, "alpha":1.4563, "fx":[-1.68594,5.15489,1.36858,-5.47166], "fy":[-5.35195,-1.56624,5.27462,1.48829]}, + {"t":4.40246, "x":6.32056, "y":4.59407, "heading":2.08602, "vx":-0.47563, "vy":1.94184, "omega":0.40729, "ax":-0.06025, "ay":-0.01478, "alpha":2.28899, "fx":[-3.34619,7.35153,1.29173,-9.39639], "fy":[-8.62364,-2.57337,8.12547,2.06568]}, + {"t":4.4288, "x":6.30801, "y":4.64522, "heading":2.09675, "vx":-0.47722, "vy":1.94145, "omega":0.46759, "ax":-0.38675, "ay":-0.09615, "alpha":3.12809, "fx":[-9.63829,4.91134,-3.57716,-18.00994], "fy":[-13.10157,-4.72588,9.88693,1.39883]}, + {"t":4.45514, "x":6.2953, "y":4.69633, "heading":2.10907, "vx":-0.4874, "vy":1.93892, "omega":0.55, "ax":-2.30065, "ay":-0.61696, "alpha":3.4971, "fx":[-41.92589,-26.72184,-36.73001,-51.15574], "fy":[-23.78812,-14.62834,3.26552,-6.82644]}, + {"t":4.48149, "x":6.28166, "y":4.7472, "heading":2.12355, "vx":-0.54801, "vy":1.92266, "omega":0.64213, "ax":-6.4601, "ay":-2.15969, "alpha":1.36363, "fx":[-108.85643,-106.68633,-110.99697,-112.99784], "fy":[-43.47143,-40.38287,-29.63857,-33.45031]}, + {"t":4.50783, "x":6.26499, "y":4.7971, "heading":2.14047, "vx":-0.7182, "vy":1.86577, "omega":0.67805, "ax":-7.64302, "ay":-3.4394, "alpha":0.43909, "fx":[-129.11692,-129.06909,-130.91174,-130.9244], "fy":[-60.80305,-60.14347,-56.14553,-56.9206]}, + {"t":4.53418, "x":6.24341, "y":4.84506, "heading":2.15833, "vx":-0.91955, "vy":1.77516, "omega":0.68962, "ax":-7.52207, "ay":-4.46729, "alpha":0.21542, "fx":[-127.37926,-127.37039,-128.52484,-128.51843], "fy":[-77.00966,-76.88876,-74.95042,-75.10013]}, + {"t":4.56052, "x":6.21658, "y":4.89027, "heading":2.1765, "vx":-1.11772, "vy":1.65747, "omega":0.6953, "ax":-7.03353, "ay":-5.41299, "alpha":0.13269, "fx":[-119.24502,-119.17923,-120.03578,-120.09332], "fy":[-92.60509,-92.65122,-91.5368,-91.50037]}, + {"t":4.58687, "x":6.18469, "y":4.93206, "heading":2.19482, "vx":-1.30301, "vy":1.51487, "omega":0.69879, "ax":-6.39063, "ay":-6.16525, "alpha":0.12339, "fx":[-108.33049,-108.18166,-109.07939,-109.21976], "fy":[-105.27317,-105.39583,-104.46197,-104.3459]}, + {"t":4.61439, "x":6.14641, "y":4.97142, "heading":2.21405, "vx":-1.47891, "vy":1.34517, "omega":0.70219, "ax":-5.48661, "ay":-6.81126, "alpha":0.21583, "fx":[-92.72774,-92.23924,-93.93718,-94.3986], "fy":[-116.3969,-116.69738,-115.31195,-115.02384]}, + {"t":4.64192, "x":6.10362, "y":5.00587, "heading":2.23338, "vx":-1.62993, "vy":1.1577, "omega":0.70813, "ax":-4.42457, "ay":-7.05115, "alpha":0.49777, "fx":[-74.17264,-72.48304,-76.4117,-77.97562], "fy":[-120.93549,-121.52677,-118.92627,-118.36391]}, + {"t":4.66944, "x":6.05708, "y":5.03506, "heading":2.25287, "vx":-1.75171, "vy":0.96362, "omega":0.72183, "ax":-2.77782, "ay":-5.60829, "alpha":1.916, "fx":[-45.65513,-37.40908,-49.28021,-56.65556], "fy":[-100.16225,-98.76591,-90.46469,-92.18856]}, + {"t":4.69696, "x":6.00781, "y":5.05946, "heading":2.27274, "vx":-1.82817, "vy":0.80925, "omega":0.77457, "ax":-0.65749, "ay":-1.53251, "alpha":4.47717, "fx":[-12.31034,6.26996,-10.25545,-28.43898], "fy":[-42.27198,-28.18688,-9.23211,-24.5791]}, + {"t":4.72449, "x":5.95725, "y":5.08115, "heading":2.29406, "vx":-1.84627, "vy":0.76707, "omega":0.8978, "ax":-0.09383, "ay":-0.22691, "alpha":4.21307, "fx":[-2.59329,14.38744,-0.62278,-17.55535], "fy":[-19.77036,-4.90441,12.13813,-2.90238]}, + {"t":4.75201, "x":5.90639, "y":5.10218, "heading":2.31877, "vx":-1.84885, "vy":0.76082, "omega":1.01376, "ax":-0.01326, "ay":-0.03225, "alpha":3.69293, "fx":[-0.75152,13.78539,0.29725,-14.23352], "fy":[-14.55304,-1.07796,13.46526,-0.02863]}, + {"t":4.77954, "x":5.8555, "y":5.12311, "heading":2.34667, "vx":-1.84921, "vy":0.75993, "omega":1.11541, "ax":-0.00187, "ay":-0.00456, "alpha":3.20664, "fx":[-0.14794,12.14086,0.08383,-12.2042], "fy":[-12.24958,-0.19393,12.09547,0.03785]}, + {"t":4.80706, "x":5.8046, "y":5.14402, "heading":2.37738, "vx":-1.84927, "vy":0.75981, "omega":1.20367, "ax":-0.00026, "ay":-0.00064, "alpha":2.75579, "fx":[0.21706,10.45472,-0.22608,-10.46365], "fy":[-10.47006,0.21059,10.44831,-0.23254]}, + {"t":4.83459, "x":5.7537, "y":5.16494, "heading":2.41051, "vx":-1.84927, "vy":0.75979, "omega":1.27952, "ax":-0.00004, "ay":-0.00009, "alpha":2.33617, "fx":[0.4808,8.85488,-0.48206,-8.85613], "fy":[-8.85703,0.47989,8.85398,-0.48296]}, + {"t":4.86211, "x":5.7028, "y":5.18585, "heading":2.44572, "vx":-1.84927, "vy":0.75979, "omega":1.34382, "ax":-0.00001, "ay":-0.00001, "alpha":1.94354, "fx":[0.65959,7.34843,-0.65976,-7.34861], "fy":[-7.34873,0.65946,7.34831,-0.65989]}, + {"t":4.88964, "x":5.6519, "y":5.20676, "heading":2.48271, "vx":-1.84927, "vy":0.75979, "omega":1.39732, "ax":0.0, "ay":0.0, "alpha":1.5737, "fx":[0.7538,5.92633,-0.75382,-5.92635], "fy":[-5.92637,0.75378,5.92631,-0.75384]}, + {"t":4.91716, "x":5.601, "y":5.22768, "heading":2.52117, "vx":-1.84927, "vy":0.75979, "omega":1.44063, "ax":0.0, "ay":0.0, "alpha":1.22254, "fx":[0.7622,4.57799,-0.7622,-4.57799], "fy":[-4.57799,0.76219,4.57798,-0.7622]}, + {"t":4.94469, "x":5.5501, "y":5.24859, "heading":2.56083, "vx":-1.84927, "vy":0.75979, "omega":1.47428, "ax":0.0, "ay":0.0, "alpha":0.88602, "fx":[0.68348,3.29333,-0.68349,-3.29333], "fy":[-3.29333,0.68348,3.29332,-0.68349]}, + {"t":4.97221, "x":5.4992, "y":5.2695, "heading":2.6014, "vx":-1.84927, "vy":0.75979, "omega":1.49867, "ax":0.0, "ay":0.0, "alpha":0.56018, "fx":[0.51624,2.06293,-0.51624,-2.06293], "fy":[-2.06294,0.51624,2.06293,-0.51625]}, + {"t":4.99974, "x":5.4483, "y":5.29041, "heading":2.64266, "vx":-1.84927, "vy":0.75979, "omega":1.51409, "ax":0.0, "ay":0.0, "alpha":0.24111, "fx":[0.25863,0.87801,-0.25863,-0.87802], "fy":[-0.87802,0.25862,0.87801,-0.25864]}, + {"t":5.02726, "x":5.3974, "y":5.31133, "heading":2.68433, "vx":-1.84927, "vy":0.75979, "omega":1.52073, "ax":0.0, "ay":0.0, "alpha":-0.07503, "fx":[-0.0918,-0.26965,0.0918,0.26965], "fy":[0.26964,-0.09181,-0.26966,0.09179]}, + {"t":5.05478, "x":5.34649, "y":5.33224, "heading":2.72619, "vx":-1.84927, "vy":0.75979, "omega":1.51866, "ax":0.0, "ay":0.0, "alpha":-0.39211, "fx":[-0.53827,-1.3878,0.53826,1.38779], "fy":[1.38779,-0.53827,-1.3878,0.53826]}, + {"t":5.08231, "x":5.29559, "y":5.35315, "heading":2.76799, "vx":-1.84927, "vy":0.75979, "omega":1.50787, "ax":0.0, "ay":0.0, "alpha":-0.71395, "fx":[-1.08481,-2.48375,1.08481,2.48374], "fy":[2.48374,-1.08482,-2.48375,1.08481]}, + {"t":5.10983, "x":5.24469, "y":5.37407, "heading":2.80949, "vx":-1.84927, "vy":0.75979, "omega":1.48822, "ax":0.0, "ay":0.0, "alpha":-1.04444, "fx":[-1.73637,-3.5645,1.73636,3.5645], "fy":[3.56449,-1.73637,-3.5645,1.73636]}, + {"t":5.13736, "x":5.19379, "y":5.39498, "heading":2.85045, "vx":-1.84927, "vy":0.75979, "omega":1.45947, "ax":0.0, "ay":0.0, "alpha":-1.3875, "fx":[-2.49868,-4.63687,2.49867,4.63686], "fy":[4.63685,-2.49869,-4.63688,2.49866]}, + {"t":5.16488, "x":5.14289, "y":5.41589, "heading":2.89063, "vx":-1.84927, "vy":0.75979, "omega":1.42128, "ax":0.0, "ay":-0.00001, "alpha":-1.74712, "fx":[-3.37827,-5.70762,3.37819,5.70755], "fy":[5.7075,-3.37831,-5.70767,3.37814]}, + {"t":5.19241, "x":5.09199, "y":5.4368, "heading":2.92975, "vx":-1.84927, "vy":0.75979, "omega":1.37319, "ax":-0.00001, "ay":-0.00004, "alpha":-2.12735, "fx":[-4.38237,-6.78382,4.38187,6.78333], "fy":[6.78297,-4.38272,-6.78417,4.38152]}, + {"t":5.21993, "x":5.04109, "y":5.45772, "heading":2.96754, "vx":-1.84927, "vy":0.75979, "omega":1.31464, "ax":-0.0001, "ay":-0.00025, "alpha":-2.53236, "fx":[-5.51957,-7.87389,5.51603,7.87042], "fy":[7.86788,-5.52207,-7.87643,5.51353]}, + {"t":5.24746, "x":4.99019, "y":5.47863, "heading":3.00373, "vx":-1.84928, "vy":0.75978, "omega":1.24493, "ax":-0.00073, "ay":-0.00179, "alpha":-2.96636, "fx":[-6.80547,-8.99373,6.78016,8.96913], "fy":[8.95104,-6.82316,-9.01182,6.76246]}, + {"t":5.27498, "x":4.93929, "y":5.49954, "heading":3.03799, "vx":-1.8493, "vy":0.75973, "omega":1.16329, "ax":-0.0052, "ay":-0.01266, "alpha":-3.43361, "fx":[-8.30444,-10.20745,8.1243,10.0339], "fy":[9.90508,-8.42929,-10.33626,7.99933]}, + {"t":5.30251, "x":4.88839, "y":5.52045, "heading":3.07001, "vx":-1.84944, "vy":0.75938, "omega":1.06878, "ax":-0.03664, "ay":-0.0894, "alpha":-3.93756, "fx":[-10.42668,-11.90681,9.15006,10.69062], "fy":[9.77394,-11.30185,-12.82283,8.2681]}, + {"t":5.33003, "x":4.83747, "y":5.54132, "heading":3.09943, "vx":-1.85045, "vy":0.75692, "omega":0.9604, "ax":-0.25291, "ay":-0.62661, "alpha":-4.43569, "fx":[-15.92175,-16.57578,7.05375,8.23619], "fy":[1.69258,-21.92026,-23.08807,0.68163]}, + {"t":5.35756, "x":4.78644, "y":5.56191, "heading":3.12586, "vx":-1.85741, "vy":0.73968, "omega":0.83831, "ax":-1.34493, "ay":-3.66072, "alpha":-3.49656, "fx":[-35.04493,-32.19881,-11.67808,-12.58581], "fy":[-52.97793,-69.05133,-71.53287,-55.50933]}, + {"t":5.38508, "x":4.73481, "y":5.58089, "heading":-3.13425, "vx":-1.89443, "vy":0.63892, "omega":0.74207, "ax":-2.06683, "ay":-7.47854, "alpha":-0.87125, "fx":[-39.96622,-37.97545,-30.51056,-32.17262], "fy":[-125.42094,-127.23107,-128.90999,-127.26932]}, + {"t":5.4126, "x":4.68188, "y":5.59564, "heading":-3.11382, "vx":-1.95132, "vy":0.43307, "omega":0.71808, "ax":-1.07076, "ay":-7.21816, "alpha":-1.51969, "fx":[-26.03326,-23.60293,-10.92237,-12.2946], "fy":[-120.42839,-123.66252,-124.99709,-122.02735]}, + {"t":5.45286, "x":4.60247, "y":5.60722, "heading":-3.08492, "vx":-1.99442, "vy":0.14253, "omega":0.65692, "ax":-0.06781, "ay":-1.12919, "alpha":-5.80435, "fx":[-18.27229,-15.63701,15.17362,14.12181], "fy":[-4.80368,-35.29656,-33.73836,-2.98991]}, + {"t":5.49311, "x":4.52214, "y":5.61204, "heading":-3.05848, "vx":-1.99715, "vy":0.09708, "omega":0.42328, "ax":-0.00308, "ay":-0.06414, "alpha":-5.01096, "fx":[-14.5908,-12.3244,14.45338,12.25251], "fy":[11.19279,-15.60791,-13.38184,13.43311]}, + {"t":5.53336, "x":4.44175, "y":5.6159, "heading":-3.04144, "vx":-1.99727, "vy":0.0945, "omega":0.22158, "ax":-0.00017, "ay":-0.00355, "alpha":-4.13986, "fx":[-12.17163,-9.94816,12.1647,9.94369], "fy":[9.88542,-12.22836,-10.00643,12.10797]}, + {"t":5.57361, "x":4.36135, "y":5.6197, "heading":-3.03252, "vx":-1.99728, "vy":0.09436, "omega":0.05495, "ax":-0.00001, "ay":-0.00019, "alpha":-3.35669, "fx":[-9.93792,-7.97618,9.93756,7.97592], "fy":[7.97274,-9.94104,-7.97936,9.93444]}, + {"t":5.61386, "x":4.28096, "y":5.6235, "heading":-3.03031, "vx":-1.99728, "vy":0.09435, "omega":-0.08016, "ax":0.0, "ay":-0.00001, "alpha":-2.64763, "fx":[-7.85243,-6.27387,7.85241,6.27385], "fy":[6.27368,-7.8526,-6.27404,7.85224]}, + {"t":5.65411, "x":4.20057, "y":5.6273, "heading":-3.03353, "vx":-1.99728, "vy":0.09435, "omega":-0.18673, "ax":0.0, "ay":0.0, "alpha":-1.99853, "fx":[-5.91198,-4.75484,5.91198,4.75484], "fy":[4.75483,-5.91199,-4.75485,5.91197]}, + {"t":5.69436, "x":4.12017, "y":5.63109, "heading":-3.04105, "vx":-1.99728, "vy":0.09435, "omega":-0.26718, "ax":0.0, "ay":0.0, "alpha":-1.39553, "fx":[-4.10314,-3.35114,4.10314,3.35114], "fy":[3.35114,-4.10314,-3.35114,4.10314]}, + {"t":5.73461, "x":4.03978, "y":5.63489, "heading":-3.05181, "vx":-1.99728, "vy":0.09435, "omega":-0.32335, "ax":0.0, "ay":0.0, "alpha":-0.82515, "fx":[-2.40465,-2.00744,2.40465,2.00744], "fy":[2.00744,-2.40465,-2.00744,2.40465]}, + {"t":5.77487, "x":3.95939, "y":5.63869, "heading":-3.06482, "vx":-1.99728, "vy":0.09435, "omega":-0.35656, "ax":0.0, "ay":0.0, "alpha":-0.27421, "fx":[-0.79036,-0.67745,0.79036,0.67745], "fy":[0.67745,-0.79036,-0.67745,0.79036]}, + {"t":5.81512, "x":3.879, "y":5.64249, "heading":-3.07917, "vx":-1.99728, "vy":0.09435, "omega":-0.3676, "ax":0.0, "ay":0.0, "alpha":0.27024, "fx":[0.76924,0.67874,-0.76924,-0.67874], "fy":[-0.67874,0.76924,0.67874,-0.76924]}, + {"t":5.85537, "x":3.7986, "y":5.64628, "heading":-3.09397, "vx":-1.99728, "vy":0.09435, "omega":-0.35672, "ax":0.0, "ay":0.0, "alpha":0.82108, "fx":[2.30646,2.09661,-2.30646,-2.09661], "fy":[-2.09661,2.30646,2.09661,-2.30646]}, + {"t":5.89562, "x":3.71821, "y":5.65008, "heading":-3.10833, "vx":-1.99728, "vy":0.09435, "omega":-0.32367, "ax":0.0, "ay":0.0, "alpha":1.39127, "fx":[3.85677,3.60834,-3.85677,-3.60834], "fy":[-3.60834,3.85677,3.60834,-3.85677]}, + {"t":5.93587, "x":3.63782, "y":5.65388, "heading":-3.12136, "vx":-1.99728, "vy":0.09435, "omega":-0.26767, "ax":0.0, "ay":0.0, "alpha":1.99398, "fx":[5.45971,5.24309,-5.45971,-5.24309], "fy":[-5.24309,5.45971,5.24309,-5.45972]}, + {"t":5.97612, "x":3.55742, "y":5.65768, "heading":-3.13213, "vx":-1.99728, "vy":0.09435, "omega":-0.18741, "ax":0.0001, "ay":0.0, "alpha":2.6427, "fx":[7.1623,7.02804,-7.15905,-7.02479], "fy":[-7.02649,7.16059,7.02634,-7.16077]}, + {"t":6.01637, "x":3.47703, "y":5.66148, "heading":-3.13967, "vx":-1.99727, "vy":0.09435, "omega":-0.08104, "ax":4.64817, "ay":-0.21957, "alpha":1.95046, "fx":[82.79501,83.03203,75.34357,75.08563], "fy":[-9.98264,2.79813,2.97591,-10.73066]}, + {"t":6.05662, "x":3.4004, "y":5.66509, "heading":3.14025, "vx":-1.81018, "vy":0.08551, "omega":-0.00253, "ax":8.96793, "ay":-0.42363, "alpha":0.02249, "fx":[152.53718,152.54843,152.54662,152.53535], "fy":[-7.32005,-7.08056,-7.09132,-7.33117]}, + {"t":6.09688, "x":3.33481, "y":5.66819, "heading":3.14015, "vx":-1.44921, "vy":0.06846, "omega":-0.00162, "ax":8.99223, "ay":-0.42478, "alpha":0.01337, "fx":[152.9523,152.95903,152.95833,152.9516], "fy":[-7.29345,-7.15063,-7.15709,-7.30004]}, + {"t":6.13713, "x":3.28376, "y":5.67061, "heading":3.14008, "vx":-1.08726, "vy":0.05136, "omega":-0.00109, "ax":9.00038, "ay":-0.42516, "alpha":0.01032, "fx":[153.09157,153.09677,153.09633,153.09112], "fy":[-7.2845,-7.17417,-7.17916,-7.28958]}, + {"t":6.17738, "x":3.24729, "y":5.67233, "heading":3.14004, "vx":-0.72498, "vy":0.03425, "omega":-0.00067, "ax":9.00447, "ay":-0.42535, "alpha":0.00879, "fx":[153.16136,153.16579,153.16547,153.16102], "fy":[-7.28001,-7.18597,-7.19023,-7.28433]}, + {"t":6.21763, "x":3.2254, "y":5.67336, "heading":3.14001, "vx":-0.36254, "vy":0.01713, "omega":-0.00032, "ax":9.00692, "ay":-0.42547, "alpha":0.00787, "fx":[153.20328,153.20726,153.20699,153.20301], "fy":[-7.27731,-7.19306,-7.19688,-7.28117]}, + {"t":6.25788, "x":3.2181, "y":5.67371, "heading":3.14, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 3543b13..b7b8b30 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -288,9 +288,12 @@ private void updateDashboard() trajectory = flip(Arrays.asList(driveOption.trajectory.asAutoTraj(_trajectoryDrawRoutine).getRawTrajectory().getPoses())); } - _previewField.setRobotPose(startPose); - _previewField.getObject("Auto End Pose").setPose(endPose); - _previewField.getObject("Auto Trajectory").setPoses(trajectory); + if (startPose != null) + { + _previewField.setRobotPose(startPose); + _previewField.getObject("Auto End Pose").setPose(endPose); + _previewField.getObject("Auto Trajectory").setPoses(trajectory); + } SmartDashboard.putString("Auto Summary", buildSummary(mode, startPosition, driveOption, _delayChooser.getSelected())); } @@ -353,6 +356,11 @@ private List flip(List bluePoses) private Pose2d flip(Pose2d bluePose) { + if (bluePose == null) + { + return null; + } + if (Utilities.isRedAlliance()) { return bluePose.rotateAround(GeneralConstants.FIELD_CENTER, Rotation2d.k180deg); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8fd275d..2458e5d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.RadiansPerSecond; @@ -311,6 +312,9 @@ public static class VisionConstants // Reject vision updates when robot is tilted more than this (on ramp) public static final Angle MAX_TILT_FOR_VISION = Degrees.of(10.0); // TODO: find the correct value + + // Reject vision updates when no AprilTag is detected within this range + public static final Distance MAX_TARGET_DISTANCE = Feet.of(10); } public static class ClimberConstants diff --git a/src/main/java/frc/robot/generated/ChoreoTraj.java b/src/main/java/frc/robot/generated/ChoreoTraj.java index 2195fe4..4e01eba 100644 --- a/src/main/java/frc/robot/generated/ChoreoTraj.java +++ b/src/main/java/frc/robot/generated/ChoreoTraj.java @@ -26,7 +26,7 @@ public record ChoreoTraj(String name, OptionalInt segment, double totalTimeSecs, public static final ChoreoTraj HubToOutpost = new ChoreoTraj("HubToOutpost", OptionalInt.empty(), 1.67965, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); public static final ChoreoTraj HubToTower = new ChoreoTraj("HubToTower", OptionalInt.empty(), 1.74265, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); public static final ChoreoTraj LeftBumpPickupFromMidScore = new ChoreoTraj( - "LeftBumpPickupFromMidScore", OptionalInt.empty(), 4.29782, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)), new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)) + "LeftBumpPickupFromMidScore", OptionalInt.empty(), 6.25788, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), new Pose2d(3.218, 5.674, Rotation2d.fromRadians(3.14)) ); public static final ChoreoTraj LeftBumpToDepot = new ChoreoTraj("LeftBumpToDepot", OptionalInt.empty(), 1.11437, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); public static final ChoreoTraj LeftBumpToTower = new ChoreoTraj("LeftBumpToTower", OptionalInt.empty(), 1.71082, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 02ef7b6..9f7bce4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -435,6 +435,25 @@ private boolean processLimelight(VisionState state) return false; } + var hasCloseTag = false; + for (var target : results.get().targets_Fiducials) + { + var targetPose = target.getTargetPose_CameraSpace(); + var distance = Meters.of(targetPose.getTranslation().getNorm()); + + if (distance.lt(VisionConstants.MAX_DETECTION_RANGE)) + { + hasCloseTag = true; + break; + } + } + + if (!hasCloseTag) + { + setVisionState(state, false, kInvalidVisionPose); + return false; + } + Optional estimate = state._poseEstimator.getPoseEstimate(); if (estimate.isEmpty()) From 4a109cf88fc95d91e22fe6c9f9a05e449265f351 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Sat, 21 Mar 2026 14:13:52 -0500 Subject: [PATCH 29/32] disable vision pose correction while shooting --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/drive/Drive.java | 17 +++++++++++++++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1cb3978..248a277 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,7 +79,7 @@ private void configureBindings() final var idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled().whileTrue(_drive.applyRequest(() -> idle).ignoringDisable(true)); - _driver.button(1).whileTrue(_shooter.shoot()); + _driver.button(1).whileTrue(Commands.parallel(_shooter.shoot(), Commands.startEnd(() -> _drive.disableVisionPoseCorrection(true), () -> _drive.disableVisionPoseCorrection(false)))); _driver.button(2).whileTrue(Commands.startEnd(() -> _driveMultiplier = DriveConstants.SLOW_MODE_SCALE, () -> _driveMultiplier = DriveConstants.FULL_SPEED_SCALE)); _driver.button(3).whileTrue(_drive.applyRequest(() -> _robotCentric.withVelocityX(getDrive()).withVelocityY(getStrafe()).withRotationalRate(getRotate()))); _driver.button(5).whileTrue(_shooter.pass()); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9f7bce4..8d2af67 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -62,6 +62,8 @@ public class Drive extends TunerSwerveDrivetrain implements Subsystem private double _robotSpeedOmegaRadiansPerSecond = 0.0; @Logged private Translation2d _fieldRelativeTranslationalVelocity = new Translation2d(); + @Logged + private boolean _disableVisionPoseCorrection = false; /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; @@ -293,6 +295,11 @@ private void startSimThread() m_simNotifier.startPeriodic(kSimLoopPeriod); } + public void disableVisionPoseCorrection(boolean disable) + { + _disableVisionPoseCorrection = disable; + } + /** * Adds a vision measurement to the Kalman Filter. This will correct the * odometry pose estimate while still accounting for measurement noise. @@ -305,7 +312,10 @@ private void startSimThread() @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + if (!_disableVisionPoseCorrection) + { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } } /** @@ -327,7 +337,10 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + if (!_disableVisionPoseCorrection) + { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } } /** From ea1fb4f509461c760b6569c475b063e3e1eff70e Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Sun, 22 Mar 2026 16:25:49 -0500 Subject: [PATCH 30/32] WIP for auto --- src/main/deploy/choreo/HubToTower.traj | 101 ++-- .../choreo/LeftBumpPickupFromMidScore.traj | 489 ++++++++-------- .../choreo/RightBumpPickupFromMidScore.traj | 532 ++++++++++-------- src/main/java/frc/robot/Autos.java | 31 +- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/generated/ChoreoTraj.java | 188 +++++-- .../java/frc/robot/generated/ChoreoVars.java | 40 +- 8 files changed, 770 insertions(+), 619 deletions(-) diff --git a/src/main/deploy/choreo/HubToTower.traj b/src/main/deploy/choreo/HubToTower.traj index 05b5137..70a7e5e 100644 --- a/src/main/deploy/choreo/HubToTower.traj +++ b/src/main/deploy/choreo/HubToTower.traj @@ -3,9 +3,8 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.61569, "y":4.03479, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.1203222274780273, "y":4.873752593994141, "heading":1.2610933822524404, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":0.7993333339691162, "y":4.8954081535339355, "heading":1.5707963267948966, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.61569, "y":4.03479, "heading":0.0, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.8513697385787964, "y":4.055983066558838, "heading":-1.554924685475252, "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}, @@ -15,9 +14,8 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"HubStart.x", "val":3.61569}, "y":{"exp":"HubStart.y", "val":4.03479}, "heading":{"exp":"HubStart.heading", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.1203222274780273 m", "val":2.1203222274780273}, "y":{"exp":"4.873752593994141 m", "val":4.873752593994141}, "heading":{"exp":"1.2610933822524404 rad", "val":1.2610933822524404}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"TowerLeft.x", "val":0.7993333339691162}, "y":{"exp":"TowerLeft.y", "val":4.8954081535339355}, "heading":{"exp":"TowerLeft.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"HubStart.x", "val":3.61569}, "y":{"exp":"HubStart.y", "val":4.03479}, "heading":{"exp":"HubStart.heading", "val":0.0}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.8513697385787964 m", "val":1.8513697385787964}, "y":{"exp":"4.055983066558838 m", "val":4.055983066558838}, "heading":{"exp":"-1.5549246854752519 rad", "val":-1.554924685475252}, "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}, @@ -53,63 +51,42 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.96954,1.74265], + "waypoints":[0.0,1.10566], "samples":[ - {"t":0.0, "x":3.61569, "y":4.03479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.81599, "ay":4.49215, "alpha":0.03342, "fx":[-132.91525,-133.0696,-132.98053,-132.82565], "fy":[76.46758,76.19819,76.3527,76.62228]}, - {"t":0.03128, "x":3.61187, "y":4.03699, "heading":0.0, "vx":-0.24445, "vy":0.14049, "omega":0.00105, "ax":-7.81427, "ay":4.49116, "alpha":0.03625, "fx":[-132.88331,-133.05065,-132.95401,-132.78605], "fy":[76.45564,76.16347,76.331,76.6234]}, - {"t":0.06255, "x":3.6004, "y":4.04358, "heading":0.00003, "vx":-0.48885, "vy":0.28096, "omega":0.00218, "ax":-7.81186, "ay":4.48978, "alpha":0.04022, "fx":[-132.83858,-133.02409,-132.91683,-132.73056], "fy":[76.43892,76.11482,76.30059,76.62496]}, - {"t":0.09383, "x":3.58129, "y":4.05456, "heading":0.0001, "vx":-0.73317, "vy":0.42138, "omega":0.00344, "ax":-7.80824, "ay":4.4877, "alpha":0.04619, "fx":[-132.77143,-132.98419,-132.86095,-132.64719], "fy":[76.4138,76.04178,76.25491,76.62728]}, - {"t":0.1251, "x":3.55454, "y":4.06993, "heading":0.00021, "vx":-0.97737, "vy":0.56173, "omega":0.00488, "ax":-7.8022, "ay":4.48422, "alpha":0.05616, "fx":[-132.6594,-132.91755,-132.76757,-132.50796], "fy":[76.37186,75.91985,76.17859,76.63112]}, - {"t":0.15638, "x":3.52016, "y":4.0897, "heading":0.00036, "vx":-1.22139, "vy":0.70198, "omega":0.00664, "ax":-7.79006, "ay":4.47725, "alpha":0.07619, "fx":[-132.43495,-132.78369,-132.57989,-132.22846], "fy":[76.28778,75.67538,76.02526,76.63862]}, - {"t":0.18765, "x":3.47815, "y":4.11384, "heading":0.00057, "vx":-1.46503, "vy":0.84201, "omega":0.00902, "ax":-7.75337, "ay":4.45616, "alpha":0.1369, "fx":[-131.75923,-132.37807,-132.01013,-131.38275], "fy":[76.03421,74.93799,75.56035,76.65967]}, - {"t":0.21893, "x":3.42854, "y":4.14235, "heading":0.00085, "vx":-1.70752, "vy":0.98138, "omega":0.0133, "ax":-0.82889, "ay":0.47634, "alpha":11.52491, "fx":[-44.01199,-45.68322,17.07099,16.22748], "fy":[37.83736,-22.14625,-23.89831,40.61701]}, - {"t":0.2502, "x":3.37473, "y":4.17328, "heading":0.00127, "vx":-1.73345, "vy":0.99628, "omega":0.37375, "ax":-0.00003, "ay":0.00005, "alpha":10.48001, "fx":[-28.16777,-28.0966,28.16694,28.09556], "fy":[28.09687,-28.1665,-28.09529,28.16821]}, - {"t":0.28148, "x":3.32051, "y":4.20444, "heading":0.01296, "vx":-1.73345, "vy":0.99628, "omega":0.70152, "ax":0.0, "ay":0.0, "alpha":9.25984, "fx":[-25.17632,-24.53226,25.17638,24.53231], "fy":[24.53233,-25.1763,-24.53224,25.17639]}, - {"t":0.31276, "x":3.2663, "y":4.2356, "heading":0.0349, "vx":-1.73345, "vy":0.99628, "omega":0.99113, "ax":0.0, "ay":0.0, "alpha":8.11157, "fx":[-22.5205,-21.00113,22.5205,21.00114], "fy":[21.00113,-22.5205,-21.00113,22.5205]}, - {"t":0.34403, "x":3.21208, "y":4.26676, "heading":0.06589, "vx":-1.73345, "vy":0.99628, "omega":1.24482, "ax":0.0, "ay":0.0, "alpha":7.03789, "fx":[-20.09494,-17.60699,20.09494,17.60699], "fy":[17.60698,-20.09495,-17.60699,20.09493]}, - {"t":0.37531, "x":3.15787, "y":4.29792, "heading":0.10483, "vx":-1.73345, "vy":0.99628, "omega":1.46493, "ax":0.0, "ay":0.0, "alpha":6.03854, "fx":[-17.81648,-14.42433,17.81648,14.42433], "fy":[14.42433,-17.81648,-14.42434,17.81647]}, - {"t":0.40658, "x":3.10365, "y":4.32908, "heading":0.15064, "vx":-1.73345, "vy":0.99628, "omega":1.65379, "ax":0.0, "ay":0.0, "alpha":5.11062, "fx":[-15.62199,-11.50438,15.622,11.50439], "fy":[11.50438,-15.62199,-11.50438,15.622]}, - {"t":0.43786, "x":3.04944, "y":4.36024, "heading":0.20237, "vx":-1.73345, "vy":0.99628, "omega":1.81363, "ax":0.0, "ay":0.0, "alpha":4.24896, "fx":[-13.46523,-8.88044,13.46525,8.88046], "fy":[8.88047,-13.46523,-8.88044,13.46526]}, - {"t":0.46913, "x":2.99522, "y":4.3914, "heading":0.25909, "vx":-1.73345, "vy":0.99628, "omega":1.94652, "ax":0.0, "ay":0.0, "alpha":3.44663, "fx":[-11.3134,-6.57273,11.31343,6.57276], "fy":[6.57277,-11.31339,-6.57272,11.31344]}, - {"t":0.50041, "x":2.94101, "y":4.42255, "heading":0.31997, "vx":-1.73345, "vy":0.99628, "omega":2.05432, "ax":0.0, "ay":0.0, "alpha":2.69523, "fx":[-9.14329,-4.59202,9.14333,4.59207], "fy":[4.59208,-9.14327,-4.59201,9.14335]}, - {"t":0.53169, "x":2.8868, "y":4.45371, "heading":0.38422, "vx":-1.73345, "vy":0.99628, "omega":2.13861, "ax":0.0, "ay":0.0, "alpha":1.98546, "fx":[-6.93872,-2.94329,6.93878,2.94334], "fy":[2.94336,-6.93871,-2.94327,6.93879]}, - {"t":0.56296, "x":2.83258, "y":4.48487, "heading":0.4511, "vx":-1.73345, "vy":0.99628, "omega":2.20071, "ax":0.0, "ay":0.0, "alpha":1.3072, "fx":[-4.68768,-1.62815,4.68772,1.6282], "fy":[1.62822,-4.68766,-1.62814,4.68774]}, - {"t":0.59424, "x":2.77837, "y":4.51603, "heading":0.51993, "vx":-1.73345, "vy":0.99628, "omega":2.24159, "ax":0.0, "ay":0.0, "alpha":0.65003, "fx":[-2.38121,-0.6474,2.38124,0.64743], "fy":[0.64744,-2.3812,-0.64739,2.38125]}, - {"t":0.62551, "x":2.72415, "y":4.54719, "heading":0.59004, "vx":-1.73345, "vy":0.99628, "omega":2.26192, "ax":0.0, "ay":0.0, "alpha":0.00324, "fx":[-0.01208,-0.00239,0.01208,0.00239], "fy":[0.0024,-0.01207,-0.00238,0.01209]}, - {"t":0.65679, "x":2.66994, "y":4.57835, "heading":0.66078, "vx":-1.73345, "vy":0.99628, "omega":2.26202, "ax":0.0, "ay":0.0, "alpha":-0.64391, "fx":[2.42543,0.30381,-2.42545,-0.30383], "fy":[-0.30384,2.42542,0.3038,-2.42546]}, - {"t":0.68806, "x":2.61572, "y":4.60951, "heading":0.73153, "vx":-1.73345, "vy":0.99628, "omega":2.24188, "ax":0.0, "ay":0.0, "alpha":-1.30206, "fx":[4.93567,0.2661,-4.93574,-0.26618], "fy":[-0.2662,4.93565,0.26608,-4.93577]}, - {"t":0.71934, "x":2.56151, "y":4.64067, "heading":0.80165, "vx":-1.73345, "vy":0.99628, "omega":2.20116, "ax":-0.00001, "ay":-0.00002, "alpha":-1.98172, "fx":[7.52185,-0.12242,-7.52223,0.12203], "fy":[0.12189,7.5217,-0.12256,-7.52237]}, - {"t":0.75061, "x":2.50729, "y":4.67183, "heading":0.87049, "vx":-1.73345, "vy":0.99628, "omega":2.13918, "ax":-0.0001, "ay":-0.00017, "alpha":-2.69306, "fx":[10.18478,-0.8705,-10.18803,0.86721], "fy":[0.866,10.18358,-0.87171,-10.18924]}, - {"t":0.78189, "x":2.45308, "y":4.70299, "heading":0.93739, "vx":-1.73345, "vy":0.99627, "omega":2.05495, "ax":-0.0009, "ay":-0.00156, "alpha":-3.44585, "fx":[12.91525,-1.99605,-12.94547,1.96518], "fy":[1.95389,12.90394,-2.00733,-12.95678]}, - {"t":0.81317, "x":2.39886, "y":4.73415, "heading":1.00166, "vx":-1.73348, "vy":0.99622, "omega":1.94718, "ax":-0.00844, "ay":-0.01469, "alpha":-4.2492, "fx":[15.61417,-3.60759,-15.89607,3.31531], "fy":[3.20965,15.50707,-3.71303,-16.0031]}, - {"t":0.84444, "x":2.34464, "y":4.7653, "heading":1.06256, "vx":-1.73374, "vy":0.99576, "omega":1.81429, "ax":-0.07856, "ay":-0.13717, "alpha":-5.10836, "fx":[17.3553,-6.68614,-19.95236,3.93818], "fy":[2.94676,16.33794,-7.65418,-20.96362]}, - {"t":0.87572, "x":2.29038, "y":4.79637, "heading":1.1193, "vx":-1.7362, "vy":0.99147, "omega":1.65452, "ax":-0.69417, "ay":-1.24782, "alpha":-5.75087, "fx":[9.40635,-19.96543,-32.09881,-4.57284], "fy":[-14.81012,-0.52773,-28.03631,-41.52592]}, - {"t":0.90699, "x":2.23574, "y":4.82677, "heading":1.17105, "vx":-1.75791, "vy":0.95245, "omega":1.47466, "ax":-2.81451, "ay":-5.89638, "alpha":-2.24807, "fx":[-38.20221,-55.8188,-56.61661,-40.85851], "fy":[-101.59212,-93.45619,-99.45744,-106.67735]}, - {"t":0.93827, "x":2.17938, "y":4.85368, "heading":1.21717, "vx":-1.84594, "vy":0.76804, "omega":1.40435, "ax":-2.71909, "ay":-8.06307, "alpha":-0.49233, "fx":[-43.73568,-48.53795,-48.68901,-44.04119], "fy":[-137.84642,-136.22033,-136.47842,-138.05668]}, - {"t":0.96954, "x":2.12032, "y":4.87375, "heading":1.26109, "vx":-1.93098, "vy":0.51586, "omega":1.38895, "ax":-1.62302, "ay":-8.35377, "alpha":-0.47188, "fx":[-24.93939,-29.66975,-30.20237,-25.61695], "fy":[-142.48361,-141.5388,-141.71308,-142.64528]}, - {"t":1.00316, "x":2.0545, "y":4.88637, "heading":1.30778, "vx":-1.98553, "vy":0.23506, "omega":1.37309, "ax":-0.40165, "ay":-5.84231, "alpha":-2.85263, "fx":[6.77776,-15.70505,-19.14868,0.74787], "fy":[-96.79953,-93.54939,-102.0377,-105.1175]}, - {"t":1.03677, "x":1.98753, "y":4.89097, "heading":1.35394, "vx":-1.99903, "vy":0.03868, "omega":1.2772, "ax":-0.01051, "ay":-0.87481, "alpha":-5.92775, "fx":[19.2516,-12.71311,-19.02049,11.76698], "fy":[-3.01629,4.03702,-26.9991,-33.54262]}, - {"t":1.07038, "x":1.92033, "y":4.89178, "heading":1.39687, "vx":-1.99939, "vy":0.00927, "omega":1.07795, "ax":-0.00031, "ay":-0.07877, "alpha":-5.2887, "fx":[16.45663,-11.55342,-16.42415,11.49962], "fy":[10.1771,15.10578,-12.87226,-17.77005]}, - {"t":1.104, "x":1.85312, "y":4.89205, "heading":1.4331, "vx":-1.9994, "vy":0.00662, "omega":0.90018, "ax":-0.00002, "ay":-0.00697, "alpha":-4.62092, "fx":[13.99034,-10.58592,-13.98813,10.58217], "fy":[10.46503,13.87105,-10.70303,-14.10739]}, - {"t":1.13761, "x":1.78592, "y":4.89227, "heading":1.46336, "vx":-1.9994, "vy":0.00639, "omega":0.74485, "ax":0.0, "ay":-0.00061, "alpha":-4.02379, "fx":[11.89718,-9.5808,-11.89704,9.58053], "fy":[9.57022,11.88671,-9.59111,-11.90751]}, - {"t":1.17122, "x":1.71871, "y":4.89248, "heading":1.4884, "vx":-1.9994, "vy":0.00637, "omega":0.6096, "ax":0.0, "ay":-0.00005, "alpha":-3.49019, "fx":[10.10812,-8.56589,-10.10812,8.56587], "fy":[8.56496,10.10721,-8.56679,-10.10903]}, - {"t":1.20484, "x":1.6515, "y":4.89269, "heading":1.50889, "vx":-1.9994, "vy":0.00637, "omega":0.49228, "ax":0.0, "ay":0.0, "alpha":-3.01261, "fx":[8.57165,-7.57098,-8.57165,7.57098], "fy":[7.5709,8.57157,-7.57106,-8.57173]}, - {"t":1.23845, "x":1.5843, "y":4.89291, "heading":1.52543, "vx":-1.9994, "vy":0.00637, "omega":0.39102, "ax":0.0, "ay":0.0, "alpha":-2.58405, "fx":[7.24383,-6.61473,-7.24383,6.61473], "fy":[6.61473,7.24382,-6.61474,-7.24383]}, - {"t":1.27207, "x":1.51709, "y":4.89312, "heading":1.53858, "vx":-1.9994, "vy":0.00637, "omega":0.30416, "ax":0.0, "ay":0.0, "alpha":-2.19799, "fx":[6.08711,-5.70698,-6.08711,5.70698], "fy":[5.70698,6.08711,-5.70698,-6.08711]}, - {"t":1.30568, "x":1.44988, "y":4.89334, "heading":1.5488, "vx":-1.9994, "vy":0.00637, "omega":0.23028, "ax":0.0, "ay":0.0, "alpha":-1.84839, "fx":[5.06959,-4.85134,-5.06959,4.85134], "fy":[4.85134,5.06959,-4.85134,-5.06959]}, - {"t":1.33929, "x":1.38268, "y":4.89355, "heading":1.55654, "vx":-1.9994, "vy":0.00637, "omega":0.16815, "ax":0.0, "ay":0.0, "alpha":-1.52965, "fx":[4.1642,-4.04714,-4.1642,4.04714], "fy":[4.04714,4.1642,-4.04714,-4.1642]}, - {"t":1.37291, "x":1.31547, "y":4.89376, "heading":1.56219, "vx":-1.9994, "vy":0.00637, "omega":0.11673, "ax":0.0, "ay":0.0, "alpha":-1.2366, "fx":[3.34788,-3.29076,-3.34788,3.29076], "fy":[3.29076,3.34788,-3.29076,-3.34788]}, - {"t":1.40652, "x":1.24826, "y":4.89398, "heading":1.56612, "vx":-1.9994, "vy":0.00637, "omega":0.07516, "ax":0.0, "ay":0.0, "alpha":-0.9644, "fx":[2.60085,-2.57662,-2.60085,2.57662], "fy":[2.57662,2.60085,-2.57662,-2.60085]}, - {"t":1.44013, "x":1.18106, "y":4.89419, "heading":1.56864, "vx":-1.9994, "vy":0.00637, "omega":0.04274, "ax":0.0, "ay":0.0, "alpha":-0.70851, "fx":[1.90595,-1.89776,-1.90595,1.89776], "fy":[1.89776,1.90595,-1.89776,-1.90595]}, - {"t":1.47375, "x":1.11385, "y":4.89441, "heading":1.57008, "vx":-1.9994, "vy":0.00637, "omega":0.01893, "ax":0.00027, "ay":0.0, "alpha":-0.46462, "fx":[1.25267,-1.24172,-1.24351,1.25089], "fy":[1.24629,1.24808,-1.24632,-1.24811]}, - {"t":1.50736, "x":1.04664, "y":4.89462, "heading":1.57072, "vx":-1.99939, "vy":0.00637, "omega":0.00331, "ax":5.49209, "ay":-0.01749, "alpha":-0.10509, "fx":[93.59718,93.24182,93.24046,93.59582], "fy":[0.08878,0.08924,-0.68557,-0.68257]}, - {"t":1.54097, "x":0.98254, "y":4.89482, "heading":1.57083, "vx":-1.81478, "vy":0.00578, "omega":-0.00022, "ax":8.95744, "ay":-0.02853, "alpha":0.00004, "fx":[152.36342,152.36342,152.36342,152.36342], "fy":[-0.4855,-0.48549,-0.48503,-0.48503]}, - {"t":1.57459, "x":0.9266, "y":4.895, "heading":1.57082, "vx":-1.51369, "vy":0.00482, "omega":-0.00022, "ax":8.9918, "ay":-0.02864, "alpha":0.00093, "fx":[152.94799,152.94803,152.94806,152.94802], "fy":[-0.49212,-0.49209,-0.48213,-0.48217]}, - {"t":1.6082, "x":0.8808, "y":4.89515, "heading":1.57081, "vx":-1.21144, "vy":0.00386, "omega":-0.00019, "ax":9.00339, "ay":-0.02868, "alpha":0.00123, "fx":[153.14512,153.14515,153.14519,153.14516], "fy":[-0.49436,-0.49431,-0.48115,-0.4812]}, - {"t":1.64181, "x":0.84516, "y":4.89526, "heading":1.57081, "vx":-0.90881, "vy":0.00289, "omega":-0.00015, "ax":9.00921, "ay":-0.02869, "alpha":0.00138, "fx":[153.24409,153.24412,153.24417,153.24414], "fy":[-0.49548,-0.49543,-0.48066,-0.48071]}, - {"t":1.67543, "x":0.8197, "y":4.89534, "heading":1.5708, "vx":-0.60598, "vy":0.00193, "omega":-0.0001, "ax":9.01271, "ay":-0.0287, "alpha":0.00147, "fx":[153.3036,153.30363,153.30368,153.30365], "fy":[-0.49615,-0.4961,-0.48037,-0.48042]}, - {"t":1.70904, "x":0.80443, "y":4.89539, "heading":1.5708, "vx":-0.30303, "vy":0.00097, "omega":-0.00005, "ax":9.01505, "ay":-0.02871, "alpha":0.00153, "fx":[153.34333,153.34335,153.3434,153.34338], "fy":[-0.4966,-0.49655,-0.48017,-0.48022]}, - {"t":1.74265, "x":0.79933, "y":4.89541, "heading":1.5708, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + {"t":0.0, "x":3.61569, "y":4.03479, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.01438, "ay":0.10829, "alpha":-0.06655, "fx":[-153.33587,-153.32727,-153.32835,-153.33687], "fy":[1.48861,2.20385,2.19363,1.48171]}, + {"t":0.0335, "x":3.61063, "y":4.03485, "heading":0.0, "vx":-0.30203, "vy":0.00363, "omega":-0.00223, "ax":-9.01204, "ay":0.10826, "alpha":-0.07355, "fx":[-153.29628,-153.28677,-153.28817,-153.29758], "fy":[1.45103,2.24141,2.22993,1.4436]}, + {"t":0.06701, "x":3.59545, "y":4.03503, "heading":-0.00007, "vx":-0.60397, "vy":0.00726, "omega":-0.00469, "ax":-9.00853, "ay":0.10822, "alpha":-0.08402, "fx":[-153.23693,-153.22608,-153.22803,-153.23876], "fy":[1.3947,2.29764,2.28428,1.38662]}, + {"t":0.10051, "x":3.57016, "y":4.03534, "heading":-0.00023, "vx":-0.9058, "vy":0.01088, "omega":-0.00751, "ax":-9.00269, "ay":0.10815, "alpha":-0.10145, "fx":[-153.13817,-153.12506,-153.12814,-153.14107], "fy":[1.301,2.39111,2.3745,1.29208]}, + {"t":0.13402, "x":3.53476, "y":4.03576, "heading":-0.00048, "vx":-1.20744, "vy":0.01451, "omega":-0.01091, "ax":-8.99106, "ay":0.10802, "alpha":-0.13613, "fx":[-152.94119,-152.9236,-152.92965,-152.94691], "fy":[1.11441,2.57713,2.55351,1.10459]}, + {"t":0.16752, "x":3.48926, "y":4.03631, "heading":-0.00085, "vx":-1.50868, "vy":0.01812, "omega":-0.01547, "ax":-8.95662, "ay":0.10763, "alpha":-0.23892, "fx":[-152.35487,-152.32402,-152.34462,-152.3745], "fy":[0.56161,3.12797,3.0787,0.55452]}, + {"t":0.20103, "x":3.43368, "y":4.03698, "heading":-0.00137, "vx":-1.80877, "vy":0.02173, "omega":-0.02347, "ax":-5.68517, "ay":0.06796, "alpha":-10.19281, "fx":[-80.59173,-79.83253,-112.98712,-113.40107], "fy":[-44.87181,47.47312,31.2997,-29.27734]}, + {"t":0.23453, "x":3.36989, "y":4.03774, "heading":-0.00215, "vx":-1.99925, "vy":0.02401, "omega":-0.36498, "ax":-0.00036, "ay":0.00021, "alpha":-18.39329, "fx":[49.26164,49.47286,-49.27249,-49.48668], "fy":[-49.47742,49.27196,49.48211,-49.26216]}, + {"t":0.26804, "x":3.3029, "y":4.03855, "heading":-0.01438, "vx":-1.99926, "vy":0.02401, "omega":-0.98125, "ax":0.0, "ay":0.00003, "alpha":-16.42803, "fx":[43.45942,44.72775,-43.45926,-44.72788], "fy":[-44.72737,43.45979,44.72826,-43.45889]}, + {"t":0.30154, "x":3.23592, "y":4.03935, "heading":-0.04726, "vx":-1.99926, "vy":0.02402, "omega":-1.53167, "ax":0.0, "ay":0.00001, "alpha":-14.37694, "fx":[36.7261,40.37243,-36.72607,-40.37244], "fy":[-40.37235,36.72617,40.37253,-36.726]}, + {"t":0.33505, "x":3.16893, "y":4.04016, "heading":-0.09858, "vx":-1.99926, "vy":0.02402, "omega":-2.01336, "ax":0.0, "ay":0.00001, "alpha":-12.27092, "fx":[29.53741,36.02101,-29.53737,-36.02101], "fy":[-36.0209,29.53751,36.02112,-29.53727]}, + {"t":0.36855, "x":3.10195, "y":4.04096, "heading":-0.16603, "vx":-1.99926, "vy":0.02402, "omega":-2.4245, "ax":0.0, "ay":0.00001, "alpha":-10.14492, "fx":[22.357,31.35852,-22.35696,-31.35853], "fy":[-31.35832,22.3572,31.35874,-22.35676]}, + {"t":0.40206, "x":3.03496, "y":4.04177, "heading":-0.24727, "vx":-1.99926, "vy":0.02402, "omega":-2.7644, "ax":0.0, "ay":0.00002, "alpha":-8.03182, "fx":[15.62733,26.18118,-15.62729,-26.18118], "fy":[-26.18089,15.62761,26.18147,-15.62701]}, + {"t":0.43556, "x":2.96798, "y":4.04257, "heading":-0.33989, "vx":-1.99926, "vy":0.02402, "omega":-3.03351, "ax":0.0, "ay":0.00001, "alpha":-5.95638, "fx":[9.74376,20.40455,-9.74374,-20.40454], "fy":[-20.4043,9.744,20.40478,-9.7435]}, + {"t":0.46907, "x":2.90099, "y":4.04338, "heading":-0.44153, "vx":-1.99926, "vy":0.02402, "omega":-3.23308, "ax":0.0, "ay":0.0, "alpha":-3.93001, "fx":[5.02977,14.04571,-5.02976,-14.0457], "fy":[-14.04569,5.02978,14.04573,-5.02974]}, + {"t":0.50257, "x":2.83401, "y":4.04418, "heading":-0.54985, "vx":-1.99926, "vy":0.02402, "omega":-3.36475, "ax":0.0, "ay":-0.00002, "alpha":-1.94912, "fx":[1.72681,7.19493,-1.72681,-7.19493], "fy":[-7.19523,1.72651,7.19463,-1.72711]}, + {"t":0.53608, "x":2.76702, "y":4.04498, "heading":-0.66259, "vx":-1.99926, "vy":0.02402, "omega":-3.43006, "ax":0.0, "ay":-0.00003, "alpha":0.00398, "fx":[-0.00186,-0.01501,0.00185,0.015], "fy":[0.01448,-0.00238,-0.01554,0.00133]}, + {"t":0.56958, "x":2.70004, "y":4.04579, "heading":-0.77751, "vx":-1.99926, "vy":0.02402, "omega":-3.42992, "ax":0.0, "ay":-0.00003, "alpha":1.95575, "fx":[-0.05858,-7.4242,0.05857,7.42418], "fy":[7.42368,-0.05908,-7.42469,0.05807]}, + {"t":0.60309, "x":2.63305, "y":4.04659, "heading":-0.89243, "vx":-1.99926, "vy":0.02401, "omega":-3.3644, "ax":0.0, "ay":-0.00002, "alpha":3.93344, "fx":[1.59512,-14.8467,-1.59514,14.84669], "fy":[14.84643,1.59486,-14.84696,-1.5954]}, + {"t":0.63659, "x":2.56607, "y":4.0474, "heading":-1.00515, "vx":-1.99926, "vy":0.02401, "omega":-3.23261, "ax":0.0, "ay":0.0, "alpha":5.95635, "fx":[4.92905,-22.06775,-4.92906,22.06774], "fy":[22.06777,4.92908,-22.06772,-4.92903]}, + {"t":0.6701, "x":2.49908, "y":4.0482, "heading":-1.11346, "vx":-1.99926, "vy":0.02401, "omega":-3.03304, "ax":0.0, "ay":0.00001, "alpha":8.02965, "fx":[9.82161,-28.85657,-9.82163,28.85658], "fy":[28.85677,9.82183,-28.85638,-9.82141]}, + {"t":0.7036, "x":2.4321, "y":4.04901, "heading":-1.21508, "vx":-1.99926, "vy":0.02401, "omega":-2.76401, "ax":0.0, "ay":0.00001, "alpha":10.14203, "fx":[16.03892,-35.00138,-16.03895,35.0014], "fy":[35.0016,16.03916,-35.00119,-16.03871]}, + {"t":0.73711, "x":2.36511, "y":4.04981, "heading":-1.30769, "vx":-1.99926, "vy":0.02401, "omega":-2.4242, "ax":0.0, "ay":0.00001, "alpha":12.26845, "fx":[23.23397,-40.36433,-23.234,40.36434], "fy":[40.36445,23.23412,-40.36422,-23.23386]}, + {"t":0.77061, "x":2.29813, "y":4.05062, "heading":-1.38891, "vx":-1.99926, "vy":0.02402, "omega":-2.01315, "ax":0.0, "ay":-0.00001, "alpha":14.37527, "fx":[30.97141,-44.93129,-30.97139,44.93124], "fy":[44.93114,30.97126,-44.93139,-30.97154]}, + {"t":0.80412, "x":2.23114, "y":4.05142, "heading":-1.45636, "vx":-1.99926, "vy":0.02401, "omega":-1.53151, "ax":0.0, "ay":-0.0001, "alpha":16.42717, "fx":[38.77257,-48.84282,-38.77207,48.84222], "fy":[48.84088,38.77053,-48.84417,-38.7741]}, + {"t":0.83762, "x":2.16416, "y":4.05223, "heading":-1.50767, "vx":-1.99926, "vy":0.02401, "omega":-0.98112, "ax":0.00036, "ay":-0.00083, "alpha":18.39311, "fx":[46.16934,-52.38619,-46.15117,52.39218], "fy":[52.37423,46.14704,-52.40413,-46.17346]}, + {"t":0.87113, "x":2.09717, "y":4.05303, "heading":-1.54055, "vx":-1.99925, "vy":0.02398, "omega":-0.36486, "ax":5.68518, "ay":-0.06697, "alpha":10.1891, "fx":[112.93305,80.07372,80.35546,113.45103], "fy":[30.30443,43.81085,-48.41553,-30.25626]}, + {"t":0.90463, "x":2.03338, "y":4.0538, "heading":-1.55277, "vx":-1.80877, "vy":0.02174, "omega":-0.02347, "ax":8.95662, "ay":-0.10774, "alpha":0.23891, "fx":[152.37439,152.35459,152.32369,152.34525], "fy":[-0.53299,-0.58817,-3.15339,-3.05611]}, + {"t":0.93814, "x":1.9778, "y":4.05446, "heading":-1.55356, "vx":-1.50868, "vy":0.01813, "omega":-0.01547, "ax":8.99106, "ay":-0.10808, "alpha":0.13612, "fx":[152.94695,152.94103,152.92342,152.9299], "fy":[-1.093,-1.12863,-2.5907,-2.54129]}, + {"t":0.97164, "x":1.9323, "y":4.05501, "heading":-1.55408, "vx":-1.20744, "vy":0.01451, "omega":-0.01091, "ax":9.00269, "ay":-0.10819, "alpha":0.10144, "fx":[153.14111,153.13806,153.12494,153.1283], "fy":[-1.28378,-1.31112,-2.40074,-2.36573]}, + {"t":1.00515, "x":1.8969, "y":4.05544, "heading":-1.55444, "vx":-0.9058, "vy":0.01088, "omega":-0.00751, "ax":9.00853, "ay":-0.10825, "alpha":0.08402, "fx":[153.23881,153.23685,153.22598,153.22815], "fy":[-1.37992,-1.4028,-2.30534,-2.27719]}, + {"t":1.03865, "x":1.87161, "y":4.05574, "heading":-1.55469, "vx":-0.60397, "vy":0.00726, "omega":-0.00469, "ax":9.01204, "ay":-0.10828, "alpha":0.07354, "fx":[153.29762,153.2962,153.28669,153.28827], "fy":[-1.43783,-1.45796,-2.24798,-2.22381]}, + {"t":1.07216, "x":1.85643, "y":4.05592, "heading":-1.55485, "vx":-0.30203, "vy":0.00363, "omega":-0.00223, "ax":9.01438, "ay":-0.10831, "alpha":0.06655, "fx":[153.33691,153.3358,153.32719,153.32843], "fy":[-1.47652,-1.4948,-2.20971,-2.18812]}, + {"t":1.10566, "x":1.85137, "y":4.05598, "heading":-1.55492, "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/LeftBumpPickupFromMidScore.traj b/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj index 5dbc19d..48880ca 100644 --- a/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj +++ b/src/main/deploy/choreo/LeftBumpPickupFromMidScore.traj @@ -3,40 +3,40 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.61569, "y":5.558789999999999, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.61945915222168, "y":5.5876383781433105, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.301177978515625, "y":5.6295905113220215, "heading":-0.5585991876326754, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.584353923797607, "y":4.738111972808838, "heading":-1.2360600261652088, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.594841480255127, "y":3.8151695728302, "heading":-1.8233502920797129, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.1019062995910645, "y":2.9971067905426025, "heading":2.3880715859232047, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.496963977813721, "y":3.878139019012451, "heading":1.774814111378504, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.184691429138184, "y":4.932059288024902, "heading":2.194818993599192, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":4.68187952041626, "y":5.595638275146484, "heading":-3.1138221488567326, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.218101739883423, "y":5.673706531524658, "heading":3.14, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.652064085006714, "y":5.513687610626221, "heading":0.0, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.420478820800781, "y":6.077170372009277, "heading":0.0, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.743437767028809, "y":7.412379264831543, "heading":-1.4094214624859571, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.927182197570801, "y":6.040421485900879, "heading":-1.4801363332370263, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.025178909301758, "y":4.8032097816467285, "heading":-1.518211867264296, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.939431667327881, "y":3.3455047607421875, "heading":-2.1669953661010677, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.726719379425049, "y":3.3700039386749268, "heading":1.774814111378504, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.184691429138184, "y":4.932059288024902, "heading":2.194818993599192, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.68187952041626, "y":5.595638275146484, "heading":-3.1138221488567326, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":1.973865985870361, "y":5.587185382843018, "heading":-2.4370992285106707, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, - {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":2.0}}, "enabled":true}], + {"from":1, "to":8, "data":{"type":"MaxVelocity", "props":{"max":2.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"LeftBump.x", "val":3.61569}, "y":{"exp":"LeftBump.y", "val":5.558789999999999}, "heading":{"exp":"-0 mrad", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"LeftMiddle.x", "val":6.61945915222168}, "y":{"exp":"LeftMiddle.y", "val":5.5876383781433105}, "heading":{"exp":"LeftMiddle.heading", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.301177978515625 m", "val":7.301177978515625}, "y":{"exp":"5.6295905113220215 m", "val":5.6295905113220215}, "heading":{"exp":"-0.5585991876326754 rad", "val":-0.5585991876326754}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.584353923797607 m", "val":7.584353923797607}, "y":{"exp":"4.738111972808838 m", "val":4.738111972808838}, "heading":{"exp":"-1.2360600261652088 rad", "val":-1.2360600261652088}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.594841480255127 m", "val":7.594841480255127}, "y":{"exp":"3.8151695728302 m", "val":3.8151695728302}, "heading":{"exp":"-1.8233502920797129 rad", "val":-1.8233502920797129}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.1019062995910645 m", "val":7.1019062995910645}, "y":{"exp":"2.9971067905426025 m", "val":2.9971067905426025}, "heading":{"exp":"2.3880715859232047 rad", "val":2.3880715859232047}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.496963977813721 m", "val":6.496963977813721}, "y":{"exp":"3.878139019012451 m", "val":3.878139019012451}, "heading":{"exp":"1.774814111378504 rad", "val":1.774814111378504}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.184691429138184 m", "val":6.184691429138184}, "y":{"exp":"4.932059288024902 m", "val":4.932059288024902}, "heading":{"exp":"2.194818993599192 rad", "val":2.194818993599192}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"4.68187952041626 m", "val":4.68187952041626}, "y":{"exp":"5.595638275146484 m", "val":5.595638275146484}, "heading":{"exp":"-3.1138221488567326 rad", "val":-3.1138221488567326}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.218101739883423 m", "val":3.218101739883423}, "y":{"exp":"5.673706531524658 m", "val":5.673706531524658}, "heading":{"exp":"3.14 rad", "val":3.14}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.652064085006714 m", "val":3.652064085006714}, "y":{"exp":"5.513687610626221 m", "val":5.513687610626221}, "heading":{"exp":"-0 mrad", "val":0.0}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.420478820800781 m", "val":6.420478820800781}, "y":{"exp":"6.077170372009277 m", "val":6.077170372009277}, "heading":{"exp":"LeftMiddle.heading", "val":0.0}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.743437767028809 m", "val":7.743437767028809}, "y":{"exp":"7.412379264831543 m", "val":7.412379264831543}, "heading":{"exp":"-1.4094214624859573 rad", "val":-1.4094214624859571}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.927182197570801 m", "val":7.927182197570801}, "y":{"exp":"6.040421485900879 m", "val":6.040421485900879}, "heading":{"exp":"-1.4801363332370265 rad", "val":-1.4801363332370263}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.025178909301758 m", "val":8.025178909301758}, "y":{"exp":"4.8032097816467285 m", "val":4.8032097816467285}, "heading":{"exp":"-1.518211867264296 rad", "val":-1.518211867264296}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.939431667327881 m", "val":7.939431667327881}, "y":{"exp":"3.3455047607421875 m", "val":3.3455047607421875}, "heading":{"exp":"-2.1669953661010677 rad", "val":-2.1669953661010677}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.726719379425049 m", "val":6.726719379425049}, "y":{"exp":"3.3700039386749268 m", "val":3.3700039386749268}, "heading":{"exp":"1.774814111378504 rad", "val":1.774814111378504}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.184691429138184 m", "val":6.184691429138184}, "y":{"exp":"4.932059288024902 m", "val":4.932059288024902}, "heading":{"exp":"2.194818993599192 rad", "val":2.194818993599192}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.68187952041626 m", "val":4.68187952041626}, "y":{"exp":"5.595638275146484 m", "val":5.595638275146484}, "heading":{"exp":"-3.1138221488567326 rad", "val":-3.1138221488567326}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"1.9738659858703613 m", "val":1.973865985870361}, "y":{"exp":"5.587185382843018 m", "val":5.587185382843018}, "heading":{"exp":"-2.4370992285106707 rad", "val":-2.4370992285106707}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, - {"from":"first", "to":"last", "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2 m / s", "val":2.0}}}, "enabled":true}], + {"from":1, "to":8, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"2.5 m / s", "val":2.5}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -67,225 +67,234 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,1.61422,1.96942,2.45471,2.91654,3.44568,4.03363,4.58687,5.4126,6.25788], + "waypoints":[0.0,0.92155,1.76954,2.41425,2.91108,3.51958,4.04843,4.72052,5.38483,6.27732], "samples":[ - {"t":0.0, "x":3.61569, "y":5.55879, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.01731, "ay":0.06273, "alpha":0.00283, "fx":[153.3818,153.38201,153.38204,153.38183], "fy":[1.08237,1.05202,1.05181,1.08216]}, - {"t":0.04612, "x":3.62528, "y":5.55886, "heading":0.0, "vx":0.41588, "vy":0.00289, "omega":0.00013, "ax":9.01424, "ay":0.06271, "alpha":0.00337, "fx":[153.32956,153.32981,153.32986,153.32961], "fy":[1.08495,1.04876,1.04851,1.08469]}, - {"t":0.09224, "x":3.65405, "y":5.55906, "heading":0.00001, "vx":0.83163, "vy":0.00579, "omega":0.00029, "ax":9.00814, "ay":0.06267, "alpha":0.00446, "fx":[153.22561,153.22594,153.22604,153.22571], "fy":[1.09007,1.04227,1.04195,1.08973]}, - {"t":0.13836, "x":3.70198, "y":5.55939, "heading":0.00002, "vx":1.24709, "vy":0.00868, "omega":0.00049, "ax":8.99007, "ay":0.06254, "alpha":0.00767, "fx":[152.91802,152.91859,152.91893,152.91836], "fy":[1.10519,1.0231,1.02256,1.1046]}, - {"t":0.18448, "x":3.76906, "y":5.55986, "heading":0.00004, "vx":1.66172, "vy":0.01156, "omega":0.00085, "ax":7.32421, "ay":0.05096, "alpha":0.33089, "fx":[124.27096,124.28756,124.89431,124.8783], "fy":[2.36154,-0.61194,-0.60043,2.31775]}, - {"t":0.2306, "x":3.85349, "y":5.56044, "heading":0.00008, "vx":1.99951, "vy":0.01391, "omega":0.01611, "ax":0.00019, "ay":0.0, "alpha":1.32941, "fx":[-3.56569,-3.56511,3.57203,3.57145], "fy":[3.56831,-3.56884,-3.56825,3.56888]}, - {"t":0.27672, "x":3.94571, "y":5.56109, "heading":0.00082, "vx":1.99952, "vy":0.01391, "omega":0.07742, "ax":0.0, "ay":0.0, "alpha":1.09723, "fx":[-2.94775,-2.9429,2.94775,2.9429], "fy":[2.9429,-2.94775,-2.9429,2.94775]}, - {"t":0.32284, "x":4.03793, "y":5.56173, "heading":0.00439, "vx":1.99952, "vy":0.01391, "omega":0.12803, "ax":0.0, "ay":0.0, "alpha":0.90032, "fx":[-2.42736,-2.40612,2.42736,2.40612], "fy":[2.40612,-2.42736,-2.40612,2.42736]}, - {"t":0.36897, "x":4.13015, "y":5.56237, "heading":0.0103, "vx":1.99952, "vy":0.01391, "omega":0.16955, "ax":0.0, "ay":0.0, "alpha":0.73244, "fx":[-1.98624,-1.94574,1.98624,1.94574], "fy":[1.94574,-1.98624,-1.94574,1.98624]}, - {"t":0.41509, "x":4.22237, "y":5.56301, "heading":0.01812, "vx":1.99952, "vy":0.01391, "omega":0.20333, "ax":0.0, "ay":0.0, "alpha":0.5882, "fx":[-1.60727,-1.55006,1.60727,1.55006], "fy":[1.55006,-1.60727,-1.55006,1.60727]}, - {"t":0.46121, "x":4.31459, "y":5.56365, "heading":0.0275, "vx":1.99952, "vy":0.01391, "omega":0.23046, "ax":0.0, "ay":0.0, "alpha":0.46299, "fx":[-1.27652,-1.20818,1.27652,1.20818], "fy":[1.20818,-1.27652,-1.20818,1.27652]}, - {"t":0.50733, "x":4.40681, "y":5.56429, "heading":0.03813, "vx":1.99952, "vy":0.01391, "omega":0.25181, "ax":0.0, "ay":0.0, "alpha":0.35278, "fx":[-0.98237,-0.91018,0.98237,0.91018], "fy":[0.91018,-0.98237,-0.91018,0.98237]}, - {"t":0.55345, "x":4.49903, "y":5.56494, "heading":0.04974, "vx":1.99952, "vy":0.01391, "omega":0.26808, "ax":0.0, "ay":0.0, "alpha":0.25399, "fx":[-0.71485,-0.64705,0.71485,0.64705], "fy":[0.64705,-0.71485,-0.64705,0.71485]}, - {"t":0.59957, "x":4.59125, "y":5.56558, "heading":0.0621, "vx":1.99952, "vy":0.01391, "omega":0.2798, "ax":0.0, "ay":0.0, "alpha":0.16344, "fx":[-0.46512,-0.41066,0.46512,0.41066], "fy":[0.41066,-0.46512,-0.41066,0.46512]}, - {"t":0.64569, "x":4.68346, "y":5.56622, "heading":0.07501, "vx":1.99952, "vy":0.01391, "omega":0.28733, "ax":0.0, "ay":0.0, "alpha":0.0782, "fx":[-0.22506,-0.1936,0.22506,0.1936], "fy":[0.1936,-0.22506,-0.1936,0.22506]}, - {"t":0.69181, "x":4.77568, "y":5.56686, "heading":0.08826, "vx":1.99952, "vy":0.01391, "omega":0.29094, "ax":0.0, "ay":0.0, "alpha":-0.00451, "fx":[0.01311,0.01098,-0.01311,-0.01098], "fy":[-0.01098,0.01311,0.01098,-0.01311]}, - {"t":0.73793, "x":4.8679, "y":5.5675, "heading":0.10168, "vx":1.99952, "vy":0.01391, "omega":0.29073, "ax":0.0, "ay":0.0, "alpha":-0.08736, "fx":[0.25709,0.20949,-0.25709,-0.20949], "fy":[-0.20949,0.25709,0.20949,-0.25709]}, - {"t":0.78405, "x":4.96012, "y":5.56814, "heading":0.11509, "vx":1.99952, "vy":0.01391, "omega":0.2867, "ax":0.0, "ay":0.0, "alpha":-0.17305, "fx":[0.51478,0.4081,-0.51478,-0.4081], "fy":[-0.4081,0.51478,0.4081,-0.51478]}, - {"t":0.83017, "x":5.05234, "y":5.56878, "heading":0.12831, "vx":1.99952, "vy":0.01391, "omega":0.27872, "ax":0.0, "ay":0.0, "alpha":-0.26435, "fx":[0.79456,0.61296,-0.79456,-0.61296], "fy":[-0.61296,0.79456,0.61296,-0.79456]}, - {"t":0.87629, "x":5.14456, "y":5.56943, "heading":0.14116, "vx":1.99952, "vy":0.01391, "omega":0.26653, "ax":0.0, "ay":0.0, "alpha":-0.36422, "fx":[1.10552,0.8304,-1.10552,-0.8304], "fy":[-0.8304,1.10552,0.8304,-1.10552]}, - {"t":0.92241, "x":5.23678, "y":5.57007, "heading":0.15346, "vx":1.99952, "vy":0.01391, "omega":0.24973, "ax":0.0, "ay":0.0, "alpha":-0.4759, "fx":[1.45772,1.06718,-1.45772,-1.06718], "fy":[-1.06718,1.45772,1.06718,-1.45772]}, - {"t":0.96853, "x":5.329, "y":5.57071, "heading":0.16497, "vx":1.99952, "vy":0.01391, "omega":0.22778, "ax":0.0, "ay":0.0, "alpha":-0.60298, "fx":[1.86244,1.3308,-1.86244,-1.3308], "fy":[-1.3308,1.86244,1.3308,-1.86244]}, - {"t":1.01465, "x":5.42122, "y":5.57135, "heading":0.17548, "vx":1.99952, "vy":0.01391, "omega":0.19997, "ax":0.0, "ay":0.0, "alpha":-0.74957, "fx":[2.33245,1.62991,-2.33245,-1.62991], "fy":[-1.62991,2.33245,1.62991,-2.33245]}, - {"t":1.06077, "x":5.51344, "y":5.57199, "heading":0.1847, "vx":1.99952, "vy":0.01391, "omega":0.1654, "ax":0.0, "ay":0.0, "alpha":-0.92036, "fx":[2.88224,1.97479,-2.88224,-1.97479], "fy":[-1.97479,2.88224,1.97479,-2.88224]}, - {"t":1.1069, "x":5.60566, "y":5.57263, "heading":0.19233, "vx":1.99952, "vy":0.01391, "omega":0.12296, "ax":0.0, "ay":0.0, "alpha":-1.1208, "fx":[3.52821,2.37803,-3.52821,-2.37803], "fy":[-2.37803,3.52821,2.37803,-3.52821]}, - {"t":1.15302, "x":5.69788, "y":5.57328, "heading":0.198, "vx":1.99952, "vy":0.01391, "omega":0.07126, "ax":0.0, "ay":0.0, "alpha":-1.35727, "fx":[4.28884,2.85547,-4.28884,-2.85547], "fy":[-2.85547,4.28884,2.85547,-4.28884]}, - {"t":1.19914, "x":5.7901, "y":5.57392, "heading":0.20129, "vx":1.99952, "vy":0.01391, "omega":0.00867, "ax":0.0, "ay":0.0, "alpha":-1.63719, "fx":[5.18466,3.42736,-5.18466,-3.42736], "fy":[-3.42736,5.18466,3.42736,-5.18466]}, - {"t":1.24526, "x":5.88231, "y":5.57456, "heading":0.20169, "vx":1.99952, "vy":0.01391, "omega":-0.06684, "ax":0.0, "ay":0.0, "alpha":-1.96929, "fx":[6.238,4.1201,-6.238,-4.1201], "fy":[-4.1201,6.238,4.1201,-6.238]}, - {"t":1.29138, "x":5.97453, "y":5.5752, "heading":0.19861, "vx":1.99952, "vy":0.01391, "omega":-0.15767, "ax":0.0, "ay":0.0, "alpha":-2.36375, "fx":[7.47224,4.96844,-7.47224,-4.96844], "fy":[-4.96844,7.47224,4.96844,-7.47224]}, - {"t":1.3375, "x":6.06675, "y":5.57584, "heading":0.19133, "vx":1.99952, "vy":0.01391, "omega":-0.26669, "ax":0.0, "ay":0.0, "alpha":-2.83244, "fx":[8.91031,6.01854,-8.91031,-6.01854], "fy":[-6.01853,8.91033,6.01855,-8.9103]}, - {"t":1.38362, "x":6.15897, "y":5.57648, "heading":0.17903, "vx":1.99952, "vy":0.01391, "omega":-0.39732, "ax":0.0, "ay":0.00002, "alpha":-3.38906, "fx":[10.57197,7.33187,-10.57197,-7.33188], "fy":[-7.33148,10.57236,7.33227,-10.57158]}, - {"t":1.42974, "x":6.25119, "y":5.57713, "heading":0.16071, "vx":1.99952, "vy":0.01391, "omega":-0.55362, "ax":0.0, "ay":0.00061, "alpha":-4.0493, "fx":[12.46895,8.99005,-12.4689,-8.9904], "fy":[-8.97975,12.47933,9.0007,-12.45852]}, - {"t":1.47586, "x":6.34341, "y":5.57777, "heading":0.13518, "vx":1.99952, "vy":0.01394, "omega":-0.74038, "ax":-0.00012, "ay":0.01624, "alpha":-4.83078, "fx":[14.59851,11.09579,-14.59486,-11.10736], "fy":[-10.82424,14.8718,11.37876,-14.32139]}, - {"t":1.52198, "x":6.43563, "y":5.57843, "heading":0.10103, "vx":1.99952, "vy":0.01469, "omega":-0.96318, "ax":-0.0052, "ay":0.42402, "alpha":-5.72816, "fx":[16.93925,13.54853,-16.83128,-14.01008], "fy":[-6.47508,23.99735,20.95436,-9.62662]}, - {"t":1.5681, "x":6.52784, "y":5.57956, "heading":0.05661, "vx":1.99928, "vy":0.03425, "omega":-1.22737, "ax":-0.55675, "ay":6.11403, "alpha":-2.56256, "fx":[1.15465,-0.049,-18.88515,-20.10126], "fy":[101.13029,108.43905,106.98105,99.44135]}, - {"t":1.61422, "x":6.61946, "y":5.58764, "heading":0.0, "vx":1.9736, "vy":0.31623, "omega":-1.34555, "ax":-1.45756, "ay":7.39043, "alpha":-0.51208, "fx":[-22.86479,-22.20022,-26.65875,-27.44664], "fy":[125.5938,126.55163,125.84492,124.84632]}, - {"t":1.63396, "x":6.65812, "y":5.59532, "heading":-0.02655, "vx":1.94484, "vy":0.46206, "omega":-1.35566, "ax":-1.3799, "ay":5.19323, "alpha":-1.29365, "fx":[-19.57232,-18.32416,-27.16223,-28.82801], "fy":[86.48428,91.28038,90.22898,85.3477]}, - {"t":1.65369, "x":6.69623, "y":5.60545, "heading":-0.0533, "vx":1.91761, "vy":0.56454, "omega":-1.38119, "ax":-0.64188, "ay":2.09658, "alpha":-2.28746, "fx":[-4.92935,-4.06471,-16.68513,-17.99376], "fy":[29.67706,41.42657,41.58158,29.96338]}, - {"t":1.67342, "x":6.73394, "y":5.61699, "heading":-0.08056, "vx":1.90494, "vy":0.60591, "omega":-1.42632, "ax":-0.18887, "ay":0.58762, "alpha":-2.43074, "fx":[2.80365,3.82517,-9.16144,-10.31806], "fy":[2.99249,15.98699,16.96445,4.03736]}, - {"t":1.69315, "x":6.7715, "y":5.62907, "heading":-0.1087, "vx":1.90121, "vy":0.61751, "omega":-1.47429, "ax":-0.03323, "ay":0.10214, "alpha":-2.34848, "fx":[5.02304,6.38146,-6.14301,-7.52267], "fy":[-5.21533,7.32335,8.68378,-3.84242]}, - {"t":1.71289, "x":6.80901, "y":5.64127, "heading":-0.1378, "vx":1.90056, "vy":0.61952, "omega":-1.52063, "ax":0.06098, "ay":-0.18771, "alpha":-2.25758, "fx":[6.19895,7.88513,-4.14172,-5.79313], "fy":[-10.01802,1.97148,3.64393,-8.36878]}, - {"t":1.73262, "x":6.84652, "y":5.65346, "heading":-0.1678, "vx":1.90176, "vy":0.61582, "omega":-1.56518, "ax":0.26666, "ay":-0.83586, "alpha":-2.14402, "fx":[9.21588,11.28134,-0.21132,-2.14298], "fy":[-20.75269,-9.52923,-7.63454,-18.95484]}, - {"t":1.75235, "x":6.8841, "y":5.66545, "heading":-0.19869, "vx":1.90702, "vy":0.59933, "omega":-1.60749, "ax":0.83926, "ay":-2.81275, "alpha":-1.71833, "fx":[17.82547,20.46385,10.58873,8.22404], "fy":[-52.55353,-44.27321,-43.0656,-51.48375]}, - {"t":1.77209, "x":6.9219, "y":5.67673, "heading":-0.23041, "vx":1.92358, "vy":0.54382, "omega":-1.6414, "ax":1.48963, "ay":-5.9546, "alpha":-0.76353, "fx":[26.92326,29.16845,23.69326,21.56793], "fy":[-102.42241,-99.83477,-100.17056,-102.71641]}, - {"t":1.79182, "x":6.96014, "y":5.6863, "heading":-0.2628, "vx":1.95298, "vy":0.42632, "omega":-1.65646, "ax":1.37127, "ay":-7.70524, "alpha":-0.28817, "fx":[24.00352,25.09007,22.63243,21.57343], "fy":[-131.20588,-130.65285,-130.93094,-131.46618]}, - {"t":1.81155, "x":6.99895, "y":5.69321, "heading":-0.29548, "vx":1.98004, "vy":0.27427, "omega":-1.66215, "ax":0.80644, "ay":-8.39855, "alpha":-0.13421, "fx":[14.08592,14.59428,13.34463,12.84429], "fy":[-142.88209,-142.7469,-142.83383,-142.96464]}, - {"t":1.83128, "x":7.03818, "y":5.69699, "heading":-0.32828, "vx":1.99595, "vy":0.10855, "omega":-1.6648, "ax":0.09595, "ay":-8.68787, "alpha":-0.07436, "fx":[1.8661,2.12327,1.39646,1.14228], "fy":[-147.79468,-147.76358,-147.76239,-147.79234]}, - {"t":1.85102, "x":7.07758, "y":5.69744, "heading":-0.36113, "vx":1.99785, "vy":-0.06289, "omega":-1.66626, "ax":-0.66481, "ay":-8.78486, "alpha":-0.0463, "fx":[-11.14506,-11.00719,-11.47197,-11.60852], "fy":[-149.44781,-149.44669,-149.40838,-149.40921]}, - {"t":1.87075, "x":7.11688, "y":5.69449, "heading":-0.39401, "vx":1.98473, "vy":-0.23624, "omega":-1.66718, "ax":-1.44158, "ay":-8.75881, "alpha":-0.03124, "fx":[-24.40044,-24.32475,-24.64174,-24.71679], "fy":[-149.00816,-149.01508,-148.9617,-148.95473]}, - {"t":1.89048, "x":7.15576, "y":5.68812, "heading":-0.42691, "vx":1.95628, "vy":-0.40908, "omega":-1.66779, "ax":-2.2182, "ay":-8.63643, "alpha":-0.02233, "fx":[-37.63862,-37.59767,-37.82361,-37.86421], "fy":[-146.92871,-146.93624,-146.87772,-146.87021]}, - {"t":1.91022, "x":7.19393, "y":5.67837, "heading":-0.45982, "vx":1.91251, "vy":-0.5795, "omega":-1.66824, "ax":-2.98581, "ay":-8.4294, "alpha":-0.01667, "fx":[-50.71512,-50.69456,-50.86053,-50.8809], "fy":[-143.40841,-143.41393,-143.35494,-143.34947]}, - {"t":1.92995, "x":7.23109, "y":5.66529, "heading":-0.49274, "vx":1.85359, "vy":-0.74584, "omega":-1.66856, "ax":-3.74461, "ay":-8.14017, "alpha":-0.01287, "fx":[-63.6367,-63.62855,-63.75292,-63.76095], "fy":[-138.48921,-138.49184,-138.43456,-138.43198]}, - {"t":1.94968, "x":7.26694, "y":5.64899, "heading":-0.52567, "vx":1.7797, "vy":-0.90647, "omega":-1.66882, "ax":-4.51324, "ay":-7.75492, "alpha":-0.01019, "fx":[-76.72208,-76.72185,-76.81592,-76.81609], "fy":[-131.93668,-131.93606,-131.88131,-131.88197]}, - {"t":1.96942, "x":7.30118, "y":5.62959, "heading":-0.5586, "vx":1.69064, "vy":-1.0595, "omega":-1.66902, "ax":-5.06559, "ay":-7.41452, "alpha":-0.0083, "fx":[-86.12626,-86.12896,-86.2021,-86.19936], "fy":[-126.14504,-126.14264,-126.09269,-126.09513]}, - {"t":1.99252, "x":7.3389, "y":5.60313, "heading":-0.59717, "vx":1.57358, "vy":-1.23084, "omega":-1.66921, "ax":-5.8615, "ay":-6.78522, "alpha":-0.00798, "fx":[-99.66697,-99.674,-99.73808,-99.73102], "fy":[-115.44567,-115.43886,-115.38359,-115.39044]}, - {"t":2.01563, "x":7.37369, "y":5.57287, "heading":-0.63574, "vx":1.43812, "vy":-1.38764, "omega":-1.6694, "ax":-6.52816, "ay":-6.11704, "alpha":-0.00711, "fx":[-111.01214,-111.02101,-111.07198,-111.06309], "fy":[-104.08151,-104.07106,-104.01684,-104.02732]}, - {"t":2.03874, "x":7.40519, "y":5.53917, "heading":-0.67432, "vx":1.28726, "vy":-1.52901, "omega":-1.66956, "ax":-7.10115, "ay":-5.38751, "alpha":-0.00501, "fx":[-120.76917,-120.77651,-120.80776,-120.80041], "fy":[-91.6658,-91.65505,-91.61414,-91.6249]}, - {"t":2.06185, "x":7.43304, "y":5.5024, "heading":-0.71291, "vx":1.12315, "vy":-1.65351, "omega":-1.66968, "ax":-7.56839, "ay":-4.5984, "alpha":-0.00001, "fx":[-128.73605,-128.73606,-128.73612,-128.7361], "fy":[-78.21755,-78.21752,-78.21744,-78.21747]}, - {"t":2.08496, "x":7.45697, "y":5.46296, "heading":-0.75149, "vx":0.94825, "vy":-1.75978, "omega":-1.66968, "ax":-7.89299, "ay":-3.75483, "alpha":0.01273, "fx":[-134.29332,-134.27605,-134.22169,-134.23896], "fy":[-63.7905,-63.8361,-63.94668,-63.90117]}, - {"t":2.10807, "x":7.47678, "y":5.42129, "heading":-0.79008, "vx":0.76585, "vy":-1.84655, "omega":-1.66938, "ax":-7.96843, "ay":-2.85834, "alpha":0.05072, "fx":[-135.64794,-135.60298,-135.43324,-135.47836], "fy":[-48.29642,-48.51065,-48.94197,-48.72896]}, - {"t":2.13118, "x":7.49235, "y":5.37785, "heading":-0.82865, "vx":0.5817, "vy":-1.9126, "omega":-1.66821, "ax":-7.37955, "ay":-1.89466, "alpha":0.19941, "fx":[-125.77068,-125.82011,-125.27621,-125.22932], "fy":[-31.00711,-31.91808,-33.44152,-32.54358]}, - {"t":2.15429, "x":7.50382, "y":5.33315, "heading":-0.86721, "vx":0.41116, "vy":-1.95639, "omega":-1.6636, "ax":-4.6444, "ay":-0.84474, "alpha":0.90354, "fx":[-79.1351,-81.55631,-78.89415,-76.41406], "fy":[-10.07969,-13.71017,-18.61145,-15.07396]}, - {"t":2.1774, "x":7.51208, "y":5.28771, "heading":-0.90565, "vx":0.30384, "vy":-1.97591, "omega":-1.64272, "ax":-1.29196, "ay":-0.18873, "alpha":1.90528, "fx":[-21.21195,-28.97537,-22.8261,-14.89009], "fy":[4.12779,-2.30128,-10.50932,-4.15795]}, - {"t":2.20051, "x":7.51876, "y":5.242, "heading":-0.94361, "vx":0.27398, "vy":-1.98027, "omega":-1.59869, "ax":-0.26819, "ay":-0.03668, "alpha":2.4016, "fx":[-3.14333,-13.5429,-6.0093,4.44853], "fy":[8.3937,0.80888,-9.62727,-2.07088]}, - {"t":2.22362, "x":7.52502, "y":5.19622, "heading":-0.98056, "vx":0.26778, "vy":-1.98112, "omega":-1.54319, "ax":-0.05461, "ay":-0.00736, "alpha":2.8447, "fx":[1.16116,-11.51852,-3.02686,9.66888], "fy":[10.47149,1.9668,-10.71734,-2.22195]}, - {"t":2.24673, "x":7.53119, "y":5.15044, "heading":-1.01622, "vx":0.26652, "vy":-1.98129, "omega":-1.47745, "ax":-0.01114, "ay":-0.0015, "alpha":3.30664, "fx":[2.6812,-12.4082,-3.06234,12.03129], "fy":[12.19502,2.84559,-12.24455,-2.89799]}, - {"t":2.26984, "x":7.53735, "y":5.10465, "heading":-1.05036, "vx":0.26626, "vy":-1.98132, "omega":-1.40104, "ax":-0.00235, "ay":-0.00032, "alpha":3.79414, "fx":[3.73158,-13.94045,-3.81221,13.86095], "fy":[13.89554,3.7663,-13.90587,-3.7775]}, - {"t":2.29295, "x":7.5435, "y":5.05886, "heading":-1.08274, "vx":0.26621, "vy":-1.98133, "omega":-1.31336, "ax":-0.00086, "ay":-0.00012, "alpha":4.31031, "fx":[4.77919,-15.65938,-4.80881,15.63026], "fy":[15.64295,4.79191,-15.64668,-4.79608]}, - {"t":2.31606, "x":7.54965, "y":5.01308, "heading":-1.11309, "vx":0.26619, "vy":-1.98133, "omega":-1.21375, "ax":-0.00202, "ay":-0.00027, "alpha":4.85793, "fx":[5.90094,-17.49436,-5.97036,17.42635], "fy":[17.45609,5.93068,-17.46462,-5.94062]}, - {"t":2.33917, "x":7.5558, "y":4.96729, "heading":-1.14114, "vx":0.26614, "vy":-1.98134, "omega":-1.10148, "ax":-0.00927, "ay":-0.00124, "alpha":5.43954, "fx":[7.03238,-19.51247,-7.35158,19.20091], "fy":[19.3377,7.16868,-19.37575,-7.21533]}, - {"t":2.36228, "x":7.56195, "y":4.9215, "heading":-1.1666, "vx":0.26593, "vy":-1.98137, "omega":-0.97578, "ax":-0.04421, "ay":-0.00592, "alpha":6.05721, "fx":[7.79165,-22.08486,-9.31699,20.60206], "fy":[21.25687,8.44101,-21.43178,-8.66905]}, - {"t":2.38539, "x":7.56808, "y":4.87571, "heading":-1.18915, "vx":0.2649, "vy":-1.9815, "omega":-0.8358, "ax":-0.20969, "ay":-0.02777, "alpha":6.70659, "fx":[6.3679,-26.91038,-13.61823,19.89342], "fy":[23.03063,9.45913,-23.81563,-10.56369]}, - {"t":2.4085, "x":7.57415, "y":4.82991, "heading":-1.20846, "vx":0.26006, "vy":-1.98215, "omega":-0.68081, "ax":-0.97803, "ay":-0.12263, "alpha":7.24078, "fx":[-5.86446,-41.15645,-27.99934,8.47626], "fy":[23.67537,8.96806,-26.94749,-14.03938]}, - {"t":2.4316, "x":7.5799, "y":4.78407, "heading":-1.22419, "vx":0.23746, "vy":-1.98498, "omega":-0.51348, "ax":-3.8687, "ay":-0.33528, "alpha":5.67378, "fx":[-59.41805,-81.63112,-73.08269,-49.08996], "fy":[18.27299,4.56528,-27.09217,-18.55846]}, - {"t":2.45471, "x":7.58435, "y":4.73811, "heading":-1.23606, "vx":0.14805, "vy":-1.99273, "omega":-0.38236, "ax":-3.97407, "ay":-0.24742, "alpha":4.21944, "fx":[-62.55981,-79.18439,-73.09968,-55.54761], "fy":[13.52665,3.78099,-20.4697,-13.67181]}, - {"t":2.47671, "x":7.58665, "y":4.69423, "heading":-1.24447, "vx":0.06066, "vy":-1.99817, "omega":-0.28957, "ax":-1.11374, "ay":-0.02701, "alpha":4.47354, "fx":[-11.67987,-33.81548,-26.47007,-3.81236], "fy":[15.17768,7.00573,-15.71555,-8.30564]}, - {"t":2.4987, "x":7.58771, "y":4.65028, "heading":-1.25084, "vx":0.03616, "vy":-1.99876, "omega":-0.19119, "ax":-0.2573, "ay":-0.00429, "alpha":2.99376, "fx":[0.70562,-14.51063,-9.48689,5.78521], "fy":[10.1105,5.01274,-10.21755,-5.19775]}, - {"t":2.52069, "x":7.58845, "y":4.60632, "heading":-1.25504, "vx":0.0305, "vy":-1.99886, "omega":-0.12535, "ax":-0.05799, "ay":-0.00087, "alpha":1.37984, "fx":[1.38355,-5.65649,-3.3575,3.68519], "fy":[4.65742,2.35506,-4.68502,-2.38641]}, - {"t":2.54268, "x":7.5891, "y":4.56236, "heading":-1.2578, "vx":0.02923, "vy":-1.99888, "omega":-0.09501, "ax":-0.01296, "ay":-0.00019, "alpha":-0.24187, "fx":[-0.63822,0.59722,0.19738,-1.03805], "fy":[-0.82084,-0.42102,0.81444,0.41459]}, - {"t":2.56467, "x":7.58974, "y":4.5184, "heading":-1.25989, "vx":0.02894, "vy":-1.99888, "omega":-0.10033, "ax":-0.00289, "ay":-0.00004, "alpha":-1.86315, "fx":[-3.28064,6.2425,3.18236,-6.34054], "fy":[-6.29215,-3.23229,6.2909,3.2307]}, - {"t":2.58667, "x":7.59038, "y":4.47444, "heading":-1.26209, "vx":0.02888, "vy":-1.99888, "omega":-0.1413, "ax":-0.00064, "ay":-0.00001, "alpha":-3.4801, "fx":[-6.07289,11.72746,6.05092,-11.74924], "fy":[-11.73844,-6.06213,11.73826,6.06168]}, - {"t":2.60866, "x":7.59101, "y":4.43049, "heading":-1.2652, "vx":0.02887, "vy":-1.99888, "omega":-0.21783, "ax":-0.00014, "ay":0.0, "alpha":-5.0888, "fx":[-8.91984,17.13442,8.91488,-17.13929], "fy":[-17.13686,-8.91743,17.13685,8.91729]}, - {"t":2.63065, "x":7.59165, "y":4.38653, "heading":-1.26999, "vx":0.02886, "vy":-1.99888, "omega":-0.32975, "ax":-0.00003, "ay":0.0, "alpha":-6.68506, "fx":[-11.82285,22.45538,11.82167,-22.45653], "fy":[-22.45595,-11.82228,22.45596,11.82224]}, - {"t":2.65264, "x":7.59228, "y":4.34257, "heading":-1.27724, "vx":0.02886, "vy":-1.99888, "omega":-0.47676, "ax":-0.00001, "ay":0.0, "alpha":-8.26439, "fx":[-14.81644,27.65418,14.81593,-27.65467], "fy":[-27.65442,-14.8162,27.65443,14.81618]}, - {"t":2.67463, "x":7.59292, "y":4.29861, "heading":-1.28773, "vx":0.02886, "vy":-1.99888, "omega":-0.65851, "ax":-0.00003, "ay":0.0, "alpha":-9.82215, "fx":[-17.95309,32.68005,17.95195,-32.68112], "fy":[-32.68056,-17.95255,32.6806,17.95249]}, - {"t":2.69662, "x":7.59355, "y":4.25465, "heading":-1.30221, "vx":0.02886, "vy":-1.99888, "omega":-0.87452, "ax":-0.00012, "ay":0.0, "alpha":-11.35364, "fx":[-21.2988,37.46971,21.2944,-37.47377], "fy":[-37.47162,-21.29677,37.47186,21.29643]}, - {"t":2.71862, "x":7.59419, "y":4.21069, "heading":-1.32144, "vx":0.02886, "vy":-1.99888, "omega":-1.12421, "ax":-0.00048, "ay":-0.00001, "alpha":-12.8544, "fx":[-24.93168,41.94547,24.91443,-41.96117], "fy":[-41.95269,-24.9239,41.95394,24.92221]}, - {"t":2.74061, "x":7.59482, "y":4.16673, "heading":-1.34617, "vx":0.02885, "vy":-1.99888, "omega":-1.4069, "ax":-0.00183, "ay":-0.00003, "alpha":-14.3204, "fx":[-28.94516,46.0078,28.87957,-46.0668], "fy":[-46.0342,-28.91635,46.04041,28.90839]}, - {"t":2.7626, "x":7.59546, "y":4.12277, "heading":-1.37711, "vx":0.02881, "vy":-1.99888, "omega":-1.72183, "ax":-0.00669, "ay":-0.0001, "alpha":-15.74836, "fx":[-33.46646,49.51249,33.22615,-49.72762], "fy":[-49.60565,-33.36406,49.63456,33.32866]}, - {"t":2.78459, "x":7.59609, "y":4.07881, "heading":-1.41497, "vx":0.02866, "vy":-1.99888, "omega":-2.06816, "ax":-0.02362, "ay":-0.00033, "alpha":-17.13577, "fx":[-38.72466,52.19818,37.8787,-52.95957], "fy":[-52.5163,-38.37707,52.64285,38.22776]}, - {"t":2.80658, "x":7.59671, "y":4.03485, "heading":-1.46046, "vx":0.02814, "vy":-1.99889, "omega":-2.44501, "ax":-0.0804, "ay":-0.00109, "alpha":-18.47954, "fx":[-45.26251,53.45234,42.40987,-56.07021], "fy":[-54.50946,-44.14395,55.03108,43.54785]}, - {"t":2.82858, "x":7.59731, "y":3.99089, "heading":-1.51423, "vx":0.02637, "vy":-1.99892, "omega":-2.85141, "ax":-0.26367, "ay":-0.00309, "alpha":-19.75927, "fx":[-54.49234,51.52336,45.3058,-60.27695], "fy":[-54.99572,-51.13195,57.02129,48.89593]}, - {"t":2.85057, "x":7.59783, "y":3.94693, "heading":-1.57693, "vx":0.02057, "vy":-1.99898, "omega":-3.28595, "ax":-0.83399, "ay":-0.00474, "alpha":-20.77077, "fx":[-69.61908,40.5632,41.37887,-69.06706], "fy":[-52.33732,-60.52764,59.68238,52.8598]}, - {"t":2.87256, "x":7.59808, "y":3.90297, "heading":-1.6492, "vx":0.00223, "vy":-1.99909, "omega":-3.74274, "ax":-2.56198, "ay":0.03338, "alpha":-19.71223, "fx":[-94.28403,-1.25604,9.67335,-88.44726], "fy":[-42.66417,-71.66101,64.97689,51.61977]}, - {"t":2.89455, "x":7.59751, "y":3.85902, "heading":-1.73151, "vx":-0.05411, "vy":-1.99835, "omega":-4.17625, "ax":-6.10873, "ay":0.41888, "alpha":-10.86067, "fx":[-123.03656,-93.0778,-83.58639,-115.93018], "fy":[-21.01519,-49.00097,54.77627,43.73989]}, - {"t":2.91654, "x":7.59484, "y":3.81517, "heading":-1.82335, "vx":-0.18845, "vy":-1.98914, "omega":-4.41509, "ax":-6.27439, "ay":0.74411, "alpha":-7.95731, "fx":[-121.61331,-101.90716,-90.77532,-112.60672], "fy":[-7.81165,-29.14369,44.37454,43.20947]}, - {"t":2.93614, "x":7.58994, "y":3.77633, "heading":-1.90988, "vx":-0.31142, "vy":-1.97456, "omega":-4.57104, "ax":-3.23134, "ay":0.56282, "alpha":-13.30622, "fx":[-93.61881,-40.71543,-11.52027,-74.00217], "fy":[-11.90861,-46.22412,42.91869,53.50749]}, - {"t":2.95574, "x":7.58322, "y":3.73774, "heading":-1.99946, "vx":-0.37474, "vy":-1.96353, "omega":-4.83181, "ax":-1.18277, "ay":0.23296, "alpha":-12.73109, "fx":[-63.62778,-5.1411,25.82248,-37.52802], "fy":[-11.97624,-43.51648,23.16957,48.17337]}, - {"t":2.97534, "x":7.57565, "y":3.6993, "heading":-2.09415, "vx":-0.39792, "vy":-1.95896, "omega":-5.08131, "ax":-0.38738, "ay":0.07947, "alpha":-8.9964, "fx":[-39.28098,1.96569,26.59813,-15.63995], "fy":[-7.31829,-31.90294,10.46531,34.16317]}, - {"t":2.99493, "x":7.56778, "y":3.66093, "heading":-2.19373, "vx":-0.40551, "vy":-1.95741, "omega":-5.25762, "ax":-0.11754, "ay":0.02443, "alpha":-4.44839, "fx":[-18.64027,0.7091,14.68415,-4.75045], "fy":[-2.3051,-16.26408,3.16107,17.07009]}, - {"t":3.01453, "x":7.55981, "y":3.62257, "heading":-2.29677, "vx":-0.40782, "vy":-1.95693, "omega":-5.3448, "ax":-0.0361, "ay":0.00753, "alpha":0.36669, "fx":[0.77556,-0.69676,-2.00356,-0.53144], "fy":[0.21079,1.51768,0.04543,-1.26153]}, - {"t":3.03413, "x":7.55181, "y":3.58422, "heading":-2.40152, "vx":-0.40853, "vy":-1.95678, "omega":-5.33761, "ax":-0.02136, "ay":0.00446, "alpha":5.13933, "fx":[19.1325,0.51475,-19.84717,-1.25323], "fy":[-0.80738,19.56519,0.96043,-19.41489]}, - {"t":3.05373, "x":7.5438, "y":3.54587, "heading":-2.50612, "vx":-0.40894, "vy":-1.95669, "omega":-5.23689, "ax":-0.04551, "ay":0.00952, "alpha":9.61451, "fx":[35.35878,4.63329,-36.81812,-6.27059], "fy":[-5.29376,36.25608,5.60918,-35.92394]}, - {"t":3.07332, "x":7.53577, "y":3.50753, "heading":-2.60875, "vx":-0.40984, "vy":-1.95651, "omega":-5.04847, "ax":-0.13394, "ay":0.02814, "alpha":13.61674, "fx":[48.01771,10.3874,-52.07017,-15.44822], "fy":[-12.51481,50.61638,13.31738,-49.50401]}, - {"t":3.09292, "x":7.52772, "y":3.46919, "heading":-2.70769, "vx":-0.41246, "vy":-1.95595, "omega":-4.78161, "ax":-0.37675, "ay":0.08019, "alpha":17.03333, "fx":[55.25057,14.81966,-66.02868,-29.6754], "fy":[-21.46537,62.69099,23.09384,-58.86354]}, - {"t":3.11252, "x":7.51956, "y":3.43088, "heading":-2.8014, "vx":-0.41984, "vy":-1.95438, "omega":-4.4478, "ax":-0.98506, "ay":0.21675, "alpha":19.61158, "fx":[52.99207,11.92967,-80.35645,-51.5877], "fy":[-31.20694,73.89933,33.55855,-61.50367]}, - {"t":3.13212, "x":7.51114, "y":3.39262, "heading":-2.88857, "vx":-0.43915, "vy":-1.95014, "omega":-4.06345, "ax":-2.4087, "ay":0.57337, "alpha":19.92668, "fx":[26.02942,-10.18891,-96.50818,-83.21776], "fy":[-38.81468,85.92592,43.2531,-51.35306]}, - {"t":3.15172, "x":7.50207, "y":3.35451, "heading":-2.9682, "vx":-0.48635, "vy":-1.9389, "omega":-3.67294, "ax":-5.21266, "ay":1.45598, "alpha":13.45895, "fx":[-66.22483,-59.44539,-112.91661,-116.07678], "fy":[-18.57329,90.70324,50.79181,-23.85839]}, - {"t":3.17131, "x":7.49154, "y":3.31679, "heading":-3.04019, "vx":-0.58851, "vy":-1.91036, "omega":-3.40917, "ax":-7.12817, "ay":2.48958, "alpha":6.61621, "fx":[-122.47311,-102.6169,-125.10641,-134.79582], "fy":[22.10962,81.87184,56.36386,9.04307]}, - {"t":3.19091, "x":7.47864, "y":3.27983, "heading":-3.107, "vx":-0.72821, "vy":-1.86157, "omega":-3.27951, "ax":-7.62336, "ay":3.34927, "alpha":3.67809, "fx":[-132.68039,-117.92625,-129.05463,-139.02338], "fy":[47.18455,79.86132,64.83375,36.00059]}, - {"t":3.21051, "x":7.4629, "y":3.24399, "heading":3.11192, "vx":-0.87761, "vy":-1.79594, "omega":-3.20742, "ax":-7.58965, "ay":4.11921, "alpha":2.34975, "fx":[-131.6493,-120.54123,-127.69934,-136.50127], "fy":[64.64547,84.51626,74.82535,56.27897]}, - {"t":3.23011, "x":7.44425, "y":3.20958, "heading":3.04906, "vx":-1.02635, "vy":-1.71521, "omega":-3.16137, "ax":-7.33111, "ay":4.83275, "alpha":1.65844, "fx":[-126.72642,-117.84342,-123.29481,-130.9355], "fy":[78.91079,92.04958,85.2526,72.60168]}, - {"t":3.2497, "x":7.42273, "y":3.1769, "heading":2.9871, "vx":-1.17002, "vy":-1.6205, "omega":-3.12887, "ax":-6.94598, "ay":5.49813, "alpha":1.25487, "fx":[-119.76149,-112.33026,-116.90344,-123.60101], "fy":[91.41033,100.59743,95.54882,86.53018]}, - {"t":3.2693, "x":7.39846, "y":3.14619, "heading":2.92578, "vx":-1.30615, "vy":-1.51275, "omega":-3.10428, "ax":-6.4714, "ay":6.11692, "alpha":0.99814, "fx":[-111.36745,-104.95661,-109.01707,-114.96543], "fy":[102.65636,109.30975,105.42117,98.80105]}, - {"t":3.2889, "x":7.37162, "y":3.11772, "heading":2.86495, "vx":-1.43297, "vy":-1.39287, "omega":-3.08472, "ax":-5.89135, "ay":6.71656, "alpha":0.82376, "fx":[-101.22406,-95.57399,-99.34708,-104.69586], "fy":[113.35125,118.21932,115.15843,110.25774]}, - {"t":3.3085, "x":7.34241, "y":3.09172, "heading":2.80449, "vx":-1.54843, "vy":-1.26124, "omega":-3.06858, "ax":-4.83927, "ay":7.53409, "alpha":0.6959, "fx":[-82.87476,-77.89234,-81.83736,-86.65418], "fy":[127.79553,130.9356,128.55819,125.3213]}, - {"t":3.3281, "x":7.31113, "y":3.06844, "heading":2.74436, "vx":-1.64327, "vy":-1.11359, "omega":-3.05494, "ax":-3.51236, "ay":8.25291, "alpha":0.59441, "fx":[-59.80646,-55.60307,-59.71076,-63.85664], "fy":[140.35814,142.10644,140.45919,138.59478]}, - {"t":3.34769, "x":7.27825, "y":3.04821, "heading":2.68449, "vx":-1.7121, "vy":-0.95185, "omega":-3.04329, "ax":-2.51441, "ay":8.62073, "alpha":0.51753, "fx":[-42.59942,-39.01761,-42.94466,-46.51598], "fy":[146.69068,147.70582,146.63105,145.51671]}, - {"t":3.36729, "x":7.24422, "y":3.03121, "heading":2.62484, "vx":-1.76138, "vy":-0.7829, "omega":-3.03315, "ax":-1.79499, "ay":8.80656, "alpha":0.46052, "fx":[-30.29743,-27.13004,-30.7644,-33.93715], "fy":[149.84971,150.47183,149.78448,149.0821]}, - {"t":3.38689, "x":7.20935, "y":3.01756, "heading":2.5654, "vx":-1.79656, "vy":-0.61031, "omega":-3.02412, "ax":-1.26597, "ay":8.90375, "alpha":0.41765, "fx":[-21.31744,-18.41512,-21.74582,-24.65658], "fy":[151.48569,151.87844,151.4478,150.98926]}, - {"t":3.40649, "x":7.1739, "y":3.0073, "heading":2.50614, "vx":-1.82137, "vy":-0.43582, "omega":-3.01594, "ax":-0.86564, "ay":8.95587, "alpha":0.38466, "fx":[-14.56651,-11.83235,-14.87884,-17.61952], "fy":[152.35661,152.60324,152.34483,152.04278]}, - {"t":3.42608, "x":7.13804, "y":3.00048, "heading":2.44703, "vx":-1.83833, "vy":-0.2603, "omega":-3.0084, "ax":-0.55436, "ay":8.98389, "alpha":0.35865, "fx":[-9.34927,-6.7211,-9.50857,-12.1394], "fy":[152.82267,152.96878,152.828,152.63395]}, - {"t":3.44568, "x":7.10191, "y":2.99711, "heading":2.38807, "vx":-1.8492, "vy":-0.08424, "omega":-3.00137, "ax":-0.24667, "ay":9.00393, "alpha":0.34676, "fx":[-4.18401,-1.56948,-4.20789,-6.82199], "fy":[153.16034,153.21509,153.17066,153.07114]}, - {"t":3.47241, "x":7.0524, "y":2.99807, "heading":2.30786, "vx":-1.85579, "vy":0.15639, "omega":-2.9921, "ax":0.22695, "ay":9.00069, "alpha":0.38942, "fx":[3.79108,6.80896,3.92787,0.9138], "fy":[153.10761,153.01049,153.11884,153.15953]}, - {"t":3.49913, "x":7.00288, "y":3.00546, "heading":2.2279, "vx":-1.84972, "vy":0.39694, "omega":-2.98169, "ax":0.91548, "ay":8.95144, "alpha":0.4498, "fx":[15.48145,18.95787,15.66239,12.18627], "fy":[152.2791,151.89604,152.28153,152.58922]}, - {"t":3.52586, "x":6.95378, "y":3.01927, "heading":2.14821, "vx":-1.82526, "vy":0.63616, "omega":-2.96967, "ax":1.97667, "ay":8.7699, "alpha":0.53928, "fx":[33.68095,37.59349,33.57884,29.63687], "fy":[149.17144,148.25117,149.22725,150.044]}, - {"t":3.55258, "x":6.9057, "y":3.0394, "heading":2.06884, "vx":-1.77243, "vy":0.87054, "omega":-2.95526, "ax":3.67113, "ay":8.19191, "alpha":0.67465, "fx":[63.0929,67.01758,61.87083,57.79843], "fy":[139.06653,137.25333,139.67276,141.37535]}, - {"t":3.57931, "x":6.85964, "y":3.06559, "heading":1.98987, "vx":-1.67432, "vy":1.08947, "omega":-2.93723, "ax":5.287, "ay":7.22984, "alpha":0.8695, "fx":[91.3832,94.95048,88.66171,84.7258], "fy":[121.9301,119.252,124.03729,126.69063]}, - {"t":3.60603, "x":6.81679, "y":3.09729, "heading":1.91137, "vx":-1.53302, "vy":1.28269, "omega":-2.91399, "ax":6.11851, "ay":6.49394, "alpha":1.17771, "fx":[106.16238,110.04657,102.34058,97.74719], "fy":[108.50258,104.74524,112.36111,116.23098]}, - {"t":3.63276, "x":6.778, "y":3.13389, "heading":1.83349, "vx":-1.36951, "vy":1.45624, "omega":-2.88252, "ax":6.79517, "ay":5.67627, "alpha":1.73951, "fx":[118.67943,123.07543,113.25025,107.33023], "fy":[92.80881,87.43652,100.00753,105.95391]}, - {"t":3.65948, "x":6.74383, "y":3.17484, "heading":1.75645, "vx":-1.1879, "vy":1.60794, "omega":-2.83603, "ax":7.2769, "ay":4.75111, "alpha":2.94058, "fx":[128.75061,133.95621,120.79129,111.61397], "fy":[72.78593,65.07243,87.59865,97.8028]}, - {"t":3.68621, "x":6.71468, "y":3.21951, "heading":1.68066, "vx":-0.99343, "vy":1.73491, "omega":-2.75744, "ax":7.28888, "ay":3.66258, "alpha":6.22413, "fx":[132.50066,140.12769,122.16117,101.13725], "fy":[40.38354,32.41972,77.61698,98.77782]}, - {"t":3.71293, "x":6.69073, "y":3.26718, "heading":1.60697, "vx":-0.79863, "vy":1.83279, "omega":-2.5911, "ax":5.19741, "ay":2.03781, "alpha":17.74404, "fx":[76.06791,131.10843,110.68181,35.76775], "fy":[-42.54742,-12.88907,76.95981,117.12727]}, - {"t":3.73966, "x":6.67125, "y":3.31689, "heading":1.53772, "vx":-0.65973, "vy":1.88726, "omega":-2.11689, "ax":1.76011, "ay":0.59101, "alpha":27.85537, "fx":[-49.40082,104.32225,98.44181,-33.60728], "fy":[-79.84632,-56.12845,72.48182,103.7049]}, - {"t":3.76638, "x":6.65424, "y":3.36754, "heading":1.48115, "vx":-0.61269, "vy":1.90305, "omega":-1.37245, "ax":0.57967, "ay":0.18405, "alpha":27.89261, "fx":[-73.42064,79.62179,88.55348,-55.31438], "fy":[-69.48997,-74.08326,67.06451,89.03144]}, - {"t":3.79311, "x":6.63807, "y":3.41846, "heading":1.44447, "vx":-0.5972, "vy":1.90797, "omega":-0.62702, "ax":0.18027, "ay":0.05618, "alpha":26.34164, "fx":[-76.68125,64.98814,81.37951,-57.42099], "fy":[-61.47216,-76.96587,61.0675,81.19275]}, - {"t":3.81983, "x":6.62218, "y":3.46947, "heading":1.42771, "vx":-0.59238, "vy":1.90947, "omega":0.07696, "ax":0.05184, "ay":0.01606, "alpha":24.22926, "fx":[-72.94854,56.1632,74.34545,-54.03277], "fy":[-55.10488,-73.10175,55.09953,74.2]}, - {"t":3.84656, "x":6.60637, "y":3.52051, "heading":1.42977, "vx":-0.591, "vy":1.9099, "omega":0.72449, "ax":0.01365, "ay":0.00422, "alpha":21.52931, "fx":[-65.14674,49.36445,65.53527,-48.82455], "fy":[-49.08042,-65.21198,49.109,65.47057]}, - {"t":3.87328, "x":6.59058, "y":3.57155, "heading":1.44913, "vx":-0.59063, "vy":1.91001, "omega":1.29986, "ax":0.0034, "ay":0.00105, "alpha":18.04581, "fx":[-53.91012,42.26774,54.01342,-42.13951], "fy":[-42.19607,-53.93358,42.21119,53.99]}, - {"t":3.90001, "x":6.57479, "y":3.6226, "heading":1.48387, "vx":-0.59054, "vy":1.91004, "omega":1.78214, "ax":0.00178, "ay":0.00055, "alpha":13.5304, "fx":[-39.30747,33.06144,39.36464,-32.99782], "fy":[-33.02345,-39.32358,33.03581,39.34853]}, - {"t":3.92673, "x":6.55901, "y":3.67364, "heading":1.5315, "vx":-0.59049, "vy":1.91006, "omega":2.14374, "ax":0.00735, "ay":0.00227, "alpha":7.80777, "fx":[-21.64254,20.24582,21.88908,-19.99207], "fy":[-20.08483,-21.72267,20.15307,21.80904]}, - {"t":3.95346, "x":6.54323, "y":3.72469, "heading":1.58879, "vx":-0.5903, "vy":1.91012, "omega":2.35241, "ax":0.04917, "ay":0.01518, "alpha":1.08641, "fx":[-2.02706,3.80474,3.69937,-2.13177], "fy":[-2.71078,-2.60473,3.22584,3.12222]}, - {"t":3.98018, "x":6.52747, "y":3.77574, "heading":1.65166, "vx":-0.58898, "vy":1.91052, "omega":2.38144, "ax":0.31548, "ay":0.09649, "alpha":-5.7843, "fx":[19.54487,-11.34002,-8.84283,22.10303], "fy":[18.26666,16.00531,-15.21165,-12.49512]}, - {"t":4.00691, "x":6.51185, "y":3.82684, "heading":1.7153, "vx":-0.58055, "vy":1.9131, "omega":2.22685, "ax":1.77129, "ay":0.48426, "alpha":-11.01231, "fx":[53.60832,-2.71571,6.90839,62.71566], "fy":[40.18021,37.23357,-28.45368,-16.01164]}, - {"t":4.03363, "x":6.49696, "y":3.87814, "heading":1.77481, "vx":-0.53321, "vy":1.92604, "omega":1.93255, "ax":1.81131, "ay":0.50685, "alpha":-9.73776, "fx":[49.90353,0.38009,12.3096,60.64619], "fy":[38.38402,32.31454,-24.93377,-11.2791]}, - {"t":4.05998, "x":6.48355, "y":3.92906, "heading":1.82573, "vx":-0.4855, "vy":1.9394, "omega":1.67601, "ax":0.32117, "ay":0.07966, "alpha":-9.39958, "fx":[23.55343,-25.36958,-12.46687,36.13539], "fy":[31.8667,19.74498,-29.7438,-16.44769]}, - {"t":4.08632, "x":6.47087, "y":3.98018, "heading":1.86988, "vx":-0.47704, "vy":1.94149, "omega":1.42839, "ax":0.05386, "ay":0.01321, "alpha":-8.33245, "fx":[15.71288,-27.06154,-13.85256,28.8658], "fy":[28.15229,15.04704,-27.77779,-14.52253]}, - {"t":4.11267, "x":6.45832, "y":4.03133, "heading":1.90751, "vx":-0.47562, "vy":1.94184, "omega":1.20888, "ax":0.00886, "ay":0.00217, "alpha":-7.28949, "fx":[12.15654,-24.78479,-11.85097,25.0819], "fy":[24.9657,12.04527,-24.90106,-11.96234]}, - {"t":4.13901, "x":6.44579, "y":4.08248, "heading":1.93936, "vx":-0.47538, "vy":1.9419, "omega":1.01684, "ax":0.00143, "ay":0.00035, "alpha":-6.29066, "fx":[9.69328,-21.81172,-9.6439,21.85994], "fy":[21.84127,9.6751,-21.83039,-9.66208]}, - {"t":4.16536, "x":6.43327, "y":4.13364, "heading":1.96614, "vx":-0.47535, "vy":1.94191, "omega":0.85111, "ax":0.00023, "ay":0.00006, "alpha":-5.33321, "fx":[7.70216,-18.72142,-7.69428,18.72915], "fy":[18.72618,7.69924,-18.72439,-7.6972]}, - {"t":4.1917, "x":6.42075, "y":4.1848, "heading":1.98857, "vx":-0.47534, "vy":1.94191, "omega":0.71061, "ax":0.00004, "ay":0.00001, "alpha":-4.41299, "fx":[6.02157,-15.63265,-6.02032,15.63387], "fy":[15.6334,6.0211,-15.63311,-6.02079]}, - {"t":4.21805, "x":6.40822, "y":4.23596, "heading":2.00729, "vx":-0.47534, "vy":1.94191, "omega":0.59436, "ax":0.00001, "ay":0.0, "alpha":-3.52511, "fx":[4.57503,-12.57563,-4.57484,12.57582], "fy":[12.57575,4.57496,-12.5757,-4.57491]}, - {"t":4.24439, "x":6.3957, "y":4.28712, "heading":2.02295, "vx":-0.47534, "vy":1.94191, "omega":0.50149, "ax":0.0, "ay":0.0, "alpha":-2.66406, "fx":[3.30822,-9.55692,-3.30822,9.55692], "fy":[9.55692,3.30822,-9.55692,-3.30822]}, - {"t":4.27073, "x":6.38318, "y":4.33828, "heading":2.03616, "vx":-0.47534, "vy":1.94191, "omega":0.43131, "ax":-0.00001, "ay":0.0, "alpha":-1.82389, "fx":[2.17818,-6.57239,-2.17836,6.57222], "fy":[6.57229,2.17825,-6.57233,-2.17829]}, - {"t":4.29708, "x":6.37066, "y":4.38943, "heading":2.04752, "vx":-0.47534, "vy":1.94191, "omega":0.38326, "ax":-0.00003, "ay":-0.00001, "alpha":-0.99833, "fx":[1.15077,-3.61131,-1.15192,3.61015], "fy":[3.61059,1.1512,-3.61087,-1.15149]}, - {"t":4.32342, "x":6.35813, "y":4.44059, "heading":2.05762, "vx":-0.47534, "vy":1.94191, "omega":0.35696, "ax":-0.00022, "ay":-0.00005, "alpha":-0.18086, "fx":[0.1982,-0.65995,-0.20572,0.65243], "fy":[0.65527,0.20104,-0.65711,-0.20288]}, - {"t":4.34977, "x":6.34561, "y":4.49175, "heading":2.06702, "vx":-0.47535, "vy":1.94191, "omega":0.35219, "ax":-0.00144, "ay":-0.00035, "alpha":0.63514, "fx":[-0.71201,2.28658,0.66312,-2.33546], "fy":[-2.317,-0.69355,2.30504,0.68157]}, - {"t":4.37611, "x":6.33309, "y":4.54291, "heading":2.0763, "vx":-0.47538, "vy":1.9419, "omega":0.36893, "ax":-0.00932, "ay":-0.00228, "alpha":1.4563, "fx":[-1.68594,5.15489,1.36858,-5.47166], "fy":[-5.35195,-1.56624,5.27462,1.48829]}, - {"t":4.40246, "x":6.32056, "y":4.59407, "heading":2.08602, "vx":-0.47563, "vy":1.94184, "omega":0.40729, "ax":-0.06025, "ay":-0.01478, "alpha":2.28899, "fx":[-3.34619,7.35153,1.29173,-9.39639], "fy":[-8.62364,-2.57337,8.12547,2.06568]}, - {"t":4.4288, "x":6.30801, "y":4.64522, "heading":2.09675, "vx":-0.47722, "vy":1.94145, "omega":0.46759, "ax":-0.38675, "ay":-0.09615, "alpha":3.12809, "fx":[-9.63829,4.91134,-3.57716,-18.00994], "fy":[-13.10157,-4.72588,9.88693,1.39883]}, - {"t":4.45514, "x":6.2953, "y":4.69633, "heading":2.10907, "vx":-0.4874, "vy":1.93892, "omega":0.55, "ax":-2.30065, "ay":-0.61696, "alpha":3.4971, "fx":[-41.92589,-26.72184,-36.73001,-51.15574], "fy":[-23.78812,-14.62834,3.26552,-6.82644]}, - {"t":4.48149, "x":6.28166, "y":4.7472, "heading":2.12355, "vx":-0.54801, "vy":1.92266, "omega":0.64213, "ax":-6.4601, "ay":-2.15969, "alpha":1.36363, "fx":[-108.85643,-106.68633,-110.99697,-112.99784], "fy":[-43.47143,-40.38287,-29.63857,-33.45031]}, - {"t":4.50783, "x":6.26499, "y":4.7971, "heading":2.14047, "vx":-0.7182, "vy":1.86577, "omega":0.67805, "ax":-7.64302, "ay":-3.4394, "alpha":0.43909, "fx":[-129.11692,-129.06909,-130.91174,-130.9244], "fy":[-60.80305,-60.14347,-56.14553,-56.9206]}, - {"t":4.53418, "x":6.24341, "y":4.84506, "heading":2.15833, "vx":-0.91955, "vy":1.77516, "omega":0.68962, "ax":-7.52207, "ay":-4.46729, "alpha":0.21542, "fx":[-127.37926,-127.37039,-128.52484,-128.51843], "fy":[-77.00966,-76.88876,-74.95042,-75.10013]}, - {"t":4.56052, "x":6.21658, "y":4.89027, "heading":2.1765, "vx":-1.11772, "vy":1.65747, "omega":0.6953, "ax":-7.03353, "ay":-5.41299, "alpha":0.13269, "fx":[-119.24502,-119.17923,-120.03578,-120.09332], "fy":[-92.60509,-92.65122,-91.5368,-91.50037]}, - {"t":4.58687, "x":6.18469, "y":4.93206, "heading":2.19482, "vx":-1.30301, "vy":1.51487, "omega":0.69879, "ax":-6.39063, "ay":-6.16525, "alpha":0.12339, "fx":[-108.33049,-108.18166,-109.07939,-109.21976], "fy":[-105.27317,-105.39583,-104.46197,-104.3459]}, - {"t":4.61439, "x":6.14641, "y":4.97142, "heading":2.21405, "vx":-1.47891, "vy":1.34517, "omega":0.70219, "ax":-5.48661, "ay":-6.81126, "alpha":0.21583, "fx":[-92.72774,-92.23924,-93.93718,-94.3986], "fy":[-116.3969,-116.69738,-115.31195,-115.02384]}, - {"t":4.64192, "x":6.10362, "y":5.00587, "heading":2.23338, "vx":-1.62993, "vy":1.1577, "omega":0.70813, "ax":-4.42457, "ay":-7.05115, "alpha":0.49777, "fx":[-74.17264,-72.48304,-76.4117,-77.97562], "fy":[-120.93549,-121.52677,-118.92627,-118.36391]}, - {"t":4.66944, "x":6.05708, "y":5.03506, "heading":2.25287, "vx":-1.75171, "vy":0.96362, "omega":0.72183, "ax":-2.77782, "ay":-5.60829, "alpha":1.916, "fx":[-45.65513,-37.40908,-49.28021,-56.65556], "fy":[-100.16225,-98.76591,-90.46469,-92.18856]}, - {"t":4.69696, "x":6.00781, "y":5.05946, "heading":2.27274, "vx":-1.82817, "vy":0.80925, "omega":0.77457, "ax":-0.65749, "ay":-1.53251, "alpha":4.47717, "fx":[-12.31034,6.26996,-10.25545,-28.43898], "fy":[-42.27198,-28.18688,-9.23211,-24.5791]}, - {"t":4.72449, "x":5.95725, "y":5.08115, "heading":2.29406, "vx":-1.84627, "vy":0.76707, "omega":0.8978, "ax":-0.09383, "ay":-0.22691, "alpha":4.21307, "fx":[-2.59329,14.38744,-0.62278,-17.55535], "fy":[-19.77036,-4.90441,12.13813,-2.90238]}, - {"t":4.75201, "x":5.90639, "y":5.10218, "heading":2.31877, "vx":-1.84885, "vy":0.76082, "omega":1.01376, "ax":-0.01326, "ay":-0.03225, "alpha":3.69293, "fx":[-0.75152,13.78539,0.29725,-14.23352], "fy":[-14.55304,-1.07796,13.46526,-0.02863]}, - {"t":4.77954, "x":5.8555, "y":5.12311, "heading":2.34667, "vx":-1.84921, "vy":0.75993, "omega":1.11541, "ax":-0.00187, "ay":-0.00456, "alpha":3.20664, "fx":[-0.14794,12.14086,0.08383,-12.2042], "fy":[-12.24958,-0.19393,12.09547,0.03785]}, - {"t":4.80706, "x":5.8046, "y":5.14402, "heading":2.37738, "vx":-1.84927, "vy":0.75981, "omega":1.20367, "ax":-0.00026, "ay":-0.00064, "alpha":2.75579, "fx":[0.21706,10.45472,-0.22608,-10.46365], "fy":[-10.47006,0.21059,10.44831,-0.23254]}, - {"t":4.83459, "x":5.7537, "y":5.16494, "heading":2.41051, "vx":-1.84927, "vy":0.75979, "omega":1.27952, "ax":-0.00004, "ay":-0.00009, "alpha":2.33617, "fx":[0.4808,8.85488,-0.48206,-8.85613], "fy":[-8.85703,0.47989,8.85398,-0.48296]}, - {"t":4.86211, "x":5.7028, "y":5.18585, "heading":2.44572, "vx":-1.84927, "vy":0.75979, "omega":1.34382, "ax":-0.00001, "ay":-0.00001, "alpha":1.94354, "fx":[0.65959,7.34843,-0.65976,-7.34861], "fy":[-7.34873,0.65946,7.34831,-0.65989]}, - {"t":4.88964, "x":5.6519, "y":5.20676, "heading":2.48271, "vx":-1.84927, "vy":0.75979, "omega":1.39732, "ax":0.0, "ay":0.0, "alpha":1.5737, "fx":[0.7538,5.92633,-0.75382,-5.92635], "fy":[-5.92637,0.75378,5.92631,-0.75384]}, - {"t":4.91716, "x":5.601, "y":5.22768, "heading":2.52117, "vx":-1.84927, "vy":0.75979, "omega":1.44063, "ax":0.0, "ay":0.0, "alpha":1.22254, "fx":[0.7622,4.57799,-0.7622,-4.57799], "fy":[-4.57799,0.76219,4.57798,-0.7622]}, - {"t":4.94469, "x":5.5501, "y":5.24859, "heading":2.56083, "vx":-1.84927, "vy":0.75979, "omega":1.47428, "ax":0.0, "ay":0.0, "alpha":0.88602, "fx":[0.68348,3.29333,-0.68349,-3.29333], "fy":[-3.29333,0.68348,3.29332,-0.68349]}, - {"t":4.97221, "x":5.4992, "y":5.2695, "heading":2.6014, "vx":-1.84927, "vy":0.75979, "omega":1.49867, "ax":0.0, "ay":0.0, "alpha":0.56018, "fx":[0.51624,2.06293,-0.51624,-2.06293], "fy":[-2.06294,0.51624,2.06293,-0.51625]}, - {"t":4.99974, "x":5.4483, "y":5.29041, "heading":2.64266, "vx":-1.84927, "vy":0.75979, "omega":1.51409, "ax":0.0, "ay":0.0, "alpha":0.24111, "fx":[0.25863,0.87801,-0.25863,-0.87802], "fy":[-0.87802,0.25862,0.87801,-0.25864]}, - {"t":5.02726, "x":5.3974, "y":5.31133, "heading":2.68433, "vx":-1.84927, "vy":0.75979, "omega":1.52073, "ax":0.0, "ay":0.0, "alpha":-0.07503, "fx":[-0.0918,-0.26965,0.0918,0.26965], "fy":[0.26964,-0.09181,-0.26966,0.09179]}, - {"t":5.05478, "x":5.34649, "y":5.33224, "heading":2.72619, "vx":-1.84927, "vy":0.75979, "omega":1.51866, "ax":0.0, "ay":0.0, "alpha":-0.39211, "fx":[-0.53827,-1.3878,0.53826,1.38779], "fy":[1.38779,-0.53827,-1.3878,0.53826]}, - {"t":5.08231, "x":5.29559, "y":5.35315, "heading":2.76799, "vx":-1.84927, "vy":0.75979, "omega":1.50787, "ax":0.0, "ay":0.0, "alpha":-0.71395, "fx":[-1.08481,-2.48375,1.08481,2.48374], "fy":[2.48374,-1.08482,-2.48375,1.08481]}, - {"t":5.10983, "x":5.24469, "y":5.37407, "heading":2.80949, "vx":-1.84927, "vy":0.75979, "omega":1.48822, "ax":0.0, "ay":0.0, "alpha":-1.04444, "fx":[-1.73637,-3.5645,1.73636,3.5645], "fy":[3.56449,-1.73637,-3.5645,1.73636]}, - {"t":5.13736, "x":5.19379, "y":5.39498, "heading":2.85045, "vx":-1.84927, "vy":0.75979, "omega":1.45947, "ax":0.0, "ay":0.0, "alpha":-1.3875, "fx":[-2.49868,-4.63687,2.49867,4.63686], "fy":[4.63685,-2.49869,-4.63688,2.49866]}, - {"t":5.16488, "x":5.14289, "y":5.41589, "heading":2.89063, "vx":-1.84927, "vy":0.75979, "omega":1.42128, "ax":0.0, "ay":-0.00001, "alpha":-1.74712, "fx":[-3.37827,-5.70762,3.37819,5.70755], "fy":[5.7075,-3.37831,-5.70767,3.37814]}, - {"t":5.19241, "x":5.09199, "y":5.4368, "heading":2.92975, "vx":-1.84927, "vy":0.75979, "omega":1.37319, "ax":-0.00001, "ay":-0.00004, "alpha":-2.12735, "fx":[-4.38237,-6.78382,4.38187,6.78333], "fy":[6.78297,-4.38272,-6.78417,4.38152]}, - {"t":5.21993, "x":5.04109, "y":5.45772, "heading":2.96754, "vx":-1.84927, "vy":0.75979, "omega":1.31464, "ax":-0.0001, "ay":-0.00025, "alpha":-2.53236, "fx":[-5.51957,-7.87389,5.51603,7.87042], "fy":[7.86788,-5.52207,-7.87643,5.51353]}, - {"t":5.24746, "x":4.99019, "y":5.47863, "heading":3.00373, "vx":-1.84928, "vy":0.75978, "omega":1.24493, "ax":-0.00073, "ay":-0.00179, "alpha":-2.96636, "fx":[-6.80547,-8.99373,6.78016,8.96913], "fy":[8.95104,-6.82316,-9.01182,6.76246]}, - {"t":5.27498, "x":4.93929, "y":5.49954, "heading":3.03799, "vx":-1.8493, "vy":0.75973, "omega":1.16329, "ax":-0.0052, "ay":-0.01266, "alpha":-3.43361, "fx":[-8.30444,-10.20745,8.1243,10.0339], "fy":[9.90508,-8.42929,-10.33626,7.99933]}, - {"t":5.30251, "x":4.88839, "y":5.52045, "heading":3.07001, "vx":-1.84944, "vy":0.75938, "omega":1.06878, "ax":-0.03664, "ay":-0.0894, "alpha":-3.93756, "fx":[-10.42668,-11.90681,9.15006,10.69062], "fy":[9.77394,-11.30185,-12.82283,8.2681]}, - {"t":5.33003, "x":4.83747, "y":5.54132, "heading":3.09943, "vx":-1.85045, "vy":0.75692, "omega":0.9604, "ax":-0.25291, "ay":-0.62661, "alpha":-4.43569, "fx":[-15.92175,-16.57578,7.05375,8.23619], "fy":[1.69258,-21.92026,-23.08807,0.68163]}, - {"t":5.35756, "x":4.78644, "y":5.56191, "heading":3.12586, "vx":-1.85741, "vy":0.73968, "omega":0.83831, "ax":-1.34493, "ay":-3.66072, "alpha":-3.49656, "fx":[-35.04493,-32.19881,-11.67808,-12.58581], "fy":[-52.97793,-69.05133,-71.53287,-55.50933]}, - {"t":5.38508, "x":4.73481, "y":5.58089, "heading":-3.13425, "vx":-1.89443, "vy":0.63892, "omega":0.74207, "ax":-2.06683, "ay":-7.47854, "alpha":-0.87125, "fx":[-39.96622,-37.97545,-30.51056,-32.17262], "fy":[-125.42094,-127.23107,-128.90999,-127.26932]}, - {"t":5.4126, "x":4.68188, "y":5.59564, "heading":-3.11382, "vx":-1.95132, "vy":0.43307, "omega":0.71808, "ax":-1.07076, "ay":-7.21816, "alpha":-1.51969, "fx":[-26.03326,-23.60293,-10.92237,-12.2946], "fy":[-120.42839,-123.66252,-124.99709,-122.02735]}, - {"t":5.45286, "x":4.60247, "y":5.60722, "heading":-3.08492, "vx":-1.99442, "vy":0.14253, "omega":0.65692, "ax":-0.06781, "ay":-1.12919, "alpha":-5.80435, "fx":[-18.27229,-15.63701,15.17362,14.12181], "fy":[-4.80368,-35.29656,-33.73836,-2.98991]}, - {"t":5.49311, "x":4.52214, "y":5.61204, "heading":-3.05848, "vx":-1.99715, "vy":0.09708, "omega":0.42328, "ax":-0.00308, "ay":-0.06414, "alpha":-5.01096, "fx":[-14.5908,-12.3244,14.45338,12.25251], "fy":[11.19279,-15.60791,-13.38184,13.43311]}, - {"t":5.53336, "x":4.44175, "y":5.6159, "heading":-3.04144, "vx":-1.99727, "vy":0.0945, "omega":0.22158, "ax":-0.00017, "ay":-0.00355, "alpha":-4.13986, "fx":[-12.17163,-9.94816,12.1647,9.94369], "fy":[9.88542,-12.22836,-10.00643,12.10797]}, - {"t":5.57361, "x":4.36135, "y":5.6197, "heading":-3.03252, "vx":-1.99728, "vy":0.09436, "omega":0.05495, "ax":-0.00001, "ay":-0.00019, "alpha":-3.35669, "fx":[-9.93792,-7.97618,9.93756,7.97592], "fy":[7.97274,-9.94104,-7.97936,9.93444]}, - {"t":5.61386, "x":4.28096, "y":5.6235, "heading":-3.03031, "vx":-1.99728, "vy":0.09435, "omega":-0.08016, "ax":0.0, "ay":-0.00001, "alpha":-2.64763, "fx":[-7.85243,-6.27387,7.85241,6.27385], "fy":[6.27368,-7.8526,-6.27404,7.85224]}, - {"t":5.65411, "x":4.20057, "y":5.6273, "heading":-3.03353, "vx":-1.99728, "vy":0.09435, "omega":-0.18673, "ax":0.0, "ay":0.0, "alpha":-1.99853, "fx":[-5.91198,-4.75484,5.91198,4.75484], "fy":[4.75483,-5.91199,-4.75485,5.91197]}, - {"t":5.69436, "x":4.12017, "y":5.63109, "heading":-3.04105, "vx":-1.99728, "vy":0.09435, "omega":-0.26718, "ax":0.0, "ay":0.0, "alpha":-1.39553, "fx":[-4.10314,-3.35114,4.10314,3.35114], "fy":[3.35114,-4.10314,-3.35114,4.10314]}, - {"t":5.73461, "x":4.03978, "y":5.63489, "heading":-3.05181, "vx":-1.99728, "vy":0.09435, "omega":-0.32335, "ax":0.0, "ay":0.0, "alpha":-0.82515, "fx":[-2.40465,-2.00744,2.40465,2.00744], "fy":[2.00744,-2.40465,-2.00744,2.40465]}, - {"t":5.77487, "x":3.95939, "y":5.63869, "heading":-3.06482, "vx":-1.99728, "vy":0.09435, "omega":-0.35656, "ax":0.0, "ay":0.0, "alpha":-0.27421, "fx":[-0.79036,-0.67745,0.79036,0.67745], "fy":[0.67745,-0.79036,-0.67745,0.79036]}, - {"t":5.81512, "x":3.879, "y":5.64249, "heading":-3.07917, "vx":-1.99728, "vy":0.09435, "omega":-0.3676, "ax":0.0, "ay":0.0, "alpha":0.27024, "fx":[0.76924,0.67874,-0.76924,-0.67874], "fy":[-0.67874,0.76924,0.67874,-0.76924]}, - {"t":5.85537, "x":3.7986, "y":5.64628, "heading":-3.09397, "vx":-1.99728, "vy":0.09435, "omega":-0.35672, "ax":0.0, "ay":0.0, "alpha":0.82108, "fx":[2.30646,2.09661,-2.30646,-2.09661], "fy":[-2.09661,2.30646,2.09661,-2.30646]}, - {"t":5.89562, "x":3.71821, "y":5.65008, "heading":-3.10833, "vx":-1.99728, "vy":0.09435, "omega":-0.32367, "ax":0.0, "ay":0.0, "alpha":1.39127, "fx":[3.85677,3.60834,-3.85677,-3.60834], "fy":[-3.60834,3.85677,3.60834,-3.85677]}, - {"t":5.93587, "x":3.63782, "y":5.65388, "heading":-3.12136, "vx":-1.99728, "vy":0.09435, "omega":-0.26767, "ax":0.0, "ay":0.0, "alpha":1.99398, "fx":[5.45971,5.24309,-5.45971,-5.24309], "fy":[-5.24309,5.45971,5.24309,-5.45972]}, - {"t":5.97612, "x":3.55742, "y":5.65768, "heading":-3.13213, "vx":-1.99728, "vy":0.09435, "omega":-0.18741, "ax":0.0001, "ay":0.0, "alpha":2.6427, "fx":[7.1623,7.02804,-7.15905,-7.02479], "fy":[-7.02649,7.16059,7.02634,-7.16077]}, - {"t":6.01637, "x":3.47703, "y":5.66148, "heading":-3.13967, "vx":-1.99727, "vy":0.09435, "omega":-0.08104, "ax":4.64817, "ay":-0.21957, "alpha":1.95046, "fx":[82.79501,83.03203,75.34357,75.08563], "fy":[-9.98264,2.79813,2.97591,-10.73066]}, - {"t":6.05662, "x":3.4004, "y":5.66509, "heading":3.14025, "vx":-1.81018, "vy":0.08551, "omega":-0.00253, "ax":8.96793, "ay":-0.42363, "alpha":0.02249, "fx":[152.53718,152.54843,152.54662,152.53535], "fy":[-7.32005,-7.08056,-7.09132,-7.33117]}, - {"t":6.09688, "x":3.33481, "y":5.66819, "heading":3.14015, "vx":-1.44921, "vy":0.06846, "omega":-0.00162, "ax":8.99223, "ay":-0.42478, "alpha":0.01337, "fx":[152.9523,152.95903,152.95833,152.9516], "fy":[-7.29345,-7.15063,-7.15709,-7.30004]}, - {"t":6.13713, "x":3.28376, "y":5.67061, "heading":3.14008, "vx":-1.08726, "vy":0.05136, "omega":-0.00109, "ax":9.00038, "ay":-0.42516, "alpha":0.01032, "fx":[153.09157,153.09677,153.09633,153.09112], "fy":[-7.2845,-7.17417,-7.17916,-7.28958]}, - {"t":6.17738, "x":3.24729, "y":5.67233, "heading":3.14004, "vx":-0.72498, "vy":0.03425, "omega":-0.00067, "ax":9.00447, "ay":-0.42535, "alpha":0.00879, "fx":[153.16136,153.16579,153.16547,153.16102], "fy":[-7.28001,-7.18597,-7.19023,-7.28433]}, - {"t":6.21763, "x":3.2254, "y":5.67336, "heading":3.14001, "vx":-0.36254, "vy":0.01713, "omega":-0.00032, "ax":9.00692, "ay":-0.42547, "alpha":0.00787, "fx":[153.20328,153.20726,153.20699,153.20301], "fy":[-7.27731,-7.19306,-7.19688,-7.28117]}, - {"t":6.25788, "x":3.2181, "y":5.67371, "heading":3.14, "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":3.65206, "y":5.51369, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.97025, "ay":0.91843, "alpha":0.22031, "fx":[152.44643,152.68925,152.71269,152.47715], "fy":[16.91971,14.56375,14.34216,16.66307]}, + {"t":0.04007, "x":3.65926, "y":5.51442, "heading":0.0, "vx":0.35942, "vy":0.0368, "omega":0.00883, "ax":8.96937, "ay":0.91939, "alpha":0.2191, "fx":[152.43192,152.67363,152.69721,152.46271], "fy":[16.92889,14.58617,14.36546,16.67353]}, + {"t":0.08014, "x":3.68087, "y":5.51664, "heading":0.00035, "vx":0.7188, "vy":0.07364, "omega":0.01761, "ax":8.96832, "ay":0.92052, "alpha":0.21768, "fx":[152.41483,152.65521,152.67888,152.4456], "fy":[16.93938,14.61219,14.39335,16.68634]}, + {"t":0.1202, "x":3.71686, "y":5.52033, "heading":0.00106, "vx":1.07814, "vy":0.11052, "omega":0.02633, "ax":8.96707, "ay":0.92188, "alpha":0.21598, "fx":[152.39434,152.63314,152.65685,152.42503], "fy":[16.95171,14.64305,14.4271,16.70208]}, + {"t":0.16027, "x":3.76726, "y":5.52549, "heading":0.00211, "vx":1.43743, "vy":0.14746, "omega":0.03498, "ax":8.96554, "ay":0.92355, "alpha":0.2139, "fx":[152.36932,152.60619,152.62989,152.39985], "fy":[16.96661,14.68052,14.4685,16.72155]}, + {"t":0.20034, "x":3.83205, "y":5.53214, "heading":0.00352, "vx":1.79665, "vy":0.18446, "omega":0.04355, "ax":8.96363, "ay":0.92563, "alpha":0.21131, "fx":[152.33806,152.57251,152.59618,152.36837], "fy":[16.98521,14.72725,14.52027,16.74599]}, + {"t":0.24041, "x":3.91123, "y":5.54028, "heading":0.00526, "vx":2.1558, "vy":0.22155, "omega":0.05202, "ax":8.96117, "ay":0.9283, "alpha":0.20798, "fx":[152.29786,152.5292,152.55284,152.32789], "fy":[17.00928,14.78737,14.58665,16.77732]}, + {"t":0.28047, "x":4.00481, "y":5.5499, "heading":0.00734, "vx":2.51486, "vy":0.25874, "omega":0.06035, "ax":8.95789, "ay":0.93187, "alpha":0.20357, "fx":[152.24425,152.47142,152.49505,152.27396], "fy":[17.04178,14.86777,14.6747,16.81878]}, + {"t":0.32054, "x":4.11276, "y":5.56102, "heading":0.00976, "vx":2.87378, "vy":0.29608, "omega":0.06851, "ax":8.9533, "ay":0.93685, "alpha":0.19743, "fx":[152.16916,152.3905,152.41418,152.19852], "fy":[17.08806,14.98074,14.79706,16.87623]}, + {"t":0.36061, "x":4.23509, "y":5.57363, "heading":0.01251, "vx":3.23251, "vy":0.33362, "omega":0.07642, "ax":8.94642, "ay":0.94431, "alpha":0.18832, "fx":[152.05652,152.26908,152.29292,152.08549], "fy":[17.15892,15.15069,14.97882,16.96146]}, + {"t":0.40068, "x":4.37179, "y":5.58776, "heading":0.01557, "vx":3.59098, "vy":0.37146, "omega":0.08396, "ax":8.93496, "ay":0.95671, "alpha":0.17338, "fx":[151.86895,152.06686,152.09098,151.89741], "fy":[17.27975,15.4338,15.27791,17.1022]}, + {"t":0.44074, "x":4.52285, "y":5.60341, "heading":0.01893, "vx":3.94898, "vy":0.40979, "omega":0.09091, "ax":8.9121, "ay":0.98135, "alpha":0.14449, "fx":[151.49513,151.66369,151.68779,151.52226], "fy":[17.52776,15.99472,15.86456,17.38262]}, + {"t":0.48081, "x":4.68823, "y":5.62061, "heading":0.02258, "vx":4.30606, "vy":0.44911, "omega":0.0967, "ax":8.84429, "ay":1.05359, "alpha":0.0651, "fx":[150.39006,150.47071,150.48733,150.40732], "fy":[18.29518,17.61184,17.54878,18.22914]}, + {"t":0.52088, "x":4.86786, "y":5.63946, "heading":0.02645, "vx":4.66043, "vy":0.49132, "omega":0.09931, "ax":5.07813, "ay":3.64652, "alpha":-2.47244, "fx":[94.91409,88.90561,77.51127,84.1792], "fy":[51.64772,66.82023,72.70435,56.93287]}, + {"t":0.56095, "x":5.05867, "y":5.66207, "heading":0.03043, "vx":4.8639, "vy":0.63743, "omega":0.00024, "ax":-1.06727, "ay":6.7103, "alpha":-0.0026, "fx":[-18.14401,-18.14222,-18.16376,-18.16556], "fy":[114.13843,114.14479,114.14214,114.13578]}, + {"t":0.60101, "x":5.2527, "y":5.693, "heading":0.03044, "vx":4.82114, "vy":0.9063, "omega":0.00014, "ax":-2.18346, "ay":7.85452, "alpha":-0.28496, "fx":[-36.10367,-35.55936,-38.15622,-38.74077], "fy":[133.71658,134.14753,133.50007,133.04849]}, + {"t":0.64108, "x":5.44412, "y":5.73561, "heading":0.03045, "vx":4.73365, "vy":1.22101, "omega":-0.01128, "ax":-8.8212, "ay":0.91183, "alpha":-1.35977, "fx":[-150.60834,-149.02468,-149.63428,-150.91691], "fy":[9.05571,23.82199,21.33282,7.82909]}, + {"t":0.68115, "x":5.6267, "y":5.78527, "heading":0.02999, "vx":4.38021, "vy":1.25754, "omega":-0.06576, "ax":-8.95561, "ay":-0.0648, "alpha":-0.75916, "fx":[-152.27145,-152.32797,-152.3964,-152.33343], "fy":[-5.15763,3.13729,2.74078,-5.12928]}, + {"t":0.72122, "x":5.79502, "y":5.8356, "heading":0.02736, "vx":4.02138, "vy":1.25495, "omega":-0.09618, "ax":-8.97405, "ay":-0.35976, "alpha":-0.57595, "fx":[-152.50204,-152.75458,-152.78353,-152.54396], "fy":[-9.29243,-3.02306,-3.0693,-9.09275]}, + {"t":0.76128, "x":5.94894, "y":5.8856, "heading":0.02351, "vx":3.66181, "vy":1.24053, "omega":-0.11925, "ax":-8.97928, "ay":-0.50138, "alpha":-0.48767, "fx":[-152.57123,-152.86909,-152.89082,-152.60848], "fy":[-11.25851,-5.96508,-5.88638,-11.00364]}, + {"t":0.80135, "x":6.08845, "y":5.9349, "heading":0.01873, "vx":3.30203, "vy":1.22044, "omega":-0.13879, "ax":-8.98124, "ay":-0.58452, "alpha":-0.43576, "fx":[-152.59928,-152.90917,-152.92957,-152.63534], "fy":[-12.40969,-7.68968,-7.54565,-12.12533]}, + {"t":0.84142, "x":6.21355, "y":5.98333, "heading":0.01317, "vx":2.94218, "vy":1.19702, "omega":-0.15625, "ax":-8.98209, "ay":-0.6392, "alpha":-0.40159, "fx":[-152.6125,-152.92446,-152.94528,-152.64871], "fy":[-13.16755,-8.82505,-8.63722,-12.86087]}, + {"t":0.88149, "x":6.32422, "y":6.03078, "heading":0.00691, "vx":2.58229, "vy":1.17141, "omega":-0.17235, "ax":-8.98247, "ay":-0.6779, "alpha":-0.37739, "fx":[-152.61919,-152.92983,-152.95177,-152.65627], "fy":[-13.70577,-9.63095,-9.4084,-13.37861]}, + {"t":0.92155, "x":6.42048, "y":6.07717, "heading":0.0, "vx":2.22238, "vy":1.14425, "omega":-0.18747, "ax":-4.51085, "ay":7.72813, "alpha":-0.96573, "fx":[-75.06194,-70.70205,-78.32261,-82.8266], "fy":[132.44348,134.90746,130.67589,127.7865]}, + {"t":0.94891, "x":6.47958, "y":6.11136, "heading":-0.00513, "vx":2.09899, "vy":1.35565, "omega":-0.21388, "ax":-5.19303, "ay":7.24781, "alpha":-1.22655, "fx":[-87.08789,-80.95201,-89.58506,-95.70286], "fy":[124.20297,128.43995,122.67202,117.81761]}, + {"t":0.97626, "x":6.53506, "y":6.15116, "heading":-0.01098, "vx":1.95694, "vy":1.5539, "omega":-0.24743, "ax":-5.83626, "ay":6.66511, "alpha":-1.65012, "fx":[-98.73228,-89.91717,-100.04422,-108.39875], "fy":[113.87899,121.27008,113.29343,105.04425]}, + {"t":1.00362, "x":6.5864, "y":6.19616, "heading":-0.01775, "vx":1.79729, "vy":1.73622, "omega":-0.29257, "ax":-6.36822, "ay":5.99325, "alpha":-2.41161, "fx":[-108.97012,-95.69007,-108.51616,-120.10979], "fy":[101.21796,114.60546,103.18785,88.76238]}, + {"t":1.03097, "x":6.63318, "y":6.24589, "heading":-0.02575, "vx":1.62309, "vy":1.90016, "omega":-0.35854, "ax":-6.68746, "ay":5.19792, "alpha":-3.99431, "fx":[-116.63777,-94.68238,-113.64616,-130.0408], "fy":[83.91331,110.47911,93.02844,66.23952]}, + {"t":1.05833, "x":6.67508, "y":6.29981, "heading":-0.03556, "vx":1.44016, "vy":2.04235, "omega":-0.4678, "ax":-6.39634, "ay":4.1235, "alpha":-8.02662, "fx":[-114.6311,-74.39019,-111.97927,-134.19891], "fy":[50.94304,112.97088,84.69847,31.94615]}, + {"t":1.08568, "x":6.71208, "y":6.35722, "heading":-0.04835, "vx":1.2652, "vy":2.15515, "omega":-0.68736, "ax":-3.94932, "ay":2.18924, "alpha":-19.37404, "fx":[-36.30807,-11.89348,-98.33738,-122.1683], "fy":[-36.74632,113.59782,81.94238,-9.84082]}, + {"t":1.11303, "x":6.74521, "y":6.41699, "heading":-0.06716, "vx":1.15717, "vy":2.21503, "omega":-1.21733, "ax":-1.49908, "ay":0.76576, "alpha":-24.92841, "fx":[37.51528,40.77425,-81.88707,-98.39828], "fy":[-70.3312,87.72969,76.05523,-41.35217]}, + {"t":1.14039, "x":6.77631, "y":6.47787, "heading":-0.10046, "vx":1.11616, "vy":2.23598, "omega":-1.89922, "ax":-0.53675, "ay":0.26575, "alpha":-23.36084, "fx":[47.49903,58.48164,-64.0435,-78.45719], "fy":[-67.65727,64.23344,70.13357,-48.62813]}, + {"t":1.16774, "x":6.80664, "y":6.53913, "heading":-0.15241, "vx":1.10148, "vy":2.24325, "omega":-2.53824, "ax":-0.17457, "ay":0.08548, "alpha":-19.93054, "fx":[41.87501,57.89745,-47.57373,-64.07626], "fy":[-60.35707,47.03857,61.67777,-42.54306]}, + {"t":1.1951, "x":6.8367, "y":6.60053, "heading":-0.22184, "vx":1.0967, "vy":2.24558, "omega":-3.08343, "ax":-0.05014, "ay":0.02446, "alpha":-15.15606, "fx":[29.88174,47.78336,-31.58459,-49.49165], "fy":[-48.35601,31.28693,48.92174,-30.18828]}, + {"t":1.22245, "x":6.86668, "y":6.66196, "heading":-0.30618, "vx":1.09533, "vy":2.24625, "omega":-3.49801, "ax":-0.01117, "ay":0.00544, "alpha":-8.86507, "fx":[15.3254,29.67438,-15.70867,-30.05122], "fy":[-29.78026,15.61978,29.9454,-15.41453]}, + {"t":1.24981, "x":6.89664, "y":6.72341, "heading":-0.40187, "vx":1.09502, "vy":2.2464, "omega":-3.74051, "ax":0.00427, "ay":-0.00208, "alpha":-1.25416, "fx":[1.85421,4.48776,-1.70892,-4.34258], "fy":[-4.45051,1.74609,4.37984,-1.81705]}, + {"t":1.27716, "x":6.9266, "y":6.78486, "heading":-0.50419, "vx":1.09514, "vy":2.24635, "omega":-3.77481, "ax":0.03199, "ay":-0.0156, "alpha":6.58276, "fx":[-6.38213,-23.47213,7.48723,24.54345], "fy":[23.75635,-7.21441,-24.25981,6.65644]}, + {"t":1.30451, "x":6.95656, "y":6.8463, "heading":-0.60744, "vx":1.09602, "vy":2.24592, "omega":-3.59475, "ax":0.13544, "ay":-0.06623, "alpha":13.34129, "fx":[-6.45506,-47.72941,11.44693,51.95245], "fy":[48.92781,-10.30818,-50.77626,7.65054]}, + {"t":1.33187, "x":6.9866, "y":6.90771, "heading":-0.70578, "vx":1.09972, "vy":2.24411, "omega":-3.22981, "ax":0.48972, "ay":-0.2418, "alpha":18.47554, "fx":[4.5941,-63.08435,15.21792,76.59266], "fy":[67.00666,-11.08849,-72.97517,0.60496]}, + {"t":1.35922, "x":7.01686, "y":6.96901, "heading":-0.79413, "vx":1.11312, "vy":2.23749, "omega":-2.72442, "ax":1.55948, "ay":-0.79456, "alpha":21.23848, "fx":[37.24879,-60.68365,30.11147,99.42894], "fy":[71.49904,-19.78173,-90.88368,-14.89445]}, + {"t":1.38658, "x":7.04789, "y":7.02991, "heading":-0.86865, "vx":1.15578, "vy":2.21576, "omega":-2.14346, "ax":4.33136, "ay":-2.41138, "alpha":15.85452, "fx":[99.21383,12.94898,64.61503,117.92323], "fy":[36.09066,-63.58119,-99.87589,-36.70115]}, + {"t":1.41393, "x":7.08113, "y":7.08962, "heading":-0.92728, "vx":1.27426, "vy":2.1498, "omega":-1.70977, "ax":6.48842, "ay":-4.2284, "alpha":6.51372, "fx":[129.78935,93.68711,93.71163,124.27687], "fy":[-33.57017,-89.27338,-101.43386,-63.41812]}, + {"t":1.44129, "x":7.11841, "y":7.14685, "heading":-0.97405, "vx":1.45174, "vy":2.03413, "omega":-1.53159, "ax":6.68904, "ay":-5.26286, "alpha":3.23573, "fx":[126.38703,105.62016,102.23145,120.87602], "fy":[-71.57445,-98.6733,-104.69454,-83.1364]}, + {"t":1.46864, "x":7.16063, "y":7.20052, "heading":-1.01595, "vx":1.63472, "vy":1.89017, "omega":-1.44308, "ax":6.34256, "ay":-6.04414, "alpha":1.97187, "fx":[116.62756,102.21986,99.73988,112.95322], "fy":[-93.04463,-108.43859,-111.49268,-98.26016]}, + {"t":1.49599, "x":7.20772, "y":7.24996, "heading":-1.05542, "vx":1.80821, "vy":1.72484, "omega":-1.38914, "ax":5.78937, "ay":-6.71628, "alpha":1.36565, "fx":[105.0022,93.88747,92.33912,102.67337], "fy":[-108.38681,-118.0766,-119.59713,-110.90746]}, + {"t":1.52335, "x":7.25934, "y":7.29463, "heading":-1.09342, "vx":1.96658, "vy":1.54112, "omega":-1.35179, "ax":4.12461, "ay":-7.91167, "alpha":1.01093, "fx":[75.06303,65.22329,65.5846,74.7626], "fy":[-131.94378,-137.07448,-137.03284,-132.24974]}, + {"t":1.5507, "x":7.31468, "y":7.33383, "heading":-1.1304, "vx":2.0794, "vy":1.3247, "omega":-1.32413, "ax":0.89607, "ay":-8.91504, "alpha":0.72278, "fx":[17.65201,10.2721,12.98724,20.05637], "fy":[-151.39382,-152.09257,-151.93534,-151.1475]}, + {"t":1.57806, "x":7.3719, "y":7.36673, "heading":-1.16662, "vx":2.10391, "vy":1.08084, "omega":-1.30436, "ax":-0.66833, "ay":-8.95532, "alpha":0.55647, "fx":[-10.06851,-15.39464,-12.60501,-7.4041], "fy":[-152.42748,-151.9964,-152.27881,-152.60715]}, + {"t":1.60541, "x":7.4292, "y":7.39294, "heading":-1.2023, "vx":2.08563, "vy":0.83587, "omega":-1.28914, "ax":-1.50173, "ay":-8.86523, "alpha":0.461, "fx":[-24.678,-28.89064,-26.3784,-22.22886], "fy":[-150.94612,-150.20688,-150.68426,-151.3428]}, + {"t":1.63276, "x":7.48569, "y":7.41249, "heading":-1.23756, "vx":2.04455, "vy":0.59337, "omega":-1.27653, "ax":-2.00617, "ay":-8.77205, "alpha":0.40087, "fx":[-33.44561,-37.01547,-34.78358,-31.25312], "fy":[-149.3697,-148.53269,-149.08226,-149.85533]}, + {"t":1.66012, "x":7.54086, "y":7.42544, "heading":-1.27248, "vx":1.98968, "vy":0.35342, "omega":-1.26556, "ax":-2.34113, "ay":-8.69354, "alpha":0.35995, "fx":[-39.22787,-42.39511,-40.40138,-37.26335], "fy":[-148.03885,-147.1687,-147.73687,-148.55421]}, + {"t":1.68747, "x":7.59441, "y":7.43185, "heading":-1.3071, "vx":1.92564, "vy":0.11561, "omega":-1.25572, "ax":-2.5787, "ay":-8.62959, "alpha":0.33045, "fx":[-43.3052,-46.20336,-44.40847,-41.53481], "fy":[-146.95746,-146.07656,-146.63889,-147.47452]}, + {"t":1.71483, "x":7.64612, "y":7.43179, "heading":-1.34145, "vx":1.8551, "vy":-0.12044, "omega":-1.24668, "ax":-2.75556, "ay":-8.57742, "alpha":0.30822, "fx":[-46.32444,-49.03368,-47.40704,-44.72011], "fy":[-146.07882,-145.19534,-145.74012,-146.58363]}, + {"t":1.74218, "x":7.69584, "y":7.42528, "heading":-1.37555, "vx":1.77972, "vy":-0.35507, "omega":-1.23825, "ax":-2.89216, "ay":-8.53442, "alpha":0.29089, "fx":[-48.64439,-51.21583,-49.7344,-47.18436], "fy":[-145.35787,-144.47474,-144.99612,-145.8432]}, + {"t":1.76954, "x":7.74344, "y":7.41238, "heading":-1.40942, "vx":1.70061, "vy":-0.58853, "omega":-1.23029, "ax":-3.02111, "ay":-8.49262, "alpha":0.27589, "fx":[-50.83312,-53.2858,-51.93299,-49.50099], "fy":[-144.65785,-143.77461,-144.27252,-145.12291]}, + {"t":1.80347, "x":7.7994, "y":7.38752, "heading":-1.45117, "vx":1.5981, "vy":-0.8767, "omega":-1.22093, "ax":-3.19915, "ay":-8.4249, "alpha":0.29911, "fx":[-53.77664,-56.44585,-55.04381,-52.40052], "fy":[-143.55175,-142.52597,-143.07809,-144.06469]}, + {"t":1.8374, "x":7.85179, "y":7.35292, "heading":-1.4926, "vx":1.48954, "vy":-1.16257, "omega":-1.21078, "ax":-3.44448, "ay":-8.32442, "alpha":0.33121, "fx":[-57.86147,-60.80203,-59.30184,-56.39331], "fy":[-141.9013,-140.66988,-141.31533,-142.49775]}, + {"t":1.87133, "x":7.90035, "y":7.30868, "heading":-1.53368, "vx":1.37266, "vy":-1.44504, "omega":-1.19954, "ax":-3.80268, "ay":-8.16221, "alpha":0.37828, "fx":[-63.87181,-67.16236,-65.47382,-62.22173], "fy":[-139.21913,-137.66694,-138.48729,-139.97395]}, + {"t":1.90527, "x":7.94474, "y":7.25495, "heading":-1.57438, "vx":1.24363, "vy":-1.722, "omega":-1.1867, "ax":-4.36955, "ay":-7.86524, "alpha":0.45332, "fx":[-73.47485,-77.21372,-75.15447,-71.45596], "fy":[-134.26415,-132.1582,-133.3549,-135.36477]}, + {"t":1.9392, "x":7.98442, "y":7.19199, "heading":-1.61465, "vx":1.09536, "vy":-1.98889, "omega":-1.17132, "ax":-5.37255, "ay":-7.20116, "alpha":0.5881, "fx":[-90.70487,-94.88361,-92.06358,-87.88981], "fy":[-123.00976,-119.83729,-122.04533,-125.06613]}, + {"t":1.97313, "x":8.0185, "y":7.12036, "heading":-1.6544, "vx":0.91306, "vy":-2.23324, "omega":-1.15137, "ax":-7.32441, "ay":-5.1547, "alpha":0.86222, "fx":[-124.94696,-128.24699,-124.36213,-120.78809], "fy":[-87.1998,-82.37132,-88.19725,-92.95152]}, + {"t":2.00706, "x":8.04526, "y":7.04161, "heading":-1.69347, "vx":0.66452, "vy":-2.40815, "omega":-1.12211, "ax":-8.70581, "ay":-1.87387, "alpha":1.29344, "fx":[-149.00087,-149.84146,-147.44688,-146.04446], "fy":[-27.49717,-23.58447,-35.82135,-40.59281]}, + {"t":2.04099, "x":8.0628, "y":6.95882, "heading":-1.73154, "vx":0.36912, "vy":-2.47173, "omega":-1.07822, "ax":-8.76137, "ay":-0.77935, "alpha":2.07868, "fx":[-149.61785,-150.20372,-148.90377,-147.38851], "fy":[-4.7156,-0.7982,-20.4893,-27.02277]}, + {"t":2.07493, "x":8.07028, "y":6.8745, "heading":-1.76813, "vx":0.07182, "vy":-2.49818, "omega":-1.00769, "ax":-8.445, "ay":0.2409, "alpha":4.21437, "fx":[-141.81052,-144.0061,-146.14028,-142.6311], "fy":[24.19079,25.64099,-10.63311,-22.80834]}, + {"t":2.10886, "x":8.06786, "y":6.78987, "heading":-1.80232, "vx":-0.21473, "vy":-2.49, "omega":-0.86468, "ax":-6.50946, "ay":0.85469, "alpha":13.18242, "fx":[-84.53394,-120.78228,-133.76338,-103.81649], "fy":[76.45312,58.31092,-17.21012,-59.40212]}, + {"t":2.14279, "x":8.05682, "y":6.70587, "heading":-1.83166, "vx":-0.43561, "vy":-2.461, "omega":-0.41738, "ax":-2.07425, "ay":0.39784, "alpha":24.91942, "fx":[47.44906,-81.39882,-109.37215,2.1921], "fy":[72.93897,79.00563,-33.45774,-91.41826]}, + {"t":2.17672, "x":8.04085, "y":6.62259, "heading":-1.84582, "vx":-0.506, "vy":-2.4475, "omega":0.4282, "ax":-0.51247, "ay":0.10784, "alpha":23.26002, "fx":[69.12176,-52.5032,-84.57852,33.09199], "fy":[48.08391,76.30102,-38.86055,-78.18681]}, + {"t":2.21065, "x":8.02338, "y":6.5396, "heading":-1.83129, "vx":-0.52339, "vy":-2.44384, "omega":1.21746, "ax":-0.11064, "ay":0.02378, "alpha":18.14214, "fx":[57.82852,-36.50205,-61.35656,32.50258], "fy":[35.29771,59.64197,-33.75072,-59.57073]}, + {"t":2.24459, "x":8.00556, "y":6.45669, "heading":-1.78998, "vx":-0.52714, "vy":-2.44304, "omega":1.83306, "ax":-0.01067, "ay":0.0023, "alpha":10.11667, "fx":[32.23255,-20.78593,-32.59029,20.41779], "fy":[20.65246,32.43935,-20.55145,-32.38362]}, + {"t":2.27852, "x":7.98766, "y":6.37379, "heading":-1.72778, "vx":-0.5275, "vy":-2.44296, "omega":2.17634, "ax":0.07424, "ay":-0.01599, "alpha":-0.85821, "fx":[-1.3727,3.17786,3.89816,-0.65216], "fy":[-2.18798,-2.90707,1.64284,2.36425]}, + {"t":2.31245, "x":7.96981, "y":6.29089, "heading":-1.65394, "vx":-0.52498, "vy":-2.4435, "omega":2.14722, "ax":0.53704, "ay":-0.11329, "alpha":-11.43851, "fx":[-23.8997,37.00665,42.22718,-18.79485], "fy":[-30.87151,-34.41266,25.52187,32.05439]}, + {"t":2.34638, "x":7.9523, "y":6.20791, "heading":-1.58108, "vx":-0.50676, "vy":-2.44735, "omega":1.75909, "ax":2.89438, "ay":-0.53914, "alpha":-16.01602, "fx":[9.86572,85.19661,89.70837,12.1597], "fy":[-65.42917,-48.42312,30.86954,46.30046]}, + {"t":2.38032, "x":7.93677, "y":6.12456, "heading":-1.52139, "vx":-0.40855, "vy":-2.46564, "omega":1.21563, "ax":7.41868, "ay":-0.81861, "alpha":-6.18042, "fx":[117.12644,130.55802,133.40349,123.67039], "fy":[-49.13482,-33.46907,12.42616,14.48068]}, + {"t":2.41425, "x":7.92718, "y":6.04042, "heading":-1.48014, "vx":-0.15682, "vy":-2.49342, "omega":1.00591, "ax":7.57402, "ay":-0.18565, "alpha":-4.27918, "fx":[124.97641,132.43041,132.23575,125.68541], "fy":[-26.92348,-18.1764,15.96672,16.50194]}, + {"t":2.44185, "x":7.92574, "y":5.97153, "heading":-1.45237, "vx":0.05224, "vy":-2.49854, "omega":0.8878, "ax":4.36469, "ay":0.1965, "alpha":-11.99392, "fx":[54.43201,101.33983,95.54412,45.65267], "fy":[-47.59912,-25.27645,39.21819,47.02722]}, + {"t":2.46945, "x":7.92884, "y":5.90264, "heading":-1.42787, "vx":0.17271, "vy":-2.49312, "omega":0.55675, "ax":1.22437, "ay":0.09314, "alpha":-15.26856, "fx":[-12.38693,65.82942,55.25293,-25.39041], "fy":[-48.56298,-31.1113,45.83257,40.17897]}, + {"t":2.49705, "x":7.93408, "y":5.83386, "heading":-1.4125, "vx":0.20651, "vy":-2.49055, "omega":0.13531, "ax":0.29481, "ay":0.02493, "alpha":-13.44232, "fx":[-24.75639,46.16756,35.04129,-36.39373], "fy":[-41.47772,-29.02432,41.24294,30.95522]}, + {"t":2.52466, "x":7.93989, "y":5.76513, "heading":-1.40876, "vx":0.21465, "vy":-2.48986, "omega":-0.23573, "ax":0.06683, "ay":0.00579, "alpha":-11.01809, "fx":[-23.25884,35.07525,25.57229,-32.84155], "fy":[-33.94558,-24.2385,33.97784,24.59995]}, + {"t":2.55226, "x":7.94584, "y":5.6964, "heading":-1.41527, "vx":0.21649, "vy":-2.4897, "omega":-0.53985, "ax":0.01437, "ay":0.00125, "alpha":-8.43678, "fx":[-18.61877,26.12379,19.11252,-25.63953], "fy":[-25.8709,-18.83407,25.89265,18.89746]}, + {"t":2.57986, "x":7.95182, "y":5.62768, "heading":-1.43017, "vx":0.21689, "vy":-2.48966, "omega":-0.77272, "ax":0.00295, "ay":0.00026, "alpha":-5.74694, "fx":[-13.0617,17.48664,13.16258,-17.38654], "fy":[-17.43321,-13.10677,17.43997,13.11753]}, + {"t":2.60746, "x":7.95781, "y":5.55896, "heading":-1.4515, "vx":0.21697, "vy":-2.48966, "omega":-0.93134, "ax":0.00059, "ay":0.00005, "alpha":-2.9815, "fx":[-6.98397,8.9089,7.00391,-8.88899], "fy":[-8.89813,-6.99301,8.89976,6.99486]}, + {"t":2.63506, "x":7.9638, "y":5.49024, "heading":-1.47721, "vx":0.21699, "vy":-2.48966, "omega":-1.01364, "ax":0.00012, "ay":0.00001, "alpha":-0.17624, "fx":[-0.42484,0.51719,0.42877,-0.51326], "fy":[-0.51505,-0.42663,0.5154,0.42697]}, + {"t":2.66266, "x":7.96979, "y":5.42153, "heading":-1.50518, "vx":0.21699, "vy":-2.48966, "omega":-1.0185, "ax":0.00004, "ay":0.0, "alpha":2.63139, "fx":[6.5858,-7.51081,-6.58458,7.51203], "fy":[7.51147,6.58524,-7.51137,-6.58514]}, + {"t":2.69027, "x":7.97578, "y":5.35281, "heading":-1.5333, "vx":0.21699, "vy":-2.48965, "omega":-0.94587, "ax":0.00008, "ay":0.00001, "alpha":5.40383, "fx":[13.95292,-15.03794,-13.9503,15.04055], "fy":[15.03933,13.95174,-15.03916,-13.95148]}, + {"t":2.71787, "x":7.98177, "y":5.28409, "heading":-1.55941, "vx":0.21699, "vy":-2.48965, "omega":-0.79672, "ax":0.00036, "ay":0.00003, "alpha":8.10503, "fx":[21.51333,-21.99685,-21.50123,22.00901], "fy":[22.00321,21.50805,-22.00265,-21.50651]}, + {"t":2.74547, "x":7.98775, "y":5.21537, "heading":-1.5814, "vx":0.217, "vy":-2.48965, "omega":-0.573, "ax":0.00166, "ay":0.00014, "alpha":10.70246, "fx":[29.05984,-28.39423,-29.00367,28.45125], "fy":[28.42323,29.0362,-28.42225,-29.02732]}, + {"t":2.77307, "x":7.99375, "y":5.14665, "heading":-1.59721, "vx":0.21705, "vy":-2.48965, "omega":-0.27759, "ax":0.00739, "ay":0.00064, "alpha":13.16943, "fx":[36.39619,-34.27738,-36.14852,34.5325], "fy":[34.40271,36.29662,-34.40727,-36.2482]}, + {"t":2.80067, "x":7.99974, "y":5.07793, "heading":-1.60487, "vx":0.21725, "vy":-2.48963, "omega":0.08591, "ax":0.03103, "ay":0.00271, "alpha":15.48681, "fx":[43.47908,-39.59075,-42.44746,40.67044], "fy":[40.10101,43.08738,-40.16219,-42.84158]}, + {"t":2.82828, "x":8.00575, "y":5.00921, "heading":-1.6025, "vx":0.21811, "vy":-2.48956, "omega":0.51337, "ax":0.12283, "ay":0.01085, "alpha":17.63999, "fx":[50.84684,-43.66761,-46.78873,47.96638], "fy":[45.62614,49.41856,-46.04599,-48.26078]}, + {"t":2.85588, "x":8.01181, "y":4.9405, "heading":-1.58833, "vx":0.2215, "vy":-2.48926, "omega":1.00027, "ax":0.45778, "ay":0.04192, "alpha":19.56094, "fx":[60.80602,-43.40858,-45.68976,59.43911], "fy":[50.6313,56.12227,-52.83791,-51.06361]}, + {"t":2.88348, "x":8.0181, "y":4.87181, "heading":-1.56072, "vx":0.23413, "vy":-2.4881, "omega":1.54018, "ax":1.61166, "ay":0.20221, "alpha":20.43304, "fx":[78.75251,-25.42953,-25.19633,81.52883], "fy":[53.72715,67.08137,-61.02953,-46.02118]}, + {"t":2.91108, "x":8.02518, "y":4.80321, "heading":-1.51821, "vx":0.27862, "vy":-2.48252, "omega":2.10417, "ax":1.88881, "ay":0.19457, "alpha":15.14551, "fx":[68.42881,-8.90856,-4.03154,73.02358], "fy":[43.17777,48.6398,-45.87362,-32.70592]}, + {"t":2.93644, "x":8.03285, "y":4.74033, "heading":-1.46486, "vx":0.32651, "vy":-2.47759, "omega":2.48818, "ax":0.56246, "ay":0.07576, "alpha":10.85982, "fx":[35.38758,-22.45391,-16.14312,41.47871], "fy":[32.76673,28.03045,-31.58154,-24.0609]}, + {"t":2.96179, "x":8.04131, "y":4.67754, "heading":-1.40178, "vx":0.34077, "vy":-2.47567, "omega":2.76352, "ax":0.14641, "ay":0.02027, "alpha":4.22892, "fx":[11.77303,-10.61185,-6.78114,15.58185], "fy":[13.42007,9.65553,-12.78452,-8.91221]}, + {"t":2.98714, "x":8.05, "y":4.61478, "heading":-1.33171, "vx":0.34448, "vy":-2.47515, "omega":2.87074, "ax":0.03367, "ay":0.00469, "alpha":-3.14693, "fx":[-5.63262,10.77899,6.78039,-9.63589], "fy":[-10.13117,-6.12366,10.28417,6.28986]}, + {"t":3.0125, "x":8.05874, "y":4.55202, "heading":-1.25893, "vx":0.34533, "vy":-2.47503, "omega":2.79095, "ax":0.00659, "ay":0.00092, "alpha":-10.01497, "fx":[-17.22242,33.94426,17.45294,-33.72668], "fy":[-33.82603,-17.31598,33.84497,17.35945]}, + {"t":3.03785, "x":8.0675, "y":4.48927, "heading":-1.18816, "vx":0.3455, "vy":-2.47501, "omega":2.53703, "ax":-0.00125, "ay":-0.00018, "alpha":-15.61732, "fx":[-23.2613,54.52316,23.21511,-54.56187], "fy":[-54.54293,-23.24381,54.5421,23.23261]}, + {"t":3.06321, "x":8.07626, "y":4.42652, "heading":-1.12384, "vx":0.34547, "vy":-2.47501, "omega":2.14107, "ax":-0.0085, "ay":-0.00119, "alpha":-19.91992, "fx":[-25.276,71.21008,24.93823,-71.45076], "fy":[-71.32492,-25.15325,71.33607,25.06123]}, + {"t":3.08856, "x":8.08501, "y":4.36377, "heading":-1.06955, "vx":0.34525, "vy":-2.47504, "omega":1.63601, "ax":-0.02454, "ay":-0.00342, "alpha":-23.18906, "fx":[-25.20134,84.18794,24.15571,-84.81216], "fy":[-84.4688,-24.82793,84.53234,24.53155]}, + {"t":3.11391, "x":8.09376, "y":4.30101, "heading":-1.02807, "vx":0.34463, "vy":-2.47513, "omega":1.04808, "ax":-0.0629, "ay":-0.00874, "alpha":-25.69957, "fx":[-24.86355,93.98508,22.01547,-95.41686], "fy":[-94.5974,-23.85181,94.80933,23.0453]}, + {"t":3.13927, "x":8.10248, "y":4.23825, "heading":-1.0015, "vx":0.34304, "vy":-2.47535, "omega":0.39649, "ax":-0.15011, "ay":-0.02068, "alpha":-27.65684, "fx":[-26.05315,101.00696,18.91638,-104.08364], "fy":[-102.25517,-23.54207,102.85039,21.53949]}, + {"t":3.16462, "x":8.11113, "y":4.17549, "heading":-0.99145, "vx":0.33923, "vy":-2.47588, "omega":-0.30473, "ax":-0.33804, "ay":-0.04571, "alpha":-29.18256, "fx":[-30.84259,105.26267,14.18407,-111.60378], "fy":[-107.65416,-25.16603,109.23726,20.47254]}, + {"t":3.18998, "x":8.11962, "y":4.1127, "heading":-0.99918, "vx":0.33066, "vy":-2.47704, "omega":-1.04463, "ax":-0.72484, "ay":-0.09399, "alpha":-30.26121, "fx":[-41.86506,105.83548,5.40153,-118.68921], "fy":[-110.19099,-30.39973,114.32441,19.8712]}, + {"t":3.21533, "x":8.12777, "y":4.04987, "heading":-1.02566, "vx":0.31228, "vy":-2.47942, "omega":-1.81187, "ax":-1.49423, "ay":-0.17644, "alpha":-30.49052, "fx":[-61.94017,98.84809,-12.80437,-125.76947], "fy":[-107.30945,-42.10213,117.53823,19.86854]}, + {"t":3.24068, "x":8.13521, "y":3.98695, "heading":-1.0716, "vx":0.2744, "vy":-2.48389, "omega":-2.58493, "ax":-3.04028, "ay":-0.28812, "alpha":-28.0126, "fx":[-91.89981,64.47613,-46.94831,-132.48519], "fy":[-92.70853,-64.3936,114.90619,22.5923]}, + {"t":3.26604, "x":8.14119, "y":3.92388, "heading":-1.13714, "vx":0.19731, "vy":-2.4912, "omega":-3.29517, "ax":-6.26405, "ay":-0.29575, "alpha":-16.79411, "fx":[-120.37635,-74.3806,-92.61912,-138.82269], "fy":[-67.89936,-70.1111,94.98141,22.90637]}, + {"t":3.29139, "x":8.14418, "y":3.86062, "heading":-1.22068, "vx":0.0385, "vy":-2.4987, "omega":-3.72097, "ax":-7.98468, "ay":0.20203, "alpha":-8.60526, "fx":[-138.79115,-133.47614,-126.97393,-144.02719], "fy":[-40.63321,-30.51113,63.11119,21.77892]}, + {"t":3.31675, "x":8.14259, "y":3.79733, "heading":-1.31503, "vx":-0.16395, "vy":-2.49357, "omega":-3.93915, "ax":-8.4879, "ay":0.93079, "alpha":-5.04219, "fx":[-147.62863,-145.96777,-137.94353,-145.96675], "fy":[-13.5798,-2.73125,51.25428,28.38672]}, + {"t":3.3421, "x":8.1357, "y":3.73441, "heading":-1.4149, "vx":-0.37915, "vy":-2.46997, "omega":-4.06699, "ax":-8.58402, "ay":1.71405, "alpha":-3.32679, "fx":[-149.92783,-148.30072,-140.35591,-145.46215], "fy":[8.87847,17.42809,52.10305,38.21219]}, + {"t":3.36746, "x":8.12333, "y":3.67234, "heading":-1.51801, "vx":-0.59679, "vy":-2.42652, "omega":-4.15134, "ax":-8.49293, "ay":2.50302, "alpha":-2.39284, "fx":[-148.66632,-146.89683,-139.20818,-143.0776], "fy":[27.93865,34.36436,58.57567,49.42383]}, + {"t":3.39281, "x":8.10547, "y":3.61162, "heading":-1.62327, "vx":-0.81212, "vy":-2.36305, "omega":-4.212, "ax":-8.28258, "ay":3.27937, "alpha":-1.83128, "fx":[-145.1531,-143.34295,-135.92233,-139.11882], "fy":[44.8022,49.63235,67.53352,61.15649]}, + {"t":3.41816, "x":8.08222, "y":3.55276, "heading":-1.73006, "vx":-1.02212, "vy":-2.27991, "omega":-4.25843, "ax":-7.97886, "ay":4.03311, "alpha":-1.46709, "fx":[-139.94458,-138.17187,-131.00032,-133.75573], "fy":[60.13068,63.7917,77.54758,72.93803]}, + {"t":3.44352, "x":8.05374, "y":3.49625, "heading":-1.83803, "vx":-1.22442, "vy":-2.17765, "omega":-4.29563, "ax":-7.59277, "ay":4.75867, "alpha":-1.21681, "fx":[-133.28721,-131.59388,-124.64628,-127.07593], "fy":[74.27882,77.07539,87.91563,84.50461]}, + {"t":3.46887, "x":8.02025, "y":3.44257, "heading":-1.94694, "vx":-1.41693, "vy":-2.057, "omega":-4.32648, "ax":-7.12166, "ay":5.46127, "alpha":-1.0368, "fx":[-125.17425,-123.56726,-116.81231,-118.99599], "fy":[87.5896,89.74667,98.40482,95.83727]}, + {"t":3.49423, "x":7.98204, "y":3.39217, "heading":-2.05663, "vx":-1.59749, "vy":-1.91854, "omega":-4.35277, "ax":-6.54372, "ay":6.15623, "alpha":-0.90227, "fx":[-115.26439,-113.71219,-107.11548,-109.13537], "fy":[100.48113,102.17445,109.08697,107.12058]}, + {"t":3.51958, "x":7.93943, "y":3.3455, "heading":-2.167, "vx":-1.7634, "vy":-1.76245, "omega":-4.37565, "ax":-6.19559, "ay":6.48906, "alpha":-0.82311, "fx":[-109.07361,-107.90034,-101.49632,-103.07085], "fy":[106.85207,107.96639,114.0229,112.66665]}, + {"t":3.53782, "x":7.90624, "y":3.31444, "heading":-2.24679, "vx":-1.87638, "vy":-1.64412, "omega":-4.39066, "ax":-5.74067, "ay":6.8842, "alpha":-0.88203, "fx":[-101.79965,-100.56564,-93.256,-94.96753], "fy":[113.63715,114.64909,120.68658,119.42001]}, + {"t":3.55605, "x":7.87107, "y":3.28561, "heading":-2.32686, "vx":-1.98107, "vy":-1.51857, "omega":-4.40674, "ax":-5.24912, "ay":7.25337, "alpha":-0.95182, "fx":[-93.96264,-92.66725,-84.32257,-86.19199], "fy":[120.00134,120.90747,126.88727,125.71485]}, + {"t":3.57429, "x":7.83407, "y":3.25912, "heading":-2.40722, "vx":-2.07679, "vy":-1.3863, "omega":-4.4241, "ax":-4.74146, "ay":7.57958, "alpha":-1.03544, "fx":[-85.90751,-84.57552,-75.04542,-77.07504], "fy":[125.65517,126.43673,132.33769,131.27629]}, + {"t":3.59252, "x":7.79541, "y":3.2351, "heading":-2.4879, "vx":-2.16326, "vy":-1.24808, "omega":-4.44298, "ax":-4.22078, "ay":7.86211, "alpha":-1.13708, "fx":[-77.70358,-76.36826,-65.45491,-67.64996], "fy":[130.59053,131.2256,137.02188,136.09121]}, + {"t":3.61076, "x":7.75526, "y":3.21365, "heading":-2.56892, "vx":-2.24023, "vy":-1.10471, "omega":-4.46372, "ax":-3.68634, "ay":8.10146, "alpha":-1.26277, "fx":[-69.36487,-68.06674,-55.50332,-57.87956], "fy":[134.82422,135.2873,140.9392,140.16334]}, + {"t":3.629, "x":7.71379, "y":3.19485, "heading":-2.65032, "vx":-2.30745, "vy":-0.95697, "omega":-4.48674, "ax":-3.13924, "ay":8.29533, "alpha":-1.42135, "fx":[-60.94394,-59.73921,-45.16072,-47.7467], "fy":[138.3308,138.58665,144.0367,143.45058]}, + {"t":3.64723, "x":7.67119, "y":3.17878, "heading":-2.73214, "vx":-2.3647, "vy":-0.80569, "omega":-4.51266, "ax":-2.58208, "ay":8.43876, "alpha":-1.62633, "fx":[-52.53156,-51.50608,-34.40118,-37.24329], "fy":[141.04816,141.04157,146.20668,145.86739]}, + {"t":3.66547, "x":7.62764, "y":3.16549, "heading":-2.81443, "vx":-2.41179, "vy":-0.65181, "omega":-4.54232, "ax":-2.01868, "ay":8.52309, "alpha":-1.89921, "fx":[-44.2579,-43.55207,-23.18298,-26.35594], "fy":[142.86836,142.50362,147.25969,147.26939]}, + {"t":3.6837, "x":7.58332, "y":3.15502, "heading":-2.89727, "vx":-2.4486, "vy":-0.49638, "omega":-4.57696, "ax":-1.45464, "ay":8.53253, "alpha":-2.27572, "fx":[-36.30845,-36.16631,-11.43599,-15.06109], "fy":[143.59926,142.69301,146.84617,147.40489]}, + {"t":3.70194, "x":7.53843, "y":3.14738, "heading":-2.98073, "vx":-2.47513, "vy":-0.34078, "omega":-4.61846, "ax":-0.89919, "ay":8.43603, "alpha":-2.8178, "fx":[-28.95337,-29.82363,0.9346,-3.33735], "fy":[142.87246,141.03418,144.2668,145.80462]}, + {"t":3.72018, "x":7.49314, "y":3.14257, "heading":-3.06496, "vx":-2.49152, "vy":-0.18694, "omega":-4.66984, "ax":-0.37017, "ay":8.16741, "alpha":-3.63451, "fx":[-22.57956,-25.31518,13.95165,8.75711], "fy":[139.92784,136.23305,138.00383,141.53649]}, + {"t":3.73841, "x":7.44765, "y":3.14052, "heading":3.13307, "vx":-2.49827, "vy":-0.038, "omega":-4.73612, "ax":0.09313, "ay":7.57903, "alpha":-4.89456, "fx":[-17.64228,-23.75953,26.98697,20.751], "fy":[133.12453,125.23243,124.6193,132.69203]}, + {"t":3.75665, "x":7.4021, "y":3.14109, "heading":3.0467, "vx":-2.49658, "vy":0.10021, "omega":-4.82538, "ax":0.40482, "ay":6.38176, "alpha":-6.61083, "fx":[-14.14296,-25.10904,36.22036,30.57525], "fy":[119.08586,101.9011,97.54629,115.67471]}, + {"t":3.77488, "x":7.35664, "y":3.14398, "heading":2.95871, "vx":-2.48919, "vy":0.21659, "omega":-4.94593, "ax":0.45663, "ay":4.41981, "alpha":-7.24034, "fx":[-9.91544,-22.31619,31.2052,32.0952], "fy":[92.7497,64.93028,56.71811,86.32088]}, + {"t":3.79312, "x":7.31133, "y":3.14866, "heading":2.86851, "vx":-2.48087, "vy":0.29719, "omega":-5.07797, "ax":0.32409, "ay":2.5095, "alpha":-3.2841, "fx":[-1.00556,-6.33797,12.59586,16.7982], "fy":[52.74956,37.36161,32.42226,48.21012]}, + {"t":3.81136, "x":7.26614, "y":3.1545, "heading":2.77591, "vx":-2.47496, "vy":0.34295, "omega":-5.13786, "ax":0.22394, "ay":1.5509, "alpha":4.20571, "fx":[10.88361,18.50201,-2.76746,-11.38154], "fy":[12.01229,32.71827,40.4478,20.34293]}, + {"t":3.82959, "x":7.22104, "y":3.16101, "heading":2.68222, "vx":-2.47087, "vy":0.37124, "omega":-5.06116, "ax":0.26089, "ay":1.66664, "alpha":11.12497, "fx":[20.62887,43.85381,-8.47877,-38.25292], "fy":[-12.00247,42.28037,65.90064,17.21767]}, + {"t":3.84783, "x":7.17603, "y":3.16806, "heading":2.58992, "vx":-2.46612, "vy":0.40163, "omega":-4.85829, "ax":0.46228, "ay":2.67147, "alpha":15.12369, "fx":[29.24788,62.63131,-4.90897,-55.51689], "fy":[-11.39224,59.86071,93.5967,39.69838]}, + {"t":3.86606, "x":7.13113, "y":3.17583, "heading":2.50133, "vx":-2.45768, "vy":0.45035, "omega":-4.58249, "ax":0.89358, "ay":4.45616, "alpha":14.32713, "fx":[39.61698,71.62596,4.26867,-54.71328], "fy":[27.21574,81.7952,115.16955,79.01137]}, + {"t":3.8843, "x":7.08646, "y":3.18478, "heading":2.41776, "vx":-2.44139, "vy":0.53161, "omega":-4.32122, "ax":1.53267, "ay":6.31126, "alpha":10.03164, "fx":[47.11042,74.06448,17.02179,-33.91573], "fy":[85.11105,101.3971,128.68434,114.21876]}, + {"t":3.90254, "x":7.0422, "y":3.19552, "heading":2.33896, "vx":-2.41344, "vy":0.6467, "omega":-4.13828, "ax":2.18248, "ay":7.31741, "alpha":6.75567, "fx":[51.45805,74.54814,30.16611,-7.6791], "fy":[114.97847,114.18644,135.38739,133.31611]}, + {"t":3.92077, "x":6.99855, "y":3.20853, "heading":2.26349, "vx":-2.37364, "vy":0.78014, "omega":-4.01509, "ax":2.79491, "ay":7.71077, "alpha":4.8268, "fx":[57.15759,76.17602,42.22449,14.60411], "fy":[125.97448,120.55026,137.54016,140.56676]}, + {"t":3.93901, "x":6.95573, "y":3.22404, "heading":2.19027, "vx":-2.32267, "vy":0.92076, "omega":-3.92707, "ax":3.37703, "ay":7.79593, "alpha":3.65439, "fx":[64.2054,79.64531,53.31765,32.60067], "fy":[129.08243,122.58777,136.89815,141.85762]}, + {"t":3.95724, "x":6.91393, "y":3.24213, "heading":2.11866, "vx":-2.26109, "vy":1.06292, "omega":-3.86042, "ax":3.93444, "ay":7.71645, "alpha":2.89794, "fx":[71.91593,84.50948,63.6608,47.60856], "fy":[128.5068,121.91332,134.47072,140.1276]}, + {"t":3.97548, "x":6.87335, "y":3.2628, "heading":2.04826, "vx":-2.18934, "vy":1.20364, "omega":-3.80758, "ax":4.46919, "ay":7.53646, "alpha":2.38231, "fx":[79.85087,90.22902,73.39074,60.60765], "fy":[125.87333,119.44879,130.7946,136.65545]}, + {"t":3.99372, "x":6.83417, "y":3.286, "heading":1.97882, "vx":-2.10784, "vy":1.34108, "omega":-3.76413, "ax":4.98143, "ay":7.28653, "alpha":2.01426, "fx":[87.7615,96.40085,82.58176,72.18681], "fy":[121.8891,115.69828,126.16014,132.01941]}, + {"t":4.01195, "x":6.79656, "y":3.31167, "heading":1.91018, "vx":-2.017, "vy":1.47395, "omega":-3.7274, "ax":5.47216, "ay":6.98121, "alpha":1.74139, "fx":[95.53014,102.77477,91.29634,82.71818], "fy":[116.87488,110.92304,120.71298,126.48255]}, + {"t":4.03019, "x":6.76069, "y":3.33971, "heading":1.84221, "vx":-1.91721, "vy":1.60126, "omega":-3.69565, "ax":5.97477, "ay":6.59789, "alpha":1.53249, "fx":[103.68267,109.69635,100.10319,93.03411], "fy":[110.42797,104.72909,114.06029,119.69535]}, + {"t":4.04843, "x":6.72672, "y":3.37, "heading":1.77481, "vx":-1.80825, "vy":1.72158, "omega":-3.6677, "ax":6.41316, "ay":6.21938, "alpha":1.50321, "fx":[110.99312,116.53404,107.69115,101.12567], "fy":[103.94146,97.8943,107.64132,113.68245]}, + {"t":4.07531, "x":6.68042, "y":3.41853, "heading":1.67621, "vx":-1.63584, "vy":1.88878, "omega":-3.62729, "ax":6.98906, "ay":5.49819, "alpha":1.95348, "fx":[121.19717,127.32349,117.42285,109.58409], "fy":[90.76904,82.40971,96.17726,104.7344]}, + {"t":4.10219, "x":6.63897, "y":3.4713, "heading":1.5787, "vx":-1.44795, "vy":2.0366, "omega":-3.57477, "ax":7.41852, "ay":4.76534, "alpha":2.69447, "fx":[129.07789,136.08671,124.86173,114.72142], "fy":[76.87244,64.88772,84.9003,97.56775]}, + {"t":4.12908, "x":6.60273, "y":3.52777, "heading":1.48259, "vx":-1.24851, "vy":2.16471, "omega":-3.50233, "ax":7.69331, "ay":3.97412, "alpha":4.0357, "fx":[134.7141,142.82925,130.26966,115.63068], "fy":[60.56402,42.77924,73.53847,93.5129]}, + {"t":4.15596, "x":6.57194, "y":3.5874, "heading":1.38844, "vx":-1.04168, "vy":2.27155, "omega":-3.39384, "ax":7.65385, "ay":3.10764, "alpha":6.75107, "fx":[135.28536,145.22797,132.79704,107.44872], "fy":[38.85781,12.63289,63.00035,96.9495]}, + {"t":4.18284, "x":6.5467, "y":3.6496, "heading":1.2972, "vx":-0.83592, "vy":2.35509, "omega":-3.21234, "ax":6.67085, "ay":2.08944, "alpha":13.00861, "fx":[109.30259,136.62035,130.81684,77.13745], "fy":[0.29369,-27.23964,56.00709,113.10206]}, + {"t":4.20973, "x":6.52664, "y":3.71366, "heading":1.21084, "vx":-0.65658, "vy":2.41126, "omega":-2.86262, "ax":3.22519, "ay":0.81663, "alpha":27.11627, "fx":[-52.85744,112.18053,126.59923,33.51562], "fy":[-50.90356,-67.34528,49.86333,123.94829]}, + {"t":4.23661, "x":6.51016, "y":3.77878, "heading":1.13388, "vx":-0.56987, "vy":2.43322, "omega":-2.13363, "ax":1.55183, "ay":0.34962, "alpha":30.64446, "fx":[-97.21833,78.41686,122.4472,1.93933], "fy":[-42.62029,-95.7225,40.36171,121.76849]}, + {"t":4.2635, "x":6.4954, "y":3.84432, "heading":1.07652, "vx":-0.52815, "vy":2.44262, "omega":-1.30978, "ax":0.72862, "ay":0.15452, "alpha":30.59765, "fx":[-105.78802,52.22686,117.07853,-13.94259], "fy":[-33.615,-105.83082,33.59155,116.36737]}, + {"t":4.29038, "x":6.48146, "y":3.91005, "heading":1.04131, "vx":-0.50857, "vy":2.44677, "omega":-0.4872, "ax":0.32285, "ay":0.06651, "alpha":29.57989, "fx":[-106.02633,36.73942,111.29712,-20.04386], "fy":[-28.23508,-106.56528,28.6765,110.64917]}, + {"t":4.31726, "x":6.4679, "y":3.97585, "heading":1.02821, "vx":-0.49989, "vy":2.44856, "omega":0.30802, "ax":0.13512, "ay":0.02748, "alpha":28.11293, "fx":[-102.38333,29.04124,104.80551,-22.26993], "fy":[-25.51431,-102.80347,25.81214,104.3754]}, + {"t":4.34415, "x":6.45451, "y":4.04169, "heading":1.03649, "vx":-0.49625, "vy":2.4493, "omega":1.06381, "ax":0.05311, "ay":0.01074, "alpha":26.24119, "fx":[-95.96004,26.02407,97.02616,-23.47668], "fy":[-24.6843,-96.19493,24.81854,96.79161]}, + {"t":4.37103, "x":6.44119, "y":4.10754, "heading":1.06509, "vx":-0.49483, "vy":2.44959, "omega":1.76927, "ax":0.01939, "ay":0.00391, "alpha":23.86649, "fx":[-86.86122,25.45069,87.30149,-24.57202], "fy":[-24.98628,-86.97371,25.03673,87.18933]}, + {"t":4.39792, "x":6.4279, "y":4.17339, "heading":1.11265, "vx":-0.4943, "vy":2.44969, "omega":2.4109, "ax":0.00631, "ay":0.00127, "alpha":20.82737, "fx":[-74.78757,25.54847,74.95014,-25.28162], "fy":[-25.40625,-74.83451,25.42387,74.90327]}, + {"t":4.4248, "x":6.41461, "y":4.23925, "heading":1.17747, "vx":-0.49414, "vy":2.44973, "omega":2.97082, "ax":0.00073, "ay":0.00014, "alpha":16.92659, "fx":[-59.37046,24.56689,59.3916,-24.5384], "fy":[-24.55145,-59.37737,24.55384,59.38469]}, + {"t":4.45168, "x":6.40133, "y":4.30511, "heading":1.25733, "vx":-0.49412, "vy":2.44973, "omega":3.42587, "ax":-0.0064, "ay":-0.0013, "alpha":11.98748, "fx":[-40.63425,20.57195,40.43083,-20.80409], "fy":[-20.70346,-40.56123,20.67261,40.50394]}, + {"t":4.47857, "x":6.38804, "y":4.37097, "heading":1.34944, "vx":-0.49429, "vy":2.4497, "omega":3.74814, "ax":-0.03643, "ay":-0.00736, "alpha":5.99376, "fx":[-19.84012,11.536,18.61732,-12.79206], "fy":[-12.27827,-19.36579,12.0501,19.09313]}, + {"t":4.50545, "x":6.37474, "y":4.43682, "heading":1.4502, "vx":-0.49527, "vy":2.4495, "omega":3.90928, "ax":-0.18402, "ay":-0.0374, "alpha":-0.69739, "fx":[-1.04726,-4.76353,-5.21235,-1.49769], "fy":[0.99819,1.4477,-2.26885,-2.7218]}, + {"t":4.53234, "x":6.36136, "y":4.50266, "heading":1.5553, "vx":-0.50021, "vy":2.44849, "omega":3.89053, "ax":-0.84492, "ay":-0.17671, "alpha":-7.19847, "fx":[5.11967,-33.39758,-33.65646,4.44665], "fy":[16.56941,16.39154,-21.67206,-23.31232]}, + {"t":4.55922, "x":6.3476, "y":4.56842, "heading":1.65989, "vx":-0.52293, "vy":2.44374, "omega":3.69701, "ax":-3.19324, "ay":-0.74255, "alpha":-10.34037, "fx":[-34.21807,-81.85528,-74.88151,-26.3093], "fy":[23.89538,13.3008,-41.28229,-46.43598]}, + {"t":4.5861, "x":6.33239, "y":4.63385, "heading":1.75928, "vx":-0.60878, "vy":2.42378, "omega":3.41902, "ax":-6.63109, "ay":-1.93057, "alpha":-5.45278, "fx":[-113.29341,-124.16431,-114.31929,-99.39486], "fy":[-7.78455,-13.35238,-51.80938,-58.40716]}, + {"t":4.61299, "x":6.31363, "y":4.69831, "heading":1.8512, "vx":-0.78705, "vy":2.37188, "omega":3.27242, "ax":-7.72187, "ay":-2.95077, "alpha":-2.50559, "fx":[-134.66423,-136.65389,-128.84373,-125.22538], "fy":[-38.35839,-38.99417,-60.271,-63.14361]}, + {"t":4.63987, "x":6.28968, "y":4.76101, "heading":1.93917, "vx":-0.99464, "vy":2.29255, "omega":3.20506, "ax":-7.78137, "ay":-3.81867, "alpha":-1.46776, "fx":[-135.27285,-136.04127,-129.82041,-128.30117], "fy":[-58.31637,-58.03961,-70.96272,-72.49932]}, + {"t":4.66675, "x":6.26013, "y":4.82127, "heading":2.02534, "vx":-1.20383, "vy":2.18989, "omega":3.1656, "ax":-7.51933, "ay":-4.61515, "alpha":-1.00232, "fx":[-130.36981,-130.89135,-125.64601,-124.69959], "fy":[-74.25776,-73.8139,-82.46324,-83.47436]}, + {"t":4.69364, "x":6.22505, "y":4.87847, "heading":2.11044, "vx":-1.40598, "vy":2.06582, "omega":3.13866, "ax":-7.07618, "ay":-5.39146, "alpha":-0.75083, "fx":[-122.50668,-122.99573,-118.35963,-117.59289], "fy":[-88.80579,-88.32162,-94.46259,-95.23855]}, + {"t":4.72052, "x":6.18469, "y":4.93206, "heading":2.19482, "vx":-1.59622, "vy":1.92087, "omega":3.11847, "ax":-6.61388, "ay":-5.93659, "alpha":-0.67985, "fx":[-114.6445,-115.12004,-110.47843,-109.75775], "fy":[-98.51066,-98.12401,-103.33818,-103.94597]}, + {"t":4.74513, "x":6.14342, "y":4.97752, "heading":2.27155, "vx":-1.75894, "vy":1.77481, "omega":3.10175, "ax":-5.99126, "ay":-6.47139, "alpha":-0.9286, "fx":[-105.05362,-105.84112,-99.0095,-97.73437], "fy":[-107.01606,-106.55087,-112.96007,-113.77906]}, + {"t":4.76973, "x":6.09833, "y":5.01923, "heading":2.34786, "vx":-1.90635, "vy":1.61559, "omega":3.0789, "ax":-5.32945, "ay":-6.85957, "alpha":-1.38203, "fx":[-95.58195,-96.85852,-86.27601,-83.89297], "fy":[-112.50528,-112.11661,-120.52507,-121.57011]}, + {"t":4.79433, "x":6.04981, "y":5.05691, "heading":2.42361, "vx":-2.03748, "vy":1.44681, "omega":3.0449, "ax":-4.57073, "ay":-7.03232, "alpha":-2.34599, "fx":[-86.34032,-88.48445,-70.69127,-65.47126], "fy":[-112.98247,-113.43051,-125.50341,-126.55493]}, + {"t":4.81894, "x":5.9983, "y":5.09037, "heading":2.49853, "vx":-2.14994, "vy":1.27379, "omega":2.98718, "ax":-3.60512, "ay":-6.63382, "alpha":-4.88196, "fx":[-78.53405,-82.24588,-49.53818,-34.97037], "fy":[-98.67358,-105.16195,-124.66728,-122.85459]}, + {"t":4.84354, "x":5.94431, "y":5.11971, "heading":2.57203, "vx":-2.23864, "vy":1.11057, "omega":2.86706, "ax":-2.10469, "ay":-4.51694, "alpha":-11.88304, "fx":[-66.28541,-78.04609,-20.47995,21.61083], "fy":[-34.70487,-78.6096,-110.75276,-83.26024]}, + {"t":4.86815, "x":5.88859, "y":5.14566, "heading":2.64257, "vx":-2.29042, "vy":0.99944, "omega":2.57469, "ax":-0.72046, "ay":-1.69262, "alpha":-17.00622, "fx":[-38.09591,-70.56845,4.1105,55.53446], "fy":[35.06772,-47.29641,-86.51017,-16.425]}, + {"t":4.89275, "x":5.83202, "y":5.16974, "heading":2.70591, "vx":-2.30815, "vy":0.95779, "omega":2.15627, "ax":-0.21938, "ay":-0.53292, "alpha":-16.37794, "fx":[-26.73529,-60.82513,16.41325,56.2208], "fy":[49.97709,-30.84073,-66.61903,11.2232]}, + {"t":4.91735, "x":5.77516, "y":5.19315, "heading":2.75897, "vx":-2.31354, "vy":0.94468, "omega":1.75331, "ax":-0.06496, "ay":-0.15947, "alpha":-14.69558, "fx":[-23.35257,-52.0777,20.42386,50.58656], "fy":[48.74466,-24.7013,-53.88397,18.99041]}, + {"t":4.94196, "x":5.71822, "y":5.21634, "heading":2.80211, "vx":-2.31514, "vy":0.94076, "omega":1.39174, "ax":-0.01855, "ay":-0.04568, "alpha":-12.85691, "fx":[-21.44783,-44.27003,20.6545,43.8012], "fy":[43.28189,-21.84971,-44.78693,20.24669]}, + {"t":4.96656, "x":5.66126, "y":5.23947, "heading":2.83635, "vx":-2.3156, "vy":0.93964, "omega":1.07541, "ax":-0.0051, "ay":-0.01258, "alpha":-10.95609, "fx":[-19.31518,-36.95892,19.10856,36.81835], "fy":[36.67843,-19.42937,-37.09869,18.99399]}, + {"t":4.99117, "x":5.60428, "y":5.26259, "heading":2.86281, "vx":-2.31572, "vy":0.93933, "omega":0.80585, "ax":-0.00136, "ay":-0.00334, "alpha":-9.00823, "fx":[-16.61909,-29.92177,16.567,29.88166], "fy":[29.84542,-16.65037,-29.95801,16.5357]}, + {"t":5.01577, "x":5.5473, "y":5.2857, "heading":2.88263, "vx":-2.31576, "vy":0.93924, "omega":0.58421, "ax":-0.00035, "ay":-0.00086, "alpha":-7.02266, "fx":[-13.40168,-23.0553,13.38885,23.04437], "fy":[23.03526,-13.40997,-23.06441,13.38056]}, + {"t":5.04037, "x":5.49033, "y":5.30881, "heading":2.89701, "vx":-2.31577, "vy":0.93922, "omega":0.41142, "ax":-0.00009, "ay":-0.00022, "alpha":-5.00858, "fx":[-9.79043,-16.30166,9.78725,16.29873], "fy":[16.29644,-9.79261,-16.30395,9.78507]}, + {"t":5.06498, "x":5.43335, "y":5.33192, "heading":2.90713, "vx":-2.31577, "vy":0.93922, "omega":0.28819, "ax":-0.00003, "ay":-0.00008, "alpha":-2.97489, "fx":[-5.91242,-9.62282,5.91133,9.62176], "fy":[9.62097,-5.9132,-9.62362,5.91055]}, + {"t":5.08958, "x":5.37637, "y":5.35502, "heading":2.91422, "vx":-2.31577, "vy":0.93921, "omega":0.215, "ax":-0.00005, "ay":-0.00012, "alpha":-0.92992, "fx":[-1.87009,-2.99547,1.86846,2.99385], "fy":[2.99265,-1.87128,-2.99667,1.86727]}, + {"t":5.11419, "x":5.3194, "y":5.37813, "heading":2.91951, "vx":-2.31577, "vy":0.93921, "omega":0.19212, "ax":-0.00018, "ay":-0.00044, "alpha":1.1185, "fx":[2.26431,3.58695,-2.27043,-3.59305], "fy":[-3.59754,2.25983,3.58247,-2.27491]}, + {"t":5.13879, "x":5.26242, "y":5.40124, "heading":2.92424, "vx":-2.31577, "vy":0.9392, "omega":0.21964, "ax":-0.00074, "ay":-0.00182, "alpha":3.16277, "fx":[6.44654,10.10859,-6.47208,-10.13332], "fy":[-10.15193,6.42831,10.08997,-6.49031]}, + {"t":5.16339, "x":5.20544, "y":5.42435, "heading":2.92964, "vx":-2.31579, "vy":0.93916, "omega":0.29746, "ax":-0.00301, "ay":-0.00742, "alpha":5.195, "fx":[10.64604,16.51769,-10.75283,-16.61551], "fy":[-16.69265,10.57317,16.44053,-10.82564]}, + {"t":5.188, "x":5.14846, "y":5.44745, "heading":2.93696, "vx":-2.31587, "vy":0.93897, "omega":0.42527, "ax":-0.01197, "ay":-0.02954, "alpha":7.20667, "fx":[14.78985,22.68611,-15.23157,-23.05899], "fy":[-23.37447,14.5071,22.37027,-15.51297]}, + {"t":5.2126, "x":5.09148, "y":5.47055, "heading":2.94742, "vx":-2.31616, "vy":0.93825, "omega":0.60259, "ax":-0.04631, "ay":-0.11452, "alpha":9.18606, "fx":[18.54746,28.27535,-20.33972,-29.63403], "fy":[-30.89857,17.4822,27.00344,-21.3792]}, + {"t":5.2372, "x":5.03448, "y":5.4936, "heading":2.96225, "vx":-2.3173, "vy":0.93543, "omega":0.8286, "ax":-0.17232, "ay":-0.42972, "alpha":11.07901, "fx":[20.54445,32.26198,-27.58931,-36.94178], "fy":[-41.85446,16.55034,27.2127,-31.14609]}, + {"t":5.26181, "x":4.97741, "y":5.51648, "heading":2.98264, "vx":-2.32154, "vy":0.92486, "omega":1.10119, "ax":-0.59797, "ay":-1.53731, "alpha":12.26408, "fx":[16.03453,30.6677,-41.58277,-45.80488], "fy":[-63.39819,-0.36357,11.03569,-51.87046]}, + {"t":5.28641, "x":4.92011, "y":5.53877, "heading":3.00973, "vx":-2.33625, "vy":0.88703, "omega":1.40293, "ax":-1.58958, "ay":-4.50366, "alpha":8.88326, "fx":[-2.06968,6.91894,-60.72786,-52.27487], "fy":[-99.95485,-66.50086,-51.21162,-88.75634]}, + {"t":5.31102, "x":4.86215, "y":5.55923, "heading":3.04425, "vx":-2.37536, "vy":0.77623, "omega":1.62149, "ax":-2.05373, "ay":-7.16826, "alpha":3.62568, "fx":[-18.89026,-19.76019,-53.90946,-47.17351], "fy":[-129.00903,-122.71037,-113.64689,-122.35386]}, + {"t":5.33562, "x":4.80309, "y":5.57616, "heading":3.08414, "vx":-2.42589, "vy":0.59986, "omega":1.7107, "ax":-1.66904, "ay":-8.18694, "alpha":1.71861, "fx":[-19.44871,-20.69561,-38.19854,-35.21655], "fy":[-141.60878,-140.11525,-136.61444,-138.69167]}, + {"t":5.36022, "x":4.74289, "y":5.58844, "heading":3.12623, "vx":-2.46696, "vy":0.39843, "omega":1.75298, "ax":-1.05255, "ay":-8.60947, "alpha":1.00638, "fx":[-12.4233,-13.12686,-23.71975,-22.34436], "fy":[-147.28642,-146.81429,-145.52752,-146.15021]}, + {"t":5.38483, "x":4.68188, "y":5.59564, "heading":-3.11382, "vx":-2.49285, "vy":0.1866, "omega":1.77774, "ax":-8.9947, "ay":-0.34193, "alpha":-0.20979, "fx":[-153.04313,-152.95866,-152.95057,-153.03661], "fy":[-4.69072,-6.91868,-6.95779,-4.69702]}, + {"t":5.4156, "x":4.6009, "y":5.60122, "heading":-3.05911, "vx":-2.76967, "vy":0.17608, "omega":1.77129, "ax":-8.98918, "ay":-0.35166, "alpha":-0.58013, "fx":[-153.03122,-152.78757,-152.77621,-153.01884], "fy":[-3.08085,-9.15183,-9.00733,-2.68633]}, + {"t":5.44638, "x":4.51141, "y":5.60647, "heading":-3.0046, "vx":-3.04632, "vy":0.16526, "omega":1.75343, "ax":-8.97956, "ay":-0.36438, "alpha":-1.07005, "fx":[-152.99016,-152.50277,-152.51284,-152.9534], "fy":[-1.2712,-12.22248,-11.5429,0.24468]}, + {"t":5.47715, "x":4.4134, "y":5.61138, "heading":-2.95064, "vx":-3.32267, "vy":0.15404, "omega":1.7205, "ax":-8.96185, "ay":-0.38174, "alpha":-1.74971, "fx":[-152.90594,-152.00178,-152.08779,-152.75869], "fy":[0.81138,-16.52952,-14.88122,4.62596]}, + {"t":5.50793, "x":4.3069, "y":5.61594, "heading":-2.89769, "vx":-3.59848, "vy":0.14229, "omega":1.66665, "ax":-8.92634, "ay":-0.40709, "alpha":-2.75751, "fx":[-152.75312,-151.05632,-151.32093,-152.20763], "fy":[3.30181,-22.784,-19.71257,11.49654]}, + {"t":5.53871, "x":4.19193, "y":5.62013, "heading":-2.84639, "vx":-3.87319, "vy":0.12976, "omega":1.58179, "ax":-8.84451, "ay":-0.44924, "alpha":-4.40965, "fx":[-152.47879,-149.09157,-149.64039,-150.5593], "fy":[6.46734,-32.36398,-27.83977,23.1706]}, + {"t":5.56948, "x":4.06854, "y":5.62391, "heading":-2.79771, "vx":-4.14538, "vy":0.11594, "omega":1.44608, "ax":-8.60313, "ay":-0.54727, "alpha":-7.62339, "fx":[-151.9531,-144.41113,-144.26066,-144.72193], "fy":[10.8819,-48.16463,-45.5903,45.63714]}, + {"t":5.60026, "x":3.93689, "y":5.62722, "heading":-2.75321, "vx":-4.41015, "vy":0.0991, "omega":1.21147, "ax":-7.46187, "ay":-1.01277, "alpha":-16.25917, "fx":[-150.8283,-131.79242,-105.90185,-119.17462], "fy":[17.46425,-74.94536,-103.33277,91.90588]}, + {"t":5.63103, "x":3.79763, "y":5.62979, "heading":-2.71593, "vx":-4.63979, "vy":0.06793, "omega":0.71108, "ax":-6.89128, "ay":-1.23288, "alpha":-18.40815, "fx":[-149.33936,-125.44907,-82.80224,-111.28387], "fy":[15.13556,-81.53362,-114.11635,96.6307]}, + {"t":5.66181, "x":3.65157, "y":5.6313, "heading":-2.69404, "vx":-4.85188, "vy":0.02998, "omega":0.14456, "ax":-1.67068, "ay":-3.68578, "alpha":-4.61176, "fx":[-47.92324,-32.50093,-8.91542,-24.3315], "fy":[-55.65567,-75.77549,-70.6492,-48.69614]}, + {"t":5.69258, "x":3.50146, "y":5.63047, "heading":-2.68959, "vx":-4.90329, "vy":-0.08345, "omega":0.00263, "ax":0.18923, "ay":-2.0915, "alpha":0.39706, "fx":[4.71157,3.75468,1.72085,2.68789], "fy":[-36.03115,-34.22037,-35.12481,-36.92695]}, + {"t":5.72336, "x":3.35065, "y":5.62692, "heading":-2.68951, "vx":-4.89747, "vy":-0.14781, "omega":0.01485, "ax":6.76364, "ay":0.53392, "alpha":17.31864, "fx":[145.34199,125.74331,88.83943,100.26568], "fy":[-22.18468,73.57569,86.65115,-101.71511]}, + {"t":5.75414, "x":3.20313, "y":5.62262, "heading":-2.68905, "vx":-4.68931, "vy":-0.13138, "omega":0.54784, "ax":8.7364, "ay":0.19272, "alpha":4.55467, "fx":[150.9612,147.3916,148.74864,147.31314], "fy":[-6.49431,32.10413,17.90253,-30.40008]}, + {"t":5.78491, "x":3.06295, "y":5.61867, "heading":-2.67219, "vx":-4.42045, "vy":-0.12545, "omega":0.68801, "ax":8.91898, "ay":0.22486, "alpha":1.32507, "fx":[152.01479,151.40397,151.63763,151.78059], "fy":[0.692,12.98427,7.37003,-5.74741]}, + {"t":5.81569, "x":2.93113, "y":5.61491, "heading":-2.65102, "vx":-4.14596, "vy":-0.11853, "omega":0.72879, "ax":8.95809, "ay":0.23887, "alpha":0.07996, "fx":[152.38427,152.36028,152.36579,152.38799], "fy":[3.87351,4.63403,4.25406,3.49066]}, + {"t":5.84646, "x":2.80778, "y":5.61138, "heading":-2.62859, "vx":-3.87027, "vy":-0.11118, "omega":0.73126, "ax":8.97173, "ay":0.24607, "alpha":-0.57791, "fx":[152.57217,152.68845,152.68968,152.47608], "fy":[5.5099,-0.01674,2.93261,8.31657]}, + {"t":5.87724, "x":2.69292, "y":5.60807, "heading":-2.60609, "vx":-3.59416, "vy":-0.10361, "omega":0.71347, "ax":8.97773, "ay":0.25045, "alpha":-0.98396, "fx":[152.69146,152.8214,152.87204,152.44981], "fy":[6.39986,-2.98297,2.31436,11.30883]}, + {"t":5.90801, "x":2.58656, "y":5.605, "heading":-2.58413, "vx":-3.31786, "vy":-0.0959, "omega":0.68319, "ax":8.98076, "ay":0.25339, "alpha":-1.25911, "fx":[152.77831,152.87858,152.9888,152.39476], "fy":[6.87966,-5.04102,2.03739,13.36445]}, + {"t":5.93879, "x":2.4887, "y":5.60217, "heading":-2.5631, "vx":-3.04148, "vy":-0.0881, "omega":0.64444, "ax":8.98242, "ay":0.25551, "alpha":-1.45759, "fx":[152.84704,152.90225,153.06973,152.33453], "fy":[7.11651,-6.55195,1.94547,14.87465]}, + {"t":5.96956, "x":2.39935, "y":5.59958, "heading":-2.54327, "vx":-2.76504, "vy":-0.08024, "omega":0.59958, "ax":8.98339, "ay":0.25711, "alpha":-1.60736, "fx":[152.90415,152.90993,153.12887,152.2765], "fy":[7.20371,-7.70653,1.9593,16.03689]}, + {"t":6.00034, "x":2.31851, "y":5.59723, "heading":-2.52482, "vx":-2.48857, "vy":-0.07233, "omega":0.55011, "ax":8.98397, "ay":0.25835, "alpha":-1.72431, "fx":[152.95295,152.90973,153.17374,152.22295], "fy":[7.19789,-8.61539,2.03391,16.9617]}, + {"t":6.03112, "x":2.24618, "y":5.59513, "heading":-2.50789, "vx":-2.21208, "vy":-0.06437, "omega":0.49705, "ax":8.98434, "ay":0.25935, "alpha":-1.81811, "fx":[152.9953,152.90563,153.20875,152.17443], "fy":[7.1357,-9.34736,2.14167,17.71595]}, + {"t":6.06189, "x":2.18236, "y":5.59327, "heading":-2.49259, "vx":-1.93558, "vy":-0.05639, "omega":0.44109, "ax":8.98457, "ay":0.26017, "alpha":-1.895, "fx":[153.03231,152.89972,153.23669,152.13085], "fy":[7.04212,-9.94779,2.26447,18.34261]}, + {"t":6.09267, "x":2.12704, "y":5.59166, "heading":-2.47902, "vx":-1.65908, "vy":-0.04839, "omega":0.38277, "ax":8.98471, "ay":0.26084, "alpha":-1.95917, "fx":[153.06474,152.89309,153.25945,152.09187], "fy":[6.93486,-10.4479,2.38992,18.87066]}, + {"t":6.12344, "x":2.08024, "y":5.59029, "heading":-2.46724, "vx":-1.38257, "vy":-0.04036, "omega":0.32248, "ax":8.98479, "ay":0.26142, "alpha":-2.01354, "fx":[153.09311,152.88632,153.27835,152.05712], "fy":[6.82689,-10.87002,2.50915,19.32046]}, + {"t":6.15422, "x":2.04195, "y":5.58918, "heading":-2.45731, "vx":-1.10606, "vy":-0.03231, "omega":0.26051, "ax":8.98484, "ay":0.26191, "alpha":-2.06021, "fx":[153.1178,152.87969,153.29435,152.02622], "fy":[6.72797,-11.23058,2.61563,19.70678]}, + {"t":6.18499, "x":2.01216, "y":5.58831, "heading":-2.44929, "vx":-0.82954, "vy":-0.02425, "omega":0.19711, "ax":8.98486, "ay":0.26233, "alpha":-2.10073, "fx":[153.13913,152.87334,153.30818,151.99882], "fy":[6.64563,-11.54195,2.70442,20.04057]}, + {"t":6.21577, "x":1.99089, "y":5.58768, "heading":-2.44323, "vx":-0.55303, "vy":-0.01618, "omega":0.13246, "ax":8.98486, "ay":0.2627, "alpha":-2.13625, "fx":[153.15732,152.86731,153.32039,151.97462], "fy":[6.58575,-11.81363,2.77169,20.33016]}, + {"t":6.24654, "x":1.97812, "y":5.58731, "heading":-2.43915, "vx":-0.27651, "vy":-0.00809, "omega":0.06671, "ax":8.98485, "ay":0.26303, "alpha":-2.16766, "fx":[153.17258,152.8616,153.33143,151.95336], "fy":[6.55302,-12.05298,2.81442,20.5819]}, + {"t":6.27732, "x":1.97387, "y":5.58719, "heading":-2.4371, "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/RightBumpPickupFromMidScore.traj b/src/main/deploy/choreo/RightBumpPickupFromMidScore.traj index aff20c4..b6e2023 100644 --- a/src/main/deploy/choreo/RightBumpPickupFromMidScore.traj +++ b/src/main/deploy/choreo/RightBumpPickupFromMidScore.traj @@ -3,40 +3,46 @@ "version":3, "snapshot":{ "waypoints":[ - {"x":3.616, "y":2.511, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.755803108215332, "y":2.5251476764678955, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.615817546844482, "y":2.630027532577514, "heading":0.9272953695519418, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.689233303070068, "y":3.1124746799468994, "heading":1.4876547290954834, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.741673469543457, "y":4.014441013336182, "heading":1.6332153136974965, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":7.479474067687988, "y":4.926895618438721, "heading":2.0778956890190523, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":6.650923252105713, "y":5.398854732513428, "heading":2.6918324705623493, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":5.360901355743408, "y":5.5561747550964355, "heading":3.0890101362203475, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.368184804916382, "y":5.5876383781433105, "heading":-1.808632057092331, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":2.780857801437378, "y":4.507376194000244, "heading":-0.8894979211695171, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":3.61569, "y":4.03479, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":3.6253833770751953, "y":2.5349528789520264, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.812593936920166, "y":2.3075690269470215, "heading":0.0, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.8845438957214355, "y":1.5387967824935913, "heading":0.5645693936681, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.248845100402832, "y":0.5642969608306885, "heading":1.4464422073687606, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.357122421264648, "y":1.7120414972305298, "heading":1.5444860228722417, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.400433540344238, "y":2.708197116851806, "heading":1.4876547290954834, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":8.270500183105469, "y":3.661041498184204, "heading":2.098871497784393, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":7.815732955932617, "y":4.9170637130737305, "heading":2.5468338404100317, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.830404758453369, "y":4.949546813964844, "heading":-2.720173097883985, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.353983402252197, "y":3.661041736602783, "heading":-2.5599506032522905, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.077489376068115, "y":2.463531970977783, "heading":2.7950533983717607, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":4.448294639587402, "y":2.5566084384918213, "heading":3.117207257508124, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":2.4559834003448486, "y":2.5349528789520264, "heading":-1.6220334941983154, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}, + {"from":1, "to":10, "data":{"type":"MaxVelocity", "props":{"max":2.5}}, "enabled":true}], "targetDt":0.05 }, "params":{ "waypoints":[ - {"x":{"exp":"RightBump.x", "val":3.616}, "y":{"exp":"RightBump.y", "val":2.511}, "heading":{"exp":"RightBump.heading", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.755803108215332 m", "val":6.755803108215332}, "y":{"exp":"2.5251476764678955 m", "val":2.5251476764678955}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.615817546844482 m", "val":7.615817546844482}, "y":{"exp":"2.6300275325775146 m", "val":2.630027532577514}, "heading":{"exp":"0.9272953695519418 rad", "val":0.9272953695519418}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.689233303070068 m", "val":7.689233303070068}, "y":{"exp":"3.1124746799468994 m", "val":3.1124746799468994}, "heading":{"exp":"1.4876547290954834 rad", "val":1.4876547290954834}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.741673469543457 m", "val":7.741673469543457}, "y":{"exp":"4.014441013336182 m", "val":4.014441013336182}, "heading":{"exp":"1.6332153136974963 rad", "val":1.6332153136974965}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"7.479474067687988 m", "val":7.479474067687988}, "y":{"exp":"4.926895618438721 m", "val":4.926895618438721}, "heading":{"exp":"2.0778956890190523 rad", "val":2.0778956890190523}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"6.650923252105713 m", "val":6.650923252105713}, "y":{"exp":"5.398854732513428 m", "val":5.398854732513428}, "heading":{"exp":"2.6918324705623493 rad", "val":2.6918324705623493}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"5.360901355743408 m", "val":5.360901355743408}, "y":{"exp":"5.5561747550964355 m", "val":5.5561747550964355}, "heading":{"exp":"3.0890101362203475 rad", "val":3.0890101362203475}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"3.368184804916382 m", "val":3.368184804916382}, "y":{"exp":"5.5876383781433105 m", "val":5.5876383781433105}, "heading":{"exp":"-1.808632057092331 rad", "val":-1.808632057092331}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"2.780857801437378 m", "val":2.780857801437378}, "y":{"exp":"4.507376194000244 m", "val":4.507376194000244}, "heading":{"exp":"-0.8894979211695171 rad", "val":-0.8894979211695171}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"HubStart.x", "val":3.61569}, "y":{"exp":"HubStart.y", "val":4.03479}, "heading":{"exp":"HubStart.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"3.6253833770751953 m", "val":3.6253833770751953}, "y":{"exp":"2.5349528789520264 m", "val":2.5349528789520264}, "heading":{"exp":"RightBump.heading", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.812593936920166 m", "val":5.812593936920166}, "y":{"exp":"2.3075690269470215 m", "val":2.3075690269470215}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.8845438957214355 m", "val":6.8845438957214355}, "y":{"exp":"1.5387967824935913 m", "val":1.5387967824935913}, "heading":{"exp":"0.5645693936681 rad", "val":0.5645693936681}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.248845100402832 m", "val":8.248845100402832}, "y":{"exp":"0.5642969608306885 m", "val":0.5642969608306885}, "heading":{"exp":"1.4464422073687606 rad", "val":1.4464422073687606}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.357122421264648 m", "val":8.357122421264648}, "y":{"exp":"1.7120414972305298 m", "val":1.7120414972305298}, "heading":{"exp":"1.5444860228722415 rad", "val":1.5444860228722417}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.400433540344238 m", "val":8.400433540344238}, "y":{"exp":"2.7081971168518066 m", "val":2.708197116851806}, "heading":{"exp":"1.4876547290954834 rad", "val":1.4876547290954834}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"8.270500183105469 m", "val":8.270500183105469}, "y":{"exp":"3.661041498184204 m", "val":3.661041498184204}, "heading":{"exp":"2.098871497784393 rad", "val":2.098871497784393}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"7.815732955932617 m", "val":7.815732955932617}, "y":{"exp":"4.9170637130737305 m", "val":4.9170637130737305}, "heading":{"exp":"2.5468338404100317 rad", "val":2.5468338404100317}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.830404758453369 m", "val":6.830404758453369}, "y":{"exp":"4.949546813964844 m", "val":4.949546813964844}, "heading":{"exp":"-2.720173097883985 rad", "val":-2.720173097883985}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.353983402252197 m", "val":6.353983402252197}, "y":{"exp":"3.661041736602783 m", "val":3.661041736602783}, "heading":{"exp":"-2.5599506032522905 rad", "val":-2.5599506032522905}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.077489376068115 m", "val":6.077489376068115}, "y":{"exp":"2.463531970977783 m", "val":2.463531970977783}, "heading":{"exp":"2.7950533983717607 rad", "val":2.7950533983717607}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"4.448294639587402 m", "val":4.448294639587402}, "y":{"exp":"2.5566084384918213 m", "val":2.5566084384918213}, "heading":{"exp":"3.1172072575081238 rad", "val":3.117207257508124}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"2.4559834003448486 m", "val":2.4559834003448486}, "y":{"exp":"2.5349528789520264 m", "val":2.5349528789520264}, "heading":{"exp":"-1.5793481695014076 rad", "val":-1.5793481695014076}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}, + {"from":1, "to":10, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"3 m / s", "val":3.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 @@ -67,219 +73,273 @@ "differentialTrackWidth":0.5588 }, "sampleType":"Swerve", - "waypoints":[0.0,0.92883,1.30612,1.53717,1.77275,2.05295,2.33895,2.62917,3.15856,3.60945,4.17676], + "waypoints":[0.0,0.78828,1.31962,2.07945,2.61312,3.01262,3.3976,3.94142,4.36253,4.92551,5.46324,5.91523,6.62509], "samples":[ - {"t":0.0, "x":3.616, "y":2.511, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.00358, "ay":0.28569, "alpha":-1.28392, "fx":[153.3749,152.97761,152.88308,153.3579], "fy":[-1.91588,11.20304,12.24589,-2.09529]}, - {"t":0.0387, "x":3.62274, "y":2.51121, "heading":0.0, "vx":0.34845, "vy":0.01106, "omega":-0.04969, "ax":9.00326, "ay":0.28565, "alpha":-1.25318, "fx":[153.36427,152.97568,152.88446,153.34691], "fy":[-1.76144,11.05775,12.06155,-1.92227]}, - {"t":0.0774, "x":3.64297, "y":2.51186, "heading":-0.00192, "vx":0.69689, "vy":0.02211, "omega":-0.09819, "ax":9.00286, "ay":0.28562, "alpha":-1.21722, "fx":[153.35146,152.97418,152.88489,153.33395], "fy":[-1.5924,10.87452,11.85821,-1.70731]}, - {"t":0.1161, "x":3.67668, "y":2.51293, "heading":-0.00572, "vx":1.04531, "vy":0.03317, "omega":-0.1453, "ax":9.00238, "ay":0.28557, "alpha":-1.17457, "fx":[153.33589,152.97272,152.88444,153.3183], "fy":[-1.39958,10.64775,11.62578,-1.44404]}, - {"t":0.15481, "x":3.72388, "y":2.51442, "heading":-0.01135, "vx":1.39372, "vy":0.04422, "omega":-0.19076, "ax":9.00176, "ay":0.28551, "alpha":-1.12318, "fx":[153.3167,152.9708,152.88317,153.29892], "fy":[-1.17004,10.36937,11.35019,-1.12341]}, - {"t":0.19351, "x":3.78456, "y":2.51635, "heading":-0.01873, "vx":1.7421, "vy":0.05527, "omega":-0.23422, "ax":9.00097, "ay":0.28544, "alpha":-1.06007, "fx":[153.29254,152.96765,152.88106,153.27427], "fy":[-0.88506,10.02738,11.01124,-0.73221]}, - {"t":0.23221, "x":3.85872, "y":2.5187, "heading":-0.02779, "vx":2.09045, "vy":0.06631, "omega":-0.27525, "ax":8.9999, "ay":0.28536, "alpha":-0.98071, "fx":[153.26125,152.96203,152.87791,153.24198], "fy":[-0.51633,9.60327,10.57885,-0.25056]}, - {"t":0.27091, "x":3.94637, "y":2.52148, "heading":-0.03845, "vx":2.43876, "vy":0.07736, "omega":-0.31321, "ax":8.99842, "ay":0.28524, "alpha":-0.8779, "fx":[153.2191,152.95189,152.87305,153.19819], "fy":[-0.01893,9.06702,10.00605,0.35294]}, - {"t":0.30961, "x":4.04749, "y":2.52469, "heading":-0.05057, "vx":2.78701, "vy":0.0884, "omega":-0.34718, "ax":8.99623, "ay":0.28507, "alpha":-0.73944, "fx":[153.15911,152.93348,152.86467,153.13619], "fy":[0.68337,8.36669,9.21487,1.13074]}, - {"t":0.34831, "x":4.16209, "y":2.52832, "heading":-0.064, "vx":3.13518, "vy":0.09943, "omega":-0.3758, "ax":8.99276, "ay":0.28482, "alpha":-0.54292, "fx":[153.06686,152.8991,152.84784,153.04316], "fy":[1.73162,7.40426,8.06514,2.17759]}, - {"t":0.38701, "x":4.29016, "y":2.53238, "heading":-0.07855, "vx":3.48321, "vy":0.11045, "omega":-0.39681, "ax":8.98659, "ay":0.2844, "alpha":-0.242, "fx":[152.90763,152.8304,152.80808,152.89139], "fy":[3.42061,5.9706,6.27587,3.68349]}, - {"t":0.42572, "x":4.43169, "y":2.53687, "heading":-0.09391, "vx":3.83101, "vy":0.12146, "omega":-0.40618, "ax":8.97358, "ay":0.28362, "alpha":0.27691, "fx":[152.57666,152.67175,152.69508,152.60872], "fy":[6.4889,3.52896,3.18705,6.09208]}, - {"t":0.46442, "x":4.58668, "y":2.54178, "heading":-0.10962, "vx":4.1783, "vy":0.13244, "omega":-0.39546, "ax":8.93571, "ay":0.28158, "alpha":1.38872, "fx":[151.60093,152.16587,152.26896,151.93973], "fy":[13.4569,-1.82167,-3.20212,10.7255]}, - {"t":0.50312, "x":4.75508, "y":2.54712, "heading":-0.12493, "vx":4.52412, "vy":0.14333, "omega":-0.34171, "ax":8.678, "ay":0.26323, "alpha":5.58229, "fx":[144.25436,147.76849,149.23929,149.17921], "fy":[41.47411,-24.59732,-22.88903,23.922]}, - {"t":0.54182, "x":4.93667, "y":2.55287, "heading":-0.13815, "vx":4.85997, "vy":0.15352, "omega":-0.12567, "ax":1.11075, "ay":0.0433, "alpha":3.21478, "fx":[11.67106,9.27831,26.18628,28.43851], "fy":[10.74299,-6.83884,-9.03953,8.08164]}, - {"t":0.58052, "x":5.12559, "y":2.55884, "heading":-0.14302, "vx":4.90296, "vy":0.1552, "omega":-0.00125, "ax":0.00132, "ay":-0.02503, "alpha":0.00115, "fx":[0.0198,0.01892,0.02505,0.02593], "fy":[-0.42217,-0.4283,-0.42918,-0.42305]}, - {"t":0.61922, "x":5.31534, "y":2.56483, "heading":-0.14307, "vx":4.90301, "vy":0.15423, "omega":-0.00121, "ax":0.0037, "ay":-0.11957, "alpha":0.0, "fx":[0.06301,0.06301,0.06301,0.06301], "fy":[-2.0338,-2.0338,-2.0338,-2.0338]}, - {"t":0.65792, "x":5.5051, "y":2.57071, "heading":-0.14311, "vx":4.90315, "vy":0.1496, "omega":-0.00121, "ax":0.01438, "ay":-0.50447, "alpha":0.0, "fx":[0.24457,0.24458,0.24455,0.24455], "fy":[-8.58099,-8.58096,-8.58096,-8.58099]}, - {"t":0.69663, "x":5.69487, "y":2.57612, "heading":-0.14316, "vx":4.90371, "vy":0.13008, "omega":-0.00121, "ax":0.03734, "ay":-2.01834, "alpha":-0.00003, "fx":[0.63522,0.63525,0.63507,0.63505], "fy":[-34.33142,-34.33126,-34.33123,-34.33139]}, - {"t":0.73533, "x":5.88467, "y":2.57964, "heading":-0.14321, "vx":4.90516, "vy":0.05196, "omega":-0.00121, "ax":-0.07528, "ay":-5.47808, "alpha":0.04544, "fx":[-1.42337,-1.46825,-1.13783,-1.09232], "fy":[-93.09222,-93.24444,-93.26887,-93.11667]}, - {"t":0.77403, "x":6.07445, "y":2.57755, "heading":-0.14325, "vx":4.90224, "vy":-0.16005, "omega":0.00055, "ax":-4.94973, "ay":-4.42809, "alpha":15.63595, "fx":[-141.16535,-113.23886,-29.5165,-52.85322], "fy":[1.1791,-90.37273,-138.71017,-73.37812]}, - {"t":0.81273, "x":6.26047, "y":2.56804, "heading":-0.14323, "vx":4.71068, "vy":-0.33142, "omega":0.60568, "ax":-6.99768, "ay":-0.59787, "alpha":19.39038, "fx":[-144.55125,-141.03239,-79.82242,-110.70821], "fy":[46.04517,-56.50611,-127.71714,97.49965]}, - {"t":0.85143, "x":6.43754, "y":2.55477, "heading":-0.11979, "vx":4.43986, "vy":-0.35456, "omega":1.35612, "ax":-8.40423, "ay":-0.75538, "alpha":9.89772, "fx":[-149.62311,-147.05614,-128.94402,-146.19081], "fy":[29.75906,-40.85397,-80.54795,40.24789]}, - {"t":0.89013, "x":6.60308, "y":2.54048, "heading":-0.06731, "vx":4.1146, "vy":-0.38379, "omega":1.73917, "ax":-8.69738, "ay":-0.63659, "alpha":6.95386, "fx":[-151.4108,-148.95655,-141.40138,-149.99135], "fy":[21.09242,-34.50219,-57.31844,27.41517]}, - {"t":0.92883, "x":6.7558, "y":2.52515, "heading":0.0, "vx":3.778, "vy":-0.40843, "omega":2.0083, "ax":-8.79344, "ay":0.09177, "alpha":5.27986, "fx":[-150.47038,-150.86173,-148.86579,-148.09799], "fy":[23.94331,-21.31702,-30.06823,33.68588]}, - {"t":0.9468, "x":6.82226, "y":2.51782, "heading":0.03608, "vx":3.62002, "vy":-0.40678, "omega":2.10315, "ax":-8.75696, "ay":1.25274, "alpha":4.44391, "fx":[-147.89975,-152.43375,-152.04849,-143.43149], "fy":[37.13088,-1.97635,-0.69394,50.77462]}, - {"t":0.96477, "x":6.88588, "y":2.51072, "heading":0.07387, "vx":3.46269, "vy":-0.38427, "omega":2.18299, "ax":-8.61819, "ay":2.16147, "alpha":3.73991, "fx":[-145.01338,-151.83172,-150.56353,-138.96291], "fy":[47.50517,14.62169,22.46575,62.47123]}, - {"t":0.98273, "x":6.9467, "y":2.50416, "heading":0.11309, "vx":3.30786, "vy":-0.34544, "omega":2.25018, "ax":-8.43917, "ay":2.86809, "alpha":3.17758, "fx":[-142.13274,-149.91988,-147.04706,-135.09196], "fy":[55.77995,28.56796,40.00254,70.79143]}, - {"t":1.0007, "x":7.00477, "y":2.49842, "heading":0.15351, "vx":3.15624, "vy":-0.29391, "omega":2.30727, "ax":-8.25257, "ay":3.42146, "alpha":2.72771, "fx":[-139.39787,-147.31249,-142.95071,-131.83406], "fy":[62.48696,40.18039,53.19364,76.93114]}, - {"t":1.01866, "x":7.06014, "y":2.49369, "heading":0.19496, "vx":3.00798, "vy":-0.23244, "omega":2.35628, "ax":-8.07342, "ay":3.86056, "alpha":2.36149, "fx":[-136.85964,-144.40463,-138.93669,-129.10562], "fy":[68.01199,49.83681,63.20982,81.60932]}, - {"t":1.03663, "x":7.11288, "y":2.49014, "heading":0.2373, "vx":2.86293, "vy":-0.16308, "omega":2.3987, "ax":-7.90773, "ay":4.21428, "alpha":2.05775, "fx":[-134.52691,-141.43358,-135.26133,-126.811], "fy":[72.63395,57.89303,70.93508,85.27263]}, - {"t":1.0546, "x":7.16304, "y":2.48789, "heading":0.28039, "vx":2.72086, "vy":-0.08737, "omega":2.43567, "ax":-7.75711, "ay":4.50351, "alpha":1.8018, "fx":[-132.38982,-138.53363,-131.99535,-124.86611], "fy":[76.55588,64.65309,76.99615,88.20883]}, - {"t":1.07256, "x":7.21067, "y":2.48705, "heading":0.32415, "vx":2.5815, "vy":-0.00646, "omega":2.46804, "ax":-7.62124, "ay":4.74336, "alpha":1.58329, "fx":[-130.43121,-135.77549,-129.13138,-123.20249], "fy":[79.92724,70.36499,81.8305,90.60986]}, - {"t":1.09053, "x":7.25582, "y":2.4877, "heading":0.36849, "vx":2.44458, "vy":0.07876, "omega":2.49649, "ax":-7.49899, "ay":4.9448, "alpha":1.39474, "fx":[-128.63206,-133.19211,-126.63288,-121.76586], "fy":[82.85947,75.22664,85.74486,92.60783]}, - {"t":1.10849, "x":7.29853, "y":2.48991, "heading":0.41334, "vx":2.30985, "vy":0.16759, "omega":2.52155, "ax":-7.38898, "ay":5.11597, "alpha":1.23057, "fx":[-126.97395,-130.79457,-124.45555,-120.51343], "fy":[85.4368,79.39456,88.95706,94.29623]}, - {"t":1.12646, "x":7.33883, "y":2.49375, "heading":0.45864, "vx":2.1771, "vy":0.25951, "omega":2.54365, "ax":-7.2898, "ay":5.26292, "alpha":1.08653, "fx":[-125.44012,-128.5818,-122.55613,-119.41141], "fy":[87.72368,82.99232,91.6244,95.7429]}, - {"t":1.14442, "x":7.37677, "y":2.49926, "heading":0.50434, "vx":2.04614, "vy":0.35406, "omega":2.56317, "ax":-7.20016, "ay":5.39028, "alpha":0.95928, "fx":[-124.01579,-126.54622,-120.89565,-118.43292], "fy":[89.77006,86.11777,93.86239,96.9981]}, - {"t":1.16239, "x":7.41237, "y":2.50649, "heading":0.55039, "vx":1.91678, "vy":0.4509, "omega":2.58041, "ax":-7.1189, "ay":5.50158, "alpha":0.84617, "fx":[-122.68814,-124.67716,-119.44025,-117.55649], "fy":[91.61507,88.84878,95.75732,98.09984]}, - {"t":1.18036, "x":7.44566, "y":2.51548, "heading":0.59675, "vx":1.78888, "vy":0.54974, "omega":2.59561, "ax":-7.04501, "ay":5.59959, "alpha":0.74507, "fx":[-121.44618,-122.9627,-118.16097,-116.76475], "fy":[93.28972,91.24774,97.37472,99.07728]}, - {"t":1.19832, "x":7.47666, "y":2.52626, "heading":0.64338, "vx":1.66231, "vy":0.65034, "omega":2.609, "ax":-6.9776, "ay":5.68649, "alpha":0.65425, "fx":[-120.28052,-121.3908,-117.03325,-116.04362], "fy":[94.81878,93.36498,98.7651,99.95318]}, - {"t":1.21629, "x":7.5054, "y":2.53886, "heading":0.69026, "vx":1.53695, "vy":0.75251, "omega":2.62075, "ax":-6.91591, "ay":5.76402, "alpha":0.57228, "fx":[-119.18319,-119.94985,-116.03627,-115.38153], "fy":[96.22224,95.24145,99.96793,100.74551]}, - {"t":1.23425, "x":7.53189, "y":2.55331, "heading":0.73734, "vx":1.4127, "vy":0.85606, "omega":2.63103, "ax":-6.85928, "ay":5.83358, "alpha":0.49797, "fx":[-118.14743,-118.62897,-115.15237,-114.76897], "fy":[97.51638,96.9107,101.01452,101.46861]}, - {"t":1.25222, "x":7.55617, "y":2.56963, "heading":0.78461, "vx":1.28947, "vy":0.96087, "omega":2.63998, "ax":-6.80714, "ay":5.89632, "alpha":0.43033, "fx":[-117.16748,-117.4181,-114.36653,-114.19803], "fy":[98.71451,98.40044,101.92994,102.13405]}, - {"t":1.27019, "x":7.57823, "y":2.58784, "heading":0.83204, "vx":1.16717, "vy":1.0668, "omega":2.64771, "ax":-6.759, "ay":5.95317, "alpha":0.36854, "fx":[-116.23847,-116.30804,-113.66592,-113.66213], "fy":[99.82764,99.73371,102.73454,102.75124]}, - {"t":1.28815, "x":7.59811, "y":2.60797, "heading":0.87961, "vx":1.04574, "vy":1.17375, "omega":2.65433, "ax":-6.71443, "ay":6.00492, "alpha":0.31188, "fx":[-115.35626,-115.29042,-113.03951,-113.15574], "fy":[100.86492,100.92984,103.44499,103.3279]}, - {"t":1.30612, "x":7.61582, "y":2.63003, "heading":0.9273, "vx":0.92511, "vy":1.28164, "omega":2.65994, "ax":-6.62934, "ay":6.09265, "alpha":0.04457, "fx":[-112.94005,-112.90754,-112.58566,-112.61926], "fy":[103.4424,103.47609,103.82639,103.79173]}, - {"t":1.32056, "x":7.62849, "y":2.64917, "heading":0.96571, "vx":0.82938, "vy":1.36962, "omega":2.66058, "ax":-6.49368, "ay":6.23461, "alpha":-0.23842, "fx":[-109.43186,-109.7348,-111.46324,-111.19299], "fy":[107.10864,106.80797,105.00137,105.27757]}, - {"t":1.335, "x":7.63979, "y":2.6696, "heading":1.00413, "vx":0.73561, "vy":1.45965, "omega":2.65714, "ax":-6.33649, "ay":6.39018, "alpha":-0.55725, "fx":[-105.19765,-106.23799,-110.27456,-109.41754], "fy":[111.22937,110.25904,106.21673,107.07516]}, - {"t":1.34944, "x":7.64975, "y":2.69134, "heading":1.0425, "vx":0.6441, "vy":1.55193, "omega":2.64909, "ax":-6.15289, "ay":6.56076, "alpha":-0.9185, "fx":[-100.06872,-102.38295,-108.99891,-107.18507], "fy":[115.82204,113.81922,107.48918,109.25646]}, - {"t":1.36388, "x":7.65841, "y":2.71444, "heading":1.08075, "vx":0.55525, "vy":1.64667, "omega":2.63582, "ax":-5.93664, "ay":6.7477, "alpha":-1.32978, "fx":[-93.83901,-98.13114,-107.60882,-104.34355], "fy":[120.87898,117.47542,108.83994,111.91157]}, - {"t":1.37832, "x":7.66581, "y":2.73892, "heading":1.11882, "vx":0.46952, "vy":1.74411, "omega":2.61662, "ax":-5.67975, "ay":6.95203, "alpha":-1.79941, "fx":[-86.26406,-93.43883,-106.06747,-100.67348], "fy":[126.34886,121.21082,110.29606,115.15223]}, - {"t":1.39276, "x":7.67199, "y":2.76483, "heading":1.1566, "vx":0.3875, "vy":1.8445, "omega":2.59064, "ax":-5.37192, "ay":7.17413, "alpha":-2.3358, "fx":[-77.06871,-88.25621,-104.32483,-95.84968], "fy":[132.11036,125.00381,111.89235,119.11295]}, - {"t":1.4072, "x":7.67703, "y":2.79222, "heading":1.19401, "vx":0.30993, "vy":1.9481, "omega":2.55691, "ax":-4.99991, "ay":7.41315, "alpha":-2.94693, "fx":[-65.97161,-82.52652,-102.31162,-89.37833], "fy":[137.93896,128.82645,113.67405,123.94294]}, - {"t":1.42164, "x":7.68098, "y":2.82112, "heading":1.23094, "vx":0.23773, "vy":2.05515, "omega":2.51435, "ax":-4.54662, "ay":7.66603, "alpha":-3.64106, "fx":[-52.73733,-76.18525,-99.92973,-80.49435], "fy":[143.47424,132.6423,115.70063,129.77041]}, - {"t":1.43608, "x":7.68394, "y":2.8516, "heading":1.26724, "vx":0.17207, "vy":2.16586, "omega":2.46177, "ax":-3.98987, "ay":7.92562, "alpha":-4.43275, "fx":[-37.26339,-69.15982,-97.03613,-68.00665], "fy":[148.20592,136.40348,118.0512,136.58973]}, - {"t":1.45052, "x":7.68601, "y":2.8837, "heading":1.30279, "vx":0.11446, "vy":2.28031, "omega":2.39776, "ax":-3.30113, "ay":8.17682, "alpha":-5.36138, "fx":[-19.69331,-61.36994,-93.41504,-50.12675], "fy":[151.50925,140.04674,120.83205,143.95329]}, - {"t":1.46496, "x":7.68732, "y":2.91748, "heading":1.33742, "vx":0.06679, "vy":2.39839, "omega":2.32034, "ax":-2.44742, "ay":8.38781, "alpha":-6.52649, "fx":[-0.51227,-52.72937,-88.72772,-24.55025], "fy":[152.75774,143.48848,124.18588,150.26458]}, - {"t":1.47941, "x":7.68803, "y":2.95299, "heading":1.37093, "vx":0.03144, "vy":2.51951, "omega":2.22609, "ax":-1.40848, "ay":8.49752, "alpha":-8.09923, "fx":[19.44957,-43.14998,-82.41882,10.28797], "fy":[151.50292,146.61885,128.29831,151.74177]}, - {"t":1.49385, "x":7.68834, "y":2.99026, "heading":1.40307, "vx":0.0111, "vy":2.64222, "omega":2.10913, "ax":-0.22347, "ay":8.42523, "alpha":-10.15048, "fx":[39.16192,-32.5505,-73.537,51.72062], "fy":[147.64262,149.29509,133.38128,142.92408]}, - {"t":1.50829, "x":7.68847, "y":3.02929, "heading":1.43353, "vx":0.00788, "vy":2.76389, "omega":1.96255, "ax":0.98765, "ay":8.14668, "alpha":-12.25669, "fx":[57.6465,-20.87385,-60.38245,90.80837], "fy":[141.46934,151.33586,139.56026,121.92514]}, - {"t":1.52273, "x":7.68869, "y":3.07006, "heading":1.46187, "vx":0.02214, "vy":2.88153, "omega":1.78556, "ax":2.13766, "ay":7.74647, "alpha":-13.55162, "fx":[74.20081,-8.11483,-39.84116,119.19882], "fy":[133.56265,152.52106,146.39601,94.58118]}, - {"t":1.53717, "x":7.68923, "y":3.11247, "heading":1.48765, "vx":0.05301, "vy":2.99339, "omega":1.58986, "ax":2.79732, "ay":7.67491, "alpha":-12.31979, "fx":[79.52028,1.1017,-14.33867,124.04302], "fy":[130.43834,152.6734,150.85072,88.22968]}, - {"t":1.55189, "x":7.69032, "y":3.15738, "heading":1.51106, "vx":0.0942, "vy":3.1064, "omega":1.40847, "ax":2.75102, "ay":7.7102, "alpha":-12.09465, "fx":[77.70072,-0.35689,-12.9361,122.76866], "fy":[131.44144,152.58601,150.76482,89.80077]}, - {"t":1.56662, "x":7.692, "y":3.20396, "heading":1.5318, "vx":0.1347, "vy":3.21993, "omega":1.23038, "ax":2.69585, "ay":7.74708, "alpha":-11.87041, "fx":[75.83438,-1.76061,-11.89361,121.24241], "fy":[132.41869,152.46641,150.60422,91.61322]}, - {"t":1.58134, "x":7.69428, "y":3.25221, "heading":1.54992, "vx":0.1744, "vy":3.334, "omega":1.0556, "ax":2.62751, "ay":7.787, "alpha":-11.64172, "fx":[73.85674,-3.14363,-11.30297,119.36235], "fy":[133.39704,152.30859,150.35926,93.75349]}, - {"t":1.59606, "x":7.69713, "y":3.30214, "heading":1.56546, "vx":0.21308, "vy":3.44865, "omega":0.88419, "ax":2.53968, "ay":7.83212, "alpha":-11.39783, "fx":[71.66529,-4.55534,-11.27104,116.95779], "fy":[134.41469,152.1011,150.01232,96.3605]}, - {"t":1.61079, "x":7.70054, "y":3.35377, "heading":1.57848, "vx":0.25048, "vy":3.56397, "omega":0.71637, "ax":2.4223, "ay":7.88585, "alpha":-11.11848, "fx":[69.08762,-6.07273,-11.93289,113.72866], "fy":[135.52934,151.82278,149.5305,99.66132]}, - {"t":1.62551, "x":7.70449, "y":3.4071, "heading":1.58903, "vx":0.28614, "vy":3.68008, "omega":0.55266, "ax":2.25776, "ay":7.95369, "alpha":-10.7644, "fx":[65.81048,-7.82675,-13.47639,109.10778], "fy":[136.83404,151.43343,148.85219,104.04035]}, - {"t":1.64024, "x":7.70895, "y":3.46215, "heading":1.59717, "vx":0.31939, "vy":3.7972, "omega":0.39416, "ax":2.01141, "ay":8.04501, "alpha":-10.25324, "fx":[61.20121,-10.06475,-16.19043,101.90775], "fy":[138.48879,150.85,147.85929,110.17507]}, - {"t":1.65496, "x":7.71387, "y":3.51893, "heading":1.60297, "vx":0.349, "vy":3.91565, "omega":0.24319, "ax":1.60424, "ay":8.17519, "alpha":-9.39029, "fx":[53.77175,-13.32411,-20.56746,89.27069], "fy":[140.77876,149.87485,146.31192,119.26488]}, - {"t":1.66969, "x":7.71919, "y":3.57747, "heading":1.60655, "vx":0.37263, "vy":4.03602, "omega":0.10493, "ax":0.81588, "ay":8.3556, "alpha":-7.64262, "fx":[39.08859,-19.04031,-27.54374,63.0069], "fy":[144.10172,147.91137,143.67025,132.82246]}, - {"t":1.68441, "x":7.72476, "y":3.6378, "heading":1.6081, "vx":0.38464, "vy":4.15905, "omega":-0.0076, "ax":-1.11246, "ay":8.37706, "alpha":-3.4299, "fx":[-2.36019,-32.69692,-39.12254,-1.51109], "fy":[145.68158,142.12208,138.50817,143.65351]}, - {"t":1.69913, "x":7.7303, "y":3.69995, "heading":1.60798, "vx":0.36826, "vy":4.2824, "omega":-0.05811, "ax":-5.77768, "ay":5.24132, "alpha":7.84513, "fx":[-131.22634,-96.40079,-60.05327,-105.42634], "fy":[48.05727,86.16143,126.07443,96.32062]}, - {"t":1.71386, "x":7.7351, "y":3.76357, "heading":1.60713, "vx":0.28319, "vy":4.35957, "omega":0.05741, "ax":-6.17095, "ay":-1.40746, "alpha":20.56709, "fx":[-126.12972,-50.38445,-99.397,-143.95296], "fy":[-75.46229,-131.96852,85.89374,25.77511]}, - {"t":1.72858, "x":7.7386, "y":3.82761, "heading":1.60797, "vx":0.19233, "vy":4.33885, "omega":0.36024, "ax":-5.95841, "ay":-4.18074, "alpha":15.11853, "fx":[-109.82436,-23.21263,-124.13217,-148.23455], "fy":[-101.80907,-146.04661,-26.64397,-9.95315]}, - {"t":1.74331, "x":7.74079, "y":3.89104, "heading":1.61328, "vx":0.10459, "vy":4.27729, "omega":0.58285, "ax":-5.18339, "ay":-5.75757, "alpha":12.79776, "fx":[-101.65756,-14.3955,-89.44463,-147.17407], "fy":[-111.53777,-149.1939,-102.74244,-28.26422]}, - {"t":1.75803, "x":7.74176, "y":3.9534, "heading":1.62186, "vx":0.02827, "vy":4.19251, "omega":0.77128, "ax":-4.67474, "ay":-6.35097, "alpha":12.45284, "fx":[-96.72706,-9.85526,-66.00541,-145.47609], "fy":[-116.64917,-150.51912,-125.79368,-39.15051]}, - {"t":1.77275, "x":7.74167, "y":4.01444, "heading":1.63322, "vx":-0.04056, "vy":4.099, "omega":0.95464, "ax":-5.20649, "ay":-6.4867, "alpha":9.73025, "fx":[-97.30413,-29.52985,-87.37173,-140.0376], "fy":[-116.56444,-148.37,-119.39713,-57.01584]}, - {"t":1.7875, "x":7.74051, "y":4.07418, "heading":1.64729, "vx":-0.11734, "vy":4.00334, "omega":1.09813, "ax":-5.76964, "ay":-6.325, "alpha":7.76734, "fx":[-101.41032,-50.90964,-103.32251,-136.91727], "fy":[-113.36033,-142.841,-108.86969,-65.27509]}, - {"t":1.80225, "x":7.73815, "y":4.13254, "heading":1.66349, "vx":-0.20243, "vy":3.91006, "omega":1.21268, "ax":-6.09147, "ay":-6.20536, "alpha":6.42684, "fx":[-104.29818,-65.56173,-110.01742,-134.57899], "fy":[-110.97334,-137.05038,-103.51957,-70.66238]}, - {"t":1.817, "x":7.7345, "y":4.18952, "heading":1.68137, "vx":-0.29226, "vy":3.81855, "omega":1.30746, "ax":-6.3024, "ay":-6.11052, "alpha":5.4408, "fx":[-106.43145,-75.92192,-113.67041,-132.78454], "fy":[-109.13522,-131.84662,-100.32393,-74.44714]}, - {"t":1.83174, "x":7.72951, "y":4.24517, "heading":1.70065, "vx":-0.3852, "vy":3.72844, "omega":1.3877, "ax":-6.45094, "ay":-6.03359, "alpha":4.68832, "fx":[-108.06864,-83.51129,-115.96479,-131.36959], "fy":[-107.67951,-127.38003,-98.20645,-77.25264]}, - {"t":1.84649, "x":7.72313, "y":4.2995, "heading":1.72112, "vx":-0.48034, "vy":3.63946, "omega":1.45684, "ax":-6.5607, "ay":-5.97029, "alpha":4.09778, "fx":[-109.36422,-89.25613,-117.53611,-130.22614], "fy":[-106.4987,-123.5909,-96.70424,-79.41783]}, - {"t":1.86124, "x":7.71533, "y":4.35252, "heading":1.7426, "vx":-0.57709, "vy":3.55141, "omega":1.51727, "ax":-6.64481, "ay":-5.91754, "alpha":3.62344, "fx":[-110.41574,-93.73062,-118.67637,-129.2824], "fy":[-105.52091,-120.37295,-95.58701,-81.14197]}, - {"t":1.87598, "x":7.7061, "y":4.40425, "heading":1.76498, "vx":-0.67508, "vy":3.46415, "omega":1.5707, "ax":-6.71112, "ay":-5.87307, "alpha":3.2349, "fx":[-111.28756,-97.30161,-119.53843,-128.48937], "fy":[-104.69651,-117.62343,-94.72744,-82.54954]}, - {"t":1.89073, "x":7.69541, "y":4.4547, "heading":1.78814, "vx":-0.77405, "vy":3.37753, "omega":1.61841, "ax":-6.76463, "ay":-5.83516, "alpha":2.91126, "fx":[-112.02372,-100.21116,-120.21002,-127.81274], "fy":[-103.99028,-115.25562,-94.04938,-83.7222]}, - {"t":1.90548, "x":7.68326, "y":4.50388, "heading":1.81201, "vx":-0.87381, "vy":3.29148, "omega":1.66134, "ax":-6.80864, "ay":-5.80251, "alpha":2.63779, "fx":[-112.65531,-102.62401,-120.7452,-127.22786], "fy":[-103.37665,-113.19967,-93.50443,-84.71565]}, - {"t":1.92023, "x":7.66963, "y":4.55179, "heading":1.83651, "vx":-0.97422, "vy":3.20591, "omega":1.70024, "ax":-6.84544, "ay":-5.77415, "alpha":2.40384, "fx":[-113.20485,-104.65546,-121.17907,-126.7166], "fy":[-102.83664,-111.40019,-93.0603,-85.56915]}, - {"t":1.93497, "x":7.65452, "y":4.59844, "heading":1.86158, "vx":-1.07517, "vy":3.12076, "omega":1.73569, "ax":-6.87663, "ay":-5.74929, "alpha":2.20154, "fx":[-113.68901,-106.38824,-121.53549,-126.26539], "fy":[-102.35591,-109.81332,-92.69458,-86.31118]}, - {"t":1.94972, "x":7.63792, "y":4.64383, "heading":1.88718, "vx":-1.17659, "vy":3.03597, "omega":1.76816, "ax":-6.90339, "ay":-5.72734, "alpha":2.02494, "fx":[-114.12036,-107.88305,-121.83126,-125.86387], "fy":[-101.92346,-108.40427,-92.39118,-86.96285]}, - {"t":1.96447, "x":7.61982, "y":4.68798, "heading":1.91326, "vx":-1.27839, "vy":2.95151, "omega":1.79802, "ax":-6.92658, "ay":-5.70783, "alpha":1.8695, "fx":[-114.50856,-109.18534,-122.07854,-125.50398], "fy":[-101.53071,-107.14521,-92.13825,-87.54013]}, - {"t":1.97922, "x":7.60021, "y":4.73089, "heading":1.93977, "vx":-1.38054, "vy":2.86733, "omega":1.82559, "ax":-6.94686, "ay":-5.69038, "alpha":1.73168, "fx":[-114.86112,-110.32973,-122.28641,-125.1794], "fy":[-101.17093,-106.01374,-91.92683,-88.05535]}, - {"t":1.99396, "x":7.5791, "y":4.77256, "heading":1.96669, "vx":-1.48299, "vy":2.78342, "omega":1.85113, "ax":-6.96475, "ay":-5.67468, "alpha":1.60868, "fx":[-115.18398,-111.34304,-122.46175,-124.88512], "fy":[-100.83871,-104.99167,-91.75002,-88.51814]}, - {"t":2.00871, "x":7.55647, "y":4.81299, "heading":1.99399, "vx":-1.5857, "vy":2.69973, "omega":1.87485, "ax":-6.98065, "ay":-5.66048, "alpha":1.49827, "fx":[-115.48186,-112.24631,-122.60993,-124.61706], "fy":[-100.52975,-104.06413,-91.60237,-88.93613]}, - {"t":2.02346, "x":7.53232, "y":4.85219, "heading":2.02164, "vx":-1.68864, "vy":2.61625, "omega":1.89695, "ax":-6.99486, "ay":-5.64757, "alpha":1.39862, "fx":[-115.75855,-113.0563,-122.73518,-124.37193], "fy":[-100.24053,-103.21885,-91.47956,-89.31546]}, - {"t":2.03821, "x":7.50666, "y":4.89015, "heading":2.04962, "vx":-1.7918, "vy":2.53297, "omega":1.91757, "ax":-7.00764, "ay":-5.63579, "alpha":1.30828, "fx":[-116.01717,-113.78647,-122.84088,-124.14703], "fy":[-99.96817,-102.44562,-91.37804,-89.66112]}, - {"t":2.05295, "x":7.47947, "y":4.9269, "heading":2.0779, "vx":-1.89514, "vy":2.44985, "omega":1.93687, "ax":-7.01288, "ay":-5.62896, "alpha":1.26484, "fx":[-116.0631,-114.1698,-122.93792,-123.97734], "fy":[-99.89313,-101.99651,-91.2253,-89.87308]}, - {"t":2.06725, "x":7.45166, "y":4.96135, "heading":2.10559, "vx":-1.99543, "vy":2.36936, "omega":1.95495, "ax":-7.01244, "ay":-5.62693, "alpha":1.28413, "fx":[-115.87337,-114.1843,-123.11938,-123.94124], "fy":[-100.07973,-101.94323,-90.94003,-89.8867]}, - {"t":2.08155, "x":7.4224, "y":4.99466, "heading":2.13355, "vx":-2.09571, "vy":2.28889, "omega":1.97332, "ax":-7.01195, "ay":-5.62466, "alpha":1.30559, "fx":[-115.67437,-114.19783,-123.30832,-123.90474], "fy":[-100.27262,-101.88682,-90.63882,-89.89689]}, - {"t":2.09585, "x":7.39172, "y":5.02682, "heading":2.16177, "vx":-2.19598, "vy":2.20846, "omega":1.99199, "ax":-7.01142, "ay":-5.6221, "alpha":1.32962, "fx":[-115.46448,-114.20961,-123.50627,-123.86835], "fy":[-100.47291,-101.82739,-90.31859,-89.90226]}, - {"t":2.11015, "x":7.3596, "y":5.05782, "heading":2.19025, "vx":-2.29624, "vy":2.12806, "omega":2.011, "ax":-7.01082, "ay":-5.61919, "alpha":1.35673, "fx":[-115.24166,-114.21863,-123.71517,-123.83274], "fy":[-100.68202,-101.76508,-89.9754,-89.90104]}, - {"t":2.12445, "x":7.32605, "y":5.08768, "heading":2.21901, "vx":-2.3965, "vy":2.04771, "omega":2.0304, "ax":-7.01016, "ay":-5.61587, "alpha":1.38755, "fx":[-115.00327,-114.22363,-123.93752,-123.79874], "fy":[-100.90179,-101.70013,-89.60418,-89.89102]}, - {"t":2.13875, "x":7.29106, "y":5.11639, "heading":2.24805, "vx":-2.49674, "vy":1.9674, "omega":2.05025, "ax":-7.00942, "ay":-5.61201, "alpha":1.42294, "fx":[-114.7459,-114.22297,-124.17659,-123.76743], "fy":[-101.13464,-101.63279,-89.19826,-89.86932]}, - {"t":2.15305, "x":7.25464, "y":5.14395, "heading":2.27736, "vx":-2.59698, "vy":1.88715, "omega":2.07059, "ax":-7.00859, "ay":-5.6075, "alpha":1.46399, "fx":[-114.46502,-114.21446,-124.43672,-123.74023], "fy":[-101.38373,-101.56344,-88.74879,-89.83219]}, - {"t":2.16735, "x":7.21678, "y":5.17036, "heading":2.30697, "vx":-2.6972, "vy":1.80696, "omega":2.09153, "ax":-7.00765, "ay":-5.60215, "alpha":1.51218, "fx":[-114.15454,-114.1952,-124.72373,-123.71907], "fy":[-101.65333,-101.49257,-88.24372,-89.77459]}, - {"t":2.18165, "x":7.1775, "y":5.19563, "heading":2.33688, "vx":-2.79741, "vy":1.72685, "omega":2.11315, "ax":-7.00658, "ay":-5.59572, "alpha":1.56956, "fx":[-113.8061,-114.16118,-125.04568,-123.70655], "fy":[-101.94933,-101.42084,-87.66633,-89.68969]}, - {"t":2.19595, "x":7.13678, "y":5.21975, "heading":2.3671, "vx":-2.89761, "vy":1.64683, "omega":2.1356, "ax":-7.00533, "ay":-5.58783, "alpha":1.63899, "fx":[-113.40787,-114.10675,-125.41388,-123.70633], "fy":[-102.27998,-101.34914,-86.99276,-89.5679]}, - {"t":2.21025, "x":7.09463, "y":5.24273, "heading":2.39764, "vx":-2.99778, "vy":1.56692, "omega":2.15904, "ax":-7.00386, "ay":-5.57798, "alpha":1.72469, "fx":[-112.94268,-114.02368,-125.84479,-123.72371], "fy":[-102.65721,-101.27875,-86.18784,-89.39539]}, - {"t":2.22455, "x":7.05104, "y":5.26457, "heading":2.42852, "vx":-3.09794, "vy":1.48716, "omega":2.1837, "ax":-7.00208, "ay":-5.56534, "alpha":1.83304, "fx":[-112.38461,-113.89947,-126.36304,-123.76657], "fy":[-103.09888,-101.21151,-85.19765,-89.15148]}, - {"t":2.23885, "x":7.00602, "y":5.28526, "heading":2.45974, "vx":-3.19807, "vy":1.40757, "omega":2.20991, "ax":-6.99984, "ay":-5.54863, "alpha":1.97418, "fx":[-111.6927,-113.71413,-127.00723,-123.84722], "fy":[-103.63293,-101.15019,-83.93539,-88.80382]}, - {"t":2.25315, "x":6.95958, "y":5.30482, "heading":2.49134, "vx":-3.29817, "vy":1.32823, "omega":2.23814, "ax":-6.99687, "ay":-5.5256, "alpha":2.16529, "fx":[-110.7984,-113.43359,-127.8409,-123.98585], "fy":[-104.30537,-101.09918,-82.25233,-88.29895]}, - {"t":2.26745, "x":6.9117, "y":5.32325, "heading":2.52335, "vx":-3.39823, "vy":1.24921, "omega":2.26911, "ax":-6.99256, "ay":-5.49212, "alpha":2.43775, "fx":[-109.57785,-112.9944,-128.97559,-124.21786], "fy":[-105.19751,-101.06603,-79.87191,-87.54192]}, - {"t":2.28175, "x":6.86239, "y":5.34056, "heading":2.5558, "vx":-3.49822, "vy":1.17067, "omega":2.30397, "ax":-6.98543, "ay":-5.43951, "alpha":2.85556, "fx":[-107.78302,-112.26303,-130.62359,-124.61106], "fy":[-106.46697,-101.06492,-76.21875,-86.34716]}, - {"t":2.29605, "x":6.81165, "y":5.35674, "heading":2.58875, "vx":-3.59811, "vy":1.09289, "omega":2.3448, "ax":-6.97071, "ay":-5.3465, "alpha":3.57129, "fx":[-104.83705,-110.90048,-133.22949,-125.31191], "fy":[-108.4617,-101.12653,-69.88249,-84.29902]}, - {"t":2.31035, "x":6.75948, "y":5.37182, "heading":2.62228, "vx":-3.69779, "vy":1.01643, "omega":2.39587, "ax":-6.92582, "ay":-5.1451, "alpha":5.05374, "fx":[-99.03835,-107.71144,-137.77762,-126.69757], "fy":[-112.11403,-101.33136,-56.37296,-80.2484]}, - {"t":2.32465, "x":6.70589, "y":5.38583, "heading":2.65654, "vx":-3.79683, "vy":0.94286, "omega":2.46814, "ax":-6.60931, "ay":-4.48625, "alpha":9.66355, "fx":[-82.54857,-93.14672,-143.95858,-130.03595], "fy":[-120.78774,-101.76633,-13.11046,-69.5751]}, - {"t":2.33895, "x":6.65092, "y":5.39885, "heading":2.69183, "vx":-3.89135, "vy":0.8787, "omega":2.60633, "ax":-5.62856, "ay":-4.44324, "alpha":-8.57316, "fx":[-124.47815,-114.44359,-71.24596,-72.79327], "fy":[-28.33313,-72.53232,-111.91802,-89.52955]}, - {"t":2.35277, "x":6.59661, "y":5.41057, "heading":2.72785, "vx":-3.96913, "vy":0.8173, "omega":2.48785, "ax":-5.29437, "ay":-4.11584, "alpha":-10.88335, "fx":[-124.49155,-114.416,-61.86977,-59.44527], "fy":[-11.70116,-70.15668,-114.83422,-83.34508]}, - {"t":2.36659, "x":6.54125, "y":5.42148, "heading":2.76223, "vx":-4.0423, "vy":0.76042, "omega":2.33745, "ax":-5.22855, "ay":-3.76799, "alpha":-11.33531, "fx":[-122.81843,-113.71159,-59.93731,-59.27701], "fy":[-4.88209,-67.57798,-112.44318,-71.4666]}, - {"t":2.38041, "x":6.48489, "y":5.43162, "heading":2.79454, "vx":-4.11456, "vy":0.70834, "omega":2.1808, "ax":-5.12345, "ay":-3.45988, "alpha":-11.6281, "fx":[-120.35623,-112.1695,-57.79742,-58.27062], "fy":[-0.59672,-65.58885,-109.5881,-59.63233]}, - {"t":2.39423, "x":6.42754, "y":5.44108, "heading":2.82467, "vx":-4.18536, "vy":0.66053, "omega":2.0201, "ax":-4.97568, "ay":-3.19591, "alpha":-11.77006, "fx":[-117.16794,-109.7873,-55.39104,-56.19297], "fy":[1.64499,-63.99721,-106.17582,-48.91832]}, - {"t":2.40805, "x":6.36922, "y":5.44991, "heading":2.85259, "vx":-4.25412, "vy":0.61636, "omega":1.85744, "ax":-4.78211, "ay":-2.97448, "alpha":-11.74621, "fx":[-113.15785,-106.51505,-52.65014,-53.04641], "fy":[2.30037,-62.58717,-102.07887,-40.01447]}, - {"t":2.42187, "x":6.30997, "y":5.45814, "heading":2.87826, "vx":-4.32021, "vy":0.57526, "omega":1.69511, "ax":-4.53865, "ay":-2.78535, "alpha":-11.53573, "fx":[-108.11683,-102.25015,-49.48959,-48.94774], "fy":[1.78592,-61.11281,-97.12942,-33.05561]}, - {"t":2.43569, "x":6.24984, "y":5.46582, "heading":2.90169, "vx":-4.38293, "vy":0.53676, "omega":1.53569, "ax":-4.24029, "ay":-2.61065, "alpha":-11.11306, "fx":[-101.75424,-96.83237,-45.83757,-44.08035], "fy":[0.47858,-59.27441,-91.09091,-27.73913]}, - {"t":2.44951, "x":6.18886, "y":5.47299, "heading":2.92291, "vx":-4.44153, "vy":0.50069, "omega":1.38211, "ax":-3.88087, "ay":-2.4268, "alpha":-10.44815, "fx":[-93.7278,-90.03767,-41.63493,-38.64988], "fy":[-1.23817,-56.69112,-83.65368,-23.53401]}, - {"t":2.46333, "x":6.12711, "y":5.47968, "heading":2.94201, "vx":-4.49517, "vy":0.46715, "omega":1.23772, "ax":-3.4534, "ay":-2.20565, "alpha":-9.509, "fx":[-83.70078,-81.57987,-36.82537,-32.85941], "fy":[-2.91113,-52.87107,-74.45361,-19.83393]}, - {"t":2.47715, "x":6.06466, "y":5.48593, "heading":2.95911, "vx":-4.54289, "vy":0.43667, "omega":1.10631, "ax":-2.95219, "ay":-1.91526, "alpha":-8.26985, "fx":[-71.46421,-71.14149,-31.36105,-26.89669], "fy":[-3.96352,-47.18912,-63.12162,-16.03759]}, - {"t":2.49097, "x":6.00159, "y":5.49178, "heading":2.9744, "vx":-4.58369, "vy":0.4102, "omega":0.99202, "ax":-2.37779, "ay":-1.52202, "alpha":-6.72806, "fx":[-57.13959,-58.47578,-25.24079,-20.92581], "fy":[-3.68825,-38.91008,-49.38032,-11.5776]}, - {"t":2.50479, "x":5.93802, "y":5.4973, "heading":2.98811, "vx":-4.61655, "vy":0.38916, "omega":0.89904, "ax":-1.74452, "ay":-0.99585, "alpha":-4.92945, "fx":[-41.38085,-43.63827,-18.58711,-15.08889], "fy":[-1.33079,-27.33739,-33.17659,-5.91146]}, - {"t":2.51861, "x":5.87405, "y":5.50258, "heading":3.00054, "vx":-4.64066, "vy":0.3754, "omega":0.83091, "ax":-1.08756, "ay":-0.31777, "alpha":-2.99336, "fx":[-25.36237,-27.35409,-11.74154,-9.53857], "fy":[3.76791,-12.15715,-14.75636,1.52505]}, - {"t":2.53243, "x":5.80982, "y":5.50774, "heading":3.01202, "vx":-4.65569, "vy":0.37101, "omega":0.78955, "ax":-0.46365, "ay":0.51767, "alpha":-1.12121, "fx":[-10.45913,-11.28224,-5.30429,-4.50069], "fy":[12.15385,6.19919,5.44206,11.42663]}, - {"t":2.54625, "x":5.74543, "y":5.51292, "heading":3.02293, "vx":-4.6621, "vy":0.37816, "omega":0.77405, "ax":0.06037, "ay":1.51642, "alpha":0.41545, "fx":[2.0352,2.2962,0.02397,-0.24803], "fy":[24.58668,26.73941,26.99999,24.84926]}, - {"t":2.56007, "x":5.68101, "y":5.51829, "heading":3.03363, "vx":-4.66126, "vy":0.39912, "omega":0.77979, "ax":0.44216, "ay":2.71522, "alpha":1.21525, "fx":[10.81925,11.30153,4.31449,3.64888], "fy":[42.81803,48.71999,49.54398,43.6584]}, - {"t":2.57389, "x":5.61663, "y":5.52406, "heading":3.04441, "vx":-4.65515, "vy":0.43664, "omega":0.79659, "ax":0.76019, "ay":4.18252, "alpha":0.69849, "fx":[15.1501,15.24673,10.76284,10.56289], "fy":[69.38608,72.32137,72.89457,69.97207]}, - {"t":2.58771, "x":5.55237, "y":5.5305, "heading":3.05541, "vx":-4.64465, "vy":0.49445, "omega":0.80624, "ax":1.08669, "ay":5.56271, "alpha":0.24916, "fx":[19.42768,19.37736,17.55057,17.5813], "fy":[94.06929,94.89997,95.16895,94.34196]}, - {"t":2.60153, "x":5.48829, "y":5.53786, "heading":3.06656, "vx":-4.62963, "vy":0.57132, "omega":0.80968, "ax":1.42128, "ay":6.46228, "alpha":0.39201, "fx":[25.88861,25.62019,22.4926,22.70066], "fy":[109.12071,110.16833,110.71424,109.68276]}, - {"t":2.61535, "x":5.42444, "y":5.54638, "heading":3.07775, "vx":-4.60999, "vy":0.66063, "omega":0.8151, "ax":1.73549, "ay":7.01476, "alpha":0.8114, "fx":[33.47013,32.46477,25.71436,26.4317], "fy":[117.6838,119.53437,120.90372,119.15447]}, - {"t":2.62917, "x":5.3609, "y":5.55617, "heading":3.08901, "vx":-4.586, "vy":0.75757, "omega":0.82631, "ax":0.73226, "ay":7.50811, "alpha":-3.22645, "fx":[-1.1798,-3.11795,28.78404,25.33585], "fy":[131.54864,126.50205,123.53558,129.25662]}, - {"t":2.64742, "x":5.27731, "y":5.57126, "heading":3.10409, "vx":-4.57264, "vy":0.89463, "omega":0.76742, "ax":0.82301, "ay":6.90458, "alpha":-2.92749, "fx":[1.9318,1.24388,27.97162,24.84949], "fy":[121.81653,115.52415,112.77923,119.65965]}, - {"t":2.66568, "x":5.19397, "y":5.58874, "heading":3.1181, "vx":-4.55761, "vy":1.02067, "omega":0.71397, "ax":1.40929, "ay":5.79245, "alpha":-0.72881, "fx":[20.9629,21.53494,27.06363,26.32519], "fy":[100.12624,97.76995,96.90822,99.30744]}, - {"t":2.68393, "x":5.11101, "y":5.60833, "heading":3.13114, "vx":-4.53189, "vy":1.12641, "omega":0.70067, "ax":2.94432, "ay":3.18752, "alpha":5.32684, "fx":[68.18278,61.18207,32.91874,38.04461], "fy":[37.14582,64.58855,71.84065,43.30022]}, - {"t":2.70219, "x":5.02877, "y":5.62943, "heading":-3.13926, "vx":-4.47814, "vy":1.1846, "omega":0.79791, "ax":4.20081, "ay":-0.43105, "alpha":11.45258, "fx":[94.22245,96.50795,48.93694,46.15123], "fy":[-37.27575,24.69816,35.78786,-52.53814]}, - {"t":2.72044, "x":4.94772, "y":5.65098, "heading":-3.12469, "vx":-4.40145, "vy":1.17673, "omega":1.00698, "ax":4.53224, "ay":-2.33921, "alpha":13.60822, "fx":[98.97159,114.13706,57.68389,37.57589], "fy":[-68.97007,3.56865,4.12315,-97.87896]}, - {"t":2.7387, "x":4.86812, "y":5.67207, "heading":-3.10631, "vx":-4.31872, "vy":1.13403, "omega":1.2554, "ax":4.62479, "ay":-3.30714, "alpha":14.26985, "fx":[100.87692,123.42002,59.28578,31.0827], "fy":[-82.39769,-5.33904,-20.79575,-116.48128]}, - {"t":2.75695, "x":4.79006, "y":5.69222, "heading":-3.08339, "vx":-4.23429, "vy":1.07366, "omega":1.51589, "ax":4.63103, "ay":-3.95802, "alpha":14.37389, "fx":[102.18712,128.94492,56.38488,27.57301], "fy":[-89.21363,-10.04429,-44.03721,-126.00426]}, - {"t":2.77521, "x":4.71353, "y":5.71116, "heading":-3.05572, "vx":-4.14975, "vy":1.00141, "omega":1.77829, "ax":4.57667, "ay":-4.51099, "alpha":14.18502, "fx":[103.03629,132.33902,49.84702,26.16879], "fy":[-93.34271,-14.19332,-67.74878,-131.63789]}, - {"t":2.79346, "x":4.63854, "y":5.72869, "heading":-3.02326, "vx":-4.0662, "vy":0.91906, "omega":2.03723, "ax":4.48797, "ay":-5.01866, "alpha":13.82543, "fx":[103.31893,134.19147,41.48731,26.35843], "fy":[-96.40617,-19.79441,-90.02522,-135.23783]}, - {"t":2.81172, "x":4.56506, "y":5.74463, "heading":-2.98607, "vx":-3.98428, "vy":0.82744, "omega":2.28962, "ax":4.38502, "ay":-5.47948, "alpha":13.35131, "fx":[102.8603,134.56741,33.43287,27.49102], "fy":[-99.26369,-27.95526,-107.92592,-137.67243]}, - {"t":2.82997, "x":4.49306, "y":5.75883, "heading":-2.94427, "vx":-3.90423, "vy":0.72742, "omega":2.53335, "ax":4.25906, "ay":-5.90669, "alpha":12.75103, "fx":[101.46309,133.15384,26.39305,28.77147], "fy":[-102.43677,-39.13685,-120.84971,-139.46104]}, - {"t":2.84823, "x":4.4225, "y":5.77112, "heading":-2.89802, "vx":-3.82648, "vy":0.61959, "omega":2.76612, "ax":4.08522, "ay":-6.32524, "alpha":11.97307, "fx":[98.94107,129.31436,20.12849,29.56994], "fy":[-106.22092,-53.37361,-129.83002,-140.93744]}, - {"t":2.86648, "x":4.35332, "y":5.78138, "heading":-2.84753, "vx":-3.7519, "vy":0.50412, "omega":2.98468, "ax":3.84176, "ay":-6.75067, "alpha":10.95776, "fx":[95.13813,122.19567,14.47133,29.58368], "fy":[-110.7249,-70.24552,-136.04878,-142.28858]}, - {"t":2.88474, "x":4.28547, "y":5.78946, "heading":-2.79304, "vx":-3.68177, "vy":0.38089, "omega":3.18472, "ax":3.52212, "ay":-7.17976, "alpha":9.6659, "fx":[89.94476,111.03072,9.76571,28.89955], "fy":[-115.8932,-88.6739,-140.3731,-143.56271]}, - {"t":2.90299, "x":4.21885, "y":5.79521, "heading":-2.73491, "vx":-3.61748, "vy":0.24982, "omega":3.36117, "ax":3.14478, "ay":-7.58989, "alpha":8.11515, "fx":[83.30028,95.79486,6.88929,27.98276], "fy":[-121.54424,-106.77007,-143.3972,-144.69581]}, - {"t":2.92125, "x":4.15334, "y":5.79851, "heading":-2.67355, "vx":-3.56007, "vy":0.11127, "omega":3.50931, "ax":2.75651, "ay":-7.94712, "alpha":6.40784, "fx":[75.18111,78.06109,6.83493,27.47291], "fy":[-127.42269,-122.1857,-145.52062,-145.58402]}, - {"t":2.9395, "x":4.08881, "y":5.79922, "heading":-2.60949, "vx":-3.50975, "vy":-0.03381, "omega":3.62628, "ax":2.41328, "ay":-8.22538, "alpha":4.69694, "fx":[65.65611,60.91754,9.88219,27.74094], "fy":[-133.2109,-133.31772,-146.93523,-146.1814]}, - {"t":2.95776, "x":4.02514, "y":5.79723, "heading":-2.54329, "vx":-3.46569, "vy":-0.18396, "omega":3.71203, "ax":2.1464, "ay":-8.42195, "alpha":3.09369, "fx":[55.05421,47.10415,15.2585,28.62181], "fy":[-138.51331,-140.29203,-147.67203,-146.54276]}, - {"t":2.97601, "x":3.96223, "y":5.79247, "heading":-2.47553, "vx":-3.42651, "vy":-0.3377, "omega":3.7685, "ax":1.95613, "ay":-8.551, "alpha":1.63345, "fx":[44.02794,37.52707,21.85471,29.68307], "fy":[-142.94878,-144.32938,-147.74983,-146.77258]}, - {"t":2.99427, "x":3.90001, "y":5.78488, "heading":-2.40673, "vx":-3.3908, "vy":-0.4938, "omega":3.79832, "ax":1.83129, "ay":-8.62908, "alpha":0.31649, "fx":[33.36308,31.73932,28.91596,30.58077], "fy":[-146.31512,-146.60727,-147.23032,-146.95985]}, - {"t":3.01252, "x":3.83841, "y":5.77442, "heading":-2.33739, "vx":-3.35737, "vy":-0.65132, "omega":3.8041, "ax":1.76175, "ay":-8.66936, "alpha":-0.86165, "fx":[23.72108,28.9119,36.09529,31.13917], "fy":[-148.64598,-147.86667,-146.18199,-147.15899]}, - {"t":3.03078, "x":3.77742, "y":5.76109, "heading":-2.26795, "vx":-3.32521, "vy":-0.80958, "omega":3.78837, "ax":1.73932, "ay":-8.68168, "alpha":-1.90731, "fx":[15.50544,28.292,43.26042,31.28352], "fy":[-150.13574,-148.49989,-144.65666,-147.3996]}, - {"t":3.04903, "x":3.717, "y":5.74486, "heading":-2.19879, "vx":-3.29346, "vy":-0.96807, "omega":3.75355, "ax":1.75616, "ay":-8.6733, "alpha":-2.83193, "fx":[8.86732,29.29475,50.34962,30.97573], "fy":[-151.02928,-148.70048,-142.69299,-147.69846]}, - {"t":3.06729, "x":3.65718, "y":5.72575, "heading":-2.13027, "vx":-3.2614, "vy":-1.1264, "omega":3.70185, "ax":1.80482, "ay":-8.64924, "alpha":-3.65479, "fx":[3.77005,31.48952,57.33847,30.19962], "fy":[-151.54648,-148.56009,-140.31616,-148.06187]}, - {"t":3.08554, "x":3.59794, "y":5.70374, "heading":-2.06269, "vx":-3.22845, "vy":-1.28429, "omega":3.63514, "ax":1.89044, "ay":-8.60748, "alpha":-4.45052, "fx":[-0.09166,34.76176,64.7746,29.17891], "fy":[-151.85116,-148.07693,-137.28038,-148.43485]}, - {"t":3.1038, "x":3.53932, "y":5.67886, "heading":-1.99633, "vx":-3.19394, "vy":-1.44142, "omega":3.55389, "ax":2.16605, "ay":-8.46402, "alpha":-5.92134, "fx":[-5.00361,41.63266,80.00321,30.74301], "fy":[-151.99374,-146.53046,-129.22497,-148.13295]}, - {"t":3.12205, "x":3.48137, "y":5.65114, "heading":-1.93146, "vx":-3.1544, "vy":-1.59593, "omega":3.4458, "ax":2.82709, "ay":-7.99452, "alpha":-8.94018, "fx":[-13.74728,55.3547,110.37715,40.36775], "fy":[-151.66853,-142.17962,-104.80827,-145.2818]}, - {"t":3.14031, "x":3.42426, "y":5.62068, "heading":-1.86856, "vx":-3.10279, "vy":-1.74187, "omega":3.2826, "ax":3.38563, "ay":-7.43863, "alpha":-11.37078, "fx":[-19.78861,67.69648,131.41953,51.02722], "fy":[-151.18699,-136.93647,-77.22399,-140.76831]}, - {"t":3.15856, "x":3.36818, "y":5.58764, "heading":-1.80863, "vx":-3.04099, "vy":-1.87766, "omega":3.07502, "ax":4.36503, "ay":-7.08998, "alpha":-10.32072, "fx":[2.86458,79.85416,134.65965,79.61304], "fy":[-152.50296,-130.27748,-71.64587,-127.96802]}, - {"t":3.17817, "x":3.30941, "y":5.54947, "heading":-1.74835, "vx":-2.95542, "vy":-2.01665, "omega":2.8727, "ax":5.41157, "ay":-6.41737, "alpha":-9.8127, "fx":[26.69513,93.90814,140.37795,107.21565], "fy":[-150.11218,-120.50709,-59.68628,-106.32503]}, - {"t":3.19777, "x":3.25251, "y":5.5087, "heading":-1.69203, "vx":-2.84933, "vy":-2.14246, "omega":2.68033, "ax":6.31975, "ay":-5.64315, "alpha":-9.21343, "fx":[52.02434,107.02283,144.76905,126.1724], "fy":[-143.26768,-109.00007,-48.12642,-83.55945]}, - {"t":3.21738, "x":3.19787, "y":5.46561, "heading":-1.63949, "vx":-2.72544, "vy":-2.25309, "omega":2.49971, "ax":7.07978, "ay":-4.82014, "alpha":-8.39952, "fx":[77.10163,118.73581,147.94912,137.91347], "fy":[-131.45797,-96.09182,-37.33564,-63.07124]}, - {"t":3.23698, "x":3.1458, "y":5.42052, "heading":-1.59049, "vx":-2.58665, "vy":-2.34758, "omega":2.33505, "ax":7.69339, "ay":-3.98167, "alpha":-7.37762, "fx":[99.81031,128.71115,150.1188,144.80914], "fy":[-115.18209,-82.24958,-27.52891,-45.94771]}, - {"t":3.25658, "x":3.09657, "y":5.37373, "heading":-1.54471, "vx":-2.43583, "vy":-2.42563, "omega":2.19042, "ax":8.1642, "ay":-3.15653, "alpha":-6.24445, "fx":[118.45732,136.78509,151.49982,148.74084], "fy":[-95.96313,-67.99793,-18.78952,-32.0164]}, - {"t":3.27619, "x":3.05039, "y":5.32557, "heading":-1.50177, "vx":-2.27578, "vy":-2.48751, "omega":2.068, "ax":8.50313, "ay":-2.37241, "alpha":-5.11058, "fx":[132.36521,142.96849,152.29626,150.91332], "fy":[-75.75629,-53.83697,-11.10601,-20.7165]}, - {"t":3.29579, "x":3.00741, "y":5.27635, "heading":-1.46123, "vx":-2.10908, "vy":-2.53402, "omega":1.96782, "ax":8.72999, "ay":-1.65037, "alpha":-4.05276, "fx":[141.84684,147.41168,152.67725,152.04271], "fy":[-56.22231,-40.18013,-4.41001,-11.47677]}, - {"t":3.31539, "x":2.96774, "y":5.22636, "heading":-1.42265, "vx":-1.93794, "vy":-2.56638, "omega":1.88837, "ax":8.86889, "ay":-1.00123, "alpha":-3.10575, "fx":[147.75686,150.35117,152.77395,152.54699], "fy":[-38.36823,-27.32389,1.39393,-3.82463]}, - {"t":3.335, "x":2.93145, "y":5.17586, "heading":-1.38563, "vx":-1.76408, "vy":-2.586, "omega":1.82748, "ax":8.94285, "ay":-0.42662, "alpha":-2.27504, "fx":[151.04855,152.05787,152.68373,152.67137], "fy":[-22.59296,-15.44717,6.40896,2.60453]}, - {"t":3.3546, "x":2.89859, "y":5.12508, "heading":-1.34981, "vx":-1.58877, "vy":-2.59437, "omega":1.78288, "ax":8.97093, "ay":0.07774, "alpha":-1.55159, "fx":[152.53547,152.79804,152.47678,152.56171], "fy":[-8.90134,-4.62965,10.73497,8.08558]}, - {"t":3.37421, "x":2.86916, "y":5.07423, "heading":-1.31485, "vx":-1.4129, "vy":-2.59284, "omega":1.75247, "ax":8.96761, "ay":0.51893, "alpha":-0.92151, "fx":[152.827,152.81012,152.20256,152.30609], "fy":[2.89668,5.12233,14.46333,12.82509]}, - {"t":3.39381, "x":2.84319, "y":5.0235, "heading":-1.2805, "vx":-1.2371, "vy":-2.58267, "omega":1.7344, "ax":8.94338, "ay":0.90482, "alpha":-0.37072, "fx":[152.34867,152.29463,151.8953,151.95845], "fy":[13.05822,13.85049,17.67498,16.97916]}, - {"t":3.41341, "x":2.82066, "y":4.97305, "heading":-1.2465, "vx":-1.06178, "vy":-2.56493, "omega":1.72713, "ax":8.90564, "ay":1.24301, "alpha":0.11327, "fx":[151.38643,151.413,151.57821,151.55211], "fy":[21.84155,21.62402,20.44036,20.66696]}, - {"t":3.43302, "x":2.80155, "y":4.923, "heading":-1.21264, "vx":-0.88719, "vy":-2.54057, "omega":1.72935, "ax":8.85955, "ay":1.54035, "alpha":0.54092, "fx":[150.12834,150.29122,151.26664,151.10772], "fy":[29.47823,28.52461,22.82025,23.98059]}, - {"t":3.45262, "x":2.78586, "y":4.8735, "heading":-1.17874, "vx":-0.71351, "vy":-2.51037, "omega":1.73996, "ax":8.80865, "ay":1.8028, "alpha":0.92087, "fx":[148.69661,149.02541,150.97037,150.63794], "fy":[36.16499,34.63665,24.86689,26.99211]}, - {"t":3.47222, "x":2.77357, "y":4.82463, "heading":-1.14463, "vx":-0.54083, "vy":-2.47503, "omega":1.75801, "ax":8.75534, "ay":2.03545, "alpha":1.26012, "fx":[147.1699,147.68777,150.69526,150.15038], "fy":[42.06457,40.04126,26.62523,29.75864]}, - {"t":3.49183, "x":2.76465, "y":4.7765, "heading":-1.11016, "vx":-0.36919, "vy":-2.43512, "omega":1.78271, "ax":8.70126, "ay":2.24259, "alpha":1.56442, "fx":[145.5984,146.33189,150.44441,149.64931], "fy":[47.30984,44.81312,28.13414,32.32602]}, - {"t":3.51143, "x":2.75908, "y":4.72919, "heading":-1.07522, "vx":-0.19861, "vy":-2.39116, "omega":1.81338, "ax":8.64751, "ay":2.42783, "alpha":1.83843, "fx":[144.01375,144.99722,150.21895,149.13683], "fy":[52.0087,49.01902,29.42745,34.73144]}, - {"t":3.53104, "x":2.75685, "y":4.68278, "heading":-1.03967, "vx":-0.02909, "vy":-2.34357, "omega":1.84942, "ax":8.5948, "ay":2.5942, "alpha":2.08598, "fx":[142.43551,143.71263,150.01865,148.61359], "fy":[56.24865,52.71758,30.53484,37.00542]}, - {"t":3.55064, "x":2.75793, "y":4.63734, "heading":-1.00341, "vx":0.1394, "vy":-2.29271, "omega":1.89032, "ax":8.54359, "ay":2.74426, "alpha":2.3102, "fx":[140.8754,142.49918,149.84232,148.07925], "fy":[60.1008,55.95948,31.48265,39.17326]}, - {"t":3.57024, "x":2.76231, "y":4.59292, "heading":-0.96635, "vx":0.30689, "vy":-2.23891, "omega":1.9356, "ax":8.49416, "ay":2.88014, "alpha":2.51369, "fx":[139.34014,141.37221,149.68808,147.5328], "fy":[63.62297,58.78806,32.29448,41.25606]}, - {"t":3.58985, "x":2.76995, "y":4.54958, "heading":-0.92841, "vx":0.47341, "vy":-2.18245, "omega":1.98488, "ax":8.44668, "ay":3.00366, "alpha":2.69859, "fx":[137.83324,140.34284,149.55359,146.97276], "fy":[66.86236,61.24007,32.99172,43.27163]}, - {"t":3.60945, "x":2.78086, "y":4.50738, "heading":-0.8895, "vx":0.639, "vy":-2.12357, "omega":2.03779, "ax":8.40795, "ay":3.1533, "alpha":2.35808, "fx":[137.53504,140.12989,148.50293,145.89958], "fy":[67.66693,61.9999,37.85306,47.02695]}, - {"t":3.63647, "x":2.80119, "y":4.45116, "heading":-0.83445, "vx":0.86613, "vy":-2.03838, "omega":2.10149, "ax":8.33223, "ay":3.35615, "alpha":2.16356, "fx":[136.25534,139.10638,147.17759,144.3758], "fy":[70.13706,64.1842,42.60387,51.42346]}, - {"t":3.66348, "x":2.82763, "y":4.39732, "heading":-0.77768, "vx":1.09123, "vy":-1.94772, "omega":2.15994, "ax":8.22934, "ay":3.60887, "alpha":1.919, "fx":[134.67151,137.63231,145.25677,142.3543], "fy":[73.04645,67.1951,48.62846,56.67346]}, - {"t":3.6905, "x":2.86011, "y":4.34602, "heading":-0.71933, "vx":1.31354, "vy":-1.85022, "omega":2.21178, "ax":8.0838, "ay":3.9312, "alpha":1.60159, "fx":[132.62891,135.4661,142.36439,139.55337], "fy":[76.58741,71.35451,56.40169,63.13079]}, - {"t":3.71751, "x":2.89854, "y":4.29747, "heading":-0.65958, "vx":1.53192, "vy":-1.74402, "omega":2.25504, "ax":7.86728, "ay":4.35362, "alpha":1.17344, "fx":[129.85275,132.19248,137.79693,135.43827], "fy":[81.07299,77.12553,66.63644,71.38068]}, - {"t":3.74452, "x":2.9428, "y":4.25194, "heading":-0.59866, "vx":1.74445, "vy":-1.62641, "omega":2.28674, "ax":7.52392, "ay":4.92389, "alpha":0.56878, "fx":[125.81538,127.05927,130.15537,128.88888], "fy":[87.03572,85.1716,80.37789,82.43094]}, - {"t":3.77154, "x":2.99267, "y":4.2098, "heading":-0.53688, "vx":1.9477, "vy":-1.4934, "omega":2.30211, "ax":6.93451, "ay":5.71387, "alpha":-0.3312, "fx":[119.39475,118.63609,116.52343,117.26188], "fy":[95.42469,96.38995,98.92538,98.02506]}, - {"t":3.79855, "x":3.04782, "y":4.17155, "heading":-0.47469, "vx":2.13504, "vy":-1.33904, "omega":2.29316, "ax":5.82773, "ay":6.80306, "alpha":-1.74612, "fx":[107.86586,104.13642,90.98856,93.52106], "fy":[107.96854,111.70682,122.62757,120.56946]}, - {"t":3.82557, "x":3.10762, "y":4.13785, "heading":-0.41274, "vx":2.29247, "vy":-1.15526, "omega":2.24599, "ax":3.62572, "ay":8.09568, "alpha":-4.04894, "fx":[83.51203,78.49357,45.00128,39.68317], "fy":[127.28854,130.80361,145.75726,146.97171]}, - {"t":3.85258, "x":3.17087, "y":4.1096, "heading":-0.35207, "vx":2.39042, "vy":-0.93656, "omega":2.13661, "ax":-0.18517, "ay":8.69427, "alpha":-6.7813, "fx":[24.22271,35.67061,-19.00554,-53.48674], "fy":[149.70236,148.15808,151.32405,142.36395]}, - {"t":3.8796, "x":3.23538, "y":4.08747, "heading":-0.29435, "vx":2.38542, "vy":-0.70168, "omega":1.95342, "ax":-4.32181, "ay":7.46234, "alpha":-7.58046, "fx":[-78.63285,-20.76848,-75.10837,-119.54145], "fy":[129.41558,150.93284,132.88171,94.49871]}, - {"t":3.90661, "x":3.29824, "y":4.07124, "heading":-0.24158, "vx":2.26866, "vy":-0.50009, "omega":1.74863, "ax":-6.72886, "ay":5.37755, "alpha":-8.01878, "fx":[-135.34457,-71.05816,-108.79122,-142.63033], "fy":[69.13621,134.90516,107.31194,54.52921]}, - {"t":3.93363, "x":3.35708, "y":4.05969, "heading":-0.19434, "vx":2.08689, "vy":-0.35482, "omega":1.53201, "ax":-7.77833, "ay":3.80307, "alpha":-7.75555, "fx":[-149.36767,-103.36434,-126.58881,-149.90793], "fy":[30.45381,112.33369,85.85051,30.11833]}, - {"t":3.96064, "x":3.41061, "y":4.05149, "heading":-0.15295, "vx":1.87676, "vy":-0.25208, "omega":1.3225, "ax":-8.26756, "ay":2.74358, "alpha":-7.2362, "fx":[-152.42361,-121.5831,-136.19474,-152.31417], "fy":[9.42403,92.56913,69.84474,14.83182]}, - {"t":3.98766, "x":3.4583, "y":4.04569, "heading":-0.11723, "vx":1.65341, "vy":-0.17797, "omega":1.12701, "ax":-8.51751, "ay":2.01403, "alpha":-6.76513, "fx":[-152.85533,-131.89794,-141.71478,-153.05389], "fy":[-3.01,77.40762,58.03296,4.60173]}, - {"t":4.01467, "x":3.49986, "y":4.04161, "heading":-0.08678, "vx":1.42332, "vy":-0.12356, "omega":0.94426, "ax":-8.65537, "ay":1.49167, "alpha":-6.38975, "fx":[-152.59909,-138.04576,-145.09356,-153.16319], "fy":[-11.05295,66.03523,49.15673,-2.64763]}, - {"t":4.04168, "x":3.53515, "y":4.03882, "heading":-0.06127, "vx":1.1895, "vy":-0.08326, "omega":0.77164, "ax":-8.73624, "ay":1.10336, "alpha":-6.09611, "fx":[-152.17263,-141.9327,-147.2753,-153.02343], "fy":[-16.64124,57.39255,42.33209,-8.01194]}, - {"t":4.0687, "x":3.56409, "y":4.03697, "heading":-0.04043, "vx":0.95349, "vy":-0.05345, "omega":0.60696, "ax":-8.78604, "ay":0.80519, "alpha":-5.86421, "fx":[-151.72781,-144.52761,-148.74618,-152.79026], "fy":[-20.75116,50.66795,36.97546,-12.10828]}, - {"t":4.09571, "x":3.58665, "y":4.03582, "heading":-0.02403, "vx":0.71614, "vy":-0.0317, "omega":0.44854, "ax":-8.81785, "ay":0.5699, "alpha":-5.67802, "fx":[-151.30809,-146.34406,-149.77237,-152.53205], "fy":[-23.91929,45.29987,32.70079,-15.30626]}, - {"t":4.12273, "x":3.60277, "y":4.03517, "heading":-0.01191, "vx":0.47793, "vy":-0.01631, "omega":0.29515, "ax":-8.83876, "ay":0.37996, "alpha":-5.52592, "fx":[-150.92205,-147.66863,-150.50792,-152.28021], "fy":[-26.46202,40.90562,29.24737,-17.83921]}, - {"t":4.14974, "x":3.61246, "y":4.03487, "heading":-0.00394, "vx":0.23915, "vy":-0.00604, "omega":0.14587, "ax":-8.85277, "ay":0.22367, "alpha":-5.39963, "fx":[-150.56704,-148.66939,-151.04641,-152.04937], "fy":[-28.5764,37.22124,26.43455,-19.86099]}, - {"t":4.17676, "x":3.61569, "y":4.03479, "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":3.62538, "y":2.53495, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.0058, "ay":-0.4558, "alpha":-0.0447, "fx":[153.17485,153.19895,153.1973,153.17305], "fy":[-7.97968,-7.50221,-7.52565,-8.0046]}, + {"t":0.03941, "x":3.63238, "y":2.5346, "heading":0.0, "vx":0.35495, "vy":-0.01796, "omega":-0.00176, "ax":9.00475, "ay":-0.46081, "alpha":-0.0442, "fx":[153.15707,153.18116,153.17946,153.15521], "fy":[-8.06223,-7.59012,-7.61357,-8.08713]}, + {"t":0.07883, "x":3.65336, "y":2.53353, "heading":-0.00007, "vx":0.70987, "vy":-0.03613, "omega":-0.0035, "ax":9.00351, "ay":-0.46673, "alpha":-0.04362, "fx":[153.13598,153.16006,153.1583,153.13408], "fy":[-8.15986,-7.69408,-7.71749,-8.18468]}, + {"t":0.11824, "x":3.68833, "y":2.53175, "heading":-0.00021, "vx":1.06473, "vy":-0.05452, "omega":-0.00522, "ax":9.00201, "ay":-0.47385, "alpha":-0.04291, "fx":[153.11058,153.13463,153.13281,153.10862], "fy":[-8.27708,-7.81889,-7.84223,-8.30179]}, + {"t":0.15766, "x":3.73729, "y":2.52923, "heading":-0.00041, "vx":1.41953, "vy":-0.0732, "omega":-0.00691, "ax":9.00018, "ay":-0.48254, "alpha":-0.04205, "fx":[153.0794,153.10339,153.1015,153.07736], "fy":[-8.42044,-7.97153,-7.99475,-8.44498]}, + {"t":0.19707, "x":3.80023, "y":2.52597, "heading":-0.00069, "vx":1.77427, "vy":-0.09222, "omega":-0.00857, "ax":8.99787, "ay":-0.49342, "alpha":-0.04098, "fx":[153.04019,153.0641,153.06212,153.03808], "fy":[-8.59977,-8.16244,-8.18549,-8.62406]}, + {"t":0.23648, "x":3.87715, "y":2.52195, "heading":-0.00102, "vx":2.12891, "vy":-0.11167, "omega":-0.01019, "ax":8.99487, "ay":-0.50741, "alpha":-0.0396, "fx":[152.98944,153.01319,153.0111,152.98722], "fy":[-8.8305,-8.40807,-8.43087,-8.85446]}, + {"t":0.2759, "x":3.96805, "y":2.51716, "heading":-0.00143, "vx":2.48343, "vy":-0.13166, "omega":-0.01175, "ax":8.99085, "ay":-0.52609, "alpha":-0.03776, "fx":[152.92114,152.94461,152.94241,152.91881], "fy":[-9.13842,-8.73581,-8.75825,-9.1619]}, + {"t":0.31531, "x":4.07291, "y":2.51156, "heading":-0.00189, "vx":2.83779, "vy":-0.1524, "omega":-0.01324, "ax":8.98514, "ay":-0.55225, "alpha":-0.03518, "fx":[152.82434,152.8473,152.84495,152.82188], "fy":[-9.56994,-9.19506,-9.2169,-9.5927]}, + {"t":0.35473, "x":4.19174, "y":2.50512, "heading":-0.00241, "vx":3.19193, "vy":-0.17417, "omega":-0.01462, "ax":8.97642, "ay":-0.59154, "alpha":-0.03132, "fx":[152.67659,152.69846,152.69597,152.674], "fy":[-10.21806,-9.88465,-9.9054,-10.23954]}, + {"t":0.39414, "x":4.32452, "y":2.4978, "heading":-0.00299, "vx":3.54573, "vy":-0.19748, "omega":-0.01586, "ax":8.96146, "ay":-0.65709, "alpha":-0.02491, "fx":[152.42354,152.44285,152.4403,152.42093], "fy":[-11.29995,-11.0353,-11.05361,-11.31871]}, + {"t":0.43355, "x":4.47123, "y":2.4895, "heading":-0.00361, "vx":3.89893, "vy":-0.22338, "omega":-0.01684, "ax":8.93002, "ay":-0.78828, "alpha":-0.01215, "fx":[151.89234,151.90361,151.90177,151.89049], "fy":[-13.46732,-13.33876,-13.34952,-13.47818]}, + {"t":0.47297, "x":4.63184, "y":2.48009, "heading":-0.00427, "vx":4.2509, "vy":-0.25445, "omega":-0.01732, "ax":8.82373, "ay":-1.18041, "alpha":0.02564, "fx":[150.10252,150.06724,150.07563,150.11079], "fy":[-19.96191,-20.2291,-20.19483,-19.92809]}, + {"t":0.51238, "x":4.80624, "y":2.46914, "heading":-0.00496, "vx":4.59868, "vy":-0.30097, "omega":-0.01631, "ax":-1.18678, "ay":-7.64465, "alpha":0.77802, "fx":[-24.26368,-23.21393,-16.26657,-17.00264], "fy":[-128.9195,-130.21021,-131.10602,-129.89762]}, + {"t":0.55179, "x":4.98657, "y":2.45134, "heading":-0.0056, "vx":4.5519, "vy":-0.60228, "omega":0.01436, "ax":-8.89572, "ay":-0.40116, "alpha":0.12511, "fx":[-151.35328,-151.2951,-151.27353,-151.33278], "fy":[-6.13214,-7.44881,-7.52071,-6.1927]}, + {"t":0.59121, "x":5.15906, "y":2.42729, "heading":-0.00503, "vx":4.20129, "vy":-0.61809, "omega":0.01929, "ax":-8.96533, "ay":-0.00963, "alpha":0.0891, "fx":[-152.50146,-152.5005,-152.49397,-152.49498], "fy":[0.31273,-0.63455,-0.64329,0.30987]}, + {"t":0.63062, "x":5.31769, "y":2.40292, "heading":-0.00427, "vx":3.84793, "vy":-0.61847, "omega":0.0228, "ax":-8.98493, "ay":0.12353, "alpha":0.07668, "fx":[-152.82734,-152.83852,-152.83473,-152.82345], "fy":[2.50599,1.68871,1.69424,2.51589]}, + {"t":0.67004, "x":5.46237, "y":2.37864, "heading":-0.00338, "vx":3.4938, "vy":-0.6136, "omega":0.02582, "ax":-8.994, "ay":0.19062, "alpha":0.07036, "fx":[-152.97893,-152.99476,-152.99198,-152.976], "fy":[3.61103,2.86025,2.87176,3.62622]}, + {"t":0.70945, "x":5.59309, "y":2.35461, "heading":-0.00236, "vx":3.13931, "vy":-0.60609, "omega":0.0286, "ax":-8.99921, "ay":0.23104, "alpha":0.06653, "fx":[-153.06613,-153.08429,-153.08199,-153.06368], "fy":[4.27685,3.56651,3.58141,4.29505]}, + {"t":0.74886, "x":5.70983, "y":2.3309, "heading":-0.00123, "vx":2.78462, "vy":-0.59698, "omega":0.03122, "ax":-9.00258, "ay":0.25807, "alpha":0.06396, "fx":[-153.12268,-153.14217,-153.14016,-153.12049], "fy":[4.72193,4.03881,4.05603,4.74219]}, + {"t":0.78828, "x":5.81259, "y":2.30757, "heading":0.0, "vx":2.42979, "vy":-0.58681, "omega":0.03374, "ax":-2.46054, "ay":-8.48859, "alpha":0.32472, "fx":[-43.89146,-42.96997,-39.8427,-40.708], "fy":[-143.76672,-144.11625,-144.99186,-144.67886]}, + {"t":0.81138, "x":5.86807, "y":2.29175, "heading":0.00078, "vx":2.37295, "vy":-0.78291, "omega":0.04124, "ax":-3.06861, "ay":-8.18241, "alpha":0.46263, "fx":[-55.1034,-53.52537,-49.33398,-50.82214], "fy":[-138.02875,-138.80146,-140.29052,-139.60103]}, + {"t":0.83448, "x":5.92207, "y":2.27148, "heading":0.00173, "vx":2.30206, "vy":-0.97194, "omega":0.05193, "ax":-3.62854, "ay":-7.73227, "alpha":0.72802, "fx":[-66.17895,-63.40263,-57.3388,-59.96133], "fy":[-129.28828,-131.06797,-133.65557,-132.08286]}, + {"t":0.85758, "x":5.97428, "y":2.24696, "heading":0.00293, "vx":2.21823, "vy":-1.15057, "omega":0.06875, "ax":-3.98516, "ay":-7.03014, "alpha":1.32971, "fx":[-75.35867,-70.28148,-60.36423,-65.14163], "fy":[-114.8766,-119.37547,-123.99611,-120.0743]}, + {"t":0.88069, "x":6.02447, "y":2.2185, "heading":0.00452, "vx":2.12617, "vy":-1.31298, "omega":0.09947, "ax":-3.7602, "ay":-5.68176, "alpha":2.97987, "fx":[-78.15417,-69.31136,-50.21059,-58.16347], "fy":[-85.87845,-99.03405,-106.70629,-94.96151]}, + {"t":0.90379, "x":6.07258, "y":2.18666, "heading":0.00682, "vx":2.0393, "vy":-1.44424, "omega":0.16831, "ax":-2.26395, "ay":-3.08017, "alpha":6.73802, "fx":[-61.61738,-53.5098,-17.47869,-21.43079], "fy":[-32.81952,-66.28263,-72.74164,-37.72753]}, + {"t":0.92689, "x":6.11909, "y":2.15247, "heading":0.01071, "vx":1.987, "vy":-1.51539, "omega":0.32397, "ax":-0.77647, "ay":-1.00585, "alpha":8.42434, "fx":[-37.25834,-34.6068,9.4372,9.59748], "fy":[4.9834,-38.92131,-40.29682,5.79769]}, + {"t":0.94999, "x":6.16478, "y":2.11719, "heading":0.01819, "vx":1.96906, "vy":-1.53863, "omega":0.51858, "ax":-0.2257, "ay":-0.28784, "alpha":7.96777, "fx":[-25.83481,-24.62834,17.78897,17.3179], "fy":[15.967,-26.47637,-26.06883,16.99416]}, + {"t":0.97309, "x":6.21021, "y":2.08157, "heading":0.03017, "vx":1.96385, "vy":-1.54528, "omega":0.70265, "ax":-0.06366, "ay":-0.08083, "alpha":7.29217, "fx":[-21.28426,-20.01384,19.03354,17.93302], "fy":[17.56421,-21.49038,-20.38954,18.81631]}, + {"t":0.99619, "x":6.25556, "y":2.04585, "heading":0.0464, "vx":1.96237, "vy":-1.54715, "omega":0.87112, "ax":-0.01775, "ay":-0.02251, "alpha":6.65312, "fx":[-18.9801,-17.30381,18.35697,16.71892], "fy":[16.61959,-19.04198,-17.40366,18.2943]}, + {"t":1.0193, "x":6.30089, "y":2.0101, "heading":0.06653, "vx":1.96196, "vy":-1.54767, "omega":1.02482, "ax":-0.00492, "ay":-0.00623, "alpha":6.06962, "fx":[-17.42565,-15.25515,17.25417,15.09214], "fy":[15.0655,-17.44369,-15.28184,17.23608]}, + {"t":1.0424, "x":6.34622, "y":1.97435, "heading":0.0902, "vx":1.96185, "vy":-1.54781, "omega":1.16503, "ax":-0.00136, "ay":-0.00172, "alpha":5.54152, "fx":[-16.17832,-13.49747,16.13113,13.45213], "fy":[13.44497,-16.18351,-13.50463,16.12595]}, + {"t":1.0655, "x":6.39154, "y":1.93859, "heading":0.11712, "vx":1.96182, "vy":-1.54785, "omega":1.29305, "ax":-0.0004, "ay":-0.0005, "alpha":5.067, "fx":[-15.10451,-11.92565,15.09075,11.91232], "fy":[11.91028,-15.10607,-11.92769,15.08919]}, + {"t":1.0886, "x":6.43686, "y":1.90283, "heading":0.14699, "vx":1.96181, "vy":-1.54786, "omega":1.41011, "ax":-0.0002, "ay":-0.00025, "alpha":4.64393, "fx":[-14.1606,-10.50903,14.15368,10.50227], "fy":[10.50127,-14.1614,-10.51003,14.15288]}, + {"t":1.1117, "x":6.48218, "y":1.86707, "heading":0.17956, "vx":1.96181, "vy":-1.54787, "omega":1.51739, "ax":-0.00039, "ay":-0.0005, "alpha":4.27007, "fx":[-13.33185,-9.23738,13.31839,9.22417], "fy":[9.22223,-13.33345,-9.23931,13.31678]}, + {"t":1.1348, "x":6.5275, "y":1.83131, "heading":0.21462, "vx":1.9618, "vy":-1.54788, "omega":1.61604, "ax":-0.00135, "ay":-0.00171, "alpha":3.94317, "fx":[-12.61929,-8.1104,12.57306,8.06481], "fy":[8.05821,-12.62494,-8.117,12.56741]}, + {"t":1.15791, "x":6.57282, "y":1.79555, "heading":0.25195, "vx":1.96177, "vy":-1.54792, "omega":1.70713, "ax":-0.00497, "ay":-0.0063, "alpha":3.66106, "fx":[-12.0521,-7.15132,11.8822,6.98316], "fy":[6.95914,-12.07319,-7.17538,11.86107]}, + {"t":1.18101, "x":6.61814, "y":1.75979, "heading":0.29139, "vx":1.96165, "vy":-1.54807, "omega":1.79171, "ax":-0.01843, "ay":-0.02335, "alpha":3.42168, "fx":[-11.75112,-6.4713,11.12182,5.84655], "fy":[5.75855,-11.83,-6.55972,11.04258]}, + {"t":1.20411, "x":6.66345, "y":1.72402, "heading":0.33278, "vx":1.96122, "vy":-1.54861, "omega":1.87076, "ax":-0.06852, "ay":-0.08668, "alpha":3.22244, "fx":[-12.17065,-6.51102,9.83414,4.18567], "fy":[3.86514,-12.46297,-6.83719,9.5373]}, + {"t":1.22721, "x":6.70874, "y":1.68823, "heading":0.376, "vx":1.95964, "vy":-1.55061, "omega":1.9452, "ax":-0.25502, "ay":-0.32103, "alpha":3.05188, "fx":[-14.98548,-8.92469,6.29859,0.2605], "fy":[-0.86892,-16.03578,-10.13062,5.19278]}, + {"t":1.25031, "x":6.75395, "y":1.65232, "heading":0.42094, "vx":1.95375, "vy":-1.55802, "omega":2.01571, "ax":-0.93462, "ay":-1.15558, "alpha":2.78874, "fx":[-25.94143,-19.43105,-5.85295,-12.36511], "fy":[-15.75751,-29.27922,-23.78515,-9.80238]}, + {"t":1.27342, "x":6.79883, "y":1.61602, "heading":0.4675, "vx":1.93216, "vy":-1.58472, "omega":2.08013, "ax":-2.81344, "ay":-3.29317, "alpha":1.79086, "fx":[-54.95722,-48.65421,-40.61798,-47.19415], "fy":[-52.61335,-61.65171,-59.6426,-50.15615]}, + {"t":1.29652, "x":6.84272, "y":1.57853, "heading":0.51556, "vx":1.86716, "vy":-1.6608, "omega":2.1215, "ax":-4.90706, "ay":-5.1098, "alpha":0.66622, "fx":[-86.34421,-82.69435,-80.51791,-84.31399], "fy":[-84.78503,-88.85289,-89.09872,-84.92801]}, + {"t":1.31962, "x":6.88454, "y":1.5388, "heading":0.56457, "vx":1.7538, "vy":-1.77884, "omega":2.13689, "ax":-5.22668, "ay":-4.80831, "alpha":0.57811, "fx":[-91.20398,-88.06692,-86.54908,-89.79722], "fy":[-79.98729,-83.71801,-83.6391,-79.80726]}, + {"t":1.34776, "x":6.93183, "y":1.48683, "heading":0.62471, "vx":1.60671, "vy":-1.91416, "omega":2.15316, "ax":-2.53545, "ay":-2.05021, "alpha":1.15932, "fx":[-47.41302,-43.28729,-38.78227,-43.02674], "fy":[-33.63742,-39.20504,-36.19333,-30.45842]}, + {"t":1.3759, "x":6.97604, "y":1.43215, "heading":0.6853, "vx":1.53536, "vy":-1.97186, "omega":2.18579, "ax":-0.54502, "ay":-0.421, "alpha":0.57481, "fx":[-11.43834,-9.47775,-7.09993,-9.06673], "fy":[-6.9323,-9.33245,-7.39353,-4.98617]}, + {"t":1.40404, "x":7.01903, "y":1.37649, "heading":0.74681, "vx":1.52002, "vy":-1.9837, "omega":2.20196, "ax":-0.1011, "ay":-0.07735, "alpha":-0.33372, "fx":[-0.45371,-1.67117,-2.98543,-1.76842], "fy":[-1.36498,-0.0497,-1.26675,-2.58164]}, + {"t":1.43219, "x":7.06177, "y":1.32064, "heading":0.80878, "vx":1.51718, "vy":-1.98588, "omega":2.19257, "ax":-0.01858, "ay":-0.01419, "alpha":-1.2542, "fx":[4.44422,-0.42765,-5.07557,-0.20496], "fy":[-0.13029,4.51878,-0.35285,-5.00104]}, + {"t":1.46033, "x":7.10446, "y":1.26475, "heading":0.87048, "vx":1.51665, "vy":-1.98628, "omega":2.15728, "ax":-0.00341, "ay":-0.0026, "alpha":-2.19442, "fx":[8.24256,-0.76608,-8.3581,0.64979], "fy":[0.66358,8.25617,-0.75229,-8.34449]}, + {"t":1.48847, "x":7.14714, "y":1.20885, "heading":0.93119, "vx":1.51656, "vy":-1.98635, "omega":2.09552, "ax":-0.00059, "ay":-0.00045, "alpha":-3.16876, "fx":[11.89161,-1.75777,-11.91168,1.7374], "fy":[1.73983,11.89394,-1.75534,-11.90934]}, + {"t":1.51661, "x":7.18982, "y":1.15295, "heading":0.99016, "vx":1.51654, "vy":-1.98637, "omega":2.00635, "ax":0.00008, "ay":0.00006, "alpha":-4.19039, "fx":[15.57661,-3.23321,-15.57389,3.23601], "fy":[3.23566,15.57629,-3.23356,-15.57421]}, + {"t":1.54475, "x":7.2325, "y":1.09705, "heading":1.04663, "vx":1.51654, "vy":-1.98636, "omega":1.88842, "ax":0.00105, "ay":0.0008, "alpha":-5.27056, "fx":[19.34684,-5.14916,-19.3118,5.18572], "fy":[5.18115,19.34292,-5.15373,-19.31571]}, + {"t":1.5729, "x":7.27517, "y":1.04115, "heading":1.09977, "vx":1.51657, "vy":-1.98634, "omega":1.7401, "ax":0.00565, "ay":0.00431, "alpha":-6.41776, "fx":[23.26219,-7.4344,-23.07605,7.63272], "fy":[7.60699,23.24244,-7.46009,-23.09583]}, + {"t":1.60104, "x":7.31786, "y":0.98525, "heading":1.14874, "vx":1.51673, "vy":-1.98622, "omega":1.55949, "ax":0.02916, "ay":0.02227, "alpha":-7.6367, "fx":[27.57135,-9.78547,-26.62386,10.82171], "fy":[10.68004,27.4786,-9.92562,-26.71757]}, + {"t":1.62918, "x":7.36055, "y":0.92936, "heading":1.19263, "vx":1.51755, "vy":-1.98559, "omega":1.34458, "ax":0.14716, "ay":0.11272, "alpha":-8.92159, "fx":[33.44325,-10.77035,-28.73958,16.07931], "fy":[15.29607,33.04736,-11.51044,-29.16393]}, + {"t":1.65732, "x":7.40332, "y":0.87353, "heading":1.23047, "vx":1.52169, "vy":-1.98242, "omega":1.09351, "ax":0.7181, "ay":0.55709, "alpha":-10.09477, "fx":[45.69354,-3.64163,-23.10693,29.91356], "fy":[25.49043,44.45635,-6.94418,-25.09909]}, + {"t":1.68546, "x":7.44642, "y":0.81796, "heading":1.26124, "vx":1.5419, "vy":-1.96674, "omega":0.80942, "ax":2.95688, "ay":2.42289, "alpha":-8.30461, "fx":[73.38965,33.57654,23.41885,70.79754], "fy":[51.40477,72.24105,31.3311,9.87378]}, + {"t":1.71361, "x":7.49099, "y":0.76357, "heading":1.28402, "vx":1.62512, "vy":-1.89856, "omega":0.57572, "ax":5.41342, "ay":5.03996, "alpha":-2.80298, "fx":[97.46431,81.16208,86.39985,103.29699], "fy":[85.26696,98.41139,87.24882,71.98597]}, + {"t":1.74175, "x":7.53886, "y":0.71214, "heading":1.30022, "vx":1.77746, "vy":-1.75672, "omega":0.49684, "ax":5.64011, "ay":6.27907, "alpha":-1.10305, "fx":[98.08977,90.56202,93.69565,101.3995], "fy":[105.63147,111.77142,108.24285,101.57509]}, + {"t":1.76989, "x":7.59112, "y":0.66519, "heading":1.3142, "vx":1.93618, "vy":-1.58002, "omega":0.46579, "ax":5.18646, "ay":7.03986, "alpha":-0.58793, "fx":[89.65079,85.07909,86.74153,91.40946], "fy":[118.87302,122.10456,120.70311,117.30351]}, + {"t":1.79803, "x":7.64766, "y":0.62351, "heading":1.32731, "vx":2.08214, "vy":-1.38191, "omega":0.44925, "ax":4.52357, "ay":7.62208, "alpha":-0.37439, "fx":[78.08967,74.85756,75.76999,79.06114], "fy":[129.02991,130.90857,130.30216,128.35713]}, + {"t":1.82617, "x":7.70805, "y":0.58764, "heading":1.33995, "vx":2.20944, "vy":-1.16741, "omega":0.43871, "ax":3.73014, "ay":8.10349, "alpha":-0.26565, "fx":[64.44678,61.95417,62.43092,64.96268], "fy":[137.4029,138.53762,138.28757,137.12396]}, + {"t":1.85431, "x":7.7717, "y":0.558, "heading":1.3523, "vx":2.31441, "vy":-0.93936, "omega":0.43124, "ax":1.35088, "ay":8.85191, "alpha":-0.19719, "fx":[24.07752,22.02024,21.86436,23.95008], "fy":[150.40819,150.72424,150.72995,150.41147]}, + {"t":1.88246, "x":7.83737, "y":0.53507, "heading":1.36443, "vx":2.35243, "vy":-0.69025, "omega":0.42569, "ax":-1.15111, "ay":8.90282, "alpha":-0.14676, "fx":[-18.59408,-20.05633,-20.57203,-19.09797], "fy":[151.56342,151.37969,151.30226,151.49246]}, + {"t":1.9106, "x":7.90311, "y":0.51917, "heading":1.37641, "vx":2.32004, "vy":-0.43971, "omega":0.42156, "ax":-2.42072, "ay":8.65793, "alpha":-0.11653, "fx":[-40.37132,-41.44114,-41.98255,-40.90802], "fy":[147.49399,147.19933,147.04114,147.34129]}, + {"t":1.93874, "x":7.96745, "y":0.51022, "heading":1.38828, "vx":2.25191, "vy":-0.19606, "omega":0.41828, "ax":-3.13943, "ay":8.43245, "alpha":-0.0979, "fx":[-52.72807,-53.57047,-54.07448,-53.23], "fy":[143.68376,143.37364,143.18124,143.49548]}, + {"t":1.96688, "x":8.02958, "y":0.50804, "heading":1.40005, "vx":2.16356, "vy":0.04125, "omega":0.41552, "ax":-3.5921, "ay":8.25547, "alpha":-0.08558, "fx":[-60.51915,-61.22157,-61.68261,-60.9792], "fy":[140.67586,140.37302,140.16886,140.47485]}, + {"t":1.99502, "x":8.08904, "y":0.51247, "heading":1.41174, "vx":2.06247, "vy":0.27357, "omega":0.41312, "ax":-3.90061, "ay":8.11831, "alpha":-0.0769, "fx":[-65.83172,-66.44185,-66.86497,-66.25434], "fy":[138.33804,138.04719,137.84108,138.13446]}, + {"t":2.02317, "x":8.14554, "y":0.52339, "heading":1.42337, "vx":1.9527, "vy":0.50204, "omega":0.41095, "ax":-4.12336, "ay":8.01051, "alpha":-0.07048, "fx":[-69.66864,-70.21438,-70.60569,-70.0597], "fy":[136.49727,136.21829,136.01455,136.29563]}, + {"t":2.05131, "x":8.19886, "y":0.54069, "heading":1.43493, "vx":1.83667, "vy":0.72747, "omega":0.40897, "ax":-4.29131, "ay":7.92415, "alpha":-0.06556, "fx":[-72.5622,-73.06096,-73.4257,-72.92681], "fy":[135.02103,134.75259,134.55311,134.82335]}, + {"t":2.07945, "x":8.24885, "y":0.5643, "heading":1.44644, "vx":1.7159, "vy":0.95047, "omega":0.40712, "ax":-4.43105, "ay":7.8477, "alpha":-0.06227, "fx":[-74.96436,-75.43143,-75.77737,-75.31026], "fy":[133.71646,133.45427,133.25715,133.52096]}, + {"t":2.1091, "x":8.29777, "y":0.59593, "heading":1.45851, "vx":1.58453, "vy":1.18314, "omega":0.40528, "ax":-4.60301, "ay":7.74566, "alpha":-0.06722, "fx":[-77.8622,-78.35621,-78.72969,-78.23572], "fy":[132.00889,131.71719,131.49308,131.78664]}, + {"t":2.13875, "x":8.34273, "y":0.63441, "heading":1.47053, "vx":1.44806, "vy":1.41278, "omega":0.40328, "ax":-4.82873, "ay":7.6037, "alpha":-0.07376, "fx":[-81.66693,-82.19218,-82.60358,-82.07857], "fy":[129.63388,129.30272,129.03872,129.37205]}, + {"t":2.16839, "x":8.38354, "y":0.67964, "heading":1.48249, "vx":1.30489, "vy":1.63822, "omega":0.4011, "ax":-5.13673, "ay":7.39443, "alpha":-0.0828, "fx":[-86.86141,-87.4217,-87.88697,-87.32735], "fy":[126.13284,125.74692,125.42005,125.80854]}, + {"t":2.19804, "x":8.41997, "y":0.73146, "heading":1.49438, "vx":1.1526, "vy":1.85745, "omega":0.39864, "ax":-5.57819, "ay":7.06013, "alpha":-0.09596, "fx":[-94.31364,-94.90822,-95.45231,-94.85941], "fy":[120.53999,120.07522,119.64008,120.10786]}, + {"t":2.22769, "x":8.45169, "y":0.78963, "heading":1.5062, "vx":0.98721, "vy":2.06678, "omega":0.3958, "ax":-6.24921, "ay":6.46122, "alpha":-0.11659, "fx":[-105.66317,-106.27049,-106.9292,-106.32605], "fy":[110.51479,109.93627,109.2908,109.87224]}, + {"t":2.25734, "x":8.47821, "y":0.85375, "heading":1.51793, "vx":0.80193, "vy":2.25834, "omega":0.39234, "ax":-7.31432, "ay":5.19814, "alpha":-0.15153, "fx":[-123.76425,-124.27772,-125.05969,-124.55663], "fy":[89.32801,88.62609,87.51036,88.21076]}, + {"t":2.28699, "x":8.49877, "y":0.92299, "heading":1.52956, "vx":0.58508, "vy":2.41246, "omega":0.38785, "ax":-8.6763, "ay":2.18168, "alpha":-0.20821, "fx":[-147.25098,-147.3865,-147.90503,-147.78273], "fy":[38.38541,37.93908,35.84771,36.26713]}, + {"t":2.31664, "x":8.5123, "y":0.99547, "heading":1.54106, "vx":0.32784, "vy":2.47714, "omega":0.38167, "ax":-8.87717, "ay":0.70917, "alpha":-0.28493, "fx":[-150.85083,-150.90743,-151.14169,-151.09266], "fy":[13.65353,13.47721,10.50132,10.61904]}, + {"t":2.34628, "x":8.51812, "y":1.06923, "heading":1.55238, "vx":0.06464, "vy":2.49816, "omega":0.37323, "ax":-8.82173, "ay":-0.23127, "alpha":-0.42632, "fx":[-150.06023,-150.1626,-150.05435,-149.94303], "fy":[-1.76489,-1.63291,-6.03801,-6.29967]}, + {"t":2.37593, "x":8.51616, "y":1.14319, "heading":1.56344, "vx":-0.1969, "vy":2.49131, "omega":0.36059, "ax":-8.55831, "ay":-1.11815, "alpha":-0.74419, "fx":[-145.82877,-146.25563,-145.36683,-144.84617], "fy":[-15.66757,-14.85452,-22.19282,-23.36257]}, + {"t":2.40558, "x":8.50656, "y":1.21656, "heading":1.57413, "vx":-0.45064, "vy":2.45816, "omega":0.33852, "ax":-7.79789, "ay":-1.81534, "alpha":-1.71242, "fx":[-133.05474,-135.43998,-132.53988,-129.52476], "fy":[-24.22532,-21.84504,-36.80173,-40.64138]}, + {"t":2.43523, "x":8.48977, "y":1.28865, "heading":1.58417, "vx":-0.68184, "vy":2.40433, "omega":0.28775, "ax":-5.04643, "ay":-1.60373, "alpha":-5.79529, "fx":[-78.46287,-99.7325,-94.14421,-71.0136], "fy":[-7.87138,-6.67308,-42.69538,-51.87582]}, + {"t":2.46488, "x":8.46734, "y":1.35922, "heading":1.5927, "vx":-0.83146, "vy":2.35679, "omega":0.11593, "ax":-1.20959, "ay":-0.43711, "alpha":-9.36759, "fx":[3.62624,-46.42636,-44.15944,4.66047], "fy":[19.5302,16.72147,-32.1547,-33.83746]}, + {"t":2.49453, "x":8.44216, "y":1.42891, "heading":1.59614, "vx":-0.86732, "vy":2.34383, "omega":-0.1618, "ax":0.18763, "ay":0.06917, "alpha":-8.08966, "fx":[24.29448,-19.02397,-17.99233,25.48779], "fy":[23.30797,22.47913,-21.2145,-19.86606]}, + {"t":2.52417, "x":8.41653, "y":1.49843, "heading":1.59134, "vx":-0.86176, "vy":2.34588, "omega":-0.40165, "ax":2.303, "ay":0.80829, "alpha":-5.63599, "fx":[52.31837,24.1112,25.77043,54.49327], "fy":[28.66209,30.8723,-2.72856,-1.81098]}, + {"t":2.55382, "x":8.39199, "y":1.56834, "heading":1.57943, "vx":-0.79348, "vy":2.36984, "omega":-0.56874, "ax":6.50903, "ay":1.89151, "alpha":-1.39218, "fx":[111.58688,107.55046,109.98704,113.74256], "fy":[36.45498,38.94488,27.5549,25.74176]}, + {"t":2.58347, "x":8.37132, "y":1.63943, "heading":1.56257, "vx":-0.60049, "vy":2.42592, "omega":-0.61002, "ax":8.19379, "ay":1.56661, "alpha":-0.26534, "fx":[139.30328,138.99837,139.4523,139.74222], "fy":[27.67852,28.16806,25.59671,25.14687]}, + {"t":2.61312, "x":8.35712, "y":1.71204, "heading":1.54449, "vx":-0.35756, "vy":2.47237, "omega":-0.61789, "ax":8.24888, "ay":0.88122, "alpha":-0.16602, "fx":[140.31435,140.15286,140.30959,140.46739], "fy":[15.6955,15.90557,14.27489,14.08145]}, + {"t":2.63809, "x":8.35077, "y":1.77405, "heading":1.52906, "vx":-0.1516, "vy":2.49437, "omega":-0.62203, "ax":7.23341, "ay":0.17823, "alpha":-0.24016, "fx":[123.25959,122.79551,122.81784,123.28026], "fy":[4.01673,4.15991,2.03259,1.91764]}, + {"t":2.66306, "x":8.34924, "y":1.83639, "heading":1.51353, "vx":0.02902, "vy":2.49882, "omega":-0.62803, "ax":4.21802, "ay":-0.13779, "alpha":-0.15334, "fx":[72.09249,71.45065,71.40196,72.04404], "fy":[-1.86685,-1.81867,-2.82303,-2.86619]}, + {"t":2.68803, "x":8.35127, "y":1.89874, "heading":1.49785, "vx":0.13434, "vy":2.49538, "omega":-0.63186, "ax":1.24216, "ay":-0.07459, "alpha":0.60005, "fx":[19.43363,22.58616,22.82304,19.6721], "fy":[-2.79463,-3.01709,0.24752,0.4889]}, + {"t":2.713, "x":8.35502, "y":1.96102, "heading":1.48207, "vx":0.16535, "vy":2.49352, "omega":-0.61687, "ax":0.29373, "ay":-0.01991, "alpha":1.48819, "fx":[0.66645,8.61759,9.32447,1.3763], "fy":[-3.97495,-4.66903,3.28413,4.0052]}, + {"t":2.73796, "x":8.35924, "y":2.02328, "heading":1.46667, "vx":0.17269, "vy":2.49302, "omega":-0.57972, "ax":0.06831, "ay":-0.00475, "alpha":2.37309, "fx":[-5.83611,6.83554,8.15872,-4.51072], "fy":[-5.75874,-7.07503,5.58904,6.92122]}, + {"t":2.76293, "x":8.36357, "y":2.08552, "heading":1.45219, "vx":0.17439, "vy":2.49291, "omega":-0.52046, "ax":0.01586, "ay":-0.00111, "alpha":3.27705, "fx":[-9.50623,7.96404,10.04523,-7.42385], "fy":[-7.71465,-9.79292,7.67336,9.75864]}, + {"t":2.7879, "x":8.36793, "y":2.14777, "heading":1.4392, "vx":0.17479, "vy":2.49288, "omega":-0.43864, "ax":0.00353, "ay":-0.00025, "alpha":4.20967, "fx":[-12.6253,9.77974,12.74512,-9.65941], "fy":[-9.72443,-12.68878,9.71473,12.68164]}, + {"t":2.81287, "x":8.37229, "y":2.21001, "heading":1.42824, "vx":0.17488, "vy":2.49287, "omega":-0.33353, "ax":0.0001, "ay":-0.00001, "alpha":5.17757, "fx":[-15.73023,11.78439,15.73347,-11.78113], "fy":[-11.7829,-15.73194,11.78262,15.73176]}, + {"t":2.83784, "x":8.37666, "y":2.27226, "heading":1.41991, "vx":0.17488, "vy":2.49287, "omega":-0.20425, "ax":-0.00302, "ay":0.00021, "alpha":6.18621, "fx":[-18.96428,13.86944,18.86201,-13.97281], "fy":[-13.91635,-18.91071,13.92591,18.91558]}, + {"t":2.86281, "x":8.38103, "y":2.3345, "heading":1.41482, "vx":0.1748, "vy":2.49288, "omega":-0.04978, "ax":-0.01323, "ay":0.00093, "alpha":7.23976, "fx":[-22.44016,15.95212,21.99366,-16.40554], "fy":[-16.15614,-22.20825,16.2017,22.22574]}, + {"t":2.88778, "x":8.38539, "y":2.39675, "heading":1.41357, "vx":0.17447, "vy":2.4929, "omega":0.13099, "ax":-0.05382, "ay":0.00375, "alpha":8.34048, "fx":[-26.52305,17.6812,24.71121,-19.53087], "fy":[-18.50604,-25.59282,18.70938,25.64473]}, + {"t":2.91275, "x":8.38973, "y":2.45899, "heading":1.41684, "vx":0.17313, "vy":2.49299, "omega":0.33924, "ax":-0.21456, "ay":0.01467, "alpha":9.48098, "fx":[-32.63865,17.5337,25.43486,-24.92815], "fy":[-20.8169,-29.01243,21.70659,29.12069]}, + {"t":2.93772, "x":8.39398, "y":2.52125, "heading":1.42531, "vx":0.16777, "vy":2.49336, "omega":0.57597, "ax":-0.8349, "ay":0.05266, "alpha":10.51935, "fx":[-45.76749,9.16769,17.79351,-37.99944], "fy":[-22.2673,-32.33344,25.94846,32.23533]}, + {"t":2.96269, "x":8.39791, "y":2.58352, "heading":1.43969, "vx":0.14692, "vy":2.49467, "omega":0.83863, "ax":-2.98105, "ay":0.13089, "alpha":9.75294, "fx":[-76.54673,-31.57872,-23.85595,-70.84585], "fy":[-20.11004,-33.29398,30.87642,31.43307]}, + {"t":2.98765, "x":8.40065, "y":2.64585, "heading":1.46063, "vx":0.07249, "vy":2.49794, "omega":1.08215, "ax":-6.50276, "ay":-0.07384, "alpha":4.52176, "fx":[-117.01697,-105.15206,-104.37615,-115.8952], "fy":[-15.47981,-23.62334,16.95785,17.12138]}, + {"t":3.01262, "x":8.40043, "y":2.7082, "heading":1.48765, "vx":-0.08988, "vy":2.4961, "omega":1.19506, "ax":-6.44305, "ay":-0.34177, "alpha":3.20658, "fx":[-113.8253,-105.1207,-105.53618,-113.89552], "fy":[-16.21132,-21.20219,6.5655,7.59464]}, + {"t":3.03187, "x":8.39751, "y":2.75618, "heading":1.51066, "vx":-0.2139, "vy":2.48952, "omega":1.25678, "ax":-4.06511, "ay":-0.41354, "alpha":5.63207, "fx":[-81.29113,-57.23484,-56.9147,-81.14483], "fy":[-21.96913,-28.32705,10.97564,11.18371]}, + {"t":3.05112, "x":8.39264, "y":2.80402, "heading":1.53485, "vx":-0.29214, "vy":2.48156, "omega":1.36519, "ax":-1.80397, "ay":-0.22514, "alpha":6.73742, "fx":[-48.36473,-13.92537,-12.73928,-47.71073], "fy":[-20.92063,-24.32837,14.99867,14.93196]}, + {"t":3.07037, "x":8.38668, "y":2.85175, "heading":1.56113, "vx":-0.32687, "vy":2.47723, "omega":1.49487, "ax":-0.68894, "ay":-0.09278, "alpha":6.1198, "fx":[-28.14378,4.43851,4.78614,-27.95545], "fy":[-17.64821,-18.55307,15.02968,14.85929]}, + {"t":3.08962, "x":8.38026, "y":2.89941, "heading":1.5899, "vx":-0.34013, "vy":2.47544, "omega":1.61267, "ax":-0.25388, "ay":-0.03514, "alpha":5.10475, "fx":[-17.73652,9.62865,9.11342,-18.27911], "fy":[-14.49879,-14.11889,13.4417,12.78521]}, + {"t":3.10887, "x":8.37367, "y":2.94706, "heading":1.62094, "vx":-0.34502, "vy":2.47476, "omega":1.71093, "ax":-0.09243, "ay":-0.01292, "alpha":4.05222, "fx":[-11.88879,9.83528,8.74552,-12.98101], "fy":[-11.61378,-10.55566,11.20607,10.08425]}, + {"t":3.12812, "x":8.36701, "y":2.99469, "heading":1.65388, "vx":-0.3468, "vy":2.47452, "omega":1.78893, "ax":-0.03342, "ay":-0.00469, "alpha":3.00961, "fx":[-7.94884,8.15289,6.81186,-9.28953], "fy":[-8.79796,-7.46354,8.64482,7.2977]}, + {"t":3.14736, "x":8.36033, "y":3.04232, "heading":1.68831, "vx":-0.34744, "vy":2.47443, "omega":1.84686, "ax":-0.01204, "ay":-0.00169, "alpha":1.97948, "fx":[-4.85874,5.69519,4.44912,-6.10462], "fy":[-5.92819,-4.68322,5.87165,4.6247]}, + {"t":3.16661, "x":8.35364, "y":3.08995, "heading":1.72386, "vx":-0.34767, "vy":2.47439, "omega":1.88496, "ax":-0.00438, "ay":-0.00062, "alpha":0.95847, "fx":[-2.22497,2.86059,2.07603,-3.0095], "fy":[-2.94547,-2.16101,2.92462,2.13999]}, + {"t":3.18586, "x":8.34695, "y":3.13758, "heading":1.76014, "vx":-0.34776, "vy":2.47438, "omega":1.90341, "ax":-0.00174, "ay":-0.00025, "alpha":-0.05792, "fx":[0.09377,-0.21163,-0.1531,0.15229], "fy":[0.17779,0.11926,-0.18613,-0.1276]}, + {"t":3.20511, "x":8.34025, "y":3.18521, "heading":1.79678, "vx":-0.34779, "vy":2.47438, "omega":1.9023, "ax":-0.00112, "ay":-0.00016, "alpha":-1.07452, "fx":[2.1457,-3.47633,-2.18377,3.43828], "fy":[3.45465,2.16205,-3.45997,-2.16742]}, + {"t":3.22436, "x":8.33356, "y":3.23283, "heading":1.8334, "vx":-0.34781, "vy":2.47437, "omega":1.88161, "ax":-0.00178, "ay":-0.00025, "alpha":-2.09615, "fx":[3.94294,-6.92472,-4.00344,6.86434], "fy":[6.89036,3.96887,-6.89869,-3.97751]}, + {"t":3.24361, "x":8.32686, "y":3.28046, "heading":1.86962, "vx":-0.34784, "vy":2.47437, "omega":1.84127, "ax":-0.00446, "ay":-0.00063, "alpha":-3.12736, "fx":[5.47544,-10.56982,-5.62746,10.4186], "fy":[10.48398,5.54039,-10.50445,-5.56251]}, + {"t":3.26286, "x":8.32017, "y":3.32809, "heading":1.90506, "vx":-0.34793, "vy":2.47436, "omega":1.78107, "ax":-0.01215, "ay":-0.00171, "alpha":-4.17211, "fx":[6.69742,-14.45918,-7.11308,14.048], "fy":[14.22649,6.87431,-14.28078,-6.93627]}, + {"t":3.2821, "x":8.31347, "y":3.37572, "heading":1.93934, "vx":-0.34816, "vy":2.47432, "omega":1.70076, "ax":-0.03331, "ay":-0.00469, "alpha":-5.23344, "fx":[7.47181,-18.72692,-8.61573,17.60455], "fy":[18.09429,7.95643,-18.23801,-8.13184]}, + {"t":3.30135, "x":8.30676, "y":3.42334, "heading":1.97208, "vx":-0.34881, "vy":2.47423, "omega":1.60002, "ax":-0.09065, "ay":-0.01281, "alpha":-6.31257, "fx":[7.41336,-23.73431,-10.54384,20.6971], "fy":[22.03146,8.73397,-22.40745,-9.22964]}, + {"t":3.3206, "x":8.30003, "y":3.47097, "heading":2.00288, "vx":-0.35055, "vy":2.47399, "omega":1.47852, "ax":-0.24411, "ay":-0.03483, "alpha":-7.40305, "fx":[5.46568,-30.41109,-13.95585,22.29197], "fy":[25.89667,9.03968,-26.87041,-10.43549]}, + {"t":3.33985, "x":8.29323, "y":3.51858, "heading":2.03134, "vx":-0.35525, "vy":2.47332, "omega":1.33602, "ax":-0.64755, "ay":-0.09468, "alpha":-8.4473, "fx":[-1.22229,-40.94104,-21.47938,19.58438], "fy":[29.25655,8.38267,-31.77949,-12.30176]}, + {"t":3.3591, "x":8.28628, "y":3.56617, "heading":2.05705, "vx":-0.36771, "vy":2.47149, "omega":1.17342, "ax":-1.66164, "ay":-0.25831, "alpha":-9.05268, "fx":[-19.8495,-59.23423,-38.61923,4.64701], "fy":[30.36606,5.46825,-37.10005,-16.30922]}, + {"t":3.37835, "x":8.27889, "y":3.6137, "heading":2.07964, "vx":-0.3997, "vy":2.46652, "omega":0.99917, "ax":-3.76239, "ay":-0.71845, "alpha":-7.67199, "fx":[-60.41804,-86.87161,-69.87077,-38.8286], "fy":[21.73598,-2.80119,-42.14627,-25.67123]}, + {"t":3.3976, "x":8.2705, "y":3.66104, "heading":2.09887, "vx":-0.47212, "vy":2.45269, "omega":0.85149, "ax":-3.66291, "ay":-0.72309, "alpha":-8.33777, "fx":[-58.95267,-87.42574,-68.44138,-34.40061], "fy":[24.55527,-2.85616,-44.69754,-26.20012]}, + {"t":3.42232, "x":8.25771, "y":3.72145, "heading":2.11992, "vx":-0.56266, "vy":2.43482, "omega":0.64539, "ax":-1.09315, "ay":-0.25899, "alpha":-9.36168, "fx":[-11.41997,-52.00934,-27.28404,16.33645], "fy":[31.20411,3.62437,-38.654,-13.79591]}, + {"t":3.44703, "x":8.24347, "y":3.78156, "heading":2.13587, "vx":-0.58968, "vy":2.42842, "omega":0.41398, "ax":-0.28407, "ay":-0.06941, "alpha":-7.70707, "fx":[1.40096,-33.22041,-11.34503,23.83683], "fy":[27.50526,5.12062,-29.63901,-7.70963]}, + {"t":3.47175, "x":8.2288, "y":3.84156, "heading":2.14611, "vx":-0.59671, "vy":2.4267, "omega":0.22346, "ax":-0.07119, "ay":-0.01753, "alpha":-5.77881, "fx":[3.34302,-22.645,-5.80505,20.26331], "fy":[21.17371,4.26209,-21.73886,-4.88982]}, + {"t":3.49647, "x":8.21403, "y":3.90154, "heading":2.15163, "vx":-0.59847, "vy":2.42627, "omega":0.08062, "ax":-0.01744, "ay":-0.0043, "alpha":-3.819, "fx":[2.6463,-14.48978,-3.24377,13.90097], "fy":[14.12393,2.87028,-14.26699,-3.01994]}, + {"t":3.52119, "x":8.19923, "y":3.96152, "heading":2.15362, "vx":-0.5989, "vy":2.42616, "omega":-0.01379, "ax":-0.00421, "ay":-0.00104, "alpha":-1.84788, "fx":[1.33954,-6.94305,-1.48312,6.79996], "fy":[6.85391,1.39355,-6.88911,-1.42912]}, + {"t":3.54591, "x":8.18443, "y":4.02149, "heading":2.15328, "vx":-0.599, "vy":2.42614, "omega":-0.05946, "ax":-0.00108, "ay":-0.00027, "alpha":0.1281, "fx":[-0.1163,0.45802,0.0797,-0.49462], "fy":[-0.48083,-0.10252,0.4718,0.09348]}, + {"t":3.57063, "x":8.16962, "y":4.08146, "heading":2.15181, "vx":-0.59903, "vy":2.42613, "omega":-0.0563, "ax":-0.00056, "ay":-0.00014, "alpha":2.10375, "fx":[-1.63053,7.81049,1.6113,-7.82964], "fy":[-7.82242,-1.6233,7.81771,1.61853]}, + {"t":3.59535, "x":8.15481, "y":4.14143, "heading":2.15042, "vx":-0.59904, "vy":2.42613, "omega":-0.00429, "ax":-0.00142, "ay":-0.00035, "alpha":4.07387, "fx":[-3.18423,15.11507,3.13563,-15.16287], "fy":[-15.14477,-3.16604,15.13318,3.15383]}, + {"t":3.62007, "x":8.14001, "y":4.20141, "heading":2.15031, "vx":-0.59908, "vy":2.42612, "omega":0.09641, "ax":-0.00556, "ay":-0.00137, "alpha":6.03255, "fx":[-4.77781,22.32437,4.58534,-22.50996], "fy":[-22.4392,-4.70625,22.39515,4.65693]}, + {"t":3.64479, "x":8.1252, "y":4.26138, "heading":2.1527, "vx":-0.59921, "vy":2.42608, "omega":0.24553, "ax":-0.02206, "ay":-0.00545, "alpha":7.97278, "fx":[-6.50368,29.27847,5.72943,-30.00489], "fy":[-29.72558,-6.2186,29.55832,6.01503]}, + {"t":3.66951, "x":8.11038, "y":4.32135, "heading":2.15877, "vx":-0.59976, "vy":2.42595, "omega":0.44261, "ax":-0.08527, "ay":-0.02112, "alpha":9.88512, "fx":[-8.88018,35.41594,5.83611,-38.17322], "fy":[-37.10549,-7.77549,36.49329,6.95073]}, + {"t":3.69423, "x":8.09552, "y":4.38131, "heading":2.16971, "vx":-0.60187, "vy":2.42543, "omega":0.68696, "ax":-0.31871, "ay":-0.07964, "alpha":11.73729, "fx":[-14.02272,38.70849,2.41517,-48.78534], "fy":[-44.90376,-9.93145,42.74092,6.67577]}, + {"t":3.71895, "x":8.08055, "y":4.44124, "heading":2.18669, "vx":-0.60975, "vy":2.42346, "omega":0.9771, "ax":-1.13916, "ay":-0.29369, "alpha":13.19179, "fx":[-28.92658,31.18586,-13.26063,-66.50606], "fy":[-53.64782,-15.29467,46.09943,2.86055]}, + {"t":3.74366, "x":8.06513, "y":4.50105, "heading":2.21084, "vx":-0.6379, "vy":2.4162, "omega":1.30319, "ax":-3.60046, "ay":-1.02239, "alpha":11.28131, "fx":[-65.69189,-20.82645,-62.77303,-95.67979], "fy":[-61.02298,-32.70889,33.38819,-9.21881]}, + {"t":3.76838, "x":8.04826, "y":4.56047, "heading":2.24306, "vx":-0.7269, "vy":2.39092, "omega":1.58205, "ax":-6.60076, "ay":-2.25902, "alpha":4.92488, "fx":[-107.17407,-101.26409,-118.06869,-122.60118], "fy":[-62.68237,-50.83522,-9.84593,-30.3373]}, + {"t":3.7931, "x":8.02828, "y":4.61888, "heading":2.28216, "vx":-0.89007, "vy":2.33508, "omega":1.70379, "ax":-7.54386, "ay":-3.23337, "alpha":2.17549, "fx":[-123.77379,-124.31963,-133.02659,-132.15573], "fy":[-66.92725,-61.5482,-41.83154,-49.68766]}, + {"t":3.81782, "x":8.00397, "y":4.67561, "heading":2.32428, "vx":-1.07655, "vy":2.25516, "omega":1.75757, "ax":-7.61072, "ay":-4.04238, "alpha":1.20696, "fx":[-126.12935,-127.04999,-132.88389,-131.76163], "fy":[-75.34604,-72.66113,-61.74047,-65.29116]}, + {"t":3.84254, "x":7.97503, "y":4.73012, "heading":2.36772, "vx":-1.26468, "vy":2.15523, "omega":1.7874, "ax":-7.37658, "ay":-4.77485, "alpha":0.78152, "fx":[-122.92544,-123.65187,-128.091,-127.22575], "fy":[-85.27092,-83.83141,-76.97821,-78.7948]}, + {"t":3.86726, "x":7.94152, "y":4.78194, "heading":2.41191, "vx":-1.44702, "vy":2.0372, "omega":1.80672, "ax":-6.9968, "ay":-5.45268, "alpha":0.55893, "fx":[-116.96562,-117.48014,-121.11159,-120.49694], "fy":[-95.4296,-94.63423,-89.97221,-90.95835]}, + {"t":3.89198, "x":7.90361, "y":4.83063, "heading":2.45657, "vx":-1.61998, "vy":1.90242, "omega":1.82054, "ax":-6.52216, "ay":-6.08084, "alpha":0.42747, "fx":[-109.23616,-109.57512,-112.68171,-112.26757], "fy":[-105.28985,-104.85792,-101.52375,-102.06192]}, + {"t":3.9167, "x":7.86157, "y":4.8758, "heading":2.50157, "vx":-1.7812, "vy":1.7521, "omega":1.8311, "ax":-5.92525, "ay":-6.70184, "alpha":0.34278, "fx":[-99.33573,-99.51321,-102.26751,-102.03077], "fy":[-115.29727,-115.10093,-112.66465,-112.92288]}, + {"t":3.94142, "x":7.81573, "y":4.91706, "heading":2.54683, "vx":-1.92767, "vy":1.58644, "omega":1.83958, "ax":-5.49294, "ay":-7.03532, "alpha":0.31997, "fx":[-92.0314,-92.16224,-94.86312,-94.67667], "fy":[-120.78449,-120.63681,-118.52814,-118.72557]}, + {"t":3.96056, "x":7.77783, "y":4.94614, "heading":2.58205, "vx":-2.03281, "vy":1.45177, "omega":1.8457, "ax":-4.93326, "ay":-7.40839, "alpha":0.38513, "fx":[-82.20728,-82.22584,-85.66347,-85.55658], "fy":[-127.18353,-127.10359,-124.8146,-124.95688]}, + {"t":3.9797, "x":7.73801, "y":4.97257, "heading":2.61737, "vx":-2.12724, "vy":1.30997, "omega":1.85307, "ax":-4.39563, "ay":-7.69841, "alpha":0.47611, "fx":[-72.6588,-72.51842,-76.94954,-76.94702], "fy":[-132.2055,-132.17982,-129.64968,-129.75597]}, + {"t":3.99884, "x":7.69649, "y":4.99624, "heading":2.65285, "vx":-2.21138, "vy":1.16261, "omega":1.86219, "ax":-3.83971, "ay":-7.93123, "alpha":0.60885, "fx":[-62.64629,-62.24046,-68.09938,-68.26318], "fy":[-136.29058,-136.30953,-133.47208,-133.5597]}, + {"t":4.01798, "x":7.65346, "y":5.01704, "heading":2.68849, "vx":-2.28487, "vy":1.0108, "omega":1.87384, "ax":-3.266, "ay":-8.0943, "alpha":0.81317, "fx":[-52.08558,-51.21675,-59.24045,-59.67224], "fy":[-139.25377,-139.27977,-136.03783,-136.15587]}, + {"t":4.03712, "x":7.60913, "y":5.0349, "heading":2.72436, "vx":-2.34739, "vy":0.85586, "omega":1.88941, "ax":-2.6776, "ay":-8.16036, "alpha":1.1497, "fx":[-40.86102,-39.13536,-50.65952,-51.52526], "fy":[-140.70295,-140.60707,-136.80891,-137.10264]}, + {"t":4.05627, "x":7.5637, "y":5.04979, "heading":2.76052, "vx":-2.39864, "vy":0.69966, "omega":1.91141, "ax":-2.07899, "ay":-8.07022, "alpha":1.75307, "fx":[-28.74127,-25.30026,-42.92878,-44.48203], "fy":[-139.84102,-139.17574,-134.56778,-135.50408]}, + {"t":4.07541, "x":7.51741, "y":5.0617, "heading":2.79711, "vx":-2.43844, "vy":0.54519, "omega":1.94497, "ax":-1.47914, "ay":-7.68497, "alpha":2.9464, "fx":[-15.36772,-8.21164,-37.28352,-39.77619], "fy":[-135.02893,-132.12705,-126.22272,-129.49754]}, + {"t":4.09455, "x":7.47046, "y":5.07073, "heading":2.83434, "vx":-2.46675, "vy":0.39809, "omega":2.00137, "ax":-0.90434, "ay":-6.69746, "alpha":5.45523, "fx":[-0.72605,14.26846,-35.99441,-39.07797], "fy":[-123.60008,-112.31068,-103.81576,-115.96067]}, + {"t":4.11369, "x":7.42308, "y":5.07713, "heading":2.87265, "vx":-2.48406, "vy":0.26989, "omega":2.10579, "ax":-0.43885, "ay":-4.89213, "alpha":9.62128, "fx":[12.80026,37.58841,-38.6225,-41.62473], "fy":[-105.75664,-73.05662,-58.77382,-95.26809]}, + {"t":4.13283, "x":7.37545, "y":5.0814, "heading":2.91296, "vx":-2.49246, "vy":0.17625, "omega":2.28995, "ax":-0.20412, "ay":-3.58735, "alpha":12.04036, "fx":[20.32537,46.31716,-38.56944,-41.96138], "fy":[-92.77771,-43.1402,-26.39937,-81.76194]}, + {"t":4.15197, "x":7.32771, "y":5.08411, "heading":2.95679, "vx":-2.49637, "vy":0.10758, "omega":2.52042, "ax":-0.11346, "ay":-4.22784, "alpha":10.43962, "fx":[21.08981,42.70344,-35.63731,-35.87582], "fy":[-96.87004,-55.82219,-45.32421,-89.64124]}, + {"t":4.17111, "x":7.2799, "y":5.0854, "heading":3.00503, "vx":-2.49854, "vy":0.02665, "omega":2.72025, "ax":0.07914, "ay":-6.13166, "alpha":6.10291, "fx":[18.60239,31.47809,-22.34175,-22.35432], "fy":[-114.15342,-96.35386,-94.63363,-112.05009]}, + {"t":4.19025, "x":7.23209, "y":5.08478, "heading":3.0571, "vx":-2.49702, "vy":-0.09071, "omega":2.83706, "ax":0.48965, "ay":-7.49597, "alpha":3.00337, "fx":[18.86385,24.73654,-4.43673,-5.8483], "fy":[-129.75459,-124.19777,-125.60901,-130.45582]}, + {"t":4.2094, "x":7.18438, "y":5.08167, "heading":3.11141, "vx":-2.48765, "vy":-0.2342, "omega":2.89455, "ax":1.02375, "ay":-8.11919, "alpha":1.58839, "fx":[23.58534,26.48934,10.51839,9.06139], "fy":[-138.25203,-136.26471,-138.14495,-139.75887]}, + {"t":4.22854, "x":7.13695, "y":5.0757, "heading":-3.11637, "vx":-2.46805, "vy":-0.38961, "omega":2.92496, "ax":1.60286, "ay":-8.35827, "alpha":0.93338, "fx":[31.01992,32.6938,23.24601,22.09711], "fy":[-141.79911,-140.87347,-142.63661,-143.37765]}, + {"t":4.24768, "x":7.09001, "y":5.06671, "heading":-3.06039, "vx":-2.43737, "vy":-0.5496, "omega":2.94282, "ax":2.19437, "ay":-8.40247, "alpha":0.59751, "fx":[39.7318,40.81479,34.81136,33.94414], "fy":[-142.44838,-141.90128,-143.44712,-143.89754]}, + {"t":4.26682, "x":7.04375, "y":5.05466, "heading":-3.00406, "vx":-2.39537, "vy":-0.71043, "omega":2.95426, "ax":2.7844, "ay":-8.33193, "alpha":0.40808, "fx":[48.97512,49.73177,45.6993,45.04096], "fy":[-141.26121,-140.87726,-142.21327,-142.54303]}, + {"t":4.28596, "x":6.99841, "y":5.03953, "heading":-2.94751, "vx":-2.34207, "vy":-0.86992, "omega":2.96207, "ax":3.36576, "ay":-8.18224, "alpha":0.29274, "fx":[58.37226,58.92867,56.10488,55.59698], "fy":[-138.7586,-138.45789,-139.61264,-139.88115]}, + {"t":4.3051, "x":6.9542, "y":5.02138, "heading":-2.89081, "vx":-2.27765, "vy":-1.02653, "omega":2.96767, "ax":3.93396, "ay":-7.97077, "alpha":0.21813, "fx":[67.71722,68.14094,66.10109,65.70279], "fy":[-135.21028,-134.95852,-135.96093,-136.19259]}, + {"t":4.32424, "x":6.91132, "y":5.00027, "heading":-2.834, "vx":-2.20235, "vy":-1.17911, "omega":2.97185, "ax":4.48788, "ay":-7.70556, "alpha":0.16749, "fx":[76.92232,77.25333,75.74568,75.4286], "fy":[-130.74503,-130.52524,-131.40034,-131.60708]}, + {"t":4.34339, "x":6.86999, "y":4.97629, "heading":-2.77712, "vx":-2.11644, "vy":-1.3266, "omega":2.97506, "ax":5.06035, "ay":-7.36715, "alpha":0.13172, "fx":[86.50317,86.77083,85.64319,85.38327], "fy":[-125.0302,-124.82821,-125.60055,-125.79374]}, + {"t":4.36253, "x":6.8304, "y":4.94955, "heading":-2.72017, "vx":-2.01958, "vy":-1.46762, "omega":2.97758, "ax":5.57025, "ay":-7.02195, "alpha":0.10363, "fx":[95.06298,95.27388,94.43169,94.2251], "fy":[-119.19845,-119.02025,-119.68706,-119.85945]}, + {"t":4.39068, "x":6.77576, "y":4.90545, "heading":-2.63636, "vx":-1.86278, "vy":-1.66528, "omega":2.98049, "ax":6.27994, "ay":-6.36075, "alpha":0.09543, "fx":[107.0736,107.2635,106.56503,106.378], "fy":[-107.95347,-107.75129,-108.43831,-108.63525]}, + {"t":4.41882, "x":6.72581, "y":4.85606, "heading":-2.55246, "vx":-1.68601, "vy":-1.84433, "omega":2.98318, "ax":6.85607, "ay":-5.66917, "alpha":0.06996, "fx":[116.78298,116.91087,116.45586,116.32914], "fy":[-96.24448,-96.07287,-96.61903,-96.78774]}, + {"t":4.44697, "x":6.68107, "y":4.80189, "heading":-2.46849, "vx":-1.49302, "vy":-2.00391, "omega":2.98515, "ax":7.32012, "ay":-4.91144, "alpha":-0.00222, "fx":[124.50866,124.50518,124.51768,124.52116], "fy":[-83.54823,-83.55435,-83.53603,-83.5299]}, + {"t":4.47512, "x":6.64194, "y":4.74354, "heading":-2.38446, "vx":-1.28696, "vy":-2.14216, "omega":2.98509, "ax":7.60436, "ay":-4.0811, "alpha":-0.22395, "fx":[128.94461,128.68999,129.74894,130.00835], "fy":[-70.03684,-70.70874,-68.81436,-68.11371]}, + {"t":4.50327, "x":6.60873, "y":4.68162, "heading":-2.30043, "vx":-1.07291, "vy":-2.25704, "omega":2.97878, "ax":7.45503, "ay":-3.13688, "alpha":-1.07509, "fx":[124.87308,124.58879,128.71215,129.05785], "fy":[-56.20005,-59.44387,-50.80148,-46.98439]}, + {"t":4.53142, "x":6.58148, "y":4.61685, "heading":-2.21658, "vx":-0.86306, "vy":-2.34534, "omega":2.94852, "ax":5.63714, "ay":-1.8632, "alpha":-5.58507, "fx":[80.65391,93.21717,109.81197,99.86161], "fy":[-43.46552,-56.9708,-24.05237,-2.28159]}, + {"t":4.55957, "x":6.55942, "y":4.55009, "heading":-2.13358, "vx":-0.70438, "vy":-2.39779, "omega":2.79131, "ax":1.76479, "ay":-0.49874, "alpha":-15.41844, "fx":[-28.96369,44.16817,82.56556,22.30411], "fy":[-26.67426,-63.70876,3.39321,53.05632]}, + {"t":4.58772, "x":6.54029, "y":4.4824, "heading":-2.05501, "vx":-0.6547, "vy":-2.41183, "omega":2.35729, "ax":0.39333, "ay":-0.10581, "alpha":-19.37989, "fx":[-64.5236,29.39399,75.8454,-13.95386], "fy":[-24.98873,-70.91896,18.96362,69.74501]}, + {"t":4.61587, "x":6.52202, "y":4.41446, "heading":-1.98865, "vx":-0.64363, "vy":-2.41481, "omega":1.81177, "ax":0.09473, "ay":-0.02519, "alpha":-21.92921, "fx":[-76.33847,31.77998,79.02741,-28.02368], "fy":[-30.76348,-77.71047,29.08414,77.67563]}, + {"t":4.64402, "x":6.50394, "y":4.34648, "heading":-1.93765, "vx":-0.64096, "vy":-2.41551, "omega":1.19448, "ax":0.02885, "ay":-0.00765, "alpha":-24.05746, "fx":[-83.03474,37.68952,83.85211,-36.54389], "fy":[-37.40902,-83.41492,36.82917,83.47418]}, + {"t":4.67217, "x":6.48591, "y":4.27848, "heading":-1.90403, "vx":-0.64015, "vy":-2.41573, "omega":0.51728, "ax":0.02172, "ay":-0.00575, "alpha":-25.86759, "fx":[-88.02216,43.33511,88.63725,-42.47213], "fy":[-43.14656,-88.28477,42.66372,88.37612]}, + {"t":4.70032, "x":6.4679, "y":4.21048, "heading":-1.88947, "vx":-0.63954, "vy":-2.41589, "omega":-0.21086, "ax":0.04516, "ay":-0.01194, "alpha":-27.41334, "fx":[-92.30144,47.72587,93.56776,-45.91961], "fy":[-47.3722,-92.80219,46.28776,93.07417]}, + {"t":4.72846, "x":6.44991, "y":4.14247, "heading":-1.8954, "vx":-0.63827, "vy":-2.41623, "omega":-0.98252, "ax":0.11703, "ay":-0.03083, "alpha":-28.73782, "fx":[-96.10971,50.87056,99.29767,-46.09593], "fy":[-50.02086,-97.29274,47.05119,98.16499]}, + {"t":4.75661, "x":6.43199, "y":4.07444, "heading":-1.92306, "vx":-0.63497, "vy":-2.4171, "omega":-1.79147, "ax":0.29795, "ay":-0.07771, "alpha":-29.8555, "fx":[-98.96472,53.6935,106.62229,-41.07883], "fy":[-51.64467,-101.70198,43.86239,104.19673]}, + {"t":4.78476, "x":6.41424, "y":4.00637, "heading":-1.97349, "vx":-0.62658, "vy":-2.41928, "omega":-2.63187, "ax":0.72958, "ay":-0.18562, "alpha":-30.64882, "fx":[-99.01638,58.60459,116.08612,-26.03444], "fy":[-53.97434,-105.24112,35.21378,111.37213]}, + {"t":4.81291, "x":6.39689, "y":3.9382, "heading":-2.04757, "vx":-0.60605, "vy":-2.42451, "omega":-3.49461, "ax":1.74273, "ay":-0.41675, "alpha":-30.32622, "fx":[-89.8387,70.40189,126.8649,11.14494], "fy":[-60.8742,-105.14134,21.24138,116.41875]}, + {"t":4.84106, "x":6.38052, "y":3.86979, "heading":-2.14594, "vx":-0.55699, "vy":-2.43624, "omega":-4.34826, "ax":4.1609, "ay":-0.84715, "alpha":-24.37203, "fx":[-25.1871,95.63616,136.51955,76.13401], "fy":[-75.5887,-92.91813,9.3659,101.50207]}, + {"t":4.86921, "x":6.36649, "y":3.80087, "heading":-2.26834, "vx":-0.43987, "vy":-2.46009, "omega":-5.03431, "ax":7.49511, "ay":-1.01191, "alpha":-10.3299, "fx":[118.25137,120.12508,143.58209,128.00022], "fy":[-42.17192,-74.72741,-6.9481,54.99798]}, + {"t":4.89736, "x":6.35708, "y":3.73122, "heading":-2.41005, "vx":-0.22889, "vy":-2.48857, "omega":-5.32509, "ax":8.45397, "ay":-0.32894, "alpha":-5.39134, "fx":[144.08605,139.97249,148.0032,143.1368], "fy":[-4.80653,-44.573,-6.24837,33.24702]}, + {"t":4.92551, "x":6.35398, "y":3.66104, "heading":-2.55995, "vx":0.00909, "vy":-2.49783, "omega":-5.47685, "ax":8.50196, "ay":0.31359, "alpha":-3.01622, "fx":[143.81665,144.44286,146.55948,143.64466], "fy":[11.31734,-16.41058,0.81912,25.61057]}, + {"t":4.94791, "x":6.35632, "y":3.60516, "heading":-2.68266, "vx":0.19957, "vy":-2.4908, "omega":-5.54443, "ax":8.09418, "ay":0.94568, "alpha":-1.63173, "fx":[136.15733,138.29258,139.33051,136.93843], "fy":[21.09576,5.62303,11.72825,25.89587]}, + {"t":4.97032, "x":6.36282, "y":3.54959, "heading":-2.80689, "vx":0.38093, "vy":-2.46962, "omega":-5.58099, "ax":6.87333, "ay":1.28116, "alpha":5.83648, "fx":[127.40687,117.54561,105.56123,117.13954], "fy":[5.82554,46.90033,45.0003,-10.5578]}, + {"t":4.99272, "x":6.37308, "y":3.49457, "heading":-2.93193, "vx":0.53493, "vy":-2.44091, "omega":-5.45022, "ax":1.89477, "ay":0.4323, "alpha":28.38471, "fx":[115.71615,88.21965,-57.52935,-17.48861], "fy":[-41.26208,85.71956,87.59246,-102.6368]}, + {"t":5.01513, "x":6.38554, "y":3.43999, "heading":-3.05404, "vx":0.57738, "vy":-2.43123, "omega":-4.81425, "ax":-0.53674, "ay":-0.12624, "alpha":33.60891, "fx":[87.71941,72.67945,-106.71793,-90.20032], "fy":[-91.11664,102.6058,73.92681,-94.00533]}, + {"t":5.03753, "x":6.39835, "y":3.38549, "heading":3.12128, "vx":0.56535, "vy":-2.43405, "omega":-4.06123, "ax":-1.7725, "ay":-0.39698, "alpha":33.97004, "fx":[47.61847,64.42321,-119.70202,-112.9384], "fy":[-123.85562,109.64383,69.21552,-82.01346]}, + {"t":5.05994, "x":6.41057, "y":3.33085, "heading":3.03028, "vx":0.52564, "vy":-2.44295, "omega":-3.30012, "ax":-2.80557, "ay":-0.56658, "alpha":32.6716, "fx":[6.74672,53.8229,-128.10891,-123.34825], "fy":[-138.48766,112.23848,62.31795,-74.61845]}, + {"t":5.08234, "x":6.42164, "y":3.27598, "heading":2.95634, "vx":0.46278, "vy":-2.45564, "omega":-2.56811, "ax":-4.03901, "ay":-0.68292, "alpha":29.19879, "fx":[-27.38011,17.25224,-135.48457,-129.19704], "fy":[-140.45016,111.75321,52.09407,-69.86213]}, + {"t":5.10475, "x":6.431, "y":3.22079, "heading":2.8988, "vx":0.37229, "vy":-2.47094, "omega":-1.9139, "ax":-6.13804, "ay":-0.74846, "alpha":21.07186, "fx":[-60.94923,-81.22395,-138.89008,-136.56227], "fy":[-132.16781,90.50078,49.27394,-58.53139]}, + {"t":5.12716, "x":6.4378, "y":3.16524, "heading":2.85592, "vx":0.23476, "vy":-2.48771, "omega":-1.44178, "ax":-7.34514, "ay":-0.44761, "alpha":15.74753, "fx":[-96.21632,-120.02558,-139.65002,-143.86314], "fy":[-111.43857,70.4245,52.24428,-41.68536]}, + {"t":5.14956, "x":6.44121, "y":3.10939, "heading":2.82362, "vx":0.07019, "vy":-2.49774, "omega":-1.08895, "ax":-7.97844, "ay":0.06379, "alpha":12.27228, "fx":[-122.29019,-132.00098,-140.26724,-148.28572], "fy":[-84.72065,61.82616,53.96689,-26.73247]}, + {"t":5.17197, "x":6.44078, "y":3.05344, "heading":2.79922, "vx":-0.10857, "vy":-2.49631, "omega":-0.81398, "ax":-8.3087, "ay":0.67699, "alpha":9.79487, "fx":[-138.44023,-135.85868,-140.25639,-150.75895], "fy":[-57.67828,60.59158,56.22993,-13.08127]}, + {"t":5.19437, "x":6.43627, "y":2.99768, "heading":2.78098, "vx":-0.29473, "vy":-2.48114, "omega":-0.59453, "ax":-8.44463, "ay":1.33872, "alpha":8.00401, "fx":[-147.28332,-136.11629,-139.36913,-151.79443], "fy":[-32.57654,63.81723,59.85064,-0.00654]}, + {"t":5.21678, "x":6.42754, "y":2.94242, "heading":2.76766, "vx":-0.48393, "vy":-2.45115, "omega":-0.41519, "ax":-8.44505, "ay":2.02003, "alpha":6.68751, "fx":[-151.13969,-134.28381,-137.57131,-151.59703], "fy":[-9.79138,69.60709,64.8238,12.80113]}, + {"t":5.23918, "x":6.41458, "y":2.88801, "heading":2.75836, "vx":-0.67315, "vy":-2.40589, "omega":-0.26536, "ax":-8.34229, "ay":2.70467, "alpha":5.69969, "fx":[-151.50242,-130.96174,-134.88316,-150.25256], "fy":[10.89731,76.81958,70.87164,25.43434]}, + {"t":5.26159, "x":6.3974, "y":2.83479, "heading":2.75242, "vx":-0.86006, "vy":-2.34529, "omega":-0.13766, "ax":-8.15353, "ay":3.38477, "alpha":4.94295, "fx":[-149.25646,-126.39979,-131.30898,-147.79172], "fy":[29.84623,84.80674,77.713,37.9297]}, + {"t":5.28399, "x":6.37609, "y":2.78309, "heading":2.74933, "vx":-1.04274, "vy":-2.26946, "omega":-0.02691, "ax":-7.87995, "ay":4.07017, "alpha":4.35117, "fx":[-144.79981,-120.51388,-126.7008,-144.12797], "fy":[47.67573,93.42002,85.3073,50.52681]}, + {"t":5.3064, "x":6.35075, "y":2.73326, "heading":2.74873, "vx":-1.21929, "vy":-2.17826, "omega":0.07058, "ax":-7.49187, "ay":4.80192, "alpha":3.87647, "fx":[-137.82178,-112.62136,-120.48866,-138.80648], "fy":[65.56991,103.08803,94.07865,63.98027]}, + {"t":5.3288, "x":6.32155, "y":2.68566, "heading":2.75031, "vx":-1.38715, "vy":-2.07067, "omega":0.15744, "ax":-7.00812, "ay":5.5197, "alpha":3.48909, "fx":[-128.61325,-103.3208,-112.99757,-131.89292], "fy":[82.43383,112.6072,103.09972,77.4131]}, + {"t":5.35121, "x":6.28871, "y":2.64065, "heading":2.75384, "vx":-1.54417, "vy":-1.947, "omega":0.23561, "ax":-6.53585, "ay":6.09587, "alpha":3.17748, "fx":[-119.24082,-94.82781,-105.87478,-124.74852], "fy":[95.66683,119.98731,110.50735,88.59468]}, + {"t":5.37361, "x":6.25247, "y":2.59856, "heading":2.75912, "vx":-1.69061, "vy":-1.81042, "omega":0.3068, "ax":-6.12364, "ay":6.52699, "alpha":2.92919, "fx":[-110.8788,-87.81923,-99.75711,-118.19043], "fy":[105.37223,125.31033,116.13941,97.26675]}, + {"t":5.39602, "x":6.21306, "y":2.55964, "heading":2.76599, "vx":-1.82781, "vy":-1.66418, "omega":0.37243, "ax":-5.77733, "ay":6.84794, "alpha":2.73008, "fx":[-103.78067,-82.1808,-94.66695,-112.45439], "fy":[112.46541,129.15531,120.38633,103.91876]}, + {"t":5.41842, "x":6.17065, "y":2.52407, "heading":2.77433, "vx":-1.95725, "vy":-1.51075, "omega":0.4336, "ax":-5.49185, "ay":7.08838, "alpha":2.56824, "fx":[-97.89437,-77.69563,-90.50252,-107.56664], "fy":[117.69839,131.96183,123.59656,109.02847]}, + {"t":5.44083, "x":6.12542, "y":2.492, "heading":2.78405, "vx":-2.0803, "vy":-1.35194, "omega":0.49114, "ax":-5.26774, "ay":7.26383, "alpha":2.4345, "fx":[-93.23235,-74.31661,-87.26853,-103.59361], "fy":[121.48303,133.94145,125.94067,112.85747]}, + {"t":5.46324, "x":6.07749, "y":2.46353, "heading":2.79505, "vx":-2.19832, "vy":-1.18919, "omega":0.54569, "ax":-6.32195, "ay":6.37699, "alpha":2.1447, "fx":[-112.0173,-96.04471,-104.2837,-117.79267], "fy":[104.40925,119.30433,112.23617,97.9335]}, + {"t":5.48378, "x":6.03099, "y":2.44045, "heading":2.80626, "vx":-2.32821, "vy":-1.05817, "omega":0.58975, "ax":-6.31995, "ay":6.37908, "alpha":2.08808, "fx":[-111.72007,-96.28999,-104.43674,-117.55565], "fy":[104.69448,119.07943,112.06654,98.18496]}, + {"t":5.50433, "x":5.98182, "y":2.42005, "heading":2.81838, "vx":-2.45806, "vy":-0.92711, "omega":0.63265, "ax":-6.31771, "ay":6.38137, "alpha":2.02481, "fx":[-111.40048,-96.56837,-104.59782,-117.28275], "fy":[104.99811,118.82351,111.8855,98.47389]}, + {"t":5.52487, "x":5.92999, "y":2.40235, "heading":2.83138, "vx":-2.58786, "vy":-0.796, "omega":0.67425, "ax":-6.31516, "ay":6.38388, "alpha":1.95365, "fx":[-111.05786,-96.88666,-104.76573,-116.96577], "fy":[105.31986,118.52995,111.69338,98.80862]}, + {"t":5.54542, "x":5.87549, "y":2.38734, "heading":2.84523, "vx":-2.71761, "vy":-0.66484, "omega":0.71439, "ax":-6.31225, "ay":6.38664, "alpha":1.87298, "fx":[-110.69137,-97.25329,-104.939,-116.59429], "fy":[105.65938,118.19042,111.49059,99.19951]}, + {"t":5.56596, "x":5.81832, "y":2.37503, "heading":2.85991, "vx":-2.84729, "vy":-0.53363, "omega":0.75287, "ax":-6.30889, "ay":6.3897, "alpha":1.78072, "fx":[-110.29999,-97.67892,-105.11579,-116.15493], "fy":[106.01628,117.79422,111.2776,99.65964]}, + {"t":5.58651, "x":5.75849, "y":2.36542, "heading":2.87538, "vx":-2.97691, "vy":-0.40235, "omega":0.78946, "ax":-6.30498, "ay":6.39308, "alpha":1.67415, "fx":[-109.88243,-98.17721,-105.29378,-115.6301], "fy":[106.3901,117.32733,111.05495,100.20578]}, + {"t":5.60705, "x":5.696, "y":2.3585, "heading":2.8916, "vx":-3.10645, "vy":-0.271, "omega":0.82385, "ax":-6.30036, "ay":6.39685, "alpha":1.54956, "fx":[-109.43705,-98.76613,-105.46998,-114.99624], "fy":[106.78032,116.77097,110.82327,100.85988]}, + {"t":5.6276, "x":5.63084, "y":2.35428, "heading":2.90852, "vx":-3.23589, "vy":-0.13957, "omega":0.85569, "ax":-6.29483, "ay":6.40105, "alpha":1.40188, "fx":[-108.96161,-99.46981,-105.64041,-114.22087], "fy":[107.18633,116.09928,110.58326,101.6513]}, + {"t":5.64814, "x":5.56303, "y":2.35277, "heading":2.9261, "vx":-3.36522, "vy":-0.00806, "omega":0.88449, "ax":-6.28806, "ay":6.40574, "alpha":1.22394, "fx":[-108.45304,-100.32165,-105.79966,-113.25795], "fy":[107.60746,115.27549,110.33564,102.62029]}, + {"t":5.66869, "x":5.49256, "y":2.35395, "heading":2.94428, "vx":-3.49441, "vy":0.12355, "omega":0.90964, "ax":-6.27959, "ay":6.41094, "alpha":1.00523, "fx":[-107.907,-101.36924,-105.94009,-112.03982], "fy":[108.04294,114.24554,110.08111,103.82364]}, + {"t":5.68923, "x":5.41944, "y":2.35784, "heading":2.96297, "vx":-3.62343, "vy":0.25526, "omega":0.93029, "ax":-6.26867, "ay":6.41666, "alpha":0.72989, "fx":[-107.31704,-102.68302,-106.05044,-110.4628], "fy":[108.49187,112.92623,109.82002,105.34427]}, + {"t":5.70978, "x":5.34368, "y":2.36444, "heading":2.98208, "vx":-3.75222, "vy":0.3871, "omega":0.94529, "ax":-6.25404, "ay":6.42274, "alpha":0.37265, "fx":[-106.67344,-104.37182,-106.11306,-108.35954], "fy":[108.95308,111.18278,109.5518,107.30798]}, + {"t":5.73033, "x":5.26527, "y":2.37375, "heading":3.0015, "vx":-3.88072, "vy":0.51905, "omega":0.95294, "ax":-6.23341, "ay":6.4286, "alpha":-0.10897, "fx":[-105.96087,-106.61261,-106.09831,-105.4421], "fy":[109.4247,108.78186,109.27318,109.91453]}, + {"t":5.75087, "x":5.18422, "y":2.38577, "heading":3.02108, "vx":-4.00878, "vy":0.65113, "omega":0.9507, "ax":-6.20218, "ay":6.43246, "alpha":-0.79211, "fx":[-105.15437,-109.71142,-105.95114,-101.17252], "fy":[109.90273,105.28354,108.97297,113.49771]}, + {"t":5.77142, "x":5.10055, "y":2.40051, "heading":3.04061, "vx":-4.13621, "vy":0.78329, "omega":0.93443, "ax":-6.14993, "ay":6.42855, "alpha":-1.83091, "fx":[-104.21228,-114.23655,-105.55363,-94.43155], "fy":[110.37582,99.75838,108.61312,118.64361]}, + {"t":5.79196, "x":5.01427, "y":2.41796, "heading":3.05981, "vx":-4.26256, "vy":0.91537, "omega":0.89681, "ax":-6.04812, "ay":6.39541, "alpha":-3.57557, "fx":[-103.06742,-121.31441,-104.59037,-82.53511], "fy":[110.8024,89.90713,108.03843,126.3885]}, + {"t":5.81251, "x":4.92542, "y":2.43811, "heading":3.07823, "vx":-4.38682, "vy":1.04676, "omega":0.82335, "ax":-5.79623, "ay":6.22948, "alpha":-6.95523, "fx":[-101.67114,-132.99592,-101.78715,-57.91456], "fy":[110.96907,68.69996,106.21454,137.96292]}, + {"t":5.83305, "x":4.83407, "y":2.46093, "heading":3.09515, "vx":-4.50591, "vy":1.17475, "omega":0.68045, "ax":-5.01793, "ay":4.88457, "alpha":-15.44867, "fx":[-101.97273,-145.78682,-85.43725,-8.21708], "fy":[108.05559,16.30279,61.73345,146.24878]}, + {"t":5.8536, "x":4.74043, "y":2.4861, "heading":3.10913, "vx":-4.609, "vy":1.27511, "omega":0.36305, "ax":-6.02259, "ay":-1.09618, "alpha":-16.69491, "fx":[-132.45725,-123.92276,-64.75903,-88.63137], "fy":[29.51725,-59.1671,-106.26032,61.32701]}, + {"t":5.87414, "x":4.64447, "y":2.51207, "heading":3.11659, "vx":-4.73274, "vy":1.25258, "omega":0.02005, "ax":-2.16472, "ay":-8.09956, "alpha":-0.49049, "fx":[-39.67571,-38.59546,-34.03014,-34.98378], "fy":[-136.8619,-137.53067,-138.64802,-138.04424]}, + {"t":5.89469, "x":4.54677, "y":2.53609, "heading":3.117, "vx":-4.77722, "vy":1.08618, "omega":0.00997, "ax":-1.55273, "ay":-8.52987, "alpha":1.19487, "fx":[-19.74922,-21.0794,-33.52344,-31.29391], "fy":[-146.48495,-145.80608,-143.54425,-144.52763]}, + {"t":5.91523, "x":4.44829, "y":2.55661, "heading":3.11721, "vx":-4.80912, "vy":0.91093, "omega":0.03452, "ax":-1.35477, "ay":-8.65104, "alpha":0.38972, "fx":[-20.82702,-21.28146,-25.31176,-24.75692], "fy":[-147.5497,-147.36096,-146.73939,-146.95705]}, + {"t":5.9389, "x":4.33412, "y":2.57574, "heading":3.11802, "vx":-4.84117, "vy":0.70623, "omega":0.04374, "ax":-0.69336, "ay":-8.66364, "alpha":1.35773, "fx":[-4.75483,-4.8634,-19.46319,-18.09418], "fy":[-148.17171,-147.65086,-146.47679,-147.16496]}, + {"t":5.96256, "x":4.21938, "y":2.59003, "heading":3.11906, "vx":-4.85758, "vy":0.50123, "omega":0.07587, "ax":0.60384, "ay":-8.50421, "alpha":4.58471, "fx":[28.04219,38.40128,-14.05196,-11.30726], "fy":[-145.25113,-140.90924,-145.07536,-147.38088]}, + {"t":5.98622, "x":4.10461, "y":2.59951, "heading":3.12085, "vx":-4.84329, "vy":0.3, "omega":0.18435, "ax":2.51407, "ay":-7.64637, "alpha":10.08497, "fx":[67.17694,106.90195,-0.94322,-2.08092], "fy":[-132.33786,-98.65687,-141.53084,-147.72489]}, + {"t":6.00988, "x":3.99071, "y":2.60446, "heading":3.12522, "vx":-4.7838, "vy":0.11908, "omega":0.42298, "ax":4.02716, "ay":-6.24844, "alpha":14.02068, "fx":[94.80598,142.53378,31.06958,5.59378], "fy":[-115.3105,-36.72639,-125.22378,-147.8759]}, + {"t":6.03354, "x":3.87864, "y":2.60553, "heading":3.13523, "vx":-4.68851, "vy":-0.02877, "omega":0.75474, "ax":5.62586, "ay":-3.44679, "alpha":18.67549, "fx":[110.96452,148.94904,109.21794,13.6457], "fy":[-101.10154,0.78896,13.63915,-147.84249]}, + {"t":6.05721, "x":3.76928, "y":2.60389, "heading":-3.1301, "vx":-4.5554, "vy":-0.11033, "omega":1.19664, "ax":6.21126, "ay":-2.12237, "alpha":20.50642, "fx":[122.5591,149.15709,116.2375,34.65324], "fy":[-87.74696,16.63819,71.55409,-144.84928]}, + {"t":6.08087, "x":3.66323, "y":2.60068, "heading":-3.10179, "vx":-4.40843, "vy":-0.16055, "omega":1.68186, "ax":6.71733, "ay":-1.52842, "alpha":19.53864, "fx":[131.51866,148.23876,119.01271,58.26948], "fy":[-74.53455,27.58469,80.48182,-137.52375]}, + {"t":6.10453, "x":3.5608, "y":2.59645, "heading":-3.06199, "vx":-4.24948, "vy":-0.19672, "omega":2.14418, "ax":7.09191, "ay":-0.92421, "alpha":18.49036, "fx":[138.54719,146.40486,117.91957,79.65385], "fy":[-61.29071,37.99824,87.22816,-126.81768]}, + {"t":6.12819, "x":3.46223, "y":2.59154, "heading":-3.01126, "vx":-4.08167, "vy":-0.21858, "omega":2.58169, "ax":7.35924, "ay":-0.38245, "alpha":17.43132, "fx":[143.53746,144.11819,116.64581,96.41255], "fy":[-49.28385,47.00518,91.40906,-115.15205]}, + {"t":6.15185, "x":3.36771, "y":2.58626, "heading":-2.95017, "vx":-3.90754, "vy":-0.22763, "omega":2.99415, "ax":7.543, "ay":0.09324, "alpha":16.46701, "fx":[147.03999,141.5322,115.80319,108.84171], "fy":[-38.42469,54.97907,93.87766,-104.08778]}, + {"t":6.17551, "x":3.27736, "y":2.5809, "heading":-2.87932, "vx":-3.72906, "vy":-0.22543, "omega":3.38379, "ax":7.68128, "ay":0.49831, "alpha":15.51442, "fx":[149.47464,138.79034,116.18963,118.17116], "fy":[-28.42755,62.02672,94.28551,-93.98024]}, + {"t":6.19918, "x":3.19128, "y":2.57571, "heading":-2.79925, "vx":-3.54731, "vy":-0.21364, "omega":3.75089, "ax":8.44824, "ay":0.55463, "alpha":9.24947, "fx":[151.62086,142.42431,140.3755,140.38769], "fy":[-13.92238,53.46348,55.18776,-56.9928]}, + {"t":6.22284, "x":3.10971, "y":2.57081, "heading":-2.7105, "vx":-3.3474, "vy":-0.20051, "omega":3.96975, "ax":8.88562, "ay":0.57061, "alpha":2.73606, "fx":[152.28996,149.76712,150.75076,151.75974], "fy":[2.33015,27.30177,19.16051,-9.96845]}, + {"t":6.2465, "x":3.03299, "y":2.56622, "heading":-2.61657, "vx":-3.13715, "vy":-0.18701, "omega":4.03449, "ax":8.93188, "ay":0.61143, "alpha":-1.15968, "fx":[151.75761,152.37797,152.27227,151.30729], "fy":[13.37702,1.99584,7.73987,18.48839]}, + {"t":6.27016, "x":2.96126, "y":2.56197, "heading":-2.52111, "vx":-2.92581, "vy":-0.17254, "omega":4.00705, "ax":8.86684, "ay":0.63568, "alpha":-3.82359, "fx":[151.16608,151.45744,152.65084,148.0156], "fy":[19.13823,-18.31994,5.13579,37.29692]}, + {"t":6.29382, "x":2.89451, "y":2.55806, "heading":-2.42629, "vx":-2.716, "vy":-0.1575, "omega":3.91658, "ax":8.7748, "ay":0.65481, "alpha":-5.65141, "fx":[151.08677,149.17488,152.77001,143.99569], "fy":[19.89073,-32.70212,6.31193,51.0518]}, + {"t":6.31749, "x":2.8327, "y":2.55452, "heading":-2.33362, "vx":-2.50837, "vy":-0.14201, "omega":3.78285, "ax":8.68705, "ay":0.66179, "alpha":-6.9153, "fx":[151.57097,146.95119,152.72431,139.81065], "fy":[15.91979,-42.04383,9.32664,61.82492]}, + {"t":6.34115, "x":2.77578, "y":2.55135, "heading":-2.24411, "vx":-2.30282, "vy":-0.12635, "omega":3.61923, "ax":8.60879, "ay":0.6447, "alpha":-7.87159, "fx":[152.23223,145.33755,152.50917,135.65337], "fy":[7.62624,-47.65744,13.27246,70.62364]}, + {"t":6.36481, "x":2.7237, "y":2.54854, "heading":-2.15847, "vx":-2.09912, "vy":-0.11109, "omega":3.43297, "ax":8.53246, "ay":0.59915, "alpha":-8.73755, "fx":[152.39536,144.40566,152.12049,131.61729], "fy":[-4.21959,-50.6608,17.66283,77.98275]}, + {"t":6.38847, "x":2.67642, "y":2.54608, "heading":-2.07724, "vx":-1.89723, "vy":-0.09692, "omega":3.22622, "ax":8.44779, "ay":0.53264, "alpha":-9.65582, "fx":[151.39961,144.04395,151.56776,127.76674], "fy":[-18.304,-51.87387,22.20784,84.20999]}, + {"t":6.41213, "x":2.63389, "y":2.54393, "heading":-2.0009, "vx":-1.69734, "vy":-0.08431, "omega":2.99775, "ax":8.3496, "ay":0.46158, "alpha":-10.66963, "fx":[148.97117,144.0982,150.87295,124.15515], "fy":[-32.93277,-51.87762,26.718,89.49768]}, + {"t":6.4358, "x":2.59607, "y":2.54207, "heading":-1.92997, "vx":-1.49977, "vy":-0.07339, "omega":2.74528, "ax":8.24086, "ay":0.4029, "alpha":-11.73501, "fx":[145.37969,144.424,150.06782,120.82717], "fy":[-46.53427,-51.09019,31.05862,93.97854]}, + {"t":6.45946, "x":2.56289, "y":2.54044, "heading":-1.86501, "vx":-1.30478, "vy":-0.06386, "omega":2.46761, "ax":8.12982, "ay":0.3665, "alpha":-12.76983, "fx":[141.23411,144.90232,149.19118,117.81587], "fy":[-58.12419,-49.82321,35.12788,97.75548]}, + {"t":6.48312, "x":2.53429, "y":2.53903, "heading":-1.80662, "vx":-1.11241, "vy":-0.05519, "omega":2.16545, "ax":8.02488, "ay":0.35345, "alpha":-13.7037, "fx":[137.13697,145.44153,148.28608,115.13935], "fy":[-67.39918,-48.31635,38.84678,100.91732]}, + {"t":6.50678, "x":2.51022, "y":2.53783, "heading":-1.75538, "vx":-0.92253, "vy":-0.04682, "omega":1.8412, "ax":7.93148, "ay":0.35889, "alpha":-14.49992, "fx":[133.47808,145.97496,147.39682,112.79909], "fy":[-74.52566,-46.75718,42.15458,103.54646]}, + {"t":6.53044, "x":2.49061, "y":2.53682, "heading":-1.71182, "vx":-0.73485, "vy":-0.03833, "omega":1.4981, "ax":7.85171, "ay":0.37566, "alpha":-15.15249, "fx":[130.41802,146.45653,146.56617,110.78092], "fy":[-79.87448,-45.29304,45.0059,105.72114]}, + {"t":6.5541, "x":2.47542, "y":2.53602, "heading":-1.67637, "vx":-0.54907, "vy":-0.02944, "omega":1.13957, "ax":7.78541, "ay":0.39683, "alpha":-15.67393, "fx":[127.96265,146.85596,145.83312,109.05898], "fy":[-83.84327,-44.03888,47.36775,107.51442]}, + {"t":6.57777, "x":2.4646, "y":2.53543, "heading":-1.64941, "vx":-0.36485, "vy":-0.02005, "omega":0.76869, "ax":7.73133, "ay":0.41667, "alpha":-16.08404, "fx":[126.04339,147.15394,145.23146,107.60186], "fy":[-86.77433,-43.08337,49.21644,108.99099]}, + {"t":6.60143, "x":2.45814, "y":2.53507, "heading":-1.63122, "vx":-0.18191, "vy":-0.01019, "omega":0.38811, "ax":7.68795, "ay":0.43084, "alpha":-16.40252, "fx":[124.57247,147.33802,144.78885,106.38014], "fy":[-88.93004,-42.49361,50.53451,110.20308]}, + {"t":6.62509, "x":2.45598, "y":2.53495, "heading":-1.62203, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], "splits":[0] }, "events":[] diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index b7b8b30..cc25176 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -29,6 +29,7 @@ import frc.robot.generated.ChoreoTraj; import frc.robot.generated.ChoreoVars; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Utilities; @@ -42,8 +43,10 @@ private enum AutoMode DriveOnly("Drive Only"), ShootOnly("Shoot Only"), ShootWithDelay("Shoot + Delay"), - ShootThenDrive("Shoot Then Drive"), - ShootWithDelayThenDrive("Shoot + Delay + Drive"); + DriveThenShoot("Drive Then Shoot"), + ShootWithDelayThenDrive("Shoot + Delay + Drive"), + DrivePickupThenShoot("Drive + Pickup + Shoot"), + DriveWithDelayPickupThenShoot("Drive + Delay + Pickup + Shoot"); // @formatter:on public final String displayName; @@ -55,7 +58,7 @@ private AutoMode(String displayName) public boolean usesDrivePath() { - return this == DriveOnly || this == ShootThenDrive || this == ShootWithDelayThenDrive; + return this == DriveOnly || this == DriveThenShoot || this == ShootWithDelayThenDrive; } public boolean usesDelay() @@ -147,6 +150,8 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. @NotLogged private final Shooter _shooterSubsystem; @NotLogged + private final Intake _intakeSubsystem; + @NotLogged private final AutoFactory _autoFactory; @NotLogged private final AutoRoutine _trajectoryDrawRoutine; @@ -173,10 +178,11 @@ private StartPosition(String displayName, Pose2d blueStartPose, AutoDriveOption. @Logged private Pose2d _currentDesiredPathPose = new Pose2d(); - public Autos(Drive driveSubsystem, Shooter shooterSubsystem) + public Autos(Drive driveSubsystem, Shooter shooterSubsystem, Intake intakeSubsystem) { _driveSubsystem = driveSubsystem; _shooterSubsystem = shooterSubsystem; + _intakeSubsystem = intakeSubsystem; _autoFactory = new AutoFactory(() -> driveSubsystem.getState().Pose, driveSubsystem::resetPose, this::followTrajectory, true, driveSubsystem); _trajectoryDrawRoutine = _autoFactory.newRoutine("Trajectory Drawing"); @@ -213,19 +219,24 @@ public Command buildAuto() var driveOption = _driveChooser.getSelected(); var startPose = getStartPose(mode, startPosition, driveOption); var resetPose = Commands.runOnce(() -> _driveSubsystem.resetPose(startPose)); - var shoot = _shooterSubsystem.shoot().withTimeout(4.0); + var shoot = _shooterSubsystem.shoot(); var delay = Commands.waitSeconds(delaySeconds); - var drive = driveOption.staysPut() ? Commands.none() : _autoFactory.trajectoryCmd(driveOption.trajectory().name()); - var stop = driveOption.staysPut() ? Commands.none() : _driveSubsystem.runOnce(() -> _driveSubsystem.setControl(_autoRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + var extend = _intakeSubsystem.getExtendCmd(); + var pickup = _intakeSubsystem.runRollersForward(); + var jiggle = _intakeSubsystem.jiggle(); + var drive = driveOption.staysPut() ? Commands.none() + : _autoFactory.trajectoryCmd(driveOption.trajectory().name()).andThen(_driveSubsystem.runOnce(() -> _driveSubsystem.setControl(_autoRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0)))); return switch (mode) { case DoNothing -> resetPose; - case DriveOnly -> Commands.sequence(resetPose, drive, stop); + case DriveOnly -> Commands.sequence(resetPose, drive); case ShootOnly -> Commands.sequence(resetPose, shoot); case ShootWithDelay -> Commands.sequence(resetPose, delay, shoot); - case ShootThenDrive -> Commands.sequence(resetPose, shoot, drive, stop); - case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive, stop); + case DriveThenShoot -> Commands.sequence(resetPose, drive, shoot); + case ShootWithDelayThenDrive -> Commands.sequence(resetPose, delay, shoot, drive); + case DrivePickupThenShoot -> Commands.sequence(resetPose, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle)); + case DriveWithDelayPickupThenShoot -> Commands.sequence(resetPose, delay, extend, Commands.deadline(drive, pickup), shoot.alongWith(jiggle)); }; } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2458e5d..a403326 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -104,7 +104,7 @@ public static class AutoConstants // Auto driving public static final double DRIVE_KP = 3.0; public static final double DRIVE_KD = 0.1; - public static final double ROTATE_KP = 0.0; + public static final double ROTATE_KP = 2.0; public static final double ROTATE_KD = 0.0; } @@ -263,7 +263,7 @@ public static class ShooterConstants // Feeder public static final Current FEEDER_CURRENT_LIMIT = Amps.of(60); public static final Voltage FEEDER_VOLTAGE = Volts.of(6); - public static final Current ROTOR_CURRENT_LIMIT = Amps.of(60); + public static final Current ROTOR_CURRENT_LIMIT = Amps.of(80); public static final Voltage ROTOR_FAST_VOLTAGE = Volts.of(3.0); public static final Voltage ROTOR_MID_VOLTAGE = Volts.of(1.5); public static final Voltage ROTOR_RETRACTED_VOLTAGE = Volts.of(1.0); @@ -291,7 +291,7 @@ public static class VisionConstants public static final String LEFT_CAMERA_NAME = "limelight-left"; public static final String RIGHT_CAMERA_NAME = "limelight-right"; public static final Distance MAX_DETECTION_RANGE = Meters.of(6.0); - public static final Distance XY_STD_DEV = Meters.of(0.7); + public static final Distance XY_STD_DEV = Meters.of(4); public static final Angle THETA_STD_DEV = Degrees.of(999999); // Trust gyro for heading, not vision public static final Matrix STD_DEVS = VecBuilder.fill(XY_STD_DEV.in(Meters), XY_STD_DEV.in(Meters), THETA_STD_DEV.in(Degrees)); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 248a277..2d06b51 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,7 +37,7 @@ public class RobotContainer private final Drive _drive = TunerConstants.createDrivetrain(); private final Intake _intake = new Intake(); private final Shooter _shooter = new Shooter(_drive::getState, _intake::isExtended, _intake::isRetracted); - private final Autos _autos = new Autos(_drive, _shooter); + private final Autos _autos = new Autos(_drive, _shooter, _intake); private Dimensionless _driveMultiplier = DriveConstants.FULL_SPEED_SCALE; private double _manualFlywheelRPM = MANUAL_FLYWHEEL_START_RPM; diff --git a/src/main/java/frc/robot/generated/ChoreoTraj.java b/src/main/java/frc/robot/generated/ChoreoTraj.java index 4e01eba..cfb27de 100644 --- a/src/main/java/frc/robot/generated/ChoreoTraj.java +++ b/src/main/java/frc/robot/generated/ChoreoTraj.java @@ -1,3 +1,4 @@ + package frc.robot.generated; import edu.wpi.first.math.geometry.Pose2d; @@ -11,68 +12,165 @@ import choreo.auto.AutoTrajectory; /** - * A class containing the name, start pose, end pose, and total time of every - * Choreo trajectory. This prevents your code from referencing deleted or - * misspelled trajectories, and removes the need for JSON parsing to load a - * trajectory's essential data. DO NOT MODIFY THIS FILE YOURSELF! It is - * automatically generated by Choreo. + * A class containing the name, start pose, end pose, and total time of every Choreo trajectory. + * This prevents your code from referencing deleted or misspelled trajectories, + * and removes the need for JSON parsing to load a trajectory's essential data. + * DO NOT MODIFY THIS FILE YOURSELF! It is automatically generated by Choreo. */ -public record ChoreoTraj(String name, OptionalInt segment, double totalTimeSecs, Pose2d initialPoseBlue, Pose2d endPoseBlue) -{ +public record ChoreoTraj( + String name, + OptionalInt segment, + double totalTimeSecs, + Pose2d initialPoseBlue, + Pose2d endPoseBlue +) { + public static final ChoreoTraj DepotToOutpost = new ChoreoTraj( + "DepotToOutpost", + OptionalInt.empty(), + 1.70772, + new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), + new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj DepotToTower = new ChoreoTraj( + "DepotToTower", + OptionalInt.empty(), + 1.1836, + new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), + new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)) + ); + public static final ChoreoTraj HubToDepot = new ChoreoTraj( + "HubToDepot", + OptionalInt.empty(), + 1.25742, + new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), + new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj HubToOutpost = new ChoreoTraj( + "HubToOutpost", + OptionalInt.empty(), + 1.67965, + new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), + new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj HubToTower = new ChoreoTraj( + "HubToTower", + OptionalInt.empty(), + 1.10566, + new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), + new Pose2d(1.851, 4.056, Rotation2d.fromRadians(-1.555)) + ); + public static final ChoreoTraj LeftBumpPickupFromMidScore = new ChoreoTraj( + "LeftBumpPickupFromMidScore", + OptionalInt.empty(), + 6.27732, + new Pose2d(3.652, 5.514, Rotation2d.fromRadians(0)), + new Pose2d(1.974, 5.587, Rotation2d.fromRadians(-2.437)) + ); + public static final ChoreoTraj LeftBumpToDepot = new ChoreoTraj( + "LeftBumpToDepot", + OptionalInt.empty(), + 1.11437, + new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), + new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj LeftBumpToTower = new ChoreoTraj( + "LeftBumpToTower", + OptionalInt.empty(), + 1.71082, + new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)), + new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)) + ); + public static final ChoreoTraj LeftTrenchToDepot = new ChoreoTraj( + "LeftTrenchToDepot", + OptionalInt.empty(), + 1.66476, + new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)), + new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj OutpostToDepot = new ChoreoTraj( + "OutpostToDepot", + OptionalInt.empty(), + 1.6899, + new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), + new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj OutpostToTower = new ChoreoTraj( + "OutpostToTower", + OptionalInt.empty(), + 0.97514, + new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), + new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)) + ); + public static final ChoreoTraj RightBumpPickupFromMidScore = new ChoreoTraj( + "RightBumpPickupFromMidScore", + OptionalInt.empty(), + 6.62509, + new Pose2d(3.625, 2.535, Rotation2d.fromRadians(0)), + new Pose2d(2.456, 2.535, Rotation2d.fromRadians(-1.622)) + ); + public static final ChoreoTraj RightBumpToOutpost = new ChoreoTraj( + "RightBumpToOutpost", + OptionalInt.empty(), + 1.32723, + new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), + new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) + ); + public static final ChoreoTraj RightBumpToTower = new ChoreoTraj( + "RightBumpToTower", + OptionalInt.empty(), + 0.76635, + new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), + new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)) + ); + public static final ChoreoTraj RightTrenchToOutpost = new ChoreoTraj( + "RightTrenchToOutpost", + OptionalInt.empty(), + 1.61166, + new Pose2d(3.616, 0.62, Rotation2d.fromRadians(0)), + new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) + ); - public static final ChoreoTraj DepotToOutpost = new ChoreoTraj("DepotToOutpost", OptionalInt.empty(), 1.70772, new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj DepotToTower = new ChoreoTraj("DepotToTower", OptionalInt.empty(), 1.1836, new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); - public static final ChoreoTraj HubToDepot = new ChoreoTraj("HubToDepot", OptionalInt.empty(), 1.25742, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj HubToOutpost = new ChoreoTraj("HubToOutpost", OptionalInt.empty(), 1.67965, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj HubToTower = new ChoreoTraj("HubToTower", OptionalInt.empty(), 1.74265, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); - public static final ChoreoTraj LeftBumpPickupFromMidScore = new ChoreoTraj( - "LeftBumpPickupFromMidScore", OptionalInt.empty(), 6.25788, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), new Pose2d(3.218, 5.674, Rotation2d.fromRadians(3.14)) - ); - public static final ChoreoTraj LeftBumpToDepot = new ChoreoTraj("LeftBumpToDepot", OptionalInt.empty(), 1.11437, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj LeftBumpToTower = new ChoreoTraj("LeftBumpToTower", OptionalInt.empty(), 1.71082, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); - public static final ChoreoTraj LeftTrenchToDepot = new ChoreoTraj("LeftTrenchToDepot", OptionalInt.empty(), 1.66476, new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj OutpostToDepot = new ChoreoTraj("OutpostToDepot", OptionalInt.empty(), 1.6899, new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj OutpostToTower = new ChoreoTraj("OutpostToTower", OptionalInt.empty(), 0.97514, new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571))); - public static final ChoreoTraj RightBumpPickupFromMidScore = new ChoreoTraj( - "RightBumpPickupFromMidScore", OptionalInt.empty(), 4.17676, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)) - ); - public static final ChoreoTraj RightBumpToOutpost = new ChoreoTraj("RightBumpToOutpost", OptionalInt.empty(), 1.32723, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); - public static final ChoreoTraj RightBumpToTower = new ChoreoTraj("RightBumpToTower", OptionalInt.empty(), 0.76635, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571))); - public static final ChoreoTraj RightTrenchToOutpost = new ChoreoTraj("RightTrenchToOutpost", OptionalInt.empty(), 1.61166, new Pose2d(3.616, 0.62, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); /** - * A map between trajectory names and their corresponding data. This allows for - * trajectory data to be looked up with strings during runtime. + * A map between trajectory names and their corresponding data. + * This allows for trajectory data to be looked up with strings during runtime. */ public static final Map ALL_TRAJECTORIES = Map.ofEntries( - Map.entry("DepotToOutpost", DepotToOutpost), Map.entry("DepotToTower", DepotToTower), Map.entry("HubToDepot", HubToDepot), Map.entry("HubToOutpost", HubToOutpost), Map.entry("HubToTower", HubToTower), - Map.entry("LeftBumpPickupFromMidScore", LeftBumpPickupFromMidScore), Map.entry("LeftBumpToDepot", LeftBumpToDepot), Map.entry("LeftBumpToTower", LeftBumpToTower), Map.entry("LeftTrenchToDepot", LeftTrenchToDepot), - Map.entry("OutpostToDepot", OutpostToDepot), Map.entry("OutpostToTower", OutpostToTower), Map.entry("RightBumpPickupFromMidScore", RightBumpPickupFromMidScore), Map.entry("RightBumpToOutpost", RightBumpToOutpost), - Map.entry("RightBumpToTower", RightBumpToTower), Map.entry("RightTrenchToOutpost", RightTrenchToOutpost) + Map.entry("DepotToOutpost", DepotToOutpost), + Map.entry("DepotToTower", DepotToTower), + Map.entry("HubToDepot", HubToDepot), + Map.entry("HubToOutpost", HubToOutpost), + Map.entry("HubToTower", HubToTower), + Map.entry("LeftBumpPickupFromMidScore", LeftBumpPickupFromMidScore), + Map.entry("LeftBumpToDepot", LeftBumpToDepot), + Map.entry("LeftBumpToTower", LeftBumpToTower), + Map.entry("LeftTrenchToDepot", LeftTrenchToDepot), + Map.entry("OutpostToDepot", OutpostToDepot), + Map.entry("OutpostToTower", OutpostToTower), + Map.entry("RightBumpPickupFromMidScore", RightBumpPickupFromMidScore), + Map.entry("RightBumpToOutpost", RightBumpToOutpost), + Map.entry("RightBumpToTower", RightBumpToTower), + Map.entry("RightTrenchToOutpost", RightTrenchToOutpost) ); + /** - * Looks up the ChoreoTraj segment of the given overall ChoreoTraj. WARNING: - * will raise an exception if not called with a valid segment index. + * Looks up the ChoreoTraj segment of the given overall ChoreoTraj. + * WARNING: will raise an exception if not called with a valid segment index. */ - public ChoreoTraj segment(int segment) - { + public ChoreoTraj segment(int segment) { var traj = ChoreoTraj.ALL_TRAJECTORIES.get(this.name + "$" + segment); - if (traj == null) - { + if (traj == null) { throw new NullPointerException("Trajectory " + this.name + " does not have segment #" + segment + "."); } return traj; } - + // If these methods cause errors because you're not using ChoreoLib, // turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. /** - * Load an AutoTrajectory directly from a ChoreoTraj, which may be a segment of - * a larger trajectory. + * Load an AutoTrajectory directly from a ChoreoTraj, which may be a segment of a larger trajectory. */ - public AutoTrajectory asAutoTraj(AutoRoutine routine) - { - if (this.segment.isPresent()) - { + public AutoTrajectory asAutoTraj(AutoRoutine routine) { + if (this.segment.isPresent()) { return routine.trajectory(this.name, this.segment.getAsInt()); } return routine.trajectory(this.name); diff --git a/src/main/java/frc/robot/generated/ChoreoVars.java b/src/main/java/frc/robot/generated/ChoreoVars.java index 404e809..cd8488e 100644 --- a/src/main/java/frc/robot/generated/ChoreoVars.java +++ b/src/main/java/frc/robot/generated/ChoreoVars.java @@ -6,30 +6,26 @@ import edu.wpi.first.units.measure.*; /** - * Generated file containing variables defined in Choreo. DO NOT MODIFY THIS - * FILE YOURSELF; instead, change these values in the Choreo GUI. + * Generated file containing variables defined in Choreo. + * DO NOT MODIFY THIS FILE YOURSELF; instead, change these values + * in the Choreo GUI. */ -public final class ChoreoVars -{ - public static final class Poses - { - public static final Pose2d Depot = new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)); - public static final Pose2d HubStart = new Pose2d(3.616, 4.035, Rotation2d.kZero); - public static final Pose2d LeftBump = new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)); - public static final Pose2d LeftMiddle = new Pose2d(6.619, 5.588, Rotation2d.kZero); - public static final Pose2d LeftTrench = new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)); - public static final Pose2d Outpost = new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)); - public static final Pose2d RightBump = new Pose2d(3.616, 2.511, Rotation2d.kZero); +public final class ChoreoVars { + + public static final class Poses { + public static final Pose2d Depot = new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)); + public static final Pose2d HubStart = new Pose2d(3.616, 4.035, Rotation2d.kZero); + public static final Pose2d LeftBump = new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)); + public static final Pose2d LeftMiddle = new Pose2d(6.619, 5.588, Rotation2d.kZero); + public static final Pose2d LeftTrench = new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)); + public static final Pose2d Outpost = new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)); + public static final Pose2d RightBump = new Pose2d(3.616, 2.511, Rotation2d.kZero); public static final Pose2d RightTrench = new Pose2d(3.616, 0.62, Rotation2d.kZero); - public static final Pose2d TowerLeft = new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)); - public static final Pose2d TowerRight = new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)); + public static final Pose2d TowerLeft = new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)); + public static final Pose2d TowerRight = new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)); - private Poses() - { - } + private Poses() {} } - private ChoreoVars() - { - } -} + private ChoreoVars() {} +} \ No newline at end of file From 3a0ba120644ff3ee80baf49587848c6312fc45b4 Mon Sep 17 00:00:00 2001 From: Collin McIntyre Date: Mon, 23 Mar 2026 20:25:38 -0500 Subject: [PATCH 31/32] updated standard deviasions for auto and teleop --- src/main/java/frc/robot/Constants.java | 6 ++++-- .../java/frc/robot/subsystems/drive/Drive.java | 14 +++++++++++++- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a403326..6f86eca 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -291,9 +291,11 @@ public static class VisionConstants public static final String LEFT_CAMERA_NAME = "limelight-left"; public static final String RIGHT_CAMERA_NAME = "limelight-right"; public static final Distance MAX_DETECTION_RANGE = Meters.of(6.0); - public static final Distance XY_STD_DEV = Meters.of(4); + public static final Distance AUTO_XY_STD_DEV = Meters.of(4); + public static final Distance TELEOP_XY_STD_DEV = Meters.of(0.7); public static final Angle THETA_STD_DEV = Degrees.of(999999); // Trust gyro for heading, not vision - public static final Matrix STD_DEVS = VecBuilder.fill(XY_STD_DEV.in(Meters), XY_STD_DEV.in(Meters), THETA_STD_DEV.in(Degrees)); + public static final Matrix AUTO_STD_DEVS = VecBuilder.fill(AUTO_XY_STD_DEV.in(Meters), AUTO_XY_STD_DEV.in(Meters), THETA_STD_DEV.in(Degrees)); + public static final Matrix TELEOP_STD_DEVS = VecBuilder.fill(TELEOP_XY_STD_DEV.in(Meters), TELEOP_XY_STD_DEV.in(Meters), THETA_STD_DEV.in(Degrees)); // Camera translations public static final Translation3d LEFT_CAMERA_TRANSLATION = new Translation3d(Inches.of(-10.67), Inches.of(-10.67), Inches.of(9.25)); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 8d2af67..38c5630 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -364,6 +364,7 @@ public class DriveVision private final VisionState _leftVision; @Logged private final VisionState _rightVision; + private Matrix _activeVisionStdDevs = null; private static final Pose2d kInvalidVisionPose = new Pose2d(-1.0, -1.0, Rotation2d.kZero); public static final class VisionState @@ -408,13 +409,15 @@ public DriveVision() _leftVision = new VisionState(null, null); _rightVision = new VisionState(null, null); } - setVisionMeasurementStdDevs(VisionConstants.STD_DEVS); + updateVisionMeasurementStdDevs(); } public void update() { if (RobotBase.isSimulation()) return; + updateVisionMeasurementStdDevs(); + var robotHeading = getState().Pose.getRotation(); var robotYawRateDegreesPerSecond = Math.toDegrees(getState().Speeds.omegaRadiansPerSecond); var imuMode = DriverStation.isDisabled() ? ImuMode.SyncInternalImu : ImuMode.InternalImuExternalAssist; @@ -425,6 +428,15 @@ public void update() _rightVision._acceptedVision = processLimelight(_rightVision); } + public void updateVisionMeasurementStdDevs() + { + Matrix desiredVisionStdDevs = DriverStation.isAutonomousEnabled() ? VisionConstants.AUTO_STD_DEVS : VisionConstants.TELEOP_STD_DEVS; + if (_activeVisionStdDevs != desiredVisionStdDevs) + { + setVisionMeasurementStdDevs(desiredVisionStdDevs); + _activeVisionStdDevs = desiredVisionStdDevs; + } + } private void setRobotOrientation(VisionState state, Rotation2d robotHeading, double robotYawRateDegreesPerSecond, ImuMode imuMode) { state._settings.withImuMode(imuMode) From 12511d773a57d03ff82fc7d01a8d249bd5385346 Mon Sep 17 00:00:00 2001 From: github-actions <41898282+github-actions[bot]@users.noreply.github.com> Date: Wed, 25 Mar 2026 02:00:12 +0000 Subject: [PATCH 32/32] Formatted code using spotless. --- .../java/frc/robot/generated/ChoreoTraj.java | 188 +++++------------- .../java/frc/robot/generated/ChoreoVars.java | 40 ++-- .../frc/robot/subsystems/drive/Drive.java | 7 +- 3 files changed, 71 insertions(+), 164 deletions(-) diff --git a/src/main/java/frc/robot/generated/ChoreoTraj.java b/src/main/java/frc/robot/generated/ChoreoTraj.java index cfb27de..775a2f9 100644 --- a/src/main/java/frc/robot/generated/ChoreoTraj.java +++ b/src/main/java/frc/robot/generated/ChoreoTraj.java @@ -1,4 +1,3 @@ - package frc.robot.generated; import edu.wpi.first.math.geometry.Pose2d; @@ -12,165 +11,68 @@ import choreo.auto.AutoTrajectory; /** - * A class containing the name, start pose, end pose, and total time of every Choreo trajectory. - * This prevents your code from referencing deleted or misspelled trajectories, - * and removes the need for JSON parsing to load a trajectory's essential data. - * DO NOT MODIFY THIS FILE YOURSELF! It is automatically generated by Choreo. + * A class containing the name, start pose, end pose, and total time of every + * Choreo trajectory. This prevents your code from referencing deleted or + * misspelled trajectories, and removes the need for JSON parsing to load a + * trajectory's essential data. DO NOT MODIFY THIS FILE YOURSELF! It is + * automatically generated by Choreo. */ -public record ChoreoTraj( - String name, - OptionalInt segment, - double totalTimeSecs, - Pose2d initialPoseBlue, - Pose2d endPoseBlue -) { - public static final ChoreoTraj DepotToOutpost = new ChoreoTraj( - "DepotToOutpost", - OptionalInt.empty(), - 1.70772, - new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), - new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) - ); - public static final ChoreoTraj DepotToTower = new ChoreoTraj( - "DepotToTower", - OptionalInt.empty(), - 1.1836, - new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), - new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)) - ); - public static final ChoreoTraj HubToDepot = new ChoreoTraj( - "HubToDepot", - OptionalInt.empty(), - 1.25742, - new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), - new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) - ); - public static final ChoreoTraj HubToOutpost = new ChoreoTraj( - "HubToOutpost", - OptionalInt.empty(), - 1.67965, - new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), - new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) - ); - public static final ChoreoTraj HubToTower = new ChoreoTraj( - "HubToTower", - OptionalInt.empty(), - 1.10566, - new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), - new Pose2d(1.851, 4.056, Rotation2d.fromRadians(-1.555)) - ); - public static final ChoreoTraj LeftBumpPickupFromMidScore = new ChoreoTraj( - "LeftBumpPickupFromMidScore", - OptionalInt.empty(), - 6.27732, - new Pose2d(3.652, 5.514, Rotation2d.fromRadians(0)), - new Pose2d(1.974, 5.587, Rotation2d.fromRadians(-2.437)) - ); - public static final ChoreoTraj LeftBumpToDepot = new ChoreoTraj( - "LeftBumpToDepot", - OptionalInt.empty(), - 1.11437, - new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), - new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) - ); - public static final ChoreoTraj LeftBumpToTower = new ChoreoTraj( - "LeftBumpToTower", - OptionalInt.empty(), - 1.71082, - new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)), - new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)) - ); - public static final ChoreoTraj LeftTrenchToDepot = new ChoreoTraj( - "LeftTrenchToDepot", - OptionalInt.empty(), - 1.66476, - new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)), - new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) - ); - public static final ChoreoTraj OutpostToDepot = new ChoreoTraj( - "OutpostToDepot", - OptionalInt.empty(), - 1.6899, - new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), - new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)) - ); - public static final ChoreoTraj OutpostToTower = new ChoreoTraj( - "OutpostToTower", - OptionalInt.empty(), - 0.97514, - new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), - new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)) - ); - public static final ChoreoTraj RightBumpPickupFromMidScore = new ChoreoTraj( - "RightBumpPickupFromMidScore", - OptionalInt.empty(), - 6.62509, - new Pose2d(3.625, 2.535, Rotation2d.fromRadians(0)), - new Pose2d(2.456, 2.535, Rotation2d.fromRadians(-1.622)) - ); - public static final ChoreoTraj RightBumpToOutpost = new ChoreoTraj( - "RightBumpToOutpost", - OptionalInt.empty(), - 1.32723, - new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), - new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) - ); - public static final ChoreoTraj RightBumpToTower = new ChoreoTraj( - "RightBumpToTower", - OptionalInt.empty(), - 0.76635, - new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), - new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)) - ); - public static final ChoreoTraj RightTrenchToOutpost = new ChoreoTraj( - "RightTrenchToOutpost", - OptionalInt.empty(), - 1.61166, - new Pose2d(3.616, 0.62, Rotation2d.fromRadians(0)), - new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)) - ); +public record ChoreoTraj(String name, OptionalInt segment, double totalTimeSecs, Pose2d initialPoseBlue, Pose2d endPoseBlue) +{ + public static final ChoreoTraj DepotToOutpost = new ChoreoTraj("DepotToOutpost", OptionalInt.empty(), 1.70772, new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj DepotToTower = new ChoreoTraj("DepotToTower", OptionalInt.empty(), 1.1836, new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); + public static final ChoreoTraj HubToDepot = new ChoreoTraj("HubToDepot", OptionalInt.empty(), 1.25742, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj HubToOutpost = new ChoreoTraj("HubToOutpost", OptionalInt.empty(), 1.67965, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj HubToTower = new ChoreoTraj("HubToTower", OptionalInt.empty(), 1.10566, new Pose2d(3.616, 4.035, Rotation2d.fromRadians(0)), new Pose2d(1.851, 4.056, Rotation2d.fromRadians(-1.555))); + public static final ChoreoTraj LeftBumpPickupFromMidScore = new ChoreoTraj( + "LeftBumpPickupFromMidScore", OptionalInt.empty(), 6.27732, new Pose2d(3.652, 5.514, Rotation2d.fromRadians(0)), new Pose2d(1.974, 5.587, Rotation2d.fromRadians(-2.437)) + ); + public static final ChoreoTraj LeftBumpToDepot = new ChoreoTraj("LeftBumpToDepot", OptionalInt.empty(), 1.11437, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(0)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj LeftBumpToTower = new ChoreoTraj("LeftBumpToTower", OptionalInt.empty(), 1.71082, new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)), new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571))); + public static final ChoreoTraj LeftTrenchToDepot = new ChoreoTraj("LeftTrenchToDepot", OptionalInt.empty(), 1.66476, new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj OutpostToDepot = new ChoreoTraj("OutpostToDepot", OptionalInt.empty(), 1.6899, new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj OutpostToTower = new ChoreoTraj("OutpostToTower", OptionalInt.empty(), 0.97514, new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)), new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571))); + public static final ChoreoTraj RightBumpPickupFromMidScore = new ChoreoTraj( + "RightBumpPickupFromMidScore", OptionalInt.empty(), 6.62509, new Pose2d(3.625, 2.535, Rotation2d.fromRadians(0)), new Pose2d(2.456, 2.535, Rotation2d.fromRadians(-1.622)) + ); + public static final ChoreoTraj RightBumpToOutpost = new ChoreoTraj("RightBumpToOutpost", OptionalInt.empty(), 1.32723, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); + public static final ChoreoTraj RightBumpToTower = new ChoreoTraj("RightBumpToTower", OptionalInt.empty(), 0.76635, new Pose2d(3.616, 2.511, Rotation2d.fromRadians(0)), new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571))); + public static final ChoreoTraj RightTrenchToOutpost = new ChoreoTraj("RightTrenchToOutpost", OptionalInt.empty(), 1.61166, new Pose2d(3.616, 0.62, Rotation2d.fromRadians(0)), new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142))); /** - * A map between trajectory names and their corresponding data. - * This allows for trajectory data to be looked up with strings during runtime. + * A map between trajectory names and their corresponding data. This allows for + * trajectory data to be looked up with strings during runtime. */ public static final Map ALL_TRAJECTORIES = Map.ofEntries( - Map.entry("DepotToOutpost", DepotToOutpost), - Map.entry("DepotToTower", DepotToTower), - Map.entry("HubToDepot", HubToDepot), - Map.entry("HubToOutpost", HubToOutpost), - Map.entry("HubToTower", HubToTower), - Map.entry("LeftBumpPickupFromMidScore", LeftBumpPickupFromMidScore), - Map.entry("LeftBumpToDepot", LeftBumpToDepot), - Map.entry("LeftBumpToTower", LeftBumpToTower), - Map.entry("LeftTrenchToDepot", LeftTrenchToDepot), - Map.entry("OutpostToDepot", OutpostToDepot), - Map.entry("OutpostToTower", OutpostToTower), - Map.entry("RightBumpPickupFromMidScore", RightBumpPickupFromMidScore), - Map.entry("RightBumpToOutpost", RightBumpToOutpost), - Map.entry("RightBumpToTower", RightBumpToTower), - Map.entry("RightTrenchToOutpost", RightTrenchToOutpost) + Map.entry("DepotToOutpost", DepotToOutpost), Map.entry("DepotToTower", DepotToTower), Map.entry("HubToDepot", HubToDepot), Map.entry("HubToOutpost", HubToOutpost), Map.entry("HubToTower", HubToTower), + Map.entry("LeftBumpPickupFromMidScore", LeftBumpPickupFromMidScore), Map.entry("LeftBumpToDepot", LeftBumpToDepot), Map.entry("LeftBumpToTower", LeftBumpToTower), Map.entry("LeftTrenchToDepot", LeftTrenchToDepot), + Map.entry("OutpostToDepot", OutpostToDepot), Map.entry("OutpostToTower", OutpostToTower), Map.entry("RightBumpPickupFromMidScore", RightBumpPickupFromMidScore), Map.entry("RightBumpToOutpost", RightBumpToOutpost), + Map.entry("RightBumpToTower", RightBumpToTower), Map.entry("RightTrenchToOutpost", RightTrenchToOutpost) ); - /** - * Looks up the ChoreoTraj segment of the given overall ChoreoTraj. - * WARNING: will raise an exception if not called with a valid segment index. + * Looks up the ChoreoTraj segment of the given overall ChoreoTraj. WARNING: + * will raise an exception if not called with a valid segment index. */ - public ChoreoTraj segment(int segment) { + public ChoreoTraj segment(int segment) + { var traj = ChoreoTraj.ALL_TRAJECTORIES.get(this.name + "$" + segment); - if (traj == null) { + if (traj == null) + { throw new NullPointerException("Trajectory " + this.name + " does not have segment #" + segment + "."); } return traj; } - + // If these methods cause errors because you're not using ChoreoLib, // turn off "Include ChoreoLib-specific Helpers" in Choreo's codegen settings. /** - * Load an AutoTrajectory directly from a ChoreoTraj, which may be a segment of a larger trajectory. + * Load an AutoTrajectory directly from a ChoreoTraj, which may be a segment of + * a larger trajectory. */ - public AutoTrajectory asAutoTraj(AutoRoutine routine) { - if (this.segment.isPresent()) { + public AutoTrajectory asAutoTraj(AutoRoutine routine) + { + if (this.segment.isPresent()) + { return routine.trajectory(this.name, this.segment.getAsInt()); } return routine.trajectory(this.name); diff --git a/src/main/java/frc/robot/generated/ChoreoVars.java b/src/main/java/frc/robot/generated/ChoreoVars.java index cd8488e..404e809 100644 --- a/src/main/java/frc/robot/generated/ChoreoVars.java +++ b/src/main/java/frc/robot/generated/ChoreoVars.java @@ -6,26 +6,30 @@ import edu.wpi.first.units.measure.*; /** - * Generated file containing variables defined in Choreo. - * DO NOT MODIFY THIS FILE YOURSELF; instead, change these values - * in the Choreo GUI. + * Generated file containing variables defined in Choreo. DO NOT MODIFY THIS + * FILE YOURSELF; instead, change these values in the Choreo GUI. */ -public final class ChoreoVars { - - public static final class Poses { - public static final Pose2d Depot = new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)); - public static final Pose2d HubStart = new Pose2d(3.616, 4.035, Rotation2d.kZero); - public static final Pose2d LeftBump = new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)); - public static final Pose2d LeftMiddle = new Pose2d(6.619, 5.588, Rotation2d.kZero); - public static final Pose2d LeftTrench = new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)); - public static final Pose2d Outpost = new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)); - public static final Pose2d RightBump = new Pose2d(3.616, 2.511, Rotation2d.kZero); +public final class ChoreoVars +{ + public static final class Poses + { + public static final Pose2d Depot = new Pose2d(1.15, 5.963, Rotation2d.fromRadians(3.142)); + public static final Pose2d HubStart = new Pose2d(3.616, 4.035, Rotation2d.kZero); + public static final Pose2d LeftBump = new Pose2d(3.616, 5.559, Rotation2d.fromRadians(3.142)); + public static final Pose2d LeftMiddle = new Pose2d(6.619, 5.588, Rotation2d.kZero); + public static final Pose2d LeftTrench = new Pose2d(3.616, 7.45, Rotation2d.fromRadians(3.142)); + public static final Pose2d Outpost = new Pose2d(0.654, 0.687, Rotation2d.fromRadians(3.142)); + public static final Pose2d RightBump = new Pose2d(3.616, 2.511, Rotation2d.kZero); public static final Pose2d RightTrench = new Pose2d(3.616, 0.62, Rotation2d.kZero); - public static final Pose2d TowerLeft = new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)); - public static final Pose2d TowerRight = new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)); + public static final Pose2d TowerLeft = new Pose2d(0.799, 4.895, Rotation2d.fromRadians(1.571)); + public static final Pose2d TowerRight = new Pose2d(1.354, 2.641, Rotation2d.fromRadians(-1.571)); - private Poses() {} + private Poses() + { + } } - private ChoreoVars() {} -} \ No newline at end of file + private ChoreoVars() + { + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 38c5630..2dde4a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -364,8 +364,8 @@ public class DriveVision private final VisionState _leftVision; @Logged private final VisionState _rightVision; - private Matrix _activeVisionStdDevs = null; - private static final Pose2d kInvalidVisionPose = new Pose2d(-1.0, -1.0, Rotation2d.kZero); + private Matrix _activeVisionStdDevs = null; + private static final Pose2d kInvalidVisionPose = new Pose2d(-1.0, -1.0, Rotation2d.kZero); public static final class VisionState { @@ -430,13 +430,14 @@ public void update() public void updateVisionMeasurementStdDevs() { - Matrix desiredVisionStdDevs = DriverStation.isAutonomousEnabled() ? VisionConstants.AUTO_STD_DEVS : VisionConstants.TELEOP_STD_DEVS; + Matrix desiredVisionStdDevs = DriverStation.isAutonomousEnabled() ? VisionConstants.AUTO_STD_DEVS : VisionConstants.TELEOP_STD_DEVS; if (_activeVisionStdDevs != desiredVisionStdDevs) { setVisionMeasurementStdDevs(desiredVisionStdDevs); _activeVisionStdDevs = desiredVisionStdDevs; } } + private void setRobotOrientation(VisionState state, Rotation2d robotHeading, double robotYawRateDegreesPerSecond, ImuMode imuMode) { state._settings.withImuMode(imuMode)